diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index b879496871..aa2555f03e 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -6,13 +6,31 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_UPDATE_DT 0.02 // update rate in seconds. update() should be called at this rate -// set_angle_targets - sets angle targets in degrees -void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan) +// set angle target in degrees +// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame +void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) { // set angle targets - _angle_ef_target_rad.x = radians(roll); - _angle_ef_target_rad.y = radians(tilt); - _angle_ef_target_rad.z = radians(pan); + mavt_target.target_type = MountTargetType::ANGLE; + mavt_target.angle_rad.roll = radians(roll_deg); + mavt_target.angle_rad.pitch = radians(pitch_deg); + mavt_target.angle_rad.yaw = radians(yaw_deg); + mavt_target.angle_rad.yaw_is_ef = yaw_is_earth_frame; + + // set the mode to mavlink targeting + set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); +} + +// sets rate target in deg/s +// yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle) +void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_is_earth_frame) +{ + // set rate targets + mavt_target.target_type = MountTargetType::RATE; + mavt_target.rate_rads.roll = radians(roll_degs); + mavt_target.rate_rads.pitch = radians(pitch_degs); + mavt_target.rate_rads.yaw = radians(yaw_degs); + mavt_target.rate_rads.yaw_is_ef = yaw_is_earth_frame; // set the mode to mavlink targeting set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); @@ -66,7 +84,7 @@ void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_ // set earth frame target angles from mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: - set_angle_targets(roll_or_lon*0.01f, pitch_or_lat*0.01f, yaw_or_alt*0.01f); + set_angle_target(roll_or_lon*0.01f, pitch_or_lat*0.01f, yaw_or_alt*0.01f, false); break; // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization @@ -116,86 +134,100 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli return true; } -// update rate and angle targets from RC input -// current angle target (in radians) should be provided in angle_rad target -// rate and angle targets are returned in rate_rads and angle_rad arguments -// angle min and max are in centi-degrees -void AP_Mount_Backend::update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_cd, float angle_max_cd) const -{ - if ((chan == nullptr) || (chan->get_radio_in() == 0)) { - rate_rads = 0; - return; - } - rate_rads = chan->norm_input_dz() * radians(_frontend._rc_rate_max); - angle_rad = constrain_float(angle_rad + (rate_rads * AP_MOUNT_UPDATE_DT), radians(angle_min_cd*0.01f), radians(angle_max_cd*0.01f)); -} - -// update_targets_from_rc - updates angle targets using input from receiver -void AP_Mount_Backend::update_targets_from_rc() +// get pilot input (in the range -1 to +1) received through RC +void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const { const RC_Channel *roll_ch = rc().channel(_state._roll_rc_in - 1); - const RC_Channel *tilt_ch = rc().channel(_state._tilt_rc_in - 1); - const RC_Channel *pan_ch = rc().channel(_state._pan_rc_in - 1); + const RC_Channel *pitch_ch = rc().channel(_state._tilt_rc_in - 1); + const RC_Channel *yaw_ch = rc().channel(_state._pan_rc_in - 1); - // if joystick_speed is defined then pilot input defines a rate of change of the angle + roll_in = 0; + if ((roll_ch != nullptr) && (roll_ch->get_radio_in() > 0)) { + roll_in = roll_ch->norm_input_dz(); + } + + pitch_in = 0; + if ((pitch_ch != nullptr) && (pitch_ch->get_radio_in() > 0)) { + pitch_in = pitch_ch->norm_input_dz(); + } + + yaw_in = 0; + if ((yaw_ch != nullptr) && (yaw_ch->get_radio_in() > 0)) { + yaw_in = yaw_ch->norm_input_dz(); + } +} + +// get rate targets (in rad/s) from pilot RC +// returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets) +bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const +{ + // exit immediately if RC is not providing rate targets + if (_frontend._rc_rate_max <= 0) { + return false; + } + + // get RC input from pilot + float roll_in, pitch_in, yaw_in; + get_rc_input(roll_in, pitch_in, yaw_in); + + // calculate rates + const float rc_rate_max_rads = radians(_frontend._rc_rate_max.get()); + rate_rads.roll = roll_in * rc_rate_max_rads; + rate_rads.pitch = pitch_in * rc_rate_max_rads; + rate_rads.yaw = yaw_in * rc_rate_max_rads; + + // yaw frame + rate_rads.yaw_is_ef = _yaw_lock; + + return true; +} + +// get angle targets (in radians) from pilot RC +// returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets) +bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const +{ + // exit immediately if RC is not providing angle targets if (_frontend._rc_rate_max > 0) { - // allow pilot position input to come directly from an RC_Channel - update_rate_and_angle_from_rc(roll_ch, _rate_target_rads.x, _angle_ef_target_rad.x, _state._roll_angle_min, _state._roll_angle_max); - update_rate_and_angle_from_rc(tilt_ch, _rate_target_rads.y, _angle_ef_target_rad.y, _state._tilt_angle_min, _state._tilt_angle_max); - update_rate_and_angle_from_rc(pan_ch, _rate_target_rads.z, _angle_ef_target_rad.z, _state._pan_angle_min, _state._pan_angle_max); - _rate_target_rads_valid = true; + return false; + } + + // get RC input from pilot + float roll_in, pitch_in, yaw_in; + get_rc_input(roll_in, pitch_in, yaw_in); + + // roll angle + angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_state._roll_angle_max - _state._roll_angle_min) + _state._roll_angle_min)*0.01f); + + // pitch angle + angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_state._tilt_angle_max - _state._tilt_angle_min) + _state._tilt_angle_min)*0.01f); + + // yaw angle + angle_rad.yaw_is_ef = _yaw_lock; + if (angle_rad.yaw_is_ef) { + // if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg + angle_rad.yaw = yaw_in * M_PI; } else { - // allow pilot rate input to come directly from an RC_Channel - if ((roll_ch != nullptr) && (roll_ch->get_radio_in() != 0)) { - _angle_ef_target_rad.x = angle_input_rad(roll_ch, _state._roll_angle_min, _state._roll_angle_max); - } - if ((tilt_ch != nullptr) && (tilt_ch->get_radio_in() != 0)) { - _angle_ef_target_rad.y = angle_input_rad(tilt_ch, _state._tilt_angle_min, _state._tilt_angle_max); - } - if ((pan_ch != nullptr) && (pan_ch->get_radio_in() != 0)) { - _angle_ef_target_rad.z = angle_input_rad(pan_ch, _state._pan_angle_min, _state._pan_angle_max); - } - // not using rate input - _rate_target_rads_valid = false; + // yaw target in body frame so apply body frame limits + angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_state._pan_angle_max - _state._pan_angle_min) + _state._pan_angle_min)*0.01f); } + + return true; } -// returns the angle (radians) that the RC_Channel input is receiving -float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max) -{ - return radians(((rc->norm_input_ignore_trim() + 1.0f) * 0.5f * (angle_max - angle_min) + angle_min)*0.01f); -} - -bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const -{ - if (!_roi_target_set) { - return false; - } - return calc_angle_to_location(_roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan); -} - -bool AP_Mount_Backend::calc_angle_to_sysid_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const -{ - if (!_target_sysid_location_set) { - return false; - } - if (!_target_sysid) { - return false; - } - return calc_angle_to_location(_target_sysid_location, angles_to_target_rad, calc_tilt, calc_pan, relative_pan); -} - -// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (in radians) to point at the given target -bool AP_Mount_Backend::calc_angle_to_location(const Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const +// get angle targets (in radians) to a Location +// returns true on success, false on failure +bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const { + // exit immediately if vehicle's location is unavailable Location current_loc; if (!AP::ahrs().get_location(current_loc)) { return false; } - const float GPS_vector_x = Location::diff_longitude(target.lng,current_loc.lng)*cosf(ToRad((current_loc.lat+target.lat)*0.00000005f))*0.01113195f; - const float GPS_vector_y = (target.lat-current_loc.lat)*0.01113195f; + + const float GPS_vector_x = Location::diff_longitude(loc.lng, current_loc.lng)*cosf(ToRad((current_loc.lat + loc.lat) * 0.00000005f)) * 0.01113195f; + const float GPS_vector_y = (loc.lat - current_loc.lat) * 0.01113195f; int32_t target_alt_cm = 0; - if (!target.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm)) { + if (!loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm)) { return false; } int32_t current_alt_cm = 0; @@ -205,23 +237,102 @@ bool AP_Mount_Backend::calc_angle_to_location(const Location &target, Vector3f& float GPS_vector_z = target_alt_cm - current_alt_cm; float target_distance = 100.0f*norm(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. - // initialise all angles to zero - angles_to_target_rad.zero(); + // calculate roll, pitch, yaw angles + angle_rad.roll = 0; + angle_rad.pitch = atan2f(GPS_vector_z, target_distance); + angle_rad.yaw = atan2f(GPS_vector_x, GPS_vector_y); + angle_rad.yaw_is_ef = true; - // tilt calcs - if (calc_tilt) { - angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance); - } - - // pan calcs - if (calc_pan) { - // calc absolute heading and then convert to vehicle relative yaw - angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y); - if (relative_pan) { - angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - AP::ahrs().yaw); - } - } return true; } +// get angle targets (in radians) to ROI location +// returns true on success, false on failure +bool AP_Mount_Backend::get_angle_target_to_roi(MountTarget& angle_rad) const +{ + if (!_roi_target_set) { + return false; + } + return get_angle_target_to_location(_roi_target, angle_rad); +} + +// return body-frame yaw angle from a mount target +float AP_Mount_Backend::get_bf_yaw_angle(const MountTarget& angle_rad) const +{ + if (angle_rad.yaw_is_ef) { + // convert to body-frame + return wrap_PI(angle_rad.yaw - AP::ahrs().yaw); + } + + // target is already body-frame + return angle_rad.yaw; +} + +// return earth-frame yaw angle from a mount target +float AP_Mount_Backend::get_ef_yaw_angle(const MountTarget& angle_rad) const +{ + if (angle_rad.yaw_is_ef) { + // target is already earth-frame + return angle_rad.yaw; + } + + // convert to earth-frame + return wrap_PI(angle_rad.yaw + AP::ahrs().yaw); +} + +// update angle targets using a given rate target +// the resulting angle_rad yaw frame will match the rate_rad yaw frame +// assumes a 50hz update rate +void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const +{ + // update roll and pitch angles and apply limits + angle_rad.roll = constrain_float(angle_rad.roll + rate_rad.roll * AP_MOUNT_UPDATE_DT, radians(_state._roll_angle_min * 0.01), radians(_state._roll_angle_max * 0.01)); + angle_rad.pitch = constrain_float(angle_rad.pitch + rate_rad.pitch * AP_MOUNT_UPDATE_DT, radians(_state._tilt_angle_min * 0.01), radians(_state._tilt_angle_max * 0.01)); + + // ensure angle yaw frames matches rate yaw frame + if (angle_rad.yaw_is_ef != rate_rad.yaw_is_ef) { + if (rate_rad.yaw_is_ef) { + angle_rad.yaw = get_ef_yaw_angle(angle_rad); + } else { + angle_rad.yaw = get_bf_yaw_angle(angle_rad); + } + angle_rad.yaw_is_ef = rate_rad.yaw_is_ef; + } + + // update yaw angle target + angle_rad.yaw = angle_rad.yaw + rate_rad.yaw * AP_MOUNT_UPDATE_DT; + if (angle_rad.yaw_is_ef) { + // if earth-frame yaw wraps between += 180 degrees + angle_rad.yaw = wrap_PI(angle_rad.yaw); + } else { + // if body-frame constrain yaw to body-frame limits + angle_rad.yaw = constrain_float(angle_rad.yaw, radians(_state._pan_angle_min * 0.01), radians(_state._pan_angle_max * 0.01)); + } +} + +// get angle targets (in radians) to home location +// returns true on success, false on failure +bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const +{ + // exit immediately if home is not set + if (!AP::ahrs().home_is_set()) { + return false; + } + return get_angle_target_to_location(AP::ahrs().get_home(), angle_rad); +} + +// get angle targets (in radians) to a vehicle with sysid of _target_sysid +// returns true on success, false on failure +bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const +{ + // exit immediately if sysid is not set or no location available + if (!_target_sysid_location_set) { + return false; + } + if (!_target_sysid) { + return false; + } + return get_angle_target_to_location(_target_sysid_location, angle_rad); +} + #endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 05ee2a6492..c97d9962c5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -59,8 +59,13 @@ public: // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; } - // set_angle_targets - sets angle targets in degrees - void set_angle_targets(float roll, float tilt, float pan); + // set angle target in degrees + // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame + void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame); + + // sets rate target in deg/s + // yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle) + void set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_is_earth_frame); // set_roi_target - sets target location that mount should attempt to point towards void set_roi_target(const Location &target_loc); @@ -97,26 +102,56 @@ public: protected: - // update_targets_from_rc - updates angle targets (i.e. _angle_ef_target_rad) using input from receiver - void update_targets_from_rc(); + enum class MountTargetType { + ANGLE, + RATE, + }; - // angle_input_rad - convert RC input into an earth-frame target angle - float angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max); + // structure for a single angle or rate target + struct MountTarget { + float roll; + float pitch; + float yaw; + bool yaw_is_ef; + }; - // calc_angle_to_location - calculates the earth-frame roll, tilt - // and pan angles (in radians) to point at the given target - bool calc_angle_to_location(const Location &target, Vector3f& angles_to_target_rad, bool yaw_is_earth_frame) const WARN_IF_UNUSED; + // get pilot input (in the range -1 to +1) received through RC + void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const; - // calc_angle_to_roi_target - calculates the earth-frame roll, tilt - // and pan angles (in radians) to point at the ROI-target (as set - // by various mavlink messages) - bool calc_angle_to_roi_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const WARN_IF_UNUSED; + // get rate targets (in rad/s) from pilot RC + // returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets) + bool get_rc_rate_target(MountTarget& rate_rads) const WARN_IF_UNUSED; - // calc_angle_to_sysid_target - calculates the earth-frame roll, tilt - // and pan angles (in radians) to point at the sysid-target (as set - // by various mavlink messages) - bool calc_angle_to_sysid_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const WARN_IF_UNUSED; + // get angle targets (in radians) from pilot RC + // returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets) + bool get_rc_angle_target(MountTarget& angle_rad) const WARN_IF_UNUSED; + // get angle targets (in radians) to a Location + // returns true on success, false on failure + bool get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const WARN_IF_UNUSED; + + // get angle targets (in radians) to ROI location + // returns true on success, false on failure + bool get_angle_target_to_roi(MountTarget& angle_rad) const WARN_IF_UNUSED; + + // get angle targets (in radians) to home location + // returns true on success, false on failure + bool get_angle_target_to_home(MountTarget& angle_rad) const WARN_IF_UNUSED; + + // get angle targets (in radians) to a vehicle with sysid of _target_sysid + // returns true on success, false on failure + bool get_angle_target_to_sysid(MountTarget& angle_rad) const WARN_IF_UNUSED; + + // return body-frame yaw angle from a mount target + float get_bf_yaw_angle(const MountTarget& angle_rad) const; + + // return earth-frame yaw angle from a mount target + float get_ef_yaw_angle(const MountTarget& angle_rad) const; + + // update angle targets using a given rate target + // the resulting angle_rad yaw frame will match the rate_rad yaw frame + // assumes a 50hz update rate + void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const; AP_Mount &_frontend; // reference to the front end which holds parameters AP_Mount::mount_state &_state; // references to the parameters and state for this backend @@ -125,23 +160,19 @@ protected: MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) bool _yaw_lock; // True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame - Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and vehicle-relative pan angles in radians - Vector3f _rate_target_rads; // desired roll, pitch, yaw rate in radians/sec - bool _rate_target_rads_valid;// true if _rate_target_rads should can be used (e.g. RC input is using rate control) + // structure for MAVLink Targeting angle and rate targets + struct { + MountTargetType target_type;// MAVLink targeting mode's current target type (e.g. angle or rate) + MountTarget angle_rad; // angle target in radians + MountTarget rate_rads; // rate target in rad/s + } mavt_target; + Location _roi_target; // roi target location bool _roi_target_set; // true if the roi target has been set uint8_t _target_sysid; // sysid to track Location _target_sysid_location;// sysid target location bool _target_sysid_location_set;// true if _target_sysid has been set - -private: - - // update rate and angle targets from RC input - // current angle target (in radians) should be provided in angle_rad target - // rate and angle targets are returned in rate_rads and angle_rad arguments - // angle min and max are in centi-degrees - void update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_cd, float angle_max_cd) const; }; #endif // HAL_MOUNT_ENABLED