<commit_before>/** \copyright
 * Copyright (c) 2014, Balazs Racz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * \file TractionDefs.hxx
 *
 * Static definitions for implementations of the NMRAnet Traction and Traction
 * Proxy protocols.
 *
 * @author Balazs Racz
 * @date 5 May 2014
 */

#ifndef _NMRANET_TRACTIONDEFS_HXX_
#define _NMRANET_TRACTIONDEFS_HXX_

#include <cmath>
#include <stdint.h>

#include "nmranet/Velocity.hxx"
#include "nmranet/Payload.hxx"

namespace nmranet {

typedef Velocity SpeedType;

/** Parses a SpeedType value from an unaligned memory address, typically from
 * the input buffer. */
SpeedType fp16_to_speed(const void *fp16);

/** Renders a SpeedType value to an unaligned memory address, typically to the
 * output buffer.
 *
 * @param speed is the speed to write
 * @param fp16 is an unaligned two-byte location to write the float16 value
 * to.*/
void speed_to_fp16(SpeedType speed, void *fp16);

/** @returns NAN as speed. */
inline SpeedType nan_to_speed() {
    SpeedType s;
    s.set_wire(0xFFFFU);
    return s;
}

/// Static constants and helper functions for the Traciton protocol family.
struct TractionDefs {
    /// This event should be produced by train nodes.
    static const uint64_t IS_TRAIN_EVENT = 0x0101000000000303ULL;
    /// This event should be produced by traction proxy nodes.
    static const uint64_t IS_PROXY_EVENT = 0x0101000000000304ULL;
    /// Producing this event causes all operations to stop (usually by turning
    /// off the command station power output).
    /// @TODO : there is a mistake in this constant. It should start with
    /// 0100 by the standard (instead of 0101).
    static const uint64_t EMERGENCY_STOP_EVENT = 0x010000000000FFFFULL;
    /// Producing this event resumes all operations (usually by turning power
    /// back on).
    static const uint64_t CLEAR_EMERGENCY_STOP_EVENT = 0x010000000000FFFEULL;

    /// Node ID space allocated for DC blocks.
    static const uint64_t NODE_ID_DC_BLOCK = 0x060000000000ULL;
    /// Node ID space allocated for DCC locomotives.
    static const uint64_t NODE_ID_DCC = 0x060100000000ULL;
    /// Node ID space allocated for TMCC protocol.
    static const uint64_t NODE_ID_TMCC = 0x060200000000ULL;
    /// Node ID space allocated for the Marklin-Motorola protocol.
    static const uint64_t NODE_ID_MARKLIN_MOTOROLA = 0x060300000000ULL;
    /// Node ID space allocated for the MTH DCS protocol.
    static const uint64_t NODE_ID_MTH_DCS = 0x060400000000ULL;

    enum {
        // Byte 0 of request commands.
        REQ_SET_SPEED = 0x00,
        REQ_SET_FN = 0x01,
        REQ_EMERGENCY_STOP = 0x02,

        REQ_QUERY_SPEED = 0x10,
        REQ_QUERY_FN = 0x11,

        REQ_CONTROLLER_CONFIG = 0x20,
        REQ_CONSIST_CONFIG = 0x30,
        REQ_TRACTION_MGMT = 0x40,

        // Byte 1 of REQ_CONTROLLER_CONFIG command
        CTRLREQ_ASSIGN_CONTROLLER = 0x01,
        CTRLREQ_RELEASE_CONTROLLER = 0x02,
        CTRLREQ_QUERY_CONTROLLER = 0x03,
        CTRLREQ_NOTIFY_CONTROLLER_CHANGED = 0x04,

        // Byte 1 of REQ_CONSIST_CONFIG command
        CNSTREQ_ATTACH_NODE = 0x01,
        CNSTREQ_DETACH_NODE = 0x02,
        CNSTREQ_QUERY_NODES = 0x03,

        // Byte 1 of REQ_TRACTION_MGMT command
        MGMTREQ_RESERVE = 0x01,
        MGMTREQ_RELEASE = 0x02,
        // Byte 0 of response commands
        RESP_QUERY_SPEED = REQ_QUERY_SPEED,
        RESP_QUERY_FN = REQ_QUERY_FN,
        RESP_CONTROLLER_CONFIG = REQ_CONTROLLER_CONFIG,
        RESP_CONSISST_CONFIG = REQ_CONSIST_CONFIG,
        RESP_TRACTION_MGMT = REQ_TRACTION_MGMT,

        // Byte 1 of Controller Configuration response
        CTRLRESP_ASSIGN_CONTROLLER = CTRLREQ_ASSIGN_CONTROLLER,
        CTRLRESP_QUERY_CONTROLLER = CTRLREQ_QUERY_CONTROLLER,
        CTRLRESP_NOTIFY_CONTROLLER_CHANGED = CTRLREQ_NOTIFY_CONTROLLER_CHANGED,

        // Byte 2 of assign controller response
        CTRLRESP_ASSIGN_ERROR_TRAIN = 0x02,
        CTRLRESP_ASSIGN_ERROR_CONTROLLER = 0x01,

        // Byte 1 of Consist Configuration response
        CNSTRESP_ATTACH_NODE = CNSTREQ_ATTACH_NODE,
        CNSTRESP_DETACH_NODE = CNSTREQ_DETACH_NODE,
        CNSTRESP_QUERY_NODES = CNSTREQ_QUERY_NODES,

        // Byte 1 of Traction Management replies
        MGMTRESP_RESERVE = MGMTREQ_RESERVE,

        PROXYREQ_ALLOCATE = 0x01,
        PROXYREQ_ATTACH = 0x02,
        PROXYREQ_DETACH = 0x03,
        PROXYREQ_MANAGE = 0x80,

        PROXYRESP_ALLOCATE = PROXYREQ_ALLOCATE,
        PROXYRESP_ATTACH = PROXYREQ_ATTACH,
        PROXYRESP_MANAGE = PROXYREQ_MANAGE,

        // byte 1 of PROXYREQ_MANAGE commands
        PROXYREQ_MANAGE_RESERVE = 0x01,
        PROXYREQ_MANAGE_RELEASE = 0x02,

        PROXYRESP_MANAGE_RESERVE_REPLY = 0x01,

        // Legacy Technology IDs from the GenTractionProxyWN.
        PROXYTYPE_DCC = 1,
        PROXYTYPE_DC = 2,
        PROXYTYPE_MARKLIN_DIGITAL = 3,
        PROXYTYPE_MARKLIN_DELTA = 4,
        PROXYTYPE_MARKLIN_MFX = 5,
        PROXYTYPE_SELECTRIX = 6,
        PROXYTYPE_MTH_DCS = 7,
        PROXYTYPE_LIONEL_TMCC = 8,

        /** This is the memory space number for accessing an NMRA DCC
         * locomotive's functions via the memory config protocol. */
        FUNCTION_CONFIG_MEMORY_SPACE = 0xF9,
        // These declare the configuration space layout. They are always the
        // third byte of the address in the configuration memory space, aka
        // address = type << 16 + offset.
        /** F0-F28 are at offset 0 to 28 here. */
        FNCONFIG_FN = 0x0,
        /** Binary State Control Instruction long form. Offset 0 to 32767 */
        FNCONFIG_BINARYSTATE_LONG = 0x1,
        /** Binary State Control Instruction short form. Offset 0 to 127 */
        FNCONFIG_BINARYSTATE_SHORT = 0x2,
        /** Analog outputs, defined by NMRA DCC WG Topic 9910241. Offset 0-255,
         * values 0-255 each. */
        FNCONFIG_ANALOG_OUTPUT = 0x3
    };

    static Payload speed_set_payload(Velocity v) {
        Payload p(3, 0);
        p[0] = REQ_SET_SPEED;
        speed_to_fp16(v, &p[1]);
        return p;
    }

    static Payload speed_get_payload() {
        Payload p(1, 0);
        p[0] = REQ_QUERY_SPEED;
        return p;
    }

    /** Parses the response payload of a GET_SPEED packet.
     * @returns true if the last_set_speed value was present and non-NaN.
     * @param p is the response payload.
     * @param v is the velocity that will be set to the speed value. */
    static bool speed_get_parse_last(const Payload &p, Velocity *v)
    {
        if (p.size() < 3)
        {
            return false;
        }
        *v = fp16_to_speed(p.data() + 1);
        if (std::isnan(v->speed()))
        {
            return false;
        }
        return true;
    }

    static Payload fn_set_payload(unsigned address, uint16_t value) {
        Payload p(6, 0);
        p[0] = REQ_SET_FN;
        p[1] = (address >> 16) & 0xff;
        p[2] = (address >> 8) & 0xff;
        p[3] = address & 0xff;
        p[4] = (value >> 8) & 0xff;
        p[5] = value & 0xff;
        return p;
    }

    static Payload fn_get_payload(unsigned address) {
        Payload p(4, 0);
        p[0] = REQ_QUERY_FN;
        p[1] = (address >> 16) & 0xff;
        p[2] = (address >> 8) & 0xff;
        p[3] = address & 0xff;
        return p;
    }

    /** Parses the response payload of a GET_FN packet.
     * @returns true if there is a valid function value.
     * @param p is the response payload.
     * @param value will be set to the output value. */
    static bool fn_get_parse(const Payload &p, uint16_t *value)
    {
        if (p.size() < 6)
        {
            return false;
        }
        *value = (((uint16_t)p[4]) << 8) | p[5];
        return true;
    }
};

}  // namespace nmranet

#endif //_NMRANET_TRACTIONDEFS_HXX_
<commit_msg>Adds assign and release controller generation helpers.<commit_after>/** \copyright
 * Copyright (c) 2014, Balazs Racz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * \file TractionDefs.hxx
 *
 * Static definitions for implementations of the NMRAnet Traction and Traction
 * Proxy protocols.
 *
 * @author Balazs Racz
 * @date 5 May 2014
 */

#ifndef _NMRANET_TRACTIONDEFS_HXX_
#define _NMRANET_TRACTIONDEFS_HXX_

#include <cmath>
#include <stdint.h>

#include "nmranet/Velocity.hxx"
#include "nmranet/Payload.hxx"
#include "nmranet/If.hxx"

namespace nmranet {

typedef Velocity SpeedType;

/** Parses a SpeedType value from an unaligned memory address, typically from
 * the input buffer. */
SpeedType fp16_to_speed(const void *fp16);

/** Renders a SpeedType value to an unaligned memory address, typically to the
 * output buffer.
 *
 * @param speed is the speed to write
 * @param fp16 is an unaligned two-byte location to write the float16 value
 * to.*/
void speed_to_fp16(SpeedType speed, void *fp16);

/** @returns NAN as speed. */
inline SpeedType nan_to_speed() {
    SpeedType s;
    s.set_wire(0xFFFFU);
    return s;
}

/// Static constants and helper functions for the Traciton protocol family.
struct TractionDefs {
    /// This event should be produced by train nodes.
    static const uint64_t IS_TRAIN_EVENT = 0x0101000000000303ULL;
    /// This event should be produced by traction proxy nodes.
    static const uint64_t IS_PROXY_EVENT = 0x0101000000000304ULL;
    /// Producing this event causes all operations to stop (usually by turning
    /// off the command station power output).
    /// @TODO : there is a mistake in this constant. It should start with
    /// 0100 by the standard (instead of 0101).
    static const uint64_t EMERGENCY_STOP_EVENT = 0x010000000000FFFFULL;
    /// Producing this event resumes all operations (usually by turning power
    /// back on).
    static const uint64_t CLEAR_EMERGENCY_STOP_EVENT = 0x010000000000FFFEULL;

    /// Node ID space allocated for DC blocks.
    static const uint64_t NODE_ID_DC_BLOCK = 0x060000000000ULL;
    /// Node ID space allocated for DCC locomotives.
    static const uint64_t NODE_ID_DCC = 0x060100000000ULL;
    /// Node ID space allocated for TMCC protocol.
    static const uint64_t NODE_ID_TMCC = 0x060200000000ULL;
    /// Node ID space allocated for the Marklin-Motorola protocol.
    static const uint64_t NODE_ID_MARKLIN_MOTOROLA = 0x060300000000ULL;
    /// Node ID space allocated for the MTH DCS protocol.
    static const uint64_t NODE_ID_MTH_DCS = 0x060400000000ULL;

    enum {
        // Byte 0 of request commands.
        REQ_SET_SPEED = 0x00,
        REQ_SET_FN = 0x01,
        REQ_EMERGENCY_STOP = 0x02,

        REQ_QUERY_SPEED = 0x10,
        REQ_QUERY_FN = 0x11,

        REQ_CONTROLLER_CONFIG = 0x20,
        REQ_CONSIST_CONFIG = 0x30,
        REQ_TRACTION_MGMT = 0x40,

        // Byte 1 of REQ_CONTROLLER_CONFIG command
        CTRLREQ_ASSIGN_CONTROLLER = 0x01,
        CTRLREQ_RELEASE_CONTROLLER = 0x02,
        CTRLREQ_QUERY_CONTROLLER = 0x03,
        CTRLREQ_NOTIFY_CONTROLLER_CHANGED = 0x04,

        // Byte 1 of REQ_CONSIST_CONFIG command
        CNSTREQ_ATTACH_NODE = 0x01,
        CNSTREQ_DETACH_NODE = 0x02,
        CNSTREQ_QUERY_NODES = 0x03,

        // Byte 1 of REQ_TRACTION_MGMT command
        MGMTREQ_RESERVE = 0x01,
        MGMTREQ_RELEASE = 0x02,
        // Byte 0 of response commands
        RESP_QUERY_SPEED = REQ_QUERY_SPEED,
        RESP_QUERY_FN = REQ_QUERY_FN,
        RESP_CONTROLLER_CONFIG = REQ_CONTROLLER_CONFIG,
        RESP_CONSISST_CONFIG = REQ_CONSIST_CONFIG,
        RESP_TRACTION_MGMT = REQ_TRACTION_MGMT,

        // Byte 1 of Controller Configuration response
        CTRLRESP_ASSIGN_CONTROLLER = CTRLREQ_ASSIGN_CONTROLLER,
        CTRLRESP_QUERY_CONTROLLER = CTRLREQ_QUERY_CONTROLLER,
        CTRLRESP_NOTIFY_CONTROLLER_CHANGED = CTRLREQ_NOTIFY_CONTROLLER_CHANGED,

        // Byte 2 of assign controller response
        CTRLRESP_ASSIGN_ERROR_TRAIN = 0x02,
        CTRLRESP_ASSIGN_ERROR_CONTROLLER = 0x01,

        // Byte 1 of Consist Configuration response
        CNSTRESP_ATTACH_NODE = CNSTREQ_ATTACH_NODE,
        CNSTRESP_DETACH_NODE = CNSTREQ_DETACH_NODE,
        CNSTRESP_QUERY_NODES = CNSTREQ_QUERY_NODES,

        // Byte 1 of Traction Management replies
        MGMTRESP_RESERVE = MGMTREQ_RESERVE,

        PROXYREQ_ALLOCATE = 0x01,
        PROXYREQ_ATTACH = 0x02,
        PROXYREQ_DETACH = 0x03,
        PROXYREQ_MANAGE = 0x80,

        PROXYRESP_ALLOCATE = PROXYREQ_ALLOCATE,
        PROXYRESP_ATTACH = PROXYREQ_ATTACH,
        PROXYRESP_MANAGE = PROXYREQ_MANAGE,

        // byte 1 of PROXYREQ_MANAGE commands
        PROXYREQ_MANAGE_RESERVE = 0x01,
        PROXYREQ_MANAGE_RELEASE = 0x02,

        PROXYRESP_MANAGE_RESERVE_REPLY = 0x01,

        // Legacy Technology IDs from the GenTractionProxyWN.
        PROXYTYPE_DCC = 1,
        PROXYTYPE_DC = 2,
        PROXYTYPE_MARKLIN_DIGITAL = 3,
        PROXYTYPE_MARKLIN_DELTA = 4,
        PROXYTYPE_MARKLIN_MFX = 5,
        PROXYTYPE_SELECTRIX = 6,
        PROXYTYPE_MTH_DCS = 7,
        PROXYTYPE_LIONEL_TMCC = 8,

        /** This is the memory space number for accessing an NMRA DCC
         * locomotive's functions via the memory config protocol. */
        FUNCTION_CONFIG_MEMORY_SPACE = 0xF9,
        // These declare the configuration space layout. They are always the
        // third byte of the address in the configuration memory space, aka
        // address = type << 16 + offset.
        /** F0-F28 are at offset 0 to 28 here. */
        FNCONFIG_FN = 0x0,
        /** Binary State Control Instruction long form. Offset 0 to 32767 */
        FNCONFIG_BINARYSTATE_LONG = 0x1,
        /** Binary State Control Instruction short form. Offset 0 to 127 */
        FNCONFIG_BINARYSTATE_SHORT = 0x2,
        /** Analog outputs, defined by NMRA DCC WG Topic 9910241. Offset 0-255,
         * values 0-255 each. */
        FNCONFIG_ANALOG_OUTPUT = 0x3
    };

    static Payload speed_set_payload(Velocity v) {
        Payload p(3, 0);
        p[0] = REQ_SET_SPEED;
        speed_to_fp16(v, &p[1]);
        return p;
    }

    static Payload speed_get_payload() {
        Payload p(1, 0);
        p[0] = REQ_QUERY_SPEED;
        return p;
    }

    /** Parses the response payload of a GET_SPEED packet.
     * @returns true if the last_set_speed value was present and non-NaN.
     * @param p is the response payload.
     * @param v is the velocity that will be set to the speed value. */
    static bool speed_get_parse_last(const Payload &p, Velocity *v)
    {
        if (p.size() < 3)
        {
            return false;
        }
        *v = fp16_to_speed(p.data() + 1);
        if (std::isnan(v->speed()))
        {
            return false;
        }
        return true;
    }

    static Payload fn_set_payload(unsigned address, uint16_t value) {
        Payload p(6, 0);
        p[0] = REQ_SET_FN;
        p[1] = (address >> 16) & 0xff;
        p[2] = (address >> 8) & 0xff;
        p[3] = address & 0xff;
        p[4] = (value >> 8) & 0xff;
        p[5] = value & 0xff;
        return p;
    }

    static Payload fn_get_payload(unsigned address) {
        Payload p(4, 0);
        p[0] = REQ_QUERY_FN;
        p[1] = (address >> 16) & 0xff;
        p[2] = (address >> 8) & 0xff;
        p[3] = address & 0xff;
        return p;
    }

    /** Parses the response payload of a GET_FN packet.
     * @returns true if there is a valid function value.
     * @param p is the response payload.
     * @param value will be set to the output value. */
    static bool fn_get_parse(const Payload &p, uint16_t *value)
    {
        if (p.size() < 6)
        {
            return false;
        }
        *value = (((uint16_t)p[4]) << 8) | p[5];
        return true;
    }

    static Payload assign_controller_payload(Node* ctrl) {
        Payload p(9, 0);
        p[0] = REQ_CONTROLLER_CONFIG;
        p[1] = CTRLREQ_ASSIGN_CONTROLLER;
        p[2] = 0;
        node_id_to_data(ctrl->node_id(), &p[3]);
        return p;
    }

    static Payload release_controller_payload(Node* ctrl) {
        Payload p(9, 0);
        p[0] = REQ_CONTROLLER_CONFIG;
        p[1] = CTRLREQ_RELEASE_CONTROLLER;
        p[2] = 0;
        node_id_to_data(ctrl->node_id(), &p[3]);
        return p;
    }
};

}  // namespace nmranet

#endif //_NMRANET_TRACTIONDEFS_HXX_
<|endoftext|>


<commit_before>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "blink.h"
#include "usbconfig.h"
#include "printf.h"
#include "gitsha1.h"

#include "bicycle.h"
#include "kalman.h"
#include "parameters.h"

#include "analog.h"
#include "encoder.h"
#include <boost/math/constants/constants.hpp>

#include "printstate.h"
#include "packet/serialize.h"
#include "packet/framing.h"

#include <type_traits>

namespace {
    using bicycle_t = model::Bicycle;
    using kalman_t = observer::Kalman<bicycle_t>;

    const float fs = 200; // sample rate [Hz]
    const float dt = 1.0/fs; // sample time [s]
    const float v0 = 5.0; // forward speed [m/s]

    /* Kalman filter variance values */
    const float sigma0 = 1 * constants::as_radians; // yaw angle measurement noise variance
    const float sigma1 = 0.008 * constants::as_radians; // steer angle measurement noise variance

    bicycle_t::input_t u; /* roll torque, steer torque */
    bicycle_t::state_t x; /* yaw angle, roll angle, steer angle, roll rate, steer rate */
    bicycle_t::output_t z; /* yaw angle, steer angle */

    /* sensors */
    Analog analog;
    Encoder encoder(&GPTD5, /* CH1, CH2 connected to PA0, PA1 and NOT enabled by board.h */
            {PAL_NOLINE, /* no index channel */
             152000, /* counts per revolution */
             EncoderConfig::filter_t::CAPTURE_64}); /* 64 * 42 MHz (TIM3 on APB1) = 1.52 us
                                                     * for valid edge */

     const float max_kistler_torque = 25.0f; /* maximum measured steer torque */
     /*
      * The voltage output of the Kistler torque sensor is ±10V. With the 12-bit ADC,
      * resolution for LSB is 4.88 mV/bit or 12.2 mNm/bit.
      */
     const float max_kollmorgen_torque = 10.0f; /* max torque at 1.00 Arms/V */

    const DACConfig dac1cfg1 = {
        .init       = 2047U, // max value is 4095 (12-bit)
        .datamode   = DAC_DHRM_12BIT_RIGHT
    };

    model::real_t wrap_angle(model::real_t angle) {
        model::real_t a = std::fmod(angle, boost::math::constants::two_pi<model::real_t>());
        if (a >= boost::math::constants::pi<model::real_t>()) {
            a -= boost::math::constants::two_pi<model::real_t>();
        }
        if (a < -boost::math::constants::pi<model::real_t>()) {
            a += boost::math::constants::two_pi<model::real_t>();
        }
        return a;
    }

     float rad_to_deg(float angle) {
         return angle * 360 / boost::math::constants::two_pi<float>();
     }

     std::array<uint8_t, BicyclePose_size> encode_buffer;
     std::array<uint8_t, BicyclePose_size + 1> frame_buffer;
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initialize a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activate the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the blink thread and print state monitor */
    chBlinkThreadCreateStatic();
    /*
     * Use LINE_TIM4_CH2 (PB7, EXT1-15, J4-B) as a button by
     * connecting/disconnecting it to ground.
     * */
    palSetLineMode(LINE_TIM4_CH2, PAL_MODE_INPUT_PULLUP);
    enablePrintStateMonitor(LINE_TIM4_CH2);

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder.start();
    analog.start(1000); /* trigger ADC conversion at 1 kHz */

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
     * The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
     * and must be changed to use as analog output.
     */
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(&DACD1, &dac1cfg1);

    /* initialize bicycle model and states */
    model::Bicycle bicycle(v0, dt);
    x.setZero();

    /* initialize Kalman filter */
    kalman_t kalman(bicycle,
            parameters::defaultvalue::kalman::Q(dt), /* process noise cov */
            (kalman_t::measurement_noise_covariance_t() <<
             sigma0,      0,
                  0, sigma1).finished(),
            bicycle_t::state_t::Zero(), /* set initial state estimate to zero */
            std::pow(x[1]/2, 2) * bicycle_t::state_matrix_t::Identity()); /* error cov */

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    rtcnt_t kalman_update_time = 0;
    bool print_version_string = true;
    u.setZero(); /* set both roll and steer torques to zero */
    BicyclePose pose = BicyclePose_init_zero;
    while (true) {
        u[1] = static_cast<float>(analog.get_adc12()*2.0f*max_kistler_torque/4096 -
                max_kistler_torque);
        //float motor_torque = static_cast<float>(
        //        analog.get_adc13()*2.0f*max_kollmorgen_torque/4096 -
        //        max_kollmorgen_torque);

        /* set measurement vector */
        z[0] = wrap_angle(x[0]); /* yaw angle, just use previous state value */

        /*
         * Steer angle, read from encoder (enccnt_t is uint32_t)
         * Convert angle from enccnt_t (unsigned) to corresponding signed type and use negative
         * values for any count over half a revolution.
         */
        auto position = static_cast<std::make_signed<enccnt_t>::type>(encoder.count());
        auto rev = static_cast<std::make_signed<enccnt_t>::type>(encoder.config().counts_per_rev);
        if (position > rev / 2) {
            position -= rev;
        }
        z[1] = static_cast<float>(position) / rev *
                boost::math::constants::two_pi<float>();

        /* observer time/measurement update (~80 us with real_t = float) */
        kalman_update_time = chSysGetRealtimeCounterX();
        kalman.time_update(u);
        kalman.measurement_update(z);
        x = kalman.x();
        x[0] = wrap_angle(x[0]);
        x[1] = wrap_angle(x[1]);
        x[2] = wrap_angle(x[2]);
        kalman_update_time = chSysGetRealtimeCounterX() - kalman_update_time;

        /* generate an example torque output for testing */
        float torque = 10.0f * std::sin(
                boost::math::constants::two_pi<float>() *
                ST2S(static_cast<float>(chVTGetSystemTime())));
        dacsample_t aout = static_cast<dacsample_t>(
                (torque/21.0f * 2048) + 2048); /* reduce output to half of full range */
        dacPutChannelX(&DACD1, 0, aout);

        uint8_t bytes_written;
        pose.x = 1;
        pose.y = 2;
        pose.yaw = x[0];
        pose.roll = x[1];
        pose.steer = x[2];
        bytes_written = packet::serialize::encode_bicycle_pose(pose,
                encode_buffer.data(), encode_buffer.size());
        packet::framing::stuff(encode_buffer.data(), frame_buffer.data(), bytes_written);

        printst_t s = getPrintState();
        if (s == printst_t::VERSION) {
            if (print_version_string) {
                printf("Running firmware version %.7s\r\n", g_GITSHA1);
                print_version_string = false;
            }
        } else if (s == printst_t::NORMAL) {
            packet::framing::unstuff(frame_buffer.data(), encode_buffer.data(), encode_buffer.size());
            if (packet::serialize::decode_bicycle_pose(encode_buffer.data(), &pose, bytes_written)) {
                printf("bicycle pose:\r\n\
                        \tx:\t%0.3f\r\n\
                        \ty:\t%0.3f\r\n\
                        \tyaw:\t%0.3f\r\n\
                        \troll:\t%0.3f\r\n\
                        \tsteer:\t%0.3f\r\n",
                        pose.x, pose.y, pose.yaw, pose.roll, pose.steer);
            }
            //printf("encoder count:\t%u\r\n", encoder.count());
            //printf("sensors:\t%0.3f\t%0.3f\t%0.3f\t%0.3f\r\n",
            //        u[1], motor_torque, rad_to_deg(z[0]), rad_to_deg(z[1]));
            //printf("state:\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\r\n",
            //        rad_to_deg(x[0]), rad_to_deg(x[1]), rad_to_deg(x[2]), rad_to_deg(x[3]), rad_to_deg(x[4]));
            //printf("kalman update time: %U us\r\n",
            //        RTC2US(STM32_SYSCLK, kalman_update_time));
        } else if (s == printst_t::NONE) {
            /* reset printing of version string */
            print_version_string = true;
        }
        chThdSleepMilliseconds(static_cast<systime_t>(1000*dt));
    }
}
<commit_msg>Print units for bicycle pose fields<commit_after>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "blink.h"
#include "usbconfig.h"
#include "printf.h"
#include "gitsha1.h"

#include "bicycle.h"
#include "kalman.h"
#include "parameters.h"

#include "analog.h"
#include "encoder.h"
#include <boost/math/constants/constants.hpp>

#include "printstate.h"
#include "packet/serialize.h"
#include "packet/framing.h"

#include <type_traits>

namespace {
    using bicycle_t = model::Bicycle;
    using kalman_t = observer::Kalman<bicycle_t>;

    const float fs = 200; // sample rate [Hz]
    const float dt = 1.0/fs; // sample time [s]
    const float v0 = 5.0; // forward speed [m/s]

    /* Kalman filter variance values */
    const float sigma0 = 1 * constants::as_radians; // yaw angle measurement noise variance
    const float sigma1 = 0.008 * constants::as_radians; // steer angle measurement noise variance

    bicycle_t::input_t u; /* roll torque, steer torque */
    bicycle_t::state_t x; /* yaw angle, roll angle, steer angle, roll rate, steer rate */
    bicycle_t::output_t z; /* yaw angle, steer angle */

    /* sensors */
    Analog analog;
    Encoder encoder(&GPTD5, /* CH1, CH2 connected to PA0, PA1 and NOT enabled by board.h */
            {PAL_NOLINE, /* no index channel */
             152000, /* counts per revolution */
             EncoderConfig::filter_t::CAPTURE_64}); /* 64 * 42 MHz (TIM3 on APB1) = 1.52 us
                                                     * for valid edge */

     const float max_kistler_torque = 25.0f; /* maximum measured steer torque */
     /*
      * The voltage output of the Kistler torque sensor is ±10V. With the 12-bit ADC,
      * resolution for LSB is 4.88 mV/bit or 12.2 mNm/bit.
      */
     const float max_kollmorgen_torque = 10.0f; /* max torque at 1.00 Arms/V */

    const DACConfig dac1cfg1 = {
        .init       = 2047U, // max value is 4095 (12-bit)
        .datamode   = DAC_DHRM_12BIT_RIGHT
    };

    model::real_t wrap_angle(model::real_t angle) {
        model::real_t a = std::fmod(angle, boost::math::constants::two_pi<model::real_t>());
        if (a >= boost::math::constants::pi<model::real_t>()) {
            a -= boost::math::constants::two_pi<model::real_t>();
        }
        if (a < -boost::math::constants::pi<model::real_t>()) {
            a += boost::math::constants::two_pi<model::real_t>();
        }
        return a;
    }

     float rad_to_deg(float angle) {
         return angle * 360 / boost::math::constants::two_pi<float>();
     }

     std::array<uint8_t, BicyclePose_size> encode_buffer;
     std::array<uint8_t, BicyclePose_size + 1> frame_buffer;
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initialize a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activate the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the blink thread and print state monitor */
    chBlinkThreadCreateStatic();
    /*
     * Use LINE_TIM4_CH2 (PB7, EXT1-15, J4-B) as a button by
     * connecting/disconnecting it to ground.
     * */
    palSetLineMode(LINE_TIM4_CH2, PAL_MODE_INPUT_PULLUP);
    enablePrintStateMonitor(LINE_TIM4_CH2);

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder.start();
    analog.start(1000); /* trigger ADC conversion at 1 kHz */

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
     * The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
     * and must be changed to use as analog output.
     */
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(&DACD1, &dac1cfg1);

    /* initialize bicycle model and states */
    model::Bicycle bicycle(v0, dt);
    x.setZero();

    /* initialize Kalman filter */
    kalman_t kalman(bicycle,
            parameters::defaultvalue::kalman::Q(dt), /* process noise cov */
            (kalman_t::measurement_noise_covariance_t() <<
             sigma0,      0,
                  0, sigma1).finished(),
            bicycle_t::state_t::Zero(), /* set initial state estimate to zero */
            std::pow(x[1]/2, 2) * bicycle_t::state_matrix_t::Identity()); /* error cov */

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    rtcnt_t kalman_update_time = 0;
    bool print_version_string = true;
    u.setZero(); /* set both roll and steer torques to zero */
    BicyclePose pose = BicyclePose_init_zero;
    while (true) {
        u[1] = static_cast<float>(analog.get_adc12()*2.0f*max_kistler_torque/4096 -
                max_kistler_torque);
        //float motor_torque = static_cast<float>(
        //        analog.get_adc13()*2.0f*max_kollmorgen_torque/4096 -
        //        max_kollmorgen_torque);

        /* set measurement vector */
        z[0] = wrap_angle(x[0]); /* yaw angle, just use previous state value */

        /*
         * Steer angle, read from encoder (enccnt_t is uint32_t)
         * Convert angle from enccnt_t (unsigned) to corresponding signed type and use negative
         * values for any count over half a revolution.
         */
        auto position = static_cast<std::make_signed<enccnt_t>::type>(encoder.count());
        auto rev = static_cast<std::make_signed<enccnt_t>::type>(encoder.config().counts_per_rev);
        if (position > rev / 2) {
            position -= rev;
        }
        z[1] = static_cast<float>(position) / rev *
                boost::math::constants::two_pi<float>();

        /* observer time/measurement update (~80 us with real_t = float) */
        kalman_update_time = chSysGetRealtimeCounterX();
        kalman.time_update(u);
        kalman.measurement_update(z);
        x = kalman.x();
        x[0] = wrap_angle(x[0]);
        x[1] = wrap_angle(x[1]);
        x[2] = wrap_angle(x[2]);
        kalman_update_time = chSysGetRealtimeCounterX() - kalman_update_time;

        /* generate an example torque output for testing */
        float torque = 10.0f * std::sin(
                boost::math::constants::two_pi<float>() *
                ST2S(static_cast<float>(chVTGetSystemTime())));
        dacsample_t aout = static_cast<dacsample_t>(
                (torque/21.0f * 2048) + 2048); /* reduce output to half of full range */
        dacPutChannelX(&DACD1, 0, aout);

        uint8_t bytes_written;
        pose.x = 1;
        pose.y = 2;
        pose.yaw = x[0];
        pose.roll = x[1];
        pose.steer = x[2];
        bytes_written = packet::serialize::encode_bicycle_pose(pose,
                encode_buffer.data(), encode_buffer.size());
        packet::framing::stuff(encode_buffer.data(), frame_buffer.data(), bytes_written);

        printst_t s = getPrintState();
        if (s == printst_t::VERSION) {
            if (print_version_string) {
                printf("Running firmware version %.7s\r\n", g_GITSHA1);
                print_version_string = false;
            }
        } else if (s == printst_t::NORMAL) {
            packet::framing::unstuff(frame_buffer.data(), encode_buffer.data(), encode_buffer.size());
            pose = BicyclePose_init_zero;
            if (packet::serialize::decode_bicycle_pose(encode_buffer.data(), &pose, bytes_written)) {
                printf("bicycle pose:\r\n"
                        "\tx:\t%0.3f m\r\n"
                        "\ty:\t%0.3f m\r\n",
                        pose.x, pose.y);
                printf("\tyaw:\t%0.3f deg\r\n"
                        "\troll:\t%0.3f deg\r\n"
                        "\tsteer:\t%0.3f deg\r\n",
                        rad_to_deg(pose.yaw), rad_to_deg(pose.roll), rad_to_deg(pose.steer));
            }
            //printf("encoder count:\t%u\r\n", encoder.count());
            //printf("sensors:\t%0.3f\t%0.3f\t%0.3f\t%0.3f\r\n",
            //        u[1], motor_torque, rad_to_deg(z[0]), rad_to_deg(z[1]));
            //printf("state:\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\r\n",
            //        rad_to_deg(x[0]), rad_to_deg(x[1]), rad_to_deg(x[2]), rad_to_deg(x[3]), rad_to_deg(x[4]));
            //printf("kalman update time: %U us\r\n",
            //        RTC2US(STM32_SYSCLK, kalman_update_time));
        } else if (s == printst_t::NONE) {
            /* reset printing of version string */
            print_version_string = true;
        }
        chThdSleepMilliseconds(static_cast<systime_t>(1000*dt));
    }
}
<|endoftext|>


<commit_before>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"
#include "chprintf.h"

#include "blink.h"
#include "usbconfig.h"
#include "bicycle.h"
#include "kalman.h"
#include "parameters.h"

#include "analog.h"
#include "encoder.h"
#include <boost/math/constants/constants.hpp>

#include <type_traits>

namespace {
    using bicycle_t = model::Bicycle;
    using kalman_t = observer::Kalman<bicycle_t>;

    const float fs = 200; // sample rate [Hz]
    const float dt = 1.0/fs; // sample time [s]
    const float v0 = 5.0; // forward speed [m/s]

    /* Kalman filter variance values */
    const float sigma0 = 1 * constants::as_radians; // yaw angle measurement noise variance
    const float sigma1 = 0.008 * constants::as_radians; // steer angle measurement noise variance

    bicycle_t::input_t u; /* roll torque, steer torque */
    bicycle_t::state_t x; /* yaw angle, roll angle, steer angle, roll rate, steer rate */
    bicycle_t::output_t z; /* yaw angle, steer angle */

    /* sensors */
    Analog analog;
    Encoder encoder(&GPTD5, /* CH1, CH2 connected to PA0, PA1 and NOT enabled by board.h */
            {PAL_NOLINE, /* no index channel */
             152000, /* counts per revolution */
             EncoderConfig::filter_t::CAPTURE_64}); /* 64 * 42 MHz (TIM3 on APB1) = 1.52 us
                                                     * for valid edge */
     const float max_kistler_torque = 25.0f; /* maximum measured steer torque */

     model::real_t wrap_angle(model::real_t angle) {
         /*
          * Angle magnitude is assumed to small enough to NOT require multiple
          * additions/subtractions of 2pi.
          */
        if (angle >= boost::math::constants::pi<float>()) {
            angle -= boost::math::constants::two_pi<float>();
        }
        if (angle < -boost::math::constants::pi<float>()) {
            angle += boost::math::constants::two_pi<float>();
        }
        return angle;
     }
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initializes a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activates the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the example thread */
    chBlinkThreadCreateStatic();

    /* initialize bicycle model and states */
    model::Bicycle bicycle(v0, dt);
    x.setZero();

    /* initialize Kalman filter */
    kalman_t kalman(bicycle,
            parameters::defaultvalue::kalman::Q(dt), /* process noise cov */
            (kalman_t::measurement_noise_covariance_t() <<
             sigma0,      0,
                  0, sigma1).finished(),
            bicycle_t::state_t::Zero(), /* set initial state estimate to zero */
            std::pow(x[1]/2, 2) * bicycle_t::state_matrix_t::Identity()); /* error cov */

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder.start();
    analog.start(1000);

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    rtcnt_t kalman_update_time = 0;
    while (true) {
        u.setZero(); /* set both roll and steer torques to zero */
        /* pulse torque measure signal and read steer torque value */
        u[1] = static_cast<float>(analog.get_adc12()*2.0f*max_kistler_torque/4096 -
                max_kistler_torque);

        /* set measurement vector */
        z[0] = x[0]; /* yaw angle, just use previous state value */

        /*
         * Steer angle, read from encoder (enccnt_t is uint32_t)
         * Convert angle from enccnt_t (unsigned) to corresponding signed type and use negative
         * values for any count over half a revolution.
         */
        auto position = static_cast<std::make_signed<enccnt_t>::type>(encoder.count());
        auto rev = static_cast<std::make_signed<enccnt_t>::type>(encoder.config().counts_per_rev);
        if (position > rev / 2) {
            position -= rev;
        }
        z[1] = static_cast<float>(position) /
                boost::math::constants::two_pi<float>();

        /* observer time/measurement update (~80 us with real_t = float) */
        kalman_update_time = chSysGetRealtimeCounterX();
        kalman.time_update(u);
        kalman.measurement_update(z);
        x = kalman.x();
        x[0] = wrap_angle(x[0]);
        x[1] = wrap_angle(x[1]);
        x[2] = wrap_angle(x[2]);
        kalman_update_time = chSysGetRealtimeCounterX() - kalman_update_time;

        chprintf((BaseSequentialStream*)&SDU1,
                "sensors:\t%0.2f\t%0.2f\t%0.2f\r\n",
                u[1], z[0], z[1]);
        chprintf((BaseSequentialStream*)&SDU1,
                "state:\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\r\n",
                x[0], x[1], x[2], x[3], x[4]);
        chprintf((BaseSequentialStream*)&SDU1,
                "kalman update time: %d us\r\n",
                RTC2US(STM32_SYSCLK, kalman_update_time));
        chThdSleepMilliseconds(static_cast<systime_t>(1000*dt));
    }
}
<commit_msg>Add note on Kistler torque sensor resolution<commit_after>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"
#include "chprintf.h"

#include "blink.h"
#include "usbconfig.h"
#include "bicycle.h"
#include "kalman.h"
#include "parameters.h"

#include "analog.h"
#include "encoder.h"
#include <boost/math/constants/constants.hpp>

#include <type_traits>

namespace {
    using bicycle_t = model::Bicycle;
    using kalman_t = observer::Kalman<bicycle_t>;

    const float fs = 200; // sample rate [Hz]
    const float dt = 1.0/fs; // sample time [s]
    const float v0 = 5.0; // forward speed [m/s]

    /* Kalman filter variance values */
    const float sigma0 = 1 * constants::as_radians; // yaw angle measurement noise variance
    const float sigma1 = 0.008 * constants::as_radians; // steer angle measurement noise variance

    bicycle_t::input_t u; /* roll torque, steer torque */
    bicycle_t::state_t x; /* yaw angle, roll angle, steer angle, roll rate, steer rate */
    bicycle_t::output_t z; /* yaw angle, steer angle */

    /* sensors */
    Analog analog;
    Encoder encoder(&GPTD5, /* CH1, CH2 connected to PA0, PA1 and NOT enabled by board.h */
            {PAL_NOLINE, /* no index channel */
             152000, /* counts per revolution */
             EncoderConfig::filter_t::CAPTURE_64}); /* 64 * 42 MHz (TIM3 on APB1) = 1.52 us
                                                     * for valid edge */

     const float max_kistler_torque = 25.0f; /* maximum measured steer torque */
     /*
      * The voltage output of the Kistler torque sensor is ±10V. With the 12-bit ADC,
      * resolution for LSB is 4.88 mV/bit or 12.2 mNm/bit.
      */

     model::real_t wrap_angle(model::real_t angle) {
         /*
          * Angle magnitude is assumed to small enough to NOT require multiple
          * additions/subtractions of 2pi.
          */
        if (angle >= boost::math::constants::pi<float>()) {
            angle -= boost::math::constants::two_pi<float>();
        }
        if (angle < -boost::math::constants::pi<float>()) {
            angle += boost::math::constants::two_pi<float>();
        }
        return angle;
     }
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initializes a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activates the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the example thread */
    chBlinkThreadCreateStatic();

    /* initialize bicycle model and states */
    model::Bicycle bicycle(v0, dt);
    x.setZero();

    /* initialize Kalman filter */
    kalman_t kalman(bicycle,
            parameters::defaultvalue::kalman::Q(dt), /* process noise cov */
            (kalman_t::measurement_noise_covariance_t() <<
             sigma0,      0,
                  0, sigma1).finished(),
            bicycle_t::state_t::Zero(), /* set initial state estimate to zero */
            std::pow(x[1]/2, 2) * bicycle_t::state_matrix_t::Identity()); /* error cov */

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder.start();
    analog.start(1000);

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    rtcnt_t kalman_update_time = 0;
    while (true) {
        u.setZero(); /* set both roll and steer torques to zero */
        /* pulse torque measure signal and read steer torque value */
        u[1] = static_cast<float>(analog.get_adc12()*2.0f*max_kistler_torque/4096 -
                max_kistler_torque);

        /* set measurement vector */
        z[0] = x[0]; /* yaw angle, just use previous state value */

        /*
         * Steer angle, read from encoder (enccnt_t is uint32_t)
         * Convert angle from enccnt_t (unsigned) to corresponding signed type and use negative
         * values for any count over half a revolution.
         */
        auto position = static_cast<std::make_signed<enccnt_t>::type>(encoder.count());
        auto rev = static_cast<std::make_signed<enccnt_t>::type>(encoder.config().counts_per_rev);
        if (position > rev / 2) {
            position -= rev;
        }
        z[1] = static_cast<float>(position) /
                boost::math::constants::two_pi<float>();

        /* observer time/measurement update (~80 us with real_t = float) */
        kalman_update_time = chSysGetRealtimeCounterX();
        kalman.time_update(u);
        kalman.measurement_update(z);
        x = kalman.x();
        x[0] = wrap_angle(x[0]);
        x[1] = wrap_angle(x[1]);
        x[2] = wrap_angle(x[2]);
        kalman_update_time = chSysGetRealtimeCounterX() - kalman_update_time;

        chprintf((BaseSequentialStream*)&SDU1,
                "sensors:\t%0.2f\t%0.2f\t%0.2f\r\n",
                u[1], z[0], z[1]);
        chprintf((BaseSequentialStream*)&SDU1,
                "state:\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\r\n",
                x[0], x[1], x[2], x[3], x[4]);
        chprintf((BaseSequentialStream*)&SDU1,
                "kalman update time: %d us\r\n",
                RTC2US(STM32_SYSCLK, kalman_update_time));
        chThdSleepMilliseconds(static_cast<systime_t>(1000*dt));
    }
}
<|endoftext|>


<commit_before>/*=========================================================================

 medInria

 Copyright (c) INRIA 2013. All rights reserved.
 See LICENSE.txt for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

=========================================================================*/

#include <medViewContainerSplitter.h>

#include <QDebug>

#include <medDataManager.h>
#include <medViewContainer.h>
#include <medAbstractData.h>
#include <QUuid>

medViewContainerSplitter::medViewContainerSplitter(QWidget *parent)
{
    this->setOrientation(Qt::Horizontal);
    this->setHandleWidth(1);
    this->setContentsMargins(0,0,0,0);
    this->setOpaqueResize(false);
}

medViewContainerSplitter::~medViewContainerSplitter()
{

}

void medViewContainerSplitter::hSplit()
{
    this->split(Qt::AlignBottom);
}

void medViewContainerSplitter::vSplit()
{
    this->split(Qt::AlignRight);
}

medViewContainer *medViewContainerSplitter::split(Qt::AlignmentFlag alignement)
{
    medViewContainer* container = dynamic_cast<medViewContainer*>(this->sender());
    if(!container)
        return NULL;
    int index = this->indexOf(container);
    int newSize = this->sizes()[index] / 2;

    medViewContainer *newContainer = new medViewContainer;

    switch(alignement)
    {
    case Qt::AlignLeft:
        if(this->orientation() == Qt::Horizontal)
        {
            this->insertViewContainer(index, newContainer);
            this->recomputeSizes(index + 1, index, newSize);
        }
        else
            this->insertNestedSplitter(index, newContainer, container);
        break;
    case Qt::AlignBottom:
        if(this->orientation() == Qt::Vertical)
        {
            this->insertViewContainer(index + 1, newContainer);
            this->recomputeSizes(index, index + 1, newSize);
        }
        else
            this->insertNestedSplitter(index, container, newContainer);
        break;
    case Qt::AlignRight:
        if(this->orientation() == Qt::Horizontal)
        {
            this->insertViewContainer(index + 1, newContainer);
            this->recomputeSizes(index, index + 1, newSize);
        }
        else
            this->insertNestedSplitter(index, container, newContainer);
        break;
    case Qt::AlignTop:
        if(this->orientation() == Qt::Vertical)
        {
            this->insertViewContainer(index, newContainer);
            this->recomputeSizes(index + 1, index, newSize);
        }
        else
            this->insertNestedSplitter(index, newContainer, container);
        break;
    }

    return newContainer;
}

void medViewContainerSplitter::split(medDataIndex index, Qt::AlignmentFlag alignement)
{
    medViewContainer *newContainer = this->split(alignement);
    newContainer->addData(medDataManager::instance()->data(index));
}

void medViewContainerSplitter::checkIfStillDeserveToLive()
{
    if(this->count() < 1)
        this->~medViewContainerSplitter();
}

void medViewContainerSplitter::insertViewContainer(int index, medViewContainer *container)
{
    connect(container, SIGNAL(hSplitRequest()), this, SLOT(hSplit()));
    connect(container, SIGNAL(vSplitRequest()), this, SLOT(vSplit()));
    connect(container, SIGNAL(splitRequest(medDataIndex, Qt::AlignmentFlag)),
            this, SLOT(split(medDataIndex, Qt::AlignmentFlag)));
    connect(container, SIGNAL(destroyed()), this, SLOT(checkIfStillDeserveToLive()));
    connect(container, SIGNAL(destroyed()), this, SIGNAL(containerRemoved()));

    emit newContainer(container->uuid());

    this->insertWidget(index, container);
    this->setCollapsible(index, false);
}

void medViewContainerSplitter::addViewContainer(medViewContainer *container)
{
    int newSize = 0;
    if(this->count() > 0)
        newSize = this->sizes()[this->count() - 1] / 2;

    this->insertViewContainer(this->count(), container);
    if(this->count() > 1)
        this->recomputeSizes(this->count() - 2, this->count() - 1, newSize);
}

void medViewContainerSplitter::recomputeSizes(int requestIndex, int newIndex, int newSize)
{
    if(requestIndex < 0 || newIndex < 0)
        return;

    QList <int> newSizes = this->sizes();
    if(requestIndex >= newSizes.size() || newIndex >= newSizes.size())
        return;

    newSizes.replace(requestIndex, newSize);
    newSizes.replace(newIndex, newSize);
    this->setSizes(newSizes);
}

void medViewContainerSplitter::insertNestedSplitter(int index,
                                                 medViewContainer *oldContainer,
                                                 medViewContainer *newContainer)
{
    Qt::Orientation ori = Qt::Vertical;
    if(this->orientation() == Qt::Vertical)
            ori = Qt::Horizontal;

    QList<int> savedSizes = this->sizes();
    oldContainer->disconnect(this);
    medViewContainerSplitter *splitter = new medViewContainerSplitter;
    splitter->setOrientation(ori);
    connect(splitter, SIGNAL(newContainer(QUuid)), this, SIGNAL(newContainer(QUuid)));
    connect(splitter, SIGNAL(containerRemoved()), this, SIGNAL(containerRemoved()));
    connect(splitter, SIGNAL(destroyed()), this, SLOT(checkIfStillDeserveToLive()));
    splitter->addViewContainer(oldContainer);
    splitter->addViewContainer(newContainer);
    this->insertWidget(index, splitter);
    this->setCollapsible(index, false);
    this->setSizes(savedSizes);
}
<commit_msg>correct resize of the splitter when adding a qvtkWidget2 directly in a new created nested splitter<commit_after>/*=========================================================================

 medInria

 Copyright (c) INRIA 2013. All rights reserved.
 See LICENSE.txt for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

=========================================================================*/

#include <medViewContainerSplitter.h>

#include <QDebug>

#include <medDataManager.h>
#include <medViewContainer.h>
#include <medAbstractData.h>
#include <QUuid>

medViewContainerSplitter::medViewContainerSplitter(QWidget *parent)
{
    this->setOrientation(Qt::Horizontal);
    this->setHandleWidth(1);
    this->setContentsMargins(0,0,0,0);
    this->setOpaqueResize(false);
}

medViewContainerSplitter::~medViewContainerSplitter()
{

}

void medViewContainerSplitter::hSplit()
{
    this->split(Qt::AlignBottom);
}

void medViewContainerSplitter::vSplit()
{
    this->split(Qt::AlignRight);
}

medViewContainer *medViewContainerSplitter::split(Qt::AlignmentFlag alignement)
{
    medViewContainer* container = dynamic_cast<medViewContainer*>(this->sender());
    if(!container)
        return NULL;
    int index = this->indexOf(container);
    int newSize = this->sizes()[index] / 2;

    medViewContainer *newContainer = new medViewContainer;

    switch(alignement)
    {
    case Qt::AlignLeft:
        if(this->orientation() == Qt::Horizontal)
        {
            this->insertViewContainer(index, newContainer);
            this->recomputeSizes(index + 1, index, newSize);
        }
        else
            this->insertNestedSplitter(index, newContainer, container);
        break;
    case Qt::AlignBottom:
        if(this->orientation() == Qt::Vertical)
        {
            this->insertViewContainer(index + 1, newContainer);
            this->recomputeSizes(index, index + 1, newSize);
        }
        else
            this->insertNestedSplitter(index, container, newContainer);
        break;
    case Qt::AlignRight:
        if(this->orientation() == Qt::Horizontal)
        {
            this->insertViewContainer(index + 1, newContainer);
            this->recomputeSizes(index, index + 1, newSize);
        }
        else
            this->insertNestedSplitter(index, container, newContainer);
        break;
    case Qt::AlignTop:
        if(this->orientation() == Qt::Vertical)
        {
            this->insertViewContainer(index, newContainer);
            this->recomputeSizes(index + 1, index, newSize);
        }
        else
            this->insertNestedSplitter(index, newContainer, container);
        break;
    }

    return newContainer;
}

void medViewContainerSplitter::split(medDataIndex index, Qt::AlignmentFlag alignement)
{
    medViewContainer *newContainer = this->split(alignement);
    newContainer->addData(medDataManager::instance()->data(index));
}

void medViewContainerSplitter::checkIfStillDeserveToLive()
{
    if(this->count() < 1)
        this->~medViewContainerSplitter();
}

void medViewContainerSplitter::insertViewContainer(int index, medViewContainer *container)
{
    connect(container, SIGNAL(hSplitRequest()), this, SLOT(hSplit()));
    connect(container, SIGNAL(vSplitRequest()), this, SLOT(vSplit()));
    connect(container, SIGNAL(splitRequest(medDataIndex, Qt::AlignmentFlag)),
            this, SLOT(split(medDataIndex, Qt::AlignmentFlag)));
    connect(container, SIGNAL(destroyed()), this, SLOT(checkIfStillDeserveToLive()));
    connect(container, SIGNAL(destroyed()), this, SIGNAL(containerRemoved()));

    emit newContainer(container->uuid());

    this->insertWidget(index, container);
    this->setCollapsible(index, false);
}

void medViewContainerSplitter::addViewContainer(medViewContainer *container)
{
    int newSize = 0;
    if(this->count() > 0)
        newSize = this->sizes()[this->count() - 1] / 2;

    this->insertViewContainer(this->count(), container);
    if(this->count() > 1)
        this->recomputeSizes(this->count() - 2, this->count() - 1, newSize);
}

void medViewContainerSplitter::recomputeSizes(int requestIndex, int newIndex, int newSize)
{
    if(requestIndex < 0 || newIndex < 0)
        return;

    QList <int> newSizes = this->sizes();
    if(requestIndex >= newSizes.size() || newIndex >= newSizes.size())
        return;

    newSizes.replace(requestIndex, newSize);
    newSizes.replace(newIndex, newSize);
    this->setSizes(newSizes);
}

void medViewContainerSplitter::insertNestedSplitter(int index,
                                                 medViewContainer *oldContainer,
                                                 medViewContainer *newContainer)
{
    Qt::Orientation ori = Qt::Vertical;
    if(this->orientation() == Qt::Vertical)
            ori = Qt::Horizontal;

    QList<int> savedSizes = this->sizes();
    oldContainer->disconnect(this);
    medViewContainerSplitter *splitter = new medViewContainerSplitter;
    splitter->setOrientation(ori);
    connect(splitter, SIGNAL(newContainer(QUuid)), this, SIGNAL(newContainer(QUuid)));
    connect(splitter, SIGNAL(containerRemoved()), this, SIGNAL(containerRemoved()));
    connect(splitter, SIGNAL(destroyed()), this, SLOT(checkIfStillDeserveToLive()));
    this->insertWidget(index, splitter);
    this->setCollapsible(index, false);
    splitter->addViewContainer(oldContainer);
    splitter->addViewContainer(newContainer);
    this->setSizes(savedSizes);

//    int newSize = 0;
//    if(splitter->orientation() == Qt::Vertical)
//        newSize = splitter->width() / 2;
//    else
//        newSize = splitter->height() / 2;
//    splitter->recomputeSizes(0, 1, newSize);

}
<|endoftext|>


<commit_before>/** \file    MediaTypeUtil.cc
 *  \brief   Implementation of Media Type utility functions.
 *  \author  Dr. Gordon W. Paynter
 *  \author  Dr. Johannes Ruscheinski
 */

/*
 *  Copyright 2004-2008 Project iVia.
 *  Copyright 2004-2008 The Regents of The University of California.
 *  Copyright 2016 Universitätsbibliothek Tübingen.
 *
 *  This file is part of the libiViaCore package.
 *
 *  The libiViaCore package is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public License as published
 *  by the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  libiViaCore is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with libiViaCore; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "MediaTypeUtil.h"
#include <stdexcept>
#include <cctype>
#include <alloca.h>
#include <magic.h>
#include "File.h"
#include "HttpHeader.h"
#include "PerlCompatRegExp.h"
#include "StringUtil.h"
#include "Url.h"
#include "WebUtil.h"

namespace MediaTypeUtil {

std::string GetHtmlMediaType(const std::string &document) {
    static const PerlCompatRegExp doctype_regexp("^\\s*<(?:!DOCTYPE\\s+HTML\\s+PUBLIC\\s+\"-//W3C//DTD\\s+){0,1}(X?HTML)",
                                                 PerlCompatRegExp::OPTIMIZE_FOR_MULTIPLE_USE, PCRE_CASELESS);

    // If we have a match we have either HTML or XHTML...
    std::string matched_substring;
    if (doctype_regexp.match(document) and doctype_regexp.getMatchedSubstring(1, &matched_substring))
        return matched_substring.length() == 4 ? "text/html" : "text/xhtml";

    // ...otherwise we have no idea what we have:
    return "";
}

static std::string LZ4_MAGIC("\000\042\115\030");

// GetMediaType -- Get the media type of a document.
//
std::string GetMediaType(const std::string &document, const bool auto_simplify) {
    if (document.empty())
        return "";

    // 1. See if we have (X)HTML:
    std::string media_type(GetHtmlMediaType(document));
    if (not media_type.empty())
        return media_type;

    // 2. Check for LZ4 compression:
    if (document.substr(0, 4) == LZ4_MAGIC)
        return "application/lz4";

    // 3. Next try libmagic:
    const magic_t cookie = ::magic_open(MAGIC_MIME);
    if (unlikely(cookie == nullptr))
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not open libmagic!");

    // Load the default "magic" definitions file:
    if (unlikely(::magic_load(cookie, nullptr /* use default magic file */) != 0)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not load libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Use magic to get the mime type of the buffer:
    const char *magic_mime_type = ::magic_buffer(cookie, document.c_str(), document.length());
    if (unlikely(magic_mime_type == nullptr)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: error in libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Attempt to remove possible leading junk (no idea why libmagic behaves in this manner every now and then):
    if (std::strncmp(magic_mime_type, "\\012- ", 6) == 0)
        magic_mime_type += 6;

    // Close the library:
    media_type = magic_mime_type;
    ::magic_close(cookie);

    // 4. If the libmagic could not determine the document's MIME type, test for XML:
    if (media_type.empty() and document.size() > 5)
        return std::strncmp(document.c_str(), "<?xml", 5) == 0 ? "text/xml" : "";

    if (auto_simplify)
        SimplifyMediaType(&media_type);

    return media_type;
}

std::string GetFileMediaType(const std::string &filename, const bool auto_simplify) {
    const magic_t cookie = ::magic_open(MAGIC_MIME | MAGIC_SYMLINK);
    if (unlikely(cookie == nullptr))
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not open libmagic!");

    // Load the default "magic" definitions file:
    if (unlikely(::magic_load(cookie, nullptr /* use default magic file */) != 0)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not load libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Use magic to get the mime type of the buffer:
    const char *magic_mime_type(::magic_file(cookie, filename.c_str()));
    if (unlikely(magic_mime_type == nullptr)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetFileMediaType: error in libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Attempt to remove possible leading junk (no idea why libmagic behaves in this manner every now and then):
    if (std::strncmp(magic_mime_type, "\\012- ", 6) == 0)
        magic_mime_type += 6;

    // Close the library:
    std::string media_type(magic_mime_type);
    ::magic_close(cookie);

    if (auto_simplify)
        SimplifyMediaType(&media_type);

    if (StringUtil::StartsWith(media_type, "application/octet-stream")) {
        File input(filename, "rb");
        char *buf = reinterpret_cast<char *>(::alloca(LZ4_MAGIC.size()));
        if ((input.read(buf, sizeof(buf)) == sizeof(buf)) and std::strncmp(LZ4_MAGIC.c_str(), buf, LZ4_MAGIC.size()) == 0)
            return "application/lz4";
    }

    return media_type;
}

// GetMediaType -- get the media type of a Web page.
//
std::string GetMediaType(const std::string &page_header, const std::string &page_body, const bool auto_simplify) {
    // First, attempt to find the media type in the header:
    HttpHeader http_header(page_header);
    if (http_header.isValid()) {
        std::string media_type(http_header.getMediaType());
        if (not media_type.empty()) {
            StringUtil::ToLower(&media_type);
            if (auto_simplify)
                SimplifyMediaType(&media_type);
            return media_type;
        }
    }

    // Otherwise, check the content:
    return GetMediaType(page_body, auto_simplify);
}

// GetMediaType -- Get the MediaType of the page.  Returns true if "media_type" is known and set.
//
bool GetMediaType(const Url &url, const HttpHeader &http_header, const std::string &page_content,
                  std::string * const media_type, const bool auto_simplify)
{
    // First, attempt to find the media type in the header:
    if (http_header.isValid()) {
        *media_type = http_header.getMediaType();
        if (not media_type->empty()) {
            if (auto_simplify)
                SimplifyMediaType(media_type);
            return true;
        }
    }

    // Second, attempt to use the "magic" library (libmagic) to analyse the page:
    *media_type = MediaTypeUtil::GetMediaType(page_content);
    if (not media_type->empty()) {
        if (auto_simplify)
            SimplifyMediaType(media_type);
        return true;
    }

    // Third, guess based on URL:
    *media_type = WebUtil::GuessMediaType(url);
    if (not media_type->empty() and auto_simplify)
        SimplifyMediaType(media_type);
    return not media_type->empty();
}

std::string GetMediaType(const HttpHeader &http_header, const std::string &page_body, const bool auto_simplify) {
    std::string media_type;

    // First, attempt to find the media type in the header:
    if (http_header.isValid()) {
        media_type = http_header.getMediaType();
        if (not media_type.empty()) {
            StringUtil::ToLower(&media_type);
            if (auto_simplify)
                SimplifyMediaType(&media_type);
            return media_type;
        }
    }

    // Second, attempt to use the "magic" library (libmagic) to analyse the page:
    media_type = MediaTypeUtil::GetMediaType(page_body, auto_simplify);

    return media_type;
}

bool GetMediaType(const std::string &url, const HttpHeader &http_header, const std::string &page_content,
                  std::string * const media_type, const bool auto_simplify)
{
    return GetMediaType(Url(url), http_header, page_content, media_type, auto_simplify);
}

bool SimplifyMediaType(std::string * const media_type) {
    if (media_type->empty())
        return false;

    // This is the format of the Content-Type field for Web pages:
    //  media-type     = type "/" subtype *( ";" parameter )
    //  type           = token
    //      subtype        = token
    // See: http://www.w3.org/Protocols/rfc2616/rfc2616-sec3.html#sec3.7

    const std::string initial_media_type(*media_type);

    // Search for a semicolon and delete any 'parameter' parts:
    const std::string::size_type semicolon_pos(media_type->find(';'));
    if (semicolon_pos != std::string::npos)
        media_type->resize(semicolon_pos);
    else { // Try a space instead of a semicolon.
        const std::string::size_type space_pos(media_type->find(' '));
        if (space_pos != std::string::npos)
            media_type->resize(space_pos);
    }

    StringUtil::TrimWhite(media_type);

    // Return if "media_type" has changed:
    return initial_media_type != *media_type;
}

} // namespace MediaTypeUtil
<commit_msg>Our File class does not support the "rb" open mode.<commit_after>/** \file    MediaTypeUtil.cc
 *  \brief   Implementation of Media Type utility functions.
 *  \author  Dr. Gordon W. Paynter
 *  \author  Dr. Johannes Ruscheinski
 */

/*
 *  Copyright 2004-2008 Project iVia.
 *  Copyright 2004-2008 The Regents of The University of California.
 *  Copyright 2016 Universitätsbibliothek Tübingen.
 *
 *  This file is part of the libiViaCore package.
 *
 *  The libiViaCore package is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public License as published
 *  by the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  libiViaCore is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with libiViaCore; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "MediaTypeUtil.h"
#include <stdexcept>
#include <cctype>
#include <alloca.h>
#include <magic.h>
#include "File.h"
#include "HttpHeader.h"
#include "PerlCompatRegExp.h"
#include "StringUtil.h"
#include "Url.h"
#include "util.h"
#include "WebUtil.h"

namespace MediaTypeUtil {

std::string GetHtmlMediaType(const std::string &document) {
    static const PerlCompatRegExp doctype_regexp("^\\s*<(?:!DOCTYPE\\s+HTML\\s+PUBLIC\\s+\"-//W3C//DTD\\s+){0,1}(X?HTML)",
                                                 PerlCompatRegExp::OPTIMIZE_FOR_MULTIPLE_USE, PCRE_CASELESS);

    // If we have a match we have either HTML or XHTML...
    std::string matched_substring;
    if (doctype_regexp.match(document) and doctype_regexp.getMatchedSubstring(1, &matched_substring))
        return matched_substring.length() == 4 ? "text/html" : "text/xhtml";

    // ...otherwise we have no idea what we have:
    return "";
}

static std::string LZ4_MAGIC("\000\042\115\030");

// GetMediaType -- Get the media type of a document.
//
std::string GetMediaType(const std::string &document, const bool auto_simplify) {
    if (document.empty())
        return "";

    // 1. See if we have (X)HTML:
    std::string media_type(GetHtmlMediaType(document));
    if (not media_type.empty())
        return media_type;

    // 2. Check for LZ4 compression:
    if (document.substr(0, 4) == LZ4_MAGIC)
        return "application/lz4";

    // 3. Next try libmagic:
    const magic_t cookie = ::magic_open(MAGIC_MIME);
    if (unlikely(cookie == nullptr))
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not open libmagic!");

    // Load the default "magic" definitions file:
    if (unlikely(::magic_load(cookie, nullptr /* use default magic file */) != 0)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not load libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Use magic to get the mime type of the buffer:
    const char *magic_mime_type = ::magic_buffer(cookie, document.c_str(), document.length());
    if (unlikely(magic_mime_type == nullptr)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: error in libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Attempt to remove possible leading junk (no idea why libmagic behaves in this manner every now and then):
    if (std::strncmp(magic_mime_type, "\\012- ", 6) == 0)
        magic_mime_type += 6;

    // Close the library:
    media_type = magic_mime_type;
    ::magic_close(cookie);

    // 4. If the libmagic could not determine the document's MIME type, test for XML:
    if (media_type.empty() and document.size() > 5)
        return std::strncmp(document.c_str(), "<?xml", 5) == 0 ? "text/xml" : "";

    if (auto_simplify)
        SimplifyMediaType(&media_type);

    return media_type;
}

std::string GetFileMediaType(const std::string &filename, const bool auto_simplify) {
    const magic_t cookie(::magic_open(MAGIC_MIME | MAGIC_SYMLINK));
    if (unlikely(cookie == nullptr))
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not open libmagic!");

    // Load the default "magic" definitions file:
    if (unlikely(::magic_load(cookie, nullptr /* use default magic file */) != 0)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetMediaType: could not load libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Use magic to get the mime type of the buffer:
    const char *magic_mime_type(::magic_file(cookie, filename.c_str()));
    if (unlikely(magic_mime_type == nullptr)) {
        ::magic_close(cookie);
        throw std::runtime_error("in MediaTypeUtil::GetFileMediaType: error in libmagic ("
                                 + std::string(::magic_error(cookie)) + ").");
    }

    // Attempt to remove possible leading junk (no idea why libmagic behaves in this manner every now and then):
    if (std::strncmp(magic_mime_type, "\\012- ", 6) == 0)
        magic_mime_type += 6;

    // Close the library:
    std::string media_type(magic_mime_type);
    ::magic_close(cookie);

    if (auto_simplify)
        SimplifyMediaType(&media_type);

    if (StringUtil::StartsWith(media_type, "application/octet-stream")) {
        File input(filename, "r");
        if (unlikely(input.anErrorOccurred()))
            logger->error("in MediaTypeUtil::GetFileMediaType: failed to open \"" + filename + "\" for reading!");
        char * const buf(reinterpret_cast<char *>(::alloca(LZ4_MAGIC.size())));
        if ((input.read(buf, sizeof(buf)) == sizeof(buf)) and std::strncmp(LZ4_MAGIC.c_str(), buf, LZ4_MAGIC.size()) == 0)
            return "application/lz4";
    }

    return media_type;
}

// GetMediaType -- get the media type of a Web page.
//
std::string GetMediaType(const std::string &page_header, const std::string &page_body, const bool auto_simplify) {
    // First, attempt to find the media type in the header:
    HttpHeader http_header(page_header);
    if (http_header.isValid()) {
        std::string media_type(http_header.getMediaType());
        if (not media_type.empty()) {
            StringUtil::ToLower(&media_type);
            if (auto_simplify)
                SimplifyMediaType(&media_type);
            return media_type;
        }
    }

    // Otherwise, check the content:
    return GetMediaType(page_body, auto_simplify);
}

// GetMediaType -- Get the MediaType of the page.  Returns true if "media_type" is known and set.
//
bool GetMediaType(const Url &url, const HttpHeader &http_header, const std::string &page_content,
                  std::string * const media_type, const bool auto_simplify)
{
    // First, attempt to find the media type in the header:
    if (http_header.isValid()) {
        *media_type = http_header.getMediaType();
        if (not media_type->empty()) {
            if (auto_simplify)
                SimplifyMediaType(media_type);
            return true;
        }
    }

    // Second, attempt to use the "magic" library (libmagic) to analyse the page:
    *media_type = MediaTypeUtil::GetMediaType(page_content);
    if (not media_type->empty()) {
        if (auto_simplify)
            SimplifyMediaType(media_type);
        return true;
    }

    // Third, guess based on URL:
    *media_type = WebUtil::GuessMediaType(url);
    if (not media_type->empty() and auto_simplify)
        SimplifyMediaType(media_type);
    return not media_type->empty();
}

std::string GetMediaType(const HttpHeader &http_header, const std::string &page_body, const bool auto_simplify) {
    std::string media_type;

    // First, attempt to find the media type in the header:
    if (http_header.isValid()) {
        media_type = http_header.getMediaType();
        if (not media_type.empty()) {
            StringUtil::ToLower(&media_type);
            if (auto_simplify)
                SimplifyMediaType(&media_type);
            return media_type;
        }
    }

    // Second, attempt to use the "magic" library (libmagic) to analyse the page:
    media_type = MediaTypeUtil::GetMediaType(page_body, auto_simplify);

    return media_type;
}

bool GetMediaType(const std::string &url, const HttpHeader &http_header, const std::string &page_content,
                  std::string * const media_type, const bool auto_simplify)
{
    return GetMediaType(Url(url), http_header, page_content, media_type, auto_simplify);
}

bool SimplifyMediaType(std::string * const media_type) {
    if (media_type->empty())
        return false;

    // This is the format of the Content-Type field for Web pages:
    //  media-type     = type "/" subtype *( ";" parameter )
    //  type           = token
    //      subtype        = token
    // See: http://www.w3.org/Protocols/rfc2616/rfc2616-sec3.html#sec3.7

    const std::string initial_media_type(*media_type);

    // Search for a semicolon and delete any 'parameter' parts:
    const std::string::size_type semicolon_pos(media_type->find(';'));
    if (semicolon_pos != std::string::npos)
        media_type->resize(semicolon_pos);
    else { // Try a space instead of a semicolon.
        const std::string::size_type space_pos(media_type->find(' '));
        if (space_pos != std::string::npos)
            media_type->resize(space_pos);
    }

    StringUtil::TrimWhite(media_type);

    // Return if "media_type" has changed:
    return initial_media_type != *media_type;
}

} // namespace MediaTypeUtil
<|endoftext|>