diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index a0786ac749..869306a293 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -9,6 +9,13 @@ #define ENABLED 1 #define DISABLED 0 +#define MASK_TILT (1<<0) +#define MASK_ROLL (1<<1) +#define MASK_YAW (1<<2) +#define MASK_RETRACT (1<<3) + + + #if defined( __AVR_ATmega1280__ ) # define MNT_JSTICK_SPD_OPTION DISABLED // Allow RC joystick to control the speed of the mount movements instead of the position of the mount # define MNT_RETRACT_OPTION DISABLED // Use a servo to retract the mount inside the fuselage (i.e. for landings) @@ -276,21 +283,31 @@ void AP_Mount::update_mount_type() { bool have_roll, have_tilt, have_pan; + have_roll = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_roll) || RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_roll); + have_tilt = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_tilt) || RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_tilt); + have_pan = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_pan) || RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_pan); - if (have_pan && have_tilt && !have_roll) { + + mount_axis_mask = 0; + + if (have_pan) + mount_axis_mask |= MASK_YAW; + if (have_tilt) + mount_axis_mask |= MASK_TILT; + if (have_roll) + mount_axis_mask |= MASK_ROLL; + + if (have_pan && have_tilt && !have_roll) _mount_type = k_pan_tilt; - } - if (!have_pan && have_tilt && have_roll) { + if (!have_pan && have_tilt && have_roll) _mount_type = k_tilt_roll; - } - if (have_pan && have_tilt && have_roll) { + if (have_pan && have_tilt && have_roll) _mount_type = k_pan_tilt_roll; - } } /// sets the servo angles for retraction, note angles are in degrees @@ -598,8 +615,18 @@ AP_Mount::calc_GPS_target_angle(const struct Location *target) float GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter). float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. _roll_control_angle = 0; - _tilt_control_angle = atan2f(GPS_vector_z, target_distance); - _pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y); + + if (mount_axis_mask & MASK_TILT) { + _tilt_control_angle = atan2f(GPS_vector_z, target_distance); + } else { + _tilt_control_angle = 0; + } + + if (mount_axis_mask & MASK_YAW) { + _pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y); + } else { + _pan_control_angle = 0; + } } /// Stabilizes mount relative to the Earth's frame diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index fa99d03d5c..92e9629ffa 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -98,6 +98,8 @@ private: uint8_t _pan_idx; ///< RC_Channel_aux mount pan function index uint8_t _open_idx; ///< RC_Channel_aux mount open function index + uint8_t mount_axis_mask; // used to track user input on each axis + float _roll_control_angle; ///< radians float _tilt_control_angle; ///< radians float _pan_control_angle; ///< radians