ArduPilot开源飞控之RC_Channels

作者：煮酒与君饮 | 2024-07-17 01:16:51

踩

开源飞控

ArduPilot开源飞控之RC_Channels

1. 源由
2. 框架设计
3. 重要例程
4. 总结
5. 参考资料

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. 总结

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

RC_Channels_Copter完善Copter模型RC_Channels类的应用实现；
RC_Channel_Copter完善Copter模型RC_Channel类的应用实现；
RC原始遥控通道数据来源于RC_Channel类的update方法；
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