mixer.h

/****************************************************************************
 *
 *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * 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 OWNER 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.
 *
 ****************************************************************************/

#pragma once

#include <stdint.h>

struct mixer_scaler_s {
    float           negative_scale;
    float           positive_scale;
    float           offset;
    float           min_output;
    float           max_output;
};

struct mixer_control_s {
    uint8_t         control_group;  
    uint8_t         control_index;  
    struct mixer_scaler_s   scaler;     
};

struct mixer_simple_s {
    uint8_t         control_count;  
    struct mixer_scaler_s   output_scaler;  
    struct mixer_control_s  controls[0];    
};

#define MIXER_SIMPLE_SIZE(_icount)  (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s))


class Mixer
{
public:
    enum class Airmode : int32_t {
        disabled = 0,
        roll_pitch = 1,
        roll_pitch_yaw = 2
    };

    Mixer               *_next;

    typedef int (* ControlCallback)(uintptr_t handle,
                        uint8_t control_group,
                        uint8_t control_index,
                        float &control);

    Mixer(ControlCallback control_cb, uintptr_t cb_handle);
    virtual ~Mixer() {}

    virtual unsigned        mix(float *outputs, unsigned space) = 0;

    virtual uint16_t        get_saturation_status(void) = 0;

    virtual void            groups_required(uint32_t &groups) = 0;

    virtual void            set_max_delta_out_once(float delta_out_max) {}

    virtual unsigned set_trim(float trim) = 0;

    virtual unsigned get_trim(float *trim) = 0;

    /*
     * @brief      Sets the thrust factor used to calculate mapping from desired thrust to pwm.
     *
     * @param[in]  val   The value
     */
    virtual void            set_thrust_factor(float val) {}

    virtual void set_airmode(Airmode airmode) {};

protected:
    ControlCallback         _control_cb;
    uintptr_t           _cb_handle;

    float               get_control(uint8_t group, uint8_t index);

    static float            scale(const mixer_scaler_s &scaler, float input);

    static int          scale_check(struct mixer_scaler_s &scaler);

    static const char       *findtag(const char *buf, unsigned &buflen, char tag);

    static char             findnexttag(const char *buf, unsigned buflen);

    static const char       *skipline(const char *buf, unsigned &buflen);

    static bool             string_well_formed(const char *buf, unsigned &buflen);

private:

    /* do not allow to copy due to pointer data members */
    Mixer(const Mixer &);
    Mixer &operator=(const Mixer &);
};

class MixerGroup : public Mixer
{
public:
    MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
    ~MixerGroup();

    unsigned        mix(float *outputs, unsigned space) override;
    uint16_t        get_saturation_status(void) override;
    void            groups_required(uint32_t &groups) override;

    void                add_mixer(Mixer *mixer);

    void                reset();

    unsigned            count();

    int             load_from_buf(const char *buf, unsigned &buflen);

    void            set_max_delta_out_once(float delta_out_max) override;

    /*
     * Invoke the set_offset method of each mixer in the group
     * for each value in page r_page_servo_control_trim
     */
    unsigned set_trims(int16_t *v, unsigned n);

    unsigned set_trim(float trim) override
    {
        return 0;
    }

    unsigned get_trims(int16_t *values);

    unsigned get_trim(float *trim) override
    {
        return 0;
    }

    void    set_thrust_factor(float val) override;

    void    set_airmode(Airmode airmode) override;

private:
    Mixer               *_first;    
    /* do not allow to copy due to pointer data members */
    MixerGroup(const MixerGroup &);
    MixerGroup operator=(const MixerGroup &);
};

class NullMixer : public Mixer
{
public:
    NullMixer();
    ~NullMixer() {}

    static NullMixer        *from_text(const char *buf, unsigned &buflen);

    unsigned        mix(float *outputs, unsigned space) override;
    uint16_t        get_saturation_status(void) override;
    void            groups_required(uint32_t &groups) override;
    unsigned set_trim(float trim) override
    {
        return 1;
    }

    unsigned get_trim(float *trim) override
    {
        return 1;
    }

};

class SimpleMixer : public Mixer
{
public:
    SimpleMixer(ControlCallback control_cb,
            uintptr_t cb_handle,
            mixer_simple_s *mixinfo);
    ~SimpleMixer();

    static SimpleMixer      *from_text(Mixer::ControlCallback control_cb,
            uintptr_t cb_handle,
            const char *buf,
            unsigned &buflen);

    static SimpleMixer      *pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
            uint16_t mid, uint16_t max);

    unsigned        mix(float *outputs, unsigned space) override;
    uint16_t        get_saturation_status(void) override;
    void            groups_required(uint32_t &groups) override;

    int             check();

    unsigned set_trim(float trim) override;

    unsigned get_trim(float *trim) override;

protected:

private:
    mixer_simple_s          *_pinfo;

    static int          parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
    static int          parse_control_scaler(const char *buf,
            unsigned &buflen,
            mixer_scaler_s &scaler,
            uint8_t &control_group,
            uint8_t &control_index);

    /* do not allow to copy due to ptr data members */
    SimpleMixer(const SimpleMixer &);
    SimpleMixer operator=(const SimpleMixer &);
};

typedef unsigned int MultirotorGeometryUnderlyingType;
enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;

class MultirotorMixer : public Mixer
{
public:
    struct Rotor {
        float   roll_scale;     
        float   pitch_scale;    
        float   yaw_scale;      
        float   thrust_scale;   
    };

    MultirotorMixer(ControlCallback control_cb,
            uintptr_t cb_handle,
            MultirotorGeometry geometry,
            float roll_scale,
            float pitch_scale,
            float yaw_scale,
            float idle_speed);

    MultirotorMixer(ControlCallback control_cb,
            uintptr_t cb_handle,
            Rotor *rotors,
            unsigned rotor_count);

    ~MultirotorMixer();

    static MultirotorMixer      *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
            unsigned &buflen);

    unsigned        mix(float *outputs, unsigned space) override;
    uint16_t        get_saturation_status(void) override;
    void            groups_required(uint32_t &groups) override;

    void            set_max_delta_out_once(float delta_out_max) override { _delta_out_max = delta_out_max; }

    unsigned set_trim(float trim) override
    {
        return _rotor_count;
    }

    unsigned get_trim(float *trim) override
    {
        return _rotor_count;
    }

    void            set_thrust_factor(float val) override { _thrust_factor = val; }

    void            set_airmode(Airmode airmode) override;

    union saturation_status {
        struct {
            uint16_t valid      : 1; // 0 - true when the saturation status is used
            uint16_t motor_pos  : 1; // 1 - true when any motor has saturated in the positive direction
            uint16_t motor_neg  : 1; // 2 - true when any motor has saturated in the negative direction
            uint16_t roll_pos   : 1; // 3 - true when a positive roll demand change will increase saturation
            uint16_t roll_neg   : 1; // 4 - true when a negative roll demand change will increase saturation
            uint16_t pitch_pos  : 1; // 5 - true when a positive pitch demand change will increase saturation
            uint16_t pitch_neg  : 1; // 6 - true when a negative pitch demand change will increase saturation
            uint16_t yaw_pos    : 1; // 7 - true when a positive yaw demand change will increase saturation
            uint16_t yaw_neg    : 1; // 8 - true when a negative yaw demand change will increase saturation
            uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation
            uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation
        } flags;
        uint16_t value;
    };

private:
    float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status,
                    float min_output, float max_output) const;

    void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status,
                 float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const;

    inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs);

    inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs);

    inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);

    inline void mix_yaw(float yaw, float *outputs);

    void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw);

    float               _roll_scale;
    float               _pitch_scale;
    float               _yaw_scale;
    float               _idle_speed;
    float               _delta_out_max;
    float               _thrust_factor;

    Airmode             _airmode;

    saturation_status _saturation_status;

    unsigned            _rotor_count;
    const Rotor         *_rotors;

    float               *_outputs_prev = nullptr;
    float               *_tmp_array = nullptr;

    /* do not allow to copy due to ptr data members */
    MultirotorMixer(const MultirotorMixer &);
    MultirotorMixer operator=(const MultirotorMixer &);
};

struct mixer_heli_servo_s {
    float angle;
    float arm_length;
    float scale;
    float offset;
    float min_output;
    float max_output;
};

#define HELI_CURVES_NR_POINTS 5

struct mixer_heli_s {
    uint8_t             control_count;  
    float               throttle_curve[HELI_CURVES_NR_POINTS];
    float               pitch_curve[HELI_CURVES_NR_POINTS];
    struct mixer_heli_servo_s   servos[4];  
};

class HelicopterMixer : public Mixer
{
public:
    HelicopterMixer(ControlCallback control_cb,
            uintptr_t cb_handle,
            mixer_heli_s *mixer_info);

    ~HelicopterMixer() = default;

    static HelicopterMixer      *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
            unsigned &buflen);

    unsigned        mix(float *outputs, unsigned space) override;
    void            groups_required(uint32_t &groups) override;

    uint16_t        get_saturation_status(void) override { return 0; }

    unsigned set_trim(float trim) override
    {
        return 4;
    }

    unsigned get_trim(float *trim) override
    {
        return 4;
    }

private:
    mixer_heli_s            _mixer_info;

    /* do not allow to copy */
    HelicopterMixer(const HelicopterMixer &);
    HelicopterMixer operator=(const HelicopterMixer &);
};