diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index c8179e325995d7..2a5f5f3c3e165a 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -234,6 +234,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Plane}: 173:Plane AUTO Mode Landing Abort // @Values{Copter, Rover, Plane, Blimp}: 174:Camera Image Tracking // @Values{Copter, Rover, Plane, Blimp}: 175:Camera Lens + // @Values{Copter, Rover, Plane}: 117:Retract Mount1 3pos // @Values{Rover}: 201:Roll // @Values{Rover}: 202:Pitch // @Values{Rover}: 207:MainSail @@ -692,6 +693,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo #if HAL_MOUNT_ENABLED case AUX_FUNC::RETRACT_MOUNT1: case AUX_FUNC::MOUNT_LOCK: + case AUX_FUNC::RETRACT_MOUNT1_3POS: #endif case AUX_FUNC::LOG_PAUSE: case AUX_FUNC::ARM_EMERGENCY_STOP: @@ -768,6 +770,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { { AUX_FUNC::CAMERA_AUTO_FOCUS, "Camera Auto Focus"}, { AUX_FUNC::CAMERA_IMAGE_TRACKING, "Camera Image Tracking"}, { AUX_FUNC::CAMERA_LENS, "Camera Lens"}, + { AUX_FUNC::RETRACT_MOUNT1_3POS,"RetractMount1 3pos"}, }; /* lookup the announcement for switch change */ @@ -1540,6 +1543,26 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH); break; } + + case AUX_FUNC::RETRACT_MOUNT1_3POS: { + AP_Mount *mount = AP::mount(); + if (mount == nullptr) { + break; + } + switch (ch_flag) { + case AuxSwitchPos::HIGH: + if (mount->get_mode(0) > MAV_MOUNT_MODE_NEUTRAL) _mount_mode_last = mount->get_mode(0); + mount->set_mode(0,MAV_MOUNT_MODE_RETRACT); + break; + case AuxSwitchPos::MIDDLE: + if (_mount_mode_last > MAV_MOUNT_MODE_NEUTRAL) mount->set_mode(0, _mount_mode_last); + break; + case AuxSwitchPos::LOW: + mount->set_mode_to_default(0); + break; + } + break; + } #endif case AUX_FUNC::LOG_PAUSE: { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 46520602a1d038..58042c268eaaf0 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -6,6 +6,7 @@ #if AP_RC_CHANNEL_ENABLED +#include #include #include #include @@ -77,7 +78,7 @@ class RC_Channel { void set_override(const uint16_t v, const uint32_t timestamp_ms); bool has_override() const; - float stick_mixing(const float servo_in); + float stick_mixing(const float servo_in); // get control input with zero deadzone int16_t get_control_in_zero_dz(void) const; @@ -212,7 +213,7 @@ class RC_Channel { 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 + 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) @@ -249,7 +250,7 @@ class RC_Channel { CAMERA_IMAGE_TRACKING = 174, // camera image tracking CAMERA_LENS = 175, // camera lens selection VFWD_THR_OVERRIDE = 176, // force enabled VTOL forward throttle method - + RETRACT_MOUNT1_3POS = 177,// Retract Mount1 3pos // inputs from 200 will eventually used to replace RCMAP ROLL = 201, // roll input @@ -422,6 +423,8 @@ class RC_Channel { static const LookupTable lookuptable[]; #endif + + enum MAV_MOUNT_MODE _mount_mode_last = MAV_MOUNT_MODE_RETRACT; }; @@ -460,7 +463,7 @@ class RC_Channels { // 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 + // helper used by scripting to convert the above function from 0 to 1 indexing // range is checked correctly by the underlying channel function RC_Channel *lua_rc_channel(const uint8_t chan) { return channel(chan -1); @@ -471,7 +474,7 @@ class RC_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 + 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