AP_Mount: backend restructure and support for ef/bf angle and rate

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
-    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;
-    } 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;
+    roll_in = 0;
+    if ((roll_ch != nullptr) && (roll_ch->get_radio_in() > 0)) {
+        roll_in = roll_ch->norm_input_dz();
     }
-}
 
-// 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);
+    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();
+    }
 }
 
-bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const
+// 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
 {
-    if (!_roi_target_set) {
+    // exit immediately if RC is not providing rate targets
+    if (_frontend._rc_rate_max <= 0) {
         return false;
     }
-    return calc_angle_to_location(_roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan);
+
+    // 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;
 }
 
-bool AP_Mount_Backend::calc_angle_to_sysid_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const
+// 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
 {
-    if (!_target_sysid_location_set) {
+    // exit immediately if RC is not providing angle targets
+    if (_frontend._rc_rate_max > 0) {
         return false;
     }
-    if (!_target_sysid) {
-        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 {
+        // 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 calc_angle_to_location(_target_sysid_location, angles_to_target_rad, calc_tilt, calc_pan, relative_pan);
+
+    return true;
 }
 
-// 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);
+    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);
+}
 
-    // 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 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;
     }
-    return true;
+
+    // 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

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,
+    };
+
+    // structure for a single angle or rate target
+    struct MountTarget {
+        float roll;
+        float pitch;
+        float yaw;
+        bool yaw_is_ef;
+    };
+
+    // 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;
+
+    // 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;
+
+    // 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;
 
-    // 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);
+    // 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;
 
-    // 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 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;
 
-    // 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 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;
 
-    // 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) 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