Copter: further precision landing rework

ArduCopter/ArduCopter.cpp

@@ -102,7 +102,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(compass_accumulate,   100,    100),
     SCHED_TASK(barometer_accumulate,  50,     90),
 #if PRECISION_LANDING == ENABLED
-    SCHED_TASK(update_precland,       50,     50),
+    SCHED_TASK(update_precland,      400,     50),
 #endif
 #if FRAME_CONFIG == HELI_FRAME
     SCHED_TASK(check_dynamic_flight,  50,     75),
@@ -428,6 +428,11 @@ void Copter::twentyfive_hz_logging()
         DataFlash.Log_Write_IMU(ins);
     }
 #endif
+
+#if PRECISION_LANDING == ENABLED
+    // log output
+    Log_Write_Precland();
+#endif
 }
 
 void Copter::dataflash_periodic(void)

ArduCopter/precision_landing.cpp

@@ -15,16 +15,15 @@ void Copter::init_precland()
 
 void Copter::update_precland()
 {
-    float final_alt = current_loc.alt;
+    int32_t height_above_ground_cm = current_loc.alt;
 
-    // use range finder altitude if it is valid
+    // use range finder altitude if it is valid, else try to get terrain alt
     if (rangefinder_alt_ok()) {
-        final_alt = rangefinder_state.alt_cm;
+        height_above_ground_cm = rangefinder_state.alt_cm;
+    } else if (terrain_use()) {
+        current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm);
     }
 
-    copter.precland.update(final_alt);
-
-    // log output
-    Log_Write_Precland();
+    copter.precland.update(height_above_ground_cm);
 }
 #endif

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -83,12 +83,19 @@ void AC_PrecLand::init()
 void AC_PrecLand::update(float alt_above_terrain_cm)
 {
     // run backend update
-    if (_backend != NULL) {
+    if (_backend != NULL && _enabled) {
         // read from sensor
         _backend->update();
-
-        // calculate angles to target and position estimate
-        calc_angles_and_pos(alt_above_terrain_cm);
+
+        if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
+            // we have a new, unique los measurement
+            _last_backend_los_meas_ms = _backend->los_meas_time_ms();
+
+            Vector3f target_vec_unit_body;
+            _backend->get_los_body(target_vec_unit_body);
+
+            calc_angles_and_pos(target_vec_unit_body, alt_above_terrain_cm);
+        }
     }
 }
 
@@ -126,55 +133,23 @@ bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret)
 //  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
-void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm)
+void AC_PrecLand::calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm)
 {
-    // exit immediately if not enabled
-    if (_backend == NULL) {
-        return;
-    }
-
-    // get angles to target from backend
-    if (!_backend->get_angle_to_target(_angle_to_target.x, _angle_to_target.y)) {
-        return;
-    }
-
-    _angle_to_target.x = -_angle_to_target.x;
-    _angle_to_target.y = -_angle_to_target.y;
-
-    // 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;
-
-    Vector3f target_vec_ned;
-
-    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);
-
-        target_vec_ned.x = axis.y*sin_theta;
-        target_vec_ned.y = -axis.x*sin_theta;
-        target_vec_ned.z = cos_theta;
-    }
-
     // rotate into NED frame
-    target_vec_ned = _ahrs.get_rotation_body_to_ned()*target_vec_ned;
+    Vector3f target_vec_unit_ned = _ahrs.get_rotation_body_to_ned()*target_vec_unit_body;
 
     // 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);
+    _angle_to_target.x = atan2f(-target_vec_unit_body.y, target_vec_unit_body.z);
+    _angle_to_target.y = atan2f( target_vec_unit_body.x, target_vec_unit_body.z);
+    _ef_angle_to_target.x = atan2f(-target_vec_unit_ned.y, target_vec_unit_ned.z);
+    _ef_angle_to_target.y = atan2f( target_vec_unit_ned.x, target_vec_unit_ned.z);
 
-    // ensure that the angle to target is no more than 70 degrees
-    if (target_vec_ned.z > 0.26f) {
+    if (target_vec_unit_ned.z > 0.0f) {
         // 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;
+        float dist = alt/target_vec_unit_ned.z;
+        _target_pos_rel.x = target_vec_unit_ned.x*dist;
+        _target_pos_rel.y = target_vec_unit_ned.y*dist;
         _target_pos_rel.z = alt;  // not used
         _target_pos = _inav.get_position()+_target_pos_rel;
 

libraries/AC_PrecLand/AC_PrecLand.h

@@ -83,7 +83,7 @@ class AC_PrecLand
     //  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);
+    void calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm);
 
     // returns enabled parameter as an behaviour
     enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
@@ -97,6 +97,7 @@ class AC_PrecLand
     AP_Int8                     _type;              // precision landing controller type
 
     uint32_t                    _last_update_ms;      // epoch time in millisecond when update is called
+    uint32_t                    _last_backend_los_meas_ms;
 
     // output from sensor (stored for logging)
     Vector2f                    _angle_to_target;   // last raw sensor angle to target

libraries/AC_PrecLand/AC_PrecLand_Backend.h

@@ -10,7 +10,6 @@
 class AC_PrecLand_Backend
 {
 public:
-
     // Constructor
     AC_PrecLand_Backend(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) :
         _frontend(frontend),
@@ -19,26 +18,26 @@ class AC_PrecLand_Backend
     // destructor
     virtual ~AC_PrecLand_Backend() {}
 
-    // init - perform any required initialisation of backend controller
+    // perform any required initialisation of backend
     virtual void init() = 0;
 
-    // update - give chance to driver to get updates from sensor
-    //  returns true if new data available
-    virtual bool update() = 0;
-    // what frame of reference is our sensor reporting in?
-    virtual MAV_FRAME get_frame_of_reference() = 0;
-
-    // get_angle_to_target - returns angles (in radians) to target
-    //  returns true if angles are available, false if not (i.e. no target)
-    //  x_angle_rad : roll direction, positive = target is to right (looking down)
-    //  y_angle_rad : pitch direction, postiive = target is forward (looking down)
-    virtual bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) = 0;
-
-    // handle_msg - parses a mavlink message from the companion computer
+    // retrieve updates from sensor
+    virtual void update() = 0;
+
+    // provides a unit vector towards the target in body frame
+    //  returns same as have_los_meas()
+    virtual bool get_los_body(Vector3f& dir_body) = 0;
+
+    // returns system time in milliseconds of last los measurement
+    virtual uint32_t los_meas_time_ms() = 0;
+
+    // return true if there is a valid los measurement available
+    virtual bool have_los_meas() = 0;
+
+    // parses a mavlink message from the companion computer
     virtual void handle_msg(mavlink_message_t* msg) = 0;
 
 protected:
-
     const AC_PrecLand&  _frontend;          // reference to precision landing front end
     AC_PrecLand::precland_state &_state;    // reference to this instances state
 };

libraries/AC_PrecLand/AC_PrecLand_Companion.cpp

@@ -7,63 +7,60 @@ extern const AP_HAL::HAL& hal;
 // Constructor
 AC_PrecLand_Companion::AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state)
     : AC_PrecLand_Backend(frontend, state),
-      _frame(MAV_FRAME_BODY_NED),
       _distance_to_target(0.0f),
       _timestamp_us(0),
-      _new_estimate(false)
+      _have_los_meas(false)
 {
 }
 
-// init - perform initialisation of this backend
+// perform any required initialisation of backend
 void AC_PrecLand_Companion::init()
 {
     // set healthy
     _state.healthy = true;
-    _new_estimate = false;
+    _have_los_meas = false;
 }
 
-// update - give chance to driver to get updates from sensor
-//  returns true if new data available
-bool AC_PrecLand_Companion::update()
+// retrieve updates from sensor
+void AC_PrecLand_Companion::update()
 {
-    // Mavlink commands are received asynchronous so all new data is processed by handle_msg()
-    return _new_estimate;
+    _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
 }
 
-MAV_FRAME AC_PrecLand_Companion::get_frame_of_reference()
-{
-    return _frame;
-}
-
-// get_angle_to_target - returns angles (in radians) to target
-//  returns true if angles are available, false if not (i.e. no target)
-//  x_angle_rad : roll direction, positive = target is to right (looking down)
-//  y_angle_rad : pitch direction, postiive = target is forward (looking down)
-bool AC_PrecLand_Companion::get_angle_to_target(float &x_angle_rad, float &y_angle_rad)
-{
-    if (_new_estimate){
-        x_angle_rad = _angle_to_target.x;
-        y_angle_rad = _angle_to_target.y;
-
-        // reset and wait for new data
-        _new_estimate = false;
+// provides a unit vector towards the target in body frame
+//  returns same as have_los_meas()
+bool AC_PrecLand_Companion::get_los_body(Vector3f& ret) {
+    if (have_los_meas()) {
+        ret = _los_meas_body;
         return true;
     }
-
     return false;
 }
 
+// returns system time in milliseconds of last los measurement
+uint32_t AC_PrecLand_Companion::los_meas_time_ms() {
+    return _los_meas_time_ms;
+}
+
+// return true if there is a valid los measurement available
+bool AC_PrecLand_Companion::have_los_meas()
+{
+    return _have_los_meas;
+}
+
 void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
 {
     // parse mavlink message
     __mavlink_landing_target_t packet;
     mavlink_msg_landing_target_decode(msg, &packet);
 
     _timestamp_us = packet.time_usec;
-    _frame = (MAV_FRAME) packet.frame;
-    _angle_to_target.x = packet.angle_x;
-    _angle_to_target.y = packet.angle_y;
     _distance_to_target = packet.distance;
-    _state.healthy = true;
-    _new_estimate = true;
+
+    // compute unit vector towards target
+    _los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
+    _los_meas_body /= _los_meas_body.length();
+
+    _los_meas_time_ms = AP_HAL::millis();
+    _have_los_meas = true;
 }

libraries/AC_PrecLand/AC_PrecLand_Companion.h

@@ -13,34 +13,33 @@
 class AC_PrecLand_Companion : public AC_PrecLand_Backend
 {
 public:
-
     // Constructor
     AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state);
-
-    // init - perform any required initialisation of backend controller
+    
+    // perform any required initialisation of backend
     void init();
 
-    // update - give chance to driver to get updates from sensor
-    //  returns true if new data available
-    bool update();
-    // what frame of reference is our sensor reporting in?
-    MAV_FRAME get_frame_of_reference();
-
-    // get_angle_to_target - returns angles (in radians) to target
-    //  returns true if angles are available, false if not (i.e. no target)
-    //  x_angle_rad : roll direction, positive = target is to right (looking down)
-    //  y_angle_rad : pitch direction, postiive = target is forward (looking down)
-    bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad);
-
-    // handle_msg - parses a mavlink message from the companion computer
+    // retrieve updates from sensor
+    void update();
+
+    // provides a unit vector towards the target in body frame
+    //  returns same as have_los_meas()
+    bool get_los_body(Vector3f& ret);
+
+    // returns system time in milliseconds of last los measurement
+    uint32_t los_meas_time_ms();
+
+    // return true if there is a valid los measurement available
+    bool have_los_meas();
+
+    // parses a mavlink message from the companion computer
     void handle_msg(mavlink_message_t* msg);
 
 private:
-
-    // output from camera
-    MAV_FRAME           _frame;                 // what frame of reference is our sensor reporting in?
-    Vector2f            _angle_to_target;       // last angle to target
+    uint64_t            _timestamp_us;          // timestamp from message
     float               _distance_to_target;    // distance from the camera to target in meters
-    uint64_t            _timestamp_us;          // timestamp when the image was captured(synced via UAVCAN)
-    bool                _new_estimate;          // true if new data from the camera has been received
+
+    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
+    bool                _have_los_meas;         // true if there is a valid measurement from the camera
+    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
 };