diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 88a31ecd95..670f620687 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/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; diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 5385d03417..6d21638a32 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/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_history; // backend state