diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index ea184522c035c..ec46a831f115a 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -13,13 +13,11 @@ void Copter::init_precland() void Copter::update_precland() { - int32_t height_above_ground_cm = current_loc.alt; - - // use range finder altitude if it is valid, otherwise use home alt - if (rangefinder_alt_ok()) { - height_above_ground_cm = rangefinder_state.alt_cm_glitch_protected; + if (!rangefinder_alt_ok() || + rangefinder_state.alt_cm_glitch_protected == 0) { + return precland.update(0, false); } - - precland.update(height_above_ground_cm, rangefinder_alt_ok()); + return precland.update(rangefinder_state.alt_cm_glitch_protected, + true); } #endif diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 224c1b8526bdf..790c534961288 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -75,6 +75,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { #if AP_AIRSPEED_AUTOCAL_ENABLE SCHED_TASK(airspeed_ratio_update, 1, 100), #endif // AP_AIRSPEED_AUTOCAL_ENABLE +#if PRECISION_LANDING == ENABLED + SCHED_TASK(update_precland, 400, 50), +#endif #if HAL_MOUNT_ENABLED SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100), #endif // HAL_MOUNT_ENABLED diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 4ffc4e3a1473b..89b9bbe0506e6 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -670,6 +670,16 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c plane.reset_offset_altitude(); } +/* + handle a LANDING_TARGET command. The timestamp has been jitter corrected +*/ +void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) +{ +#if PRECISION_LANDING == ENABLED + plane.g2.precland.handle_msg(packet, timestamp_ms); +#endif +} + MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_long_t &packet) { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 7b3e93d0847d2..cdcf5f84571e3 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -25,6 +25,8 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_long_t &packet) override; + void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; + void send_position_target_global_int() override; virtual bool in_hil_mode() const override; diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index e85be2360313e..290117048e6f8 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -20,6 +20,16 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; } +#if PRECISION_LANDING == ENABLED + if (plane.g2.precland.enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; + } + if (plane.g2.precland.enabled() && plane.g2.precland.healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; + } +#endif + // reverse thrust if (plane.have_reverse_thrust()) { control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index b5df76d21048e..a4890b0cdac6f 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1238,6 +1238,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(guidedHeading, "GUIDED_", 28, ParametersG2, AC_PID), #endif // OFFBOARD_GUIDED == ENABLED +#if PRECISION_LANDING == ENABLED + // @Group: PLND_ + // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp + AP_SUBGROUPINFO(precland, "PLND_", 29, ParametersG2, AC_PrecLand), +#endif + AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 017a248bf3d28..d35bb86c20492 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -575,6 +575,10 @@ class ParametersG2 { // min initial climb in RTL AP_Int16 rtl_climb_min; + +#if PRECISION_LANDING == ENABLED + AC_PrecLand precland; +#endif }; extern const AP_Param::Info var_info[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2621bbb9f0f39..32b13329241e9 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -110,6 +110,10 @@ #include #endif +#if PRECISION_LANDING == ENABLED +#include +#endif + #include "RC_Channel.h" // RC Channel Library #include "Parameters.h" #if HAL_ADSB_ENABLED @@ -1078,6 +1082,10 @@ class Plane : public AP_Vehicle { #if HAL_SOARING_ENABLED void update_soaring(); #endif + + // precision_landing.cpp + void init_precland(); + void update_precland(); // vehicle specific waypoint info helpers bool get_wp_distance_m(float &distance) const override; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index d5ab778a92d5a..295c1d8946a2e 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -188,6 +188,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, break; case AUX_FUNC::CROW_SELECT: + case AUX_FUNC::PRECISION_LOITER: do_aux_function(ch_option, ch_flag); break; @@ -305,6 +306,22 @@ case AUX_FUNC::ARSPD_CALIBRATE: break; case AuxSwitchPos::LOW: plane.airspeed.set_calibration_enabled(false); + break; + } +#endif + break; + + case AUX_FUNC::PRECISION_LOITER: +#if PRECISION_LANDING == ENABLED + switch (ch_flag) { + case AuxSwitchPos::HIGH: + plane.quadplane.set_precision_loiter_enabled(true); + break; + case AuxSwitchPos::MIDDLE: + // nothing + break; + case AuxSwitchPos::LOW: + plane.quadplane.set_precision_loiter_enabled(false); break; } #endif diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 5e43e3627668e..c56dea9eb86dc 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -24,6 +24,13 @@ # define MAV_SYSTEM_ID 1 #endif +////////////////////////////////////////////////////////////////////////////// +// Precision Landing system +// +#ifndef PRECISION_LANDING +#define PRECISION_LANDING !HAL_MINIMIZE_FEATURES +#endif + ////////////////////////////////////////////////////////////////////////////// // Advanced Failsafe support // diff --git a/ArduPlane/precision_landing.cpp b/ArduPlane/precision_landing.cpp new file mode 100644 index 0000000000000..2cda0508a06f1 --- /dev/null +++ b/ArduPlane/precision_landing.cpp @@ -0,0 +1,22 @@ +// +// functions to support precision landing +// + +#include "Plane.h" + +#if PRECISION_LANDING == ENABLED +void Plane::init_precland() +{ + plane.g2.precland.init(400); // this must match the number on the scheduler table +} + +void Plane::update_precland() +{ + if (plane.g.rangefinder_landing && + rangefinder_state.in_range) { + return g2.precland.update(rangefinder_state.height_estimate, + true); + } + return g2.precland.update(0, false); +} +#endif diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ed196c9e558ee..531a3735d134e 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1418,12 +1418,40 @@ bool QuadPlane::is_flying_vtol(void) const */ float QuadPlane::landing_descent_rate_cms(float height_above_ground) const { - float ret = linear_interpolate(land_speed_cms, wp_nav->get_default_speed_down(), + float speed_down = wp_nav->get_default_speed_down(); + float ret = linear_interpolate(land_speed_cms, speed_down, height_above_ground, land_final_alt, land_final_alt+6); +#if PRECISION_LANDING == ENABLED + if (precland_active()) { + const float precland_acceptable_error = 15.0f; + const float precland_min_descent_speed = 10.0f; + float land_slowdown = MAX(0.0f, pos_control->get_pos_error_xy()*(speed_down/precland_acceptable_error)); + ret = MAX(precland_min_descent_speed, ret - land_slowdown); + } +#endif return ret; } +// return true if precision landing is active +bool QuadPlane::precland_active(void) const +{ +#if PRECISION_LANDING == ENABLED + if (plane.control_mode != &plane.mode_qland && + plane.control_mode != &plane.mode_qloiter && + !in_vtol_land_descent()) { + return false; + } + if (plane.control_mode == &plane.mode_qloiter && + !_precision_loiter_enabled) { + return false; + } + AC_PrecLand &precland = plane.g2.precland; + return pos_control->is_active_xy() && precland.target_acquired(); +#else + return false; +#endif +} // run quadplane loiter controller void QuadPlane::control_loiter() @@ -1466,6 +1494,24 @@ void QuadPlane::control_loiter() // run loiter controller loiter_nav->update(); +#if PRECISION_LANDING == ENABLED + if (precland_active()) { + AC_PrecLand &precland = plane.g2.precland; + + Vector2f target_pos, target_vel_rel; + if (!precland.get_target_position_cm(target_pos)) { + target_pos.x = inertial_nav.get_position().x; + target_pos.y = inertial_nav.get_position().y; + } + if (!precland.get_target_velocity_relative_cms(target_vel_rel)) { + target_vel_rel.x = -inertial_nav.get_velocity().x; + target_vel_rel.y = -inertial_nav.get_velocity().y; + } + pos_control->set_xy_target(target_pos.x, target_pos.y); + pos_control->override_vehicle_velocity_xy(-target_vel_rel); + } +#endif + // nav roll and pitch are controller by loiter controller plane.nav_roll_cd = loiter_nav->get_roll(); plane.nav_pitch_cd = loiter_nav->get_pitch(); @@ -2580,8 +2626,7 @@ void QuadPlane::vtol_position_controller(void) plane.nav_controller->update_waypoint(plane.prev_WP_loc, loc); FALLTHROUGH; - case QPOS_LAND_FINAL: - + case QPOS_LAND_FINAL: { // set position controller desired velocity and acceleration to zero pos_control->set_desired_velocity_xy(0.0f,0.0f); pos_control->set_desired_accel_xy(0.0f,0.0f); @@ -2592,6 +2637,24 @@ void QuadPlane::vtol_position_controller(void) } else { pos_control->set_xy_target(poscontrol.target.x, poscontrol.target.y); } + +#if PRECISION_LANDING == ENABLED + // run precision landing + if (precland_active()) { + Vector2f target_pos, target_vel_rel; + AC_PrecLand &precland = plane.g2.precland; + if (!precland.get_target_position_cm(target_pos)) { + target_pos.x = inertial_nav.get_position().x; + target_pos.y = inertial_nav.get_position().y; + } + if (!precland.get_target_velocity_relative_cms(target_vel_rel)) { + target_vel_rel.x = -inertial_nav.get_velocity().x; + target_vel_rel.y = -inertial_nav.get_velocity().y; + } + pos_control->set_xy_target(target_pos.x, target_pos.y); + pos_control->override_vehicle_velocity_xy(-target_vel_rel); + } +#endif pos_control->update_xy_controller(); // nav roll and pitch are controller by position controller @@ -2603,6 +2666,7 @@ void QuadPlane::vtol_position_controller(void) plane.nav_pitch_cd, get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); break; + } case QPOS_LAND_COMPLETE: // nothing to do @@ -2655,7 +2719,7 @@ void QuadPlane::vtol_position_controller(void) ahrs.setTouchdownExpected(true); } break; - + case QPOS_LAND_COMPLETE: break; } @@ -3075,7 +3139,8 @@ bool QuadPlane::verify_vtol_land(void) return true; } if (poscontrol.state == QPOS_POSITION2 && - plane.auto_state.wp_distance < 2) { + ((plane.auto_state.wp_distance < 2) || precland_active())) { + poscontrol.state = QPOS_LAND_DESCEND; #if AC_FENCE == ENABLED plane.fence.auto_disable_fence_for_landing(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 83ad208186c91..5fb372caf0d7c 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -10,6 +10,7 @@ #include #include #include +#include "config.h" #include "qautotune.h" #include "defines.h" @@ -51,6 +52,10 @@ class QuadPlane static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info2[]; +#if PRECISION_LANDING == ENABLED + void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; } +#endif + void control_run(void); void control_auto(void); bool init_mode(void); @@ -62,7 +67,7 @@ class QuadPlane void waypoint_controller(void); void update_throttle_mix(void); - + // update transition handling void update(void); @@ -297,7 +302,12 @@ class QuadPlane // calculate a stopping distance for fixed-wing to vtol transitions float stopping_distance(void); - + +#if PRECISION_LANDING == ENABLED + bool _precision_loiter_enabled; +#endif + bool precland_active() const; + AP_Int16 transition_time_ms; // transition deceleration, m/s/s diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index fe3324291792e..582dc033599ea 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -111,6 +111,11 @@ void Plane::init_ardupilot() camera_mount.init(); #endif +#if PRECISION_LANDING == ENABLED + // initialise precision landing + init_precland(); +#endif + #if LANDING_GEAR_ENABLED == ENABLED // initialise landing gear position g2.landing_gear.init(); @@ -390,7 +395,7 @@ void Plane::startup_INS_ground(void) ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING); ahrs.set_wind_estimation(true); - ins.init(scheduler.get_loop_rate_hz()); + ins.init(400); // this must be the same number as in the scheduler table ahrs.reset(); // read Baro pressure at ground diff --git a/ArduPlane/wscript b/ArduPlane/wscript index dde7456ca2535..29fbf289fee2a 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -12,6 +12,7 @@ def build(bld): 'AP_Avoidance', 'AP_Arming', 'AP_Camera', + 'AP_IRLock', 'AP_L1_Control', 'AP_Navigation', 'AP_RCMapper', @@ -30,6 +31,7 @@ def build(bld): 'AP_OSD', 'AC_AutoTune', 'AP_KDECAN', + 'AC_PrecLand', ], ) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index c842615ad09c8..9c2fd9d6f780a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -206,9 +206,13 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) _inertial_history->push_force(inertial_data_newest); // update estimator of target position - if (_backend != nullptr && _enabled) { + if (_enabled) { _backend->update(); run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid); + // Output prediction + if (target_acquired()) { + run_output_prediction(); + } } const uint32_t now = AP_HAL::millis(); @@ -275,96 +279,100 @@ void AC_PrecLand::handle_msg(const mavlink_landing_target_t &packet, uint32_t ti // Private methods // -void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid) +void AC_PrecLand::run_estimator_raw(const struct AC_PrecLand::inertial_data_frame_s *inertial_data_delayed, float rangefinder_alt_m, bool rangefinder_alt_valid) { - const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; + // Return if there's any invalid velocity data + for (uint8_t i=0; i<_inertial_history->available(); i++) { + const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i]; + if (!inertial_data->inertialNavVelocityValid) { + _target_acquired = false; + return; + } + } - switch ((EstimatorType)_estimator_type.get()) { - case EstimatorType::RAW_SENSOR: { - // Return if there's any invalid velocity data - for (uint8_t i=0; i<_inertial_history->available(); i++) { - const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i]; - if (!inertial_data->inertialNavVelocityValid) { - _target_acquired = false; - return; - } - } + // Predict + if (target_acquired()) { + _target_pos_rel_est_NE.x -= inertial_data_delayed->inertialNavVelocity.x * inertial_data_delayed->dt; + _target_pos_rel_est_NE.y -= inertial_data_delayed->inertialNavVelocity.y * inertial_data_delayed->dt; + _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x; + _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; + } - // Predict - if (target_acquired()) { - _target_pos_rel_est_NE.x -= inertial_data_delayed->inertialNavVelocity.x * inertial_data_delayed->dt; - _target_pos_rel_est_NE.y -= inertial_data_delayed->inertialNavVelocity.y * inertial_data_delayed->dt; - _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x; - _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; - } + // Update if a new Line-Of-Sight measurement is available + if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { + _target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x; + _target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y; + _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x; + _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; + + _last_update_ms = AP_HAL::millis(); + _target_acquired = true; + } +} + +void AC_PrecLand::run_estimator_kf(const struct AC_PrecLand::inertial_data_frame_s *inertial_data_delayed, float rangefinder_alt_m, bool rangefinder_alt_valid) +{ + // Predict + if (target_acquired()) { + const float& dt = inertial_data_delayed->dt; + const Vector3f& vehicleDelVel = inertial_data_delayed->correctedVehicleDeltaVelocityNED; - // Update if a new Line-Of-Sight measurement is available - if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { - _target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x; - _target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y; - _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x; - _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; + _ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt); + _ekf_y.predict(dt, -vehicleDelVel.y, _accel_noise*dt); + } + // Update if a new Line-Of-Sight measurement is available + if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { + float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f); + if (!target_acquired()) { + // reset filter state + if (inertial_data_delayed->inertialNavVelocityValid) { + _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.x, sq(2.0f)); + _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.y, sq(2.0f)); + } else { + _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f)); + _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f)); + } + _last_update_ms = AP_HAL::millis(); + _target_acquired = true; + } else { + float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var); + float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var); + if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) { + _outlier_reject_count = 0; + _ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var); + _ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var); _last_update_ms = AP_HAL::millis(); _target_acquired = true; + } else { + _outlier_reject_count++; } - - // Output prediction - if (target_acquired()) { - run_output_prediction(); - } - break; } - case EstimatorType::KALMAN_FILTER: { - // Predict - if (target_acquired()) { - const float& dt = inertial_data_delayed->dt; - const Vector3f& vehicleDelVel = inertial_data_delayed->correctedVehicleDeltaVelocityNED; - - _ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt); - _ekf_y.predict(dt, -vehicleDelVel.y, _accel_noise*dt); - } + } - // Update if a new Line-Of-Sight measurement is available - if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { - float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f); - if (!target_acquired()) { - // reset filter state - if (inertial_data_delayed->inertialNavVelocityValid) { - _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.x, sq(2.0f)); - _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.y, sq(2.0f)); - } else { - _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f)); - _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f)); - } - _last_update_ms = AP_HAL::millis(); - _target_acquired = true; - } else { - float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var); - float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var); - if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) { - _outlier_reject_count = 0; - _ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var); - _ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var); - _last_update_ms = AP_HAL::millis(); - _target_acquired = true; - } else { - _outlier_reject_count++; - } - } - } + // Output prediction + if (target_acquired()) { + _target_pos_rel_est_NE.x = _ekf_x.getPos(); + _target_pos_rel_est_NE.y = _ekf_y.getPos(); + _target_vel_rel_est_NE.x = _ekf_x.getVel(); + _target_vel_rel_est_NE.y = _ekf_y.getVel(); + } +} - // Output prediction - if (target_acquired()) { - _target_pos_rel_est_NE.x = _ekf_x.getPos(); - _target_pos_rel_est_NE.y = _ekf_y.getPos(); - _target_vel_rel_est_NE.x = _ekf_x.getVel(); - _target_vel_rel_est_NE.y = _ekf_y.getVel(); +void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid) +{ + const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; - run_output_prediction(); - } + switch ((EstimatorType)_estimator_type.get()) { + case EstimatorType::RAW_SENSOR: + run_estimator_raw(inertial_data_delayed, rangefinder_alt_m, rangefinder_alt_valid); break; - } + case EstimatorType::KALMAN_FILTER: + run_estimator_kf(inertial_data_delayed, rangefinder_alt_m, rangefinder_alt_valid); + break; + default: + // invalid parameter value + return; } } @@ -393,32 +401,42 @@ bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body) bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid) { Vector3f target_vec_unit_body; - if (retrieve_los_meas(target_vec_unit_body)) { - const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; - - Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; - bool target_vec_valid = target_vec_unit_ned.z > 0.0f; - bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f); - if (target_vec_valid && alt_valid) { - float dist, alt; - if (_backend->distance_to_target() > 0.0f) { - dist = _backend->distance_to_target(); - alt = dist * target_vec_unit_ned.z; - } else { - alt = MAX(rangefinder_alt_m, 0.0f); - dist = alt / target_vec_unit_ned.z; - } + if (!retrieve_los_meas(target_vec_unit_body)) { + return false; + } - // Compute camera position relative to IMU - Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); - Vector3f cam_pos_ned = inertial_data_delayed->Tbn * (_cam_offset.get() - accel_body_offset); + const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; - // Compute target position relative to IMU - _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt) + cam_pos_ned; - return true; - } + const Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; + if (target_vec_unit_ned.z <= 0.0f) { + // invalid height transformation + return false; + } + + float dist, alt; + if (_backend->distance_to_target() > 0.0f) { + dist = _backend->distance_to_target(); + alt = dist * target_vec_unit_ned.z; + } else if (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) { + alt = MAX(rangefinder_alt_m, 0.0f); + dist = alt / target_vec_unit_ned.z; + } else { + // no altitude source + return false; } - return false; + + // Compute camera position relative to IMU + const Vector3f &accel_body_offset = AP::ins().get_imu_pos_offset(); + const Vector3f cam_pos_ned {inertial_data_delayed->Tbn * (_cam_offset.get() - accel_body_offset)}; + + // Compute target position relative to IMU + _target_pos_rel_meas_NED = Vector3f{ + target_vec_unit_ned.x*dist, + target_vec_unit_ned.y*dist, + alt + } + cam_pos_ned; + + return true; } void AC_PrecLand::run_output_prediction() diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 26817568427e7..c61464a498ae7 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -138,6 +138,10 @@ class AC_PrecLand }; ObjectArray *_inertial_history; + // run different target position estimator types: + void run_estimator_raw(const struct inertial_data_frame_s *inertial_data_delayed, float rangefinder_alt_m, bool rangefinder_alt_valid); + void run_estimator_kf(const struct inertial_data_frame_s *inertial_data_delayed, float rangefinder_alt_m, bool rangefinder_alt_valid); + // backend state struct precland_state { bool healthy; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index da8d111f71828..0bf4487f08566 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -96,7 +96,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Description: Function assigned to this RC channel // @Values{Copter}: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 37:Throw, 38:ADSB Avoidance En, 39:PrecLoiter, 40:Proximity Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 46:RC Override Enable, 47:User Function 1, 48:User Function 2, 49:User Function 3, 52:Acro, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints, 60:ZigZag, 61:ZigZag SaveWP, 62:Compass Learn, 65:GPS Disable, 66:Relay5 On/Off, 67:Relay6 On/Off, 68:Stabilize, 69:PosHold, 70:AltHold, 71:FlowHold, 72:Circle, 73:Drift, 75:SurfaceTrackingUpDown, 76:Standby Mode, 78:RunCam Control, 79:RunCam OSD Control, 80:Viso Align, 81:Disarm, 83:ZigZag Auto, 84:Air Mode, 85:Generator, 90:EKF Pos Source, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8 // @Values{Rover}: 0:Do Nothing, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 11:Fence, 16:Auto, 19:Gripper, 24:Auto Mission Reset, 27:Retract Mount, 28:Relay On/Off, 30:Lost Rover Sound, 31:Motor Emergency Stop, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 40:Proximity Avoidance, 41:ArmDisarm, 42:SmartRTL, 46:RC Override Enable, 50:LearnCruise, 51:Manual, 52:Acro, 53:Steering, 54:Hold, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints, 59:Simple Mode, 62:Compass Learn, 63:Sailboat Tack, 65:GPS Disable, 66:Relay5 On/Off, 67:Relay6 On/Off, 74:Sailboat motoring 3pos, 78:RunCam Control, 79:RunCam OSD Control, 80:Viso Align, 81:Disarm, 90:EKF Pos Source, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 201:Roll, 202:Pitch, 203:Walking Height, 207:MainSail, 208:Flap, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8 - // @Values{Plane}: 0:Do Nothing, 4:ModeRTL, 9:Camera Trigger, 16:ModeAuto, 24:Auto Mission Reset, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Plane Sound, 31:Motor Emergency Stop, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 38:ADSB Avoidance En, 41:ArmDisarm, 43:InvertedFlight, 46:RC Override Enable, 51:ModeManual, 55:ModeGuided, 56:ModeLoiter, 58:Clear Waypoints, 62:Compass Learn, 64:Reverse Throttle, 65:GPS Disable, 66:Relay5 On/Off, 67:Relay6 On/Off, 72:ModeCircle, 77:ModeTakeoff, 78:RunCam Control, 79:RunCam OSD Control, 81:Disarm, 82:QAssist 3pos, 84:Air Mode, 85:Generator, 86: Non Auto Terrain Follow Disable, 87:Crow Select, 88:Soaring Enable, 89:Landing Flare, 90:EKF Pos Source, 91:Airspeed Ratio Calibration, 92:FBWA, 95:FBWA taildragger takeoff mode, 96:trigger re-reading of mode switch, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 208:Flap, 209: Forward Throttle, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8 + // @Values{Plane}: 0:Do Nothing, 4:ModeRTL, 9:Camera Trigger, 16:ModeAuto, 24:Auto Mission Reset, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Plane Sound, 31:Motor Emergency Stop, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 38:ADSB Avoidance En, 39:PrecLoiter, 41:ArmDisarm, 43:InvertedFlight, 46:RC Override Enable, 51:ModeManual, 55:ModeGuided, 56:ModeLoiter, 58:Clear Waypoints, 62:Compass Learn, 64:Reverse Throttle, 65:GPS Disable, 66:Relay5 On/Off, 67:Relay6 On/Off, 72:ModeCircle, 77:ModeTakeoff, 78:RunCam Control, 79:RunCam OSD Control, 81:Disarm, 82:QAssist 3pos, 84:Air Mode, 85:Generator, 86: Non Auto Terrain Follow Disable, 87:Crow Select, 88:Soaring Enable, 89:Landing Flare, 90:EKF Pos Source, 91:Airspeed Ratio Calibration, 92:FBWA, 95:FBWA taildragger takeoff mode, 96:trigger re-reading of mode switch, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 208:Flap, 209: Forward Throttle, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8 // @User: Standard AP_GROUPINFO_FRAME("OPTION", 6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER|AP_PARAM_FRAME_PLANE),