AC_PrecLand: non functional changes including adding comments

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -52,8 +52,8 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
 
     // @Param: EST_TYPE
     // @DisplayName: Precision Land Estimator Type
-    // @Description: Specifies the estimation method used.
-    // @Values: 0:RAW_MEASUREMENTS, 1:TWO_STATE_KF_PER_AXIS
+    // @Description: Specifies the estimation method to be used
+    // @Values: 0:RawSensor, 1:KalmanFilter
     // @User: Advanced
     AP_GROUPINFO("EST_TYPE",    5, AC_PrecLand, _estimator_type, 1),
 
@@ -81,7 +81,6 @@ AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
     _backend_state.healthy = false;
 }
 
-
 // init - perform any required initialisation of backends
 void AC_PrecLand::init()
 {
@@ -129,14 +128,15 @@ void AC_PrecLand::init()
 // update - give chance to driver to get updates from sensor
 void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
 {
-    // Collect inertial data and append it to the buffer
+    // append current velocity and attitude correction into history buffer
     struct inertial_data_frame_s inertial_data_newest;
     _ahrs.getCorrectedDeltaVelocityNED(inertial_data_newest.correctedVehicleDeltaVelocityNED, inertial_data_newest.dt);
     inertial_data_newest.Tbn = _ahrs.get_rotation_body_to_ned();
     inertial_data_newest.inertialNavVelocity = _inav.get_velocity()*0.01f;
     inertial_data_newest.inertialNavVelocityValid = _inav.get_filter_status().flags.horiz_vel;
     _inertial_history.push_back(inertial_data_newest);
-    
+
+    // update estimator of target position
     if (_backend != nullptr && _enabled) {
         _backend->update();
         run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid);
@@ -195,7 +195,7 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
     const struct inertial_data_frame_s& inertial_data_delayed = _inertial_history.front();
 
     switch (_estimator_type) {
-        case ESTIMATOR_TYPE_RAW_MEASUREMENTS: {
+        case ESTIMATOR_TYPE_RAW_SENSOR: {
             // Return if there's any invalid velocity data
             for (uint8_t i=0; i<_inertial_history.size(); i++) {
                 const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i);
@@ -245,7 +245,7 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
             }
             break;
         }
-        case ESTIMATOR_TYPE_TWO_STATE_KF_PER_AXIS: {
+        case ESTIMATOR_TYPE_KALMAN_FILTER: {
             // Predict
             if (target_acquired()) {
                 const float& dt = inertial_data_delayed.dt;
@@ -349,7 +349,7 @@ void AC_PrecLand::run_output_prediction()
         _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data.dt;
     }
 
-    // Apply offset
+    // compensate for camera offset from the center of the vehicle
     Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f;
     _target_pos_rel_out_NE.x += land_ofs_ned_m.x;
     _target_pos_rel_out_NE.y += land_ofs_ned_m.y;

libraries/AC_PrecLand/AC_PrecLand.h

@@ -51,13 +51,16 @@ public:
     // returns true if precision landing is healthy
     bool healthy() const { return _backend_state.healthy; }
 
+    // returns true if precision landing is enabled (used only for logging)
+    bool enabled() const { return _enabled.get(); }
+
     // returns time of last update
     uint32_t last_update_ms() const { return _last_update_ms; }
 
-    // give chance to driver to get updates from sensor
+    // give chance to driver to get updates from sensor, should be called at 400hz
     void update(float rangefinder_alt_cm, bool rangefinder_alt_valid);
 
-    // returns target position relative to origin
+    // returns target position relative to the EKF origin
     bool get_target_position_cm(Vector2f& ret);
 
     // returns target position relative to vehicle
@@ -72,23 +75,26 @@ public:
     // process a LANDING_TARGET mavlink message
     void handle_msg(mavlink_message_t* msg);
 
-    // accessors for logging
-    bool enabled() const { return _enabled; }
-
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
     enum estimator_type_t {
-        ESTIMATOR_TYPE_RAW_MEASUREMENTS=0,
-        ESTIMATOR_TYPE_TWO_STATE_KF_PER_AXIS=1
+        ESTIMATOR_TYPE_RAW_SENSOR = 0,
+        ESTIMATOR_TYPE_KALMAN_FILTER = 1
     };
 
     // returns enabled parameter as an behaviour
     enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
 
+    // run target position estimator
     void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid);
+
+    // get vehicle body frame 3D vector from vehicle to target.  returns true on success, false on failure
     bool retrieve_los_meas(Vector3f& target_vec_unit_body);
+
+    // calculate target's position and velocity relative to the vehicle (used as input to position controller)
+    // results are stored in_target_pos_rel_out_NE, _target_vel_rel_out_NE
     void run_output_prediction();
 
     // references to inertial nav and ahrs libraries
@@ -103,29 +109,29 @@ private:
     AP_Float                    _land_ofs_cm_x;     // Desired landing position of the camera forward of the target in vehicle body frame
     AP_Float                    _land_ofs_cm_y;     // Desired landing position of the camera right of the target in vehicle body frame
 
-    uint32_t                    _last_update_ms;      // epoch time in millisecond when update is called
-    bool                        _target_acquired;
-    uint32_t                    _last_backend_los_meas_ms;
+    uint32_t                    _last_update_ms;    // system time in millisecond when update was last called
+    bool                        _target_acquired;   // true if target has been seen recently
+    uint32_t                    _last_backend_los_meas_ms;  // system time target was last seen
 
-    PosVelEKF                   _ekf_x, _ekf_y;
-    uint32_t                    _outlier_reject_count;
+    PosVelEKF                   _ekf_x, _ekf_y;     // Kalman Filter for x and y axis
+    uint32_t                    _outlier_reject_count;  // mini-EKF's outlier counter (3 consecutive outliers lead to EKF accepting updates)
     
-    Vector3f                    _target_pos_rel_meas_NED;
+    Vector3f                    _target_pos_rel_meas_NED; // target's relative position as 3D vector
 
-    Vector2f                    _target_pos_rel_est_NE;
-    Vector2f                    _target_vel_rel_est_NE;
+    Vector2f                    _target_pos_rel_est_NE; // target's relative position based on latest sensor data (i.e. not compensated for lag)
+    Vector2f                    _target_vel_rel_est_NE; // target's relative velocity based on latest sensor data (i.e. not compensated for lag)
 
-    Vector2f                    _target_pos_rel_out_NE;
-    Vector2f                    _target_vel_rel_out_NE;
+    Vector2f                    _target_pos_rel_out_NE; // target's relative position, fed into position controller
+    Vector2f                    _target_vel_rel_out_NE; // target's relative velocity, fed into position controller
 
+    // structure and buffer to hold a short history of vehicle velocity
     struct inertial_data_frame_s {
-        Matrix3f Tbn;
+        Matrix3f Tbn;                               // dcm rotation matrix to rotate body frame to north
         Vector3f correctedVehicleDeltaVelocityNED;
         Vector3f inertialNavVelocity;
         bool inertialNavVelocityValid;
         float dt;
     };
-
     AP_Buffer<inertial_data_frame_s,8>       _inertial_history;
 
     // backend state