当前位置:   article > 正文

ArduPilot开源飞控之RC_Channels

开源飞控

1. 源由

Ardupilot最为主要的一个控制方式就是远程遥控控制。
RC_Channels就是遥控通道实现控制的应用类。该应用类,将RC数据从RCInput按照规定协议获取,经过数据处理、规范化。再使用特定模型应用转化方法进行应用和解释,最终得到控制飞控的应用参数,比如:当前飞行模式。

2. 框架设计

2.1 继承关系

2.1.1 RC_Channel_Copter

继承实现RC_Channel的虚函数。

class RC_Channel_Copter : public RC_Channel
{

public:

protected:

    void init_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
    bool do_aux_function(aux_func_t ch_option, AuxSwitchPos) override;

private:

    void do_aux_function_change_mode(const Mode::Number mode,
                                     const AuxSwitchPos ch_flag);
    void do_aux_function_change_air_mode(const AuxSwitchPos ch_flag);
    void do_aux_function_change_force_flying(const AuxSwitchPos ch_flag);

    // called when the mode switch changes position:
    void mode_switch_changed(modeswitch_pos_t new_pos) override;

};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

2.1.2 RC_Channels_Copter

继承实现RC_Channels的虚函数。

class RC_Channels_Copter : public RC_Channels
{
public:

    bool has_valid_input() const override;
    bool in_rc_failsafe() const override;

    RC_Channel *get_arming_channel(void) const override;

    RC_Channel_Copter obj_channels[NUM_RC_CHANNELS];
    RC_Channel_Copter *channel(const uint8_t chan) override {
        if (chan >= NUM_RC_CHANNELS) {
            return nullptr;
        }
        return &obj_channels[chan];
    }

    // returns true if throttle arming checks should be run
    bool arming_check_throttle() const override;

protected:

    int8_t flight_mode_channel_number() const override;

};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25

2.1.3 RC_Channels

所有RC通道。

/*
  class	RC_Channels. Hold the full set of RC_Channel objects
*/
class RC_Channels {
public:
    friend class SRV_Channels;
    friend class RC_Channel;
    // constructor
    RC_Channels(void);

    void init(void);

    // get singleton instance
    static RC_Channels *get_singleton() {
        return _singleton;
    }

    static const struct AP_Param::GroupInfo var_info[];

    // compatability functions for Plane:
    static uint16_t get_radio_in(const uint8_t chan) {
        RC_Channel *c = _singleton->channel(chan);
        if (c == nullptr) {
            return 0;
        }
        return c->get_radio_in();
    }
    static RC_Channel *rc_channel(const uint8_t chan) {
        return _singleton->channel(chan);
    }
    //end compatability functions for Plane

    // this function is implemented in the child class in the vehicle
    // code
    virtual RC_Channel *channel(uint8_t chan) = 0;
    // helper used by scripting to convert the above function from 0 to 1 indexeing
    // range is checked correctly by the underlying channel function
    RC_Channel *lua_rc_channel(const uint8_t chan) {
        return channel(chan -1);
    }

    uint8_t get_radio_in(uint16_t *chans, const uint8_t num_channels); // reads a block of chanel radio_in values starting from channel 0
                                                                       // returns the number of valid channels

    static uint8_t get_valid_channel_count(void);                      // returns the number of valid channels in the last read
    static int16_t get_receiver_rssi(void);                            // returns [0, 255] for receiver RSSI (0 is no link) if present, otherwise -1
    static int16_t get_receiver_link_quality(void);                         // returns 0-100 % of last 100 packets received at receiver are valid
    bool read_input(void);                                             // returns true if new input has been read in
    static void clear_overrides(void);                                 // clears any active overrides
    static bool receiver_bind(const int dsmMode);                      // puts the receiver in bind mode if present, returns true if success
    static void set_override(const uint8_t chan, const int16_t value, const uint32_t timestamp_ms = 0); // set a channels override value
    static bool has_active_overrides(void);                            // returns true if there are overrides applied that are valid

    // returns a mask indicating which channels have overrides.  Bit 0
    // is RC channel 1.  Beware this is not a cheap call.
    uint16_t get_override_mask() const;

    class RC_Channel *find_channel_for_option(const RC_Channel::aux_func_t option);
    bool duplicate_options_exist();
    RC_Channel::AuxSwitchPos get_channel_pos(const uint8_t rcmapchan) const;
    void convert_options(const RC_Channel::aux_func_t old_option, const RC_Channel::aux_func_t new_option);

    void init_aux_all();
    void read_aux_all();

    // mode switch handling
    void reset_mode_switch();
    virtual void read_mode_switch();

    virtual bool in_rc_failsafe() const { return true; };
    virtual bool has_valid_input() const { return false; };

    virtual RC_Channel *get_arming_channel(void) const { return nullptr; };

    bool gcs_overrides_enabled() const { return _gcs_overrides_enabled; }
    void set_gcs_overrides_enabled(bool enable) {
        _gcs_overrides_enabled = enable;
        if (!_gcs_overrides_enabled) {
            clear_overrides();
        }
    }

    enum class Option {
        IGNORE_RECEIVER         = (1U << 0), // RC receiver modules
        IGNORE_OVERRIDES        = (1U << 1), // MAVLink overrides
        IGNORE_FAILSAFE         = (1U << 2), // ignore RC failsafe bits
        FPORT_PAD               = (1U << 3), // pad fport telem output
        LOG_RAW_DATA            = (1U << 4), // log rc input bytes
        ARMING_CHECK_THROTTLE   = (1U << 5), // run an arming check for neutral throttle
        ARMING_SKIP_CHECK_RPY   = (1U << 6), // skip the an arming checks for the roll/pitch/yaw channels
        ALLOW_SWITCH_REV        = (1U << 7), // honor the reversed flag on switches
        CRSF_CUSTOM_TELEMETRY   = (1U << 8), // use passthrough data for crsf telemetry
        SUPPRESS_CRSF_MESSAGE   = (1U << 9), // suppress CRSF mode/rate message for ELRS systems
        MULTI_RECEIVER_SUPPORT  = (1U << 10), // allow multiple receivers
        USE_CRSF_LQ_AS_RSSI     = (1U << 11), // returns CRSF link quality as RSSI value, instead of RSSI
        CRSF_FM_DISARM_STAR     = (1U << 12), // when disarmed, add a star at the end of the flight mode in CRSF telemetry
        ELRS_420KBAUD           = (1U << 13), // use 420kbaud for ELRS protocol
    };

    bool option_is_enabled(Option option) const {
        return _options & uint32_t(option);
    }

    virtual bool arming_check_throttle() const {
        return option_is_enabled(Option::ARMING_CHECK_THROTTLE);
    }

    // returns true if overrides should time out.  If true is returned
    // then returned_timeout_ms will contain the timeout in
    // milliseconds, with 0 meaning overrides are disabled.
    bool get_override_timeout_ms(uint32_t &returned_timeout_ms) const {
        const float value = _override_timeout.get();
        if (is_positive(value)) {
            returned_timeout_ms = uint32_t(value * 1e3f);
            return true;
        }
        if (is_zero(value)) {
            returned_timeout_ms = 0;
            return true;
        }
        // overrides will not time out
        return false;
    }

    // get mask of enabled protocols
    uint32_t enabled_protocols() const;

    // returns true if we have had a direct detach RC receiver, does not include overrides
    bool has_had_rc_receiver() const { return _has_had_rc_receiver; }

    // returns true if we have had an override on any channel
    bool has_had_rc_override() const { return _has_had_override; }

    /*
      get the RC input PWM value given a channel number.  Note that
      channel numbers start at 1, as this API is designed for use in
      LUA
    */
    bool get_pwm(uint8_t channel, uint16_t &pwm) const;

    uint32_t last_input_ms() const { return last_update_ms; };

    // method for other parts of the system (e.g. Button and mavlink)
    // to trigger auxiliary functions
    bool run_aux_function(RC_Channel::AUX_FUNC ch_option, RC_Channel::AuxSwitchPos pos, RC_Channel::AuxFuncTriggerSource source) {
        return rc_channel(0)->run_aux_function(ch_option, pos, source);
    }

    // check if flight mode channel is assigned RC option
    // return true if assigned
    bool flight_mode_channel_conflicts_with_rc_option() const;

    // flight_mode_channel_number must be overridden in vehicle specific code
    virtual int8_t flight_mode_channel_number() const = 0;

    // set and get calibrating flag, stops arming if true
    void calibrating(bool b) { gcs_is_calibrating = b; }
    bool calibrating() { return gcs_is_calibrating; }

#if AP_SCRIPTING_ENABLED
    // get last aux cached value for scripting. Returns false if never set, otherwise 0,1,2
    bool get_aux_cached(RC_Channel::aux_func_t aux_fn, uint8_t &pos);
#endif

    // get failsafe timeout in milliseconds
    uint32_t get_fs_timeout_ms() const { return MAX(_fs_timeout * 1000, 100); }

protected:

    void new_override_received() {
        has_new_overrides = true;
        _has_had_override = true;
    }

private:
    static RC_Channels *_singleton;
    // this static arrangement is to avoid static pointers in AP_Param tables
    static RC_Channel *channels;

    uint32_t last_update_ms;
    bool has_new_overrides;
    bool _has_had_rc_receiver; // true if we have had a direct detach RC receiver, does not include overrides
    bool _has_had_override; // true if we have had an override on any channel

    AP_Float _override_timeout;
    AP_Int32  _options;
    AP_Int32  _protocols;
    AP_Float _fs_timeout;

    RC_Channel *flight_mode_channel() const;

    // Allow override by default at start
    bool _gcs_overrides_enabled = true;

    // true if GCS is performing a RC calibration
    bool gcs_is_calibrating;

#if AP_SCRIPTING_ENABLED
    // bitmask of last aux function value, 2 bits per function
    // value 0 means never set, otherwise level+1
    HAL_Semaphore aux_cache_sem;
    Bitmask<unsigned(RC_Channel::AUX_FUNC::AUX_FUNCTION_MAX)*2> aux_cached;

    void set_aux_cached(RC_Channel::aux_func_t aux_fn, RC_Channel::AuxSwitchPos pos);
#endif
};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206

2.1.4 RC_Channel

单独一个RC通道。

/// @class	RC_Channel
/// @brief	Object managing one RC channel
class RC_Channel {
public:
    friend class SRV_Channels;
    friend class RC_Channels;
    // Constructor
    RC_Channel(void);

    enum class ControlType {
        ANGLE = 0,
        RANGE = 1,
    };

    // setup the control preferences
    void        set_range(uint16_t high);
    uint16_t    get_range() const { return high_in; }
    void        set_angle(uint16_t angle);
    bool        get_reverse(void) const;
    void        set_default_dead_zone(int16_t dzone);
    uint16_t    get_dead_zone(void) const { return dead_zone; }

    // get the center stick position expressed as a control_in value
    int16_t     get_control_mid() const;

    // read input from hal.rcin - create a control_in value
    bool        update(void);

    // calculate an angle given dead_zone and trim. This is used by the quadplane code
    // for hover throttle
    int16_t     pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim) const;

    // return a normalised input for a channel, in range -1 to 1,
    // centered around the channel trim. Ignore deadzone.
    float       norm_input() const;

    // return a normalised input for a channel, in range -1 to 1,
    // centered around the channel trim. Take into account the deadzone
    float       norm_input_dz() const;

    // return a normalised input for a channel, in range -1 to 1,
    // ignores trim and deadzone
    float       norm_input_ignore_trim() const;

    // returns true if input is within deadzone of min
    bool        in_min_dz() const;

    uint8_t     percent_input() const;

    static const struct AP_Param::GroupInfo var_info[];

    // return true if input is within deadzone of trim
    bool       in_trim_dz() const;

    int16_t    get_radio_in() const { return radio_in;}
    void       set_radio_in(int16_t val) {radio_in = val;}

    int16_t    get_control_in() const { return control_in;}
    void       set_control_in(int16_t val) { control_in = val;}

    void       clear_override();
    void       set_override(const uint16_t v, const uint32_t timestamp_ms);
    bool       has_override() const;

    float    stick_mixing(const float servo_in);

    // get control input with zero deadzone
    int16_t    get_control_in_zero_dz(void) const;

    int16_t    get_radio_min() const {return radio_min.get();}

    int16_t    get_radio_max() const {return radio_max.get();}

    int16_t    get_radio_trim() const { return radio_trim.get();}

    void       set_and_save_trim() { radio_trim.set_and_save_ifchanged(radio_in);}

    // set and save trim if changed
    void       set_and_save_radio_trim(int16_t val) { radio_trim.set_and_save_ifchanged(val);}

    // check if any of the trim/min/max param are configured, this would indicate that the user has done a calibration at somepoint
    bool       configured() { return radio_min.configured() || radio_max.configured() || radio_trim.configured(); }

    ControlType get_type(void) const { return type_in; }

    AP_Int16    option; // e.g. activate EPM gripper / enable fence

    // auxiliary switch support
    void init_aux();
    bool read_aux();

    // Aux Switch enumeration
    enum class AUX_FUNC {
        DO_NOTHING =           0, // aux switch disabled
        FLIP =                 2, // flip
        SIMPLE_MODE =          3, // change to simple mode
        RTL =                  4, // change to RTL flight mode
        SAVE_TRIM =            5, // save current position as level
        SAVE_WP =              7, // save mission waypoint or RTL if in auto mode
        CAMERA_TRIGGER =       9, // trigger camera servo or relay
        RANGEFINDER =         10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground
        FENCE =               11, // allow enabling or disabling fence in flight
        RESETTOARMEDYAW =     12, // UNUSED
        SUPERSIMPLE_MODE =    13, // change to simple mode in middle, super simple at top
        ACRO_TRAINER =        14, // low = disabled, middle = leveled, high = leveled and limited
        SPRAYER =             15, // enable/disable the crop sprayer
        AUTO =                16, // change to auto flight mode
        AUTOTUNE =            17, // auto tune
        LAND =                18, // change to LAND flight mode
        GRIPPER =             19, // Operate cargo grippers low=off, middle=neutral, high=on
        PARACHUTE_ENABLE  =   21, // Parachute enable/disable
        PARACHUTE_RELEASE =   22, // Parachute release
        PARACHUTE_3POS =      23, // Parachute disable, enable, release with 3 position switch
        MISSION_RESET =       24, // Reset auto mission to start from first command
        ATTCON_FEEDFWD =      25, // enable/disable the roll and pitch rate feed forward
        ATTCON_ACCEL_LIM =    26, // enable/disable the roll, pitch and yaw accel limiting
        RETRACT_MOUNT1 =      27, // Retract Mount1
        RELAY =               28, // Relay pin on/off (only supports first relay)
        LANDING_GEAR =        29, // Landing gear controller
        LOST_VEHICLE_SOUND =  30, // Play lost vehicle sound
        MOTOR_ESTOP =         31, // Emergency Stop Switch
        MOTOR_INTERLOCK =     32, // Motor On/Off switch
        BRAKE =               33, // Brake flight mode
        RELAY2 =              34, // Relay2 pin on/off
        RELAY3 =              35, // Relay3 pin on/off
        RELAY4 =              36, // Relay4 pin on/off
        THROW =               37, // change to THROW flight mode
        AVOID_ADSB =          38, // enable AP_Avoidance library
        PRECISION_LOITER =    39, // enable precision loiter
        AVOID_PROXIMITY =     40, // enable object avoidance using proximity sensors (ie. horizontal lidar)
        ARMDISARM_UNUSED =    41, // UNUSED
        SMART_RTL =           42, // change to SmartRTL flight mode
        INVERTED  =           43, // enable inverted flight
        WINCH_ENABLE =        44, // winch enable/disable
        WINCH_CONTROL =       45, // winch control
        RC_OVERRIDE_ENABLE =  46, // enable RC Override
        USER_FUNC1 =          47, // user function #1
        USER_FUNC2 =          48, // user function #2
        USER_FUNC3 =          49, // user function #3
        LEARN_CRUISE =        50, // learn cruise throttle (Rover)
        MANUAL       =        51, // manual mode
        ACRO         =        52, // acro mode
        STEERING     =        53, // steering mode
        HOLD         =        54, // hold mode
        GUIDED       =        55, // guided mode
        LOITER       =        56, // loiter mode
        FOLLOW       =        57, // follow mode
        CLEAR_WP     =        58, // clear waypoints
        SIMPLE       =        59, // simple mode
        ZIGZAG       =        60, // zigzag mode
        ZIGZAG_SaveWP =       61, // zigzag save waypoint
        COMPASS_LEARN =       62, // learn compass offsets
        SAILBOAT_TACK =       63, // rover sailboat tack
        REVERSE_THROTTLE =    64, // reverse throttle input
        GPS_DISABLE  =        65, // disable GPS for testing
        RELAY5 =              66, // Relay5 pin on/off
        RELAY6 =              67, // Relay6 pin on/off
        STABILIZE =           68, // stabilize mode
        POSHOLD   =           69, // poshold mode
        ALTHOLD   =           70, // althold mode
        FLOWHOLD  =           71, // flowhold mode
        CIRCLE    =           72, // circle mode
        DRIFT     =           73, // drift mode
        SAILBOAT_MOTOR_3POS = 74, // Sailboat motoring 3pos
        SURFACE_TRACKING =    75, // Surface tracking upwards or downwards
        STANDBY  =            76, // Standby mode
        TAKEOFF   =           77, // takeoff
        RUNCAM_CONTROL =      78, // control RunCam device
        RUNCAM_OSD_CONTROL =  79, // control RunCam OSD
        VISODOM_ALIGN =       80, // align visual odometry camera's attitude to AHRS
        DISARM =              81, // disarm vehicle
        Q_ASSIST =            82, // disable, enable and force Q assist
        ZIGZAG_Auto =         83, // zigzag auto switch
        AIRMODE =             84, // enable / disable airmode for copter
        GENERATOR   =         85, // generator control
        TER_DISABLE =         86, // disable terrain following in CRUISE/FBWB modes
        CROW_SELECT =         87, // select CROW mode for diff spoilers;high disables,mid forces progressive
        SOARING =             88, // three-position switch to set soaring mode
        LANDING_FLARE =       89, // force flare, throttle forced idle, pitch to LAND_PITCH_CD, tilts up
        EKF_POS_SOURCE =      90, // change EKF position source between primary, secondary and tertiary sources
        ARSPD_CALIBRATE=      91, // calibrate airspeed ratio 
        FBWA =                92, // Fly-By-Wire-A
        RELOCATE_MISSION =    93, // used in separate branch MISSION_RELATIVE
        VTX_POWER =           94, // VTX power level
        FBWA_TAILDRAGGER =    95, // enables FBWA taildragger takeoff mode. Once this feature is enabled it will stay enabled until the aircraft goes above TKOFF_TDRAG_SPD1 airspeed, changes mode, or the pitch goes above the initial pitch when this is engaged or goes below 0 pitch. When enabled the elevator will be forced to TKOFF_TDRAG_ELEV. This option allows for easier takeoffs on taildraggers in FBWA mode, and also makes it easier to test auto-takeoff steering handling in FBWA.
        MODE_SWITCH_RESET =   96, // trigger re-reading of mode switch
        WIND_VANE_DIR_OFSSET= 97, // flag for windvane direction offset input, used with windvane type 2
        TRAINING            = 98, // mode training
        AUTO_RTL =            99, // AUTO RTL via DO_LAND_START

        // entries from 100-150  are expected to be developer
        // options used for testing
        KILL_IMU1 =          100, // disable first IMU (for IMU failure testing)
        KILL_IMU2 =          101, // disable second IMU (for IMU failure testing)
        CAM_MODE_TOGGLE =    102, // Momentary switch to cycle camera modes
        EKF_LANE_SWITCH =    103, // trigger lane switch attempt
        EKF_YAW_RESET =      104, // trigger yaw reset attempt
        GPS_DISABLE_YAW =    105, // disable GPS yaw for testing
        DISABLE_AIRSPEED_USE = 106, // equivalent to AIRSPEED_USE 0
        FW_AUTOTUNE =          107, // fixed wing auto tune
        QRTL =               108, // QRTL mode
        CUSTOM_CONTROLLER =  109,  // use Custom Controller
        KILL_IMU3 =          110, // disable third IMU (for IMU failure testing)
        LOWEHEISER_STARTER = 111,  // allows for manually running starter

        // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp!
        // also, if you add an option >255, you will need to fix duplicate_options_exist

        // options 150-199 continue user rc switch options
        CRUISE =             150,  // CRUISE mode
        TURTLE =             151,  // Turtle mode - flip over after crash
        SIMPLE_HEADING_RESET = 152, // reset simple mode reference heading to current
        ARMDISARM =          153, // arm or disarm vehicle
        ARMDISARM_AIRMODE =  154, // arm or disarm vehicle enabling airmode
        TRIM_TO_CURRENT_SERVO_RC = 155, // trim to current servo and RC
        TORQEEDO_CLEAR_ERR = 156, // clear torqeedo error
        EMERGENCY_LANDING_EN = 157, //Force long FS action to FBWA for landing out of range
        OPTFLOW_CAL =        158, // optical flow calibration
        FORCEFLYING =        159, // enable or disable land detection for GPS based manual modes preventing land detection and maintainting set_throttle_mix_max
        WEATHER_VANE_ENABLE = 160, // enable/disable weathervaning
        TURBINE_START =      161, // initialize turbine start sequence
        FFT_NOTCH_TUNE =     162, // FFT notch tuning function
        MOUNT_LOCK =         163, // Mount yaw lock vs follow
        LOG_PAUSE =          164, // Pauses logging if under logging rate control
        ARM_EMERGENCY_STOP = 165, // ARM on high, MOTOR_ESTOP on low
        CAMERA_REC_VIDEO =   166, // start recording on high, stop recording on low
        CAMERA_ZOOM =        167, // camera zoom high = zoom in, middle = hold, low = zoom out
        CAMERA_MANUAL_FOCUS = 168,// camera manual focus.  high = long shot, middle = stop focus, low = close shot
        CAMERA_AUTO_FOCUS =  169, // camera auto focus
        QSTABILIZE =         170, // QuadPlane QStabilize mode
        MAG_CAL =            171, // Calibrate compasses (disarmed only)
        BATTERY_MPPT_ENABLE = 172,// Battery MPPT Power enable. high = ON, mid = auto (controlled by mppt/batt driver), low = OFF. This effects all MPPTs.
        PLANE_AUTO_LANDING_ABORT = 173, // Abort Glide-slope or VTOL landing during payload place or do_land type mission items
        CAMERA_IMAGE_TRACKING = 174, // camera image tracking
        CAMERA_LENS =        175, // camera lens selection
        VFWD_THR_OVERRIDE =  176, // force enabled VTOL forward throttle method


        // inputs from 200 will eventually used to replace RCMAP
        ROLL =               201, // roll input
        PITCH =              202, // pitch input
        THROTTLE =           203, // throttle pilot input
        YAW =                204, // yaw pilot input
        MAINSAIL =           207, // mainsail input
        FLAP =               208, // flap input
        FWD_THR =            209, // VTOL manual forward throttle
        AIRBRAKE =           210, // manual airbrake control
        WALKING_HEIGHT =     211, // walking robot height input
        MOUNT1_ROLL =        212, // mount1 roll input
        MOUNT1_PITCH =       213, // mount1 pitch input
        MOUNT1_YAW =         214, // mount1 yaw input
        MOUNT2_ROLL =        215, // mount2 roll input
        MOUNT2_PITCH =       216, // mount3 pitch input
        MOUNT2_YAW =         217, // mount4 yaw input
        LOWEHEISER_THROTTLE= 218,  // allows for throttle on slider

        // inputs 248-249 are reserved for the Skybrush fork at
        // https://github.com/skybrush-io/ardupilot

        // inputs for the use of onboard lua scripting
        SCRIPTING_1 =        300,
        SCRIPTING_2 =        301,
        SCRIPTING_3 =        302,
        SCRIPTING_4 =        303,
        SCRIPTING_5 =        304,
        SCRIPTING_6 =        305,
        SCRIPTING_7 =        306,
        SCRIPTING_8 =        307,

        // this must be higher than any aux function above
        AUX_FUNCTION_MAX =   308,
    };
    typedef enum AUX_FUNC aux_func_t;

    // auxiliary switch handling (n.b.: we store this as 2-bits!):
    enum class AuxSwitchPos : uint8_t {
        LOW,       // indicates auxiliary switch is in the low position (pwm <1200)
        MIDDLE,    // indicates auxiliary switch is in the middle position (pwm >1200, <1800)
        HIGH       // indicates auxiliary switch is in the high position (pwm >1800)
    };

    enum class AuxFuncTriggerSource : uint8_t {
        INIT,
        RC,
        BUTTON,
        MAVLINK,
        MISSION,
        SCRIPTING,
    };

    AuxSwitchPos get_aux_switch_pos() const;

    // wrapper function around do_aux_function which allows us to log
    bool run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source);

#if AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED
    const char *string_for_aux_function(AUX_FUNC function) const;
    const char *string_for_aux_pos(AuxSwitchPos pos) const;
#endif
    // pwm value under which we consider that Radio value is invalid
    static const uint16_t RC_MIN_LIMIT_PWM = 800;
    // pwm value above which we consider that Radio value is invalid
    static const uint16_t RC_MAX_LIMIT_PWM = 2200;

    // pwm value above which we condider that Radio min value is invalid
    static const uint16_t RC_CALIB_MIN_LIMIT_PWM = 1300;
    // pwm value under which we condider that Radio max value is invalid
    static const uint16_t RC_CALIB_MAX_LIMIT_PWM = 1700;

    // pwm value above which the switch/button will be invoked:
    static const uint16_t AUX_SWITCH_PWM_TRIGGER_HIGH = 1800;
    // pwm value below which the switch/button will be disabled:
    static const uint16_t AUX_SWITCH_PWM_TRIGGER_LOW = 1200;

    // pwm value above which the option will be invoked:
    static const uint16_t AUX_PWM_TRIGGER_HIGH = 1700;
    // pwm value below which the option will be disabled:
    static const uint16_t AUX_PWM_TRIGGER_LOW = 1300;

protected:

    virtual void init_aux_function(aux_func_t ch_option, AuxSwitchPos);

    // virtual function to be overridden my subclasses
    virtual bool do_aux_function(aux_func_t ch_option, AuxSwitchPos);

    void do_aux_function_armdisarm(const AuxSwitchPos ch_flag);
    void do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag);
    void do_aux_function_avoid_proximity(const AuxSwitchPos ch_flag);
    void do_aux_function_camera_trigger(const AuxSwitchPos ch_flag);
    bool do_aux_function_record_video(const AuxSwitchPos ch_flag);
    bool do_aux_function_camera_zoom(const AuxSwitchPos ch_flag);
    bool do_aux_function_camera_manual_focus(const AuxSwitchPos ch_flag);
    bool do_aux_function_camera_auto_focus(const AuxSwitchPos ch_flag);
    bool do_aux_function_camera_image_tracking(const AuxSwitchPos ch_flag);
    bool do_aux_function_camera_lens(const AuxSwitchPos ch_flag);
    void do_aux_function_runcam_control(const AuxSwitchPos ch_flag);
    void do_aux_function_runcam_osd_control(const AuxSwitchPos ch_flag);
    void do_aux_function_fence(const AuxSwitchPos ch_flag);
    void do_aux_function_clear_wp(const AuxSwitchPos ch_flag);
    void do_aux_function_gripper(const AuxSwitchPos ch_flag);
    void do_aux_function_lost_vehicle_sound(const AuxSwitchPos ch_flag);
    void do_aux_function_mission_reset(const AuxSwitchPos ch_flag);
    void do_aux_function_rc_override_enable(const AuxSwitchPos ch_flag);
    void do_aux_function_relay(uint8_t relay, bool val);
    void do_aux_function_sprayer(const AuxSwitchPos ch_flag);
    void do_aux_function_generator(const AuxSwitchPos ch_flag);
    void do_aux_function_fft_notch_tune(const AuxSwitchPos ch_flag);

    typedef int8_t modeswitch_pos_t;
    virtual void mode_switch_changed(modeswitch_pos_t new_pos) {
        // no action by default (e.g. Tracker, Sub, who do their own thing)
    };


private:

    // pwm is stored here
    int16_t     radio_in;

    // value generated from PWM normalised to configured scale
    int16_t    control_in;

    AP_Int16    radio_min;
    AP_Int16    radio_trim;
    AP_Int16    radio_max;

    AP_Int8     reversed;
    AP_Int16    dead_zone;

    ControlType type_in;
    int16_t     high_in;

    // the input channel this corresponds to
    uint8_t     ch_in;

    // overrides
    uint16_t override_value;
    uint32_t last_override_time;

    int16_t pwm_to_angle() const;
    int16_t pwm_to_angle_dz(uint16_t dead_zone) const;

    int16_t pwm_to_range() const;
    int16_t pwm_to_range_dz(uint16_t dead_zone) const;

    bool read_3pos_switch(AuxSwitchPos &ret) const WARN_IF_UNUSED;
    bool read_6pos_switch(int8_t& position) WARN_IF_UNUSED;

    // Structure used to detect and debounce switch changes
    struct {
        int8_t debounce_position = -1;
        int8_t current_position = -1;
        uint32_t last_edge_time_ms;
    } switch_state;

    void reset_mode_switch();
    void read_mode_switch();
    bool debounce_completed(int8_t position);

#if AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED
    // Structure to lookup switch change announcements
    struct LookupTable{
       AUX_FUNC option;
       const char *announcement;
    };

    static const LookupTable lookuptable[];
#endif
};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268
  • 269
  • 270
  • 271
  • 272
  • 273
  • 274
  • 275
  • 276
  • 277
  • 278
  • 279
  • 280
  • 281
  • 282
  • 283
  • 284
  • 285
  • 286
  • 287
  • 288
  • 289
  • 290
  • 291
  • 292
  • 293
  • 294
  • 295
  • 296
  • 297
  • 298
  • 299
  • 300
  • 301
  • 302
  • 303
  • 304
  • 305
  • 306
  • 307
  • 308
  • 309
  • 310
  • 311
  • 312
  • 313
  • 314
  • 315
  • 316
  • 317
  • 318
  • 319
  • 320
  • 321
  • 322
  • 323
  • 324
  • 325
  • 326
  • 327
  • 328
  • 329
  • 330
  • 331
  • 332
  • 333
  • 334
  • 335
  • 336
  • 337
  • 338
  • 339
  • 340
  • 341
  • 342
  • 343
  • 344
  • 345
  • 346
  • 347
  • 348
  • 349
  • 350
  • 351
  • 352
  • 353
  • 354
  • 355
  • 356
  • 357
  • 358
  • 359
  • 360
  • 361
  • 362
  • 363
  • 364
  • 365
  • 366
  • 367
  • 368
  • 369
  • 370
  • 371
  • 372
  • 373
  • 374
  • 375
  • 376
  • 377
  • 378
  • 379
  • 380
  • 381
  • 382
  • 383
  • 384
  • 385
  • 386
  • 387
  • 388
  • 389
  • 390
  • 391
  • 392
  • 393
  • 394
  • 395
  • 396
  • 397
  • 398
  • 399
  • 400
  • 401
  • 402
  • 403
  • 404
  • 405
  • 406
  • 407
  • 408
  • 409
  • 410

2.2 启动代码

Copter::init_ardupilot
 └──> RC_Channels::init
  • 1
  • 2

2.3 任务代码

SCHED_TASK(rc_loop,              250,    130,  3),
 └──> Copter::rc_loop
     └──> Copter::read_radio
         └──> RC_Channels::read_input
  • 1
  • 2
  • 3
  • 4

3. 重要例程

3.1 RC_Channels

RC通道唯一实例对象初始化。

RC_Channels::RC_Channels
 │
 │  // set defaults from the parameter table
 ├──> AP_Param::setup_object_defaults(this, var_info)
 │
 │  // only one instance
 ├──> <_singleton != nullptr>
 │   └──> AP_HAL::panic("RC_Channels must be singleton")
 └──> _singleton = this
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

3.2 init

初始化所有RC通道。

RC_Channels::init
 │
 │   // setup ch_in on channels
 ├──> <for (uint8_t i=0 i<NUM_RC_CHANNELS i++)>
 │   └──> channel(i)->ch_in = i
 └──> init_aux_all()
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
RC_Channels::init_aux_all
 ├──> <for (uint8_t i=0 i<NUM_RC_CHANNELS i++)>
 │   ├──> RC_Channel *c = channel(i)
 │   ├──> <c == nullptr>
 │   │   └──> continue
 │   └──> c->init_aux()
 └──> reset_mode_switch()
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

3.3 read_input

// update all the input channels
RC_Channels::read_input
 │
 │   //check rc new input
 ├──> <hal.rcin->new_input()) {
 │   └──> _has_had_rc_receiver = true
 ├──> < else if (!has_new_overrides) >
 │   └──> return false
 ├──> has_new_overrides = false
 │
 ├──> last_update_ms = AP_HAL::millis()
 │
 │   //update all rc channels
 ├──> bool success = false
 ├──> <for (uint8_t i=0 i<NUM_RC_CHANNELS i++)>
 │   └──> success |= channel(i)->update()
 └──> return success
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17

3.4 update

RC遥控数据,详见:ArduPilot开源代码之RCInput

// read input from hal.rcin or overrides
RC_Channel::update
 │
 │   //update rc channel in
 ├──> <has_override() && !rc().option_is_enabled(RC_Channels::Option::IGNORE_OVERRIDES)>
 │   └──> radio_in = override_value
 ├──> < else if (rc().has_had_rc_receiver() && !rc().option_is_enabled(RC_Channels::Option::IGNORE_RECEIVER))>
 │   └──> radio_in = hal.rcin->read(ch_in)
 ├──> < else >
 │   └──> return false
 │
 │   //calculate deadzone
 ├──> <type_in == ControlType::RANGE>
 │   └──> control_in = pwm_to_range()
 ├──> < else >
 │   └──> control_in = pwm_to_angle() // // ControlType::ANGLE
 └──> return true
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17

4. 总结

从整个代码设计逻辑上可以分析出以下几点:

  1. RC_Channels_Copter完善Copter模型RC_Channels类的应用实现;
  2. RC_Channel_Copter完善Copter模型RC_Channel类的应用实现;
  3. RC原始遥控通道数据来源于RC_Channel类的update方法;
  4. RC遥控协议的解析主要在RCInput完成;

RC数据流方向:RCInput ==》 RC_Channel + RC_Channel_Copter ==》 RC_Channels + RC_Channels_Copter

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/煮酒与君饮/article/detail/837242
推荐阅读
相关标签
  

闽ICP备14008679号