AC_PrecLand: redo math, simplify interface

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -32,7 +32,7 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
 AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
     _ahrs(ahrs),
     _inav(inav),
-    _have_estimate(false),
+    _last_update_ms(0),
     _backend(NULL)
 {
     // set parameters to defaults
@@ -92,29 +92,37 @@ void AC_PrecLand::update(float alt_above_terrain_cm)
     }
 }
 
-// get_target_shift - returns 3D vector of earth-frame position adjustments to target
-Vector3f AC_PrecLand::get_target_shift(const Vector3f &orig_target)
+bool AC_PrecLand::target_acquired()
 {
-    Vector3f shift; // default shift initialised to zero
+    return (AP_HAL::millis()-_last_update_ms) < 1000;
+}
 
-    // do not shift target if not enabled or no position estimate
-    if (_backend == NULL || !_have_estimate) {
-        return shift;
+bool AC_PrecLand::get_target_position(Vector3f& ret)
+{
+    if (!target_acquired()) {
+        return false;
     }
 
-    // shift is target_offset - (original target - current position)
-    Vector3f curr_offset_from_target = orig_target - _inav.get_position();
-    shift = _target_pos_offset - curr_offset_from_target;
-    shift.z = 0.0f;
+    ret = _target_pos;
+    return true;
+}
+
+bool AC_PrecLand::get_target_position_relative(Vector3f& ret)
+{
+    if (!target_acquired()) {
+        return false;
+    }
 
-    // record we have consumed this reading (perhaps there is a cleaner way to do this using timestamps)
-    _have_estimate = false;
+    ret = _target_pos_rel;
+    return true;
+}
 
-    // return adjusted target
-    return shift;
+bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret)
+{
+    return false;
 }
 
-// calc_angles_and_pos - converts sensor's body-frame angles to earth-frame angles and position estimate
+// converts sensor's body-frame angles to earth-frame angles and position estimate
 //  raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame)
 //  earth-frame angles stored in _ef_angle_to_target
 //  position estimate is stored in _target_pos
@@ -122,42 +130,56 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm)
 {
     // exit immediately if not enabled
     if (_backend == NULL) {
-        _have_estimate = false;
         return;
     }
 
     // get angles to target from backend
     if (!_backend->get_angle_to_target(_angle_to_target.x, _angle_to_target.y)) {
-        _have_estimate = false;
         return;
     }
 
-    float x_rad;
-    float y_rad;
-
-    if(_backend->get_frame_of_reference() == MAV_FRAME_LOCAL_NED){
-        //don't subtract vehicle lean angles
-        x_rad = _angle_to_target.x;
-        y_rad = -_angle_to_target.y;
-    }else{ // assume MAV_FRAME_BODY_NED (i.e. a hard-mounted sensor)
-        // subtract vehicle lean angles
-        x_rad = _angle_to_target.x - _ahrs.roll;
-        y_rad = -_angle_to_target.y + _ahrs.pitch;
-    }
+    _angle_to_target.x = -_angle_to_target.x;
+    _angle_to_target.y = -_angle_to_target.y;
 
-    // rotate to earth-frame angles
-    _ef_angle_to_target.x = y_rad*_ahrs.cos_yaw() - x_rad*_ahrs.sin_yaw();
-    _ef_angle_to_target.y = y_rad*_ahrs.sin_yaw() + x_rad*_ahrs.cos_yaw();
+    // compensate for delay
+    _angle_to_target.x -= _ahrs.get_gyro().x*4.0e-2f;
+    _angle_to_target.y -= _ahrs.get_gyro().y*4.0e-2f;
 
-    // get current altitude (constrained to no lower than 50cm)
-    float alt = MAX(alt_above_terrain_cm, 50.0f);
+    Vector3f target_vec_ned;
 
-    // convert earth-frame angles to earth-frame position offset
-    _target_pos_offset.x = alt*tanf(_ef_angle_to_target.x);
-    _target_pos_offset.y = alt*tanf(_ef_angle_to_target.y);
-    _target_pos_offset.z = 0;  // not used
+    if (_angle_to_target.is_zero()) {
+        target_vec_ned = Vector3f(0.0f,0.0f,1.0f);
+    } else {
+        float theta = _angle_to_target.length();
+        Vector3f axis = Vector3f(_angle_to_target.x, _angle_to_target.y, 0.0f)/theta;
+        float sin_theta = sinf(theta);
+        float cos_theta = cosf(theta);
 
-    _have_estimate = true;
+        target_vec_ned.x = axis.x*axis.z*(1.0f - cos_theta) + axis.y*sin_theta;
+        target_vec_ned.y = axis.y*axis.z*(1.0f - cos_theta) - axis.x*sin_theta;
+        target_vec_ned.z = cos_theta + sq(axis.z)*(1.0f-cos_theta);
+    }
+
+    // rotate into NED frame
+    target_vec_ned = _ahrs.get_rotation_body_to_ned()*target_vec_ned;
+
+    // extract the angles to target (logging only)
+    _ef_angle_to_target.x = atan2f(target_vec_ned.z,target_vec_ned.x);
+    _ef_angle_to_target.y = atan2f(target_vec_ned.z,target_vec_ned.y);
+
+    // ensure that the angle to target is no more than 70 degrees
+    if (target_vec_ned.z > 0.26f) {
+        // get current altitude (constrained to be positive)
+        float alt = MAX(alt_above_terrain_cm, 0.0f);
+        float dist = alt/target_vec_ned.z;
+        //
+        _target_pos_rel.x = target_vec_ned.x*dist;
+        _target_pos_rel.y = target_vec_ned.y*dist;
+        _target_pos_rel.z = alt;  // not used
+        _target_pos = _inav.get_position()+_target_pos_rel;
+
+        _last_update_ms = AP_HAL::millis();
+    }
 }
 
 // handle_msg - Process a LANDING_TARGET mavlink message

libraries/AC_PrecLand/AC_PrecLand.h

@@ -4,6 +4,8 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_InertialNav/AP_InertialNav.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <stdint.h>
 
 // declare backend classes
 class AC_PrecLand_Backend;
@@ -33,42 +35,57 @@ public:
         PRECLAND_TYPE_IRLOCK
     };
 
-    // Constructor
+    // constructor
     AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav);
 
-    // init - perform any required initialisation of landing controllers
+    // perform any required initialisation of landing controllers
     void init();
 
-    // healthy - returns true if precision landing is healthy
+    // returns true if precision landing is healthy
     bool healthy() { return _backend_state.healthy; }
 
-    // update - give chance to driver to get updates from sensor
+    // returns time of last update
+    uint32_t last_update_ms() { return _last_update_ms; }
+
+    // give chance to driver to get updates from sensor
     void update(float alt_above_terrain_cm);
 
-    // get_target_shift - returns 3D vector of earth-frame position adjustments to target
+    // returns 3D vector of earth-frame position adjustments to target
     Vector3f get_target_shift(const Vector3f& orig_target);
 
-    // handle_msg - Process a LANDING_TARGET mavlink message
+    // returns target position relative to origin
+    bool get_target_position(Vector3f& ret);
+
+    // returns target position relative to vehicle
+    bool get_target_position_relative(Vector3f& ret);
+
+    // returns target velocity relative to vehicle
+    bool get_target_velocity_relative(Vector3f& ret);
+
+    // returns true when the landing target has been detected
+    bool target_acquired();
+
+    // process a LANDING_TARGET mavlink message
     void handle_msg(mavlink_message_t* msg);
 
     // accessors for logging
     bool enabled() const { return _enabled; }
     const Vector2f& last_bf_angle_to_target() const { return _angle_to_target; }
     const Vector2f& last_ef_angle_to_target() const { return _ef_angle_to_target; }
-    const Vector3f& last_target_pos_offset() const { return _target_pos_offset; }
+    const Vector3f& last_target_pos_offset() const { return _target_pos_rel; }
 
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
 
-    // calc_angles_and_pos - converts sensor's body-frame angles to earth-frame angles and position estimate
+    // converts sensor's body-frame angles to earth-frame angles and position estimate
     //  angles stored in _angle_to_target
     //  earth-frame angles stored in _ef_angle_to_target
     //  position estimate is stored in _target_pos
     void calc_angles_and_pos(float alt_above_terrain_cm);
 
-    // get_behaviour - returns enabled parameter as an behaviour
+    // returns enabled parameter as an behaviour
     enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
 
     // references to inertial nav and ahrs libraries
@@ -79,13 +96,15 @@ private:
     AP_Int8                     _enabled;           // enabled/disabled and behaviour
     AP_Int8                     _type;              // precision landing controller type
 
+    uint32_t                    _last_update_ms;      // epoch time in millisecond when update is called
+
     // output from sensor (stored for logging)
     Vector2f                    _angle_to_target;   // last raw sensor angle to target
     Vector2f                    _ef_angle_to_target;// last earth-frame angle to target
 
-    // output from controller
-    bool                        _have_estimate;     // true if we have a recent estimated position offset
-    Vector3f                    _target_pos_offset; // estimate target position offset from vehicle in earth-frame
+    // estimator output
+    Vector3f                    _target_pos_rel;    // estimate target position relative to vehicle in NEU cm
+    Vector3f                    _target_pos;        // estimate target position in NEU cm
 
     // backend state
     struct precland_state {