Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Separate landing and terrain following.

This commit is contained in:
Leonard Hall 2021-08-31 13:47:59 +09:30 committed by Randy Mackay
parent 00ff3cec46
commit 251b1fb9ea
15 changed files with 83 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/Copter.h
View File

@ -266,8 +266,9 @@ private:
class SurfaceTracking {
public:
// get desired climb rate (in cm/s) to achieve surface tracking
float adjust_climb_rate(float target_rate);
// update_surface_offset - manages the vertical offset of the position controller to follow the
// measured ground or ceiling level measured using the range finder.
void update_surface_offset();
// get/set target altitude (in cm) above ground
bool get_target_alt_cm(float &target_alt_cm) const;
@ -289,7 +290,6 @@ private:
private:
Surface surface = Surface::GROUND;
float target_dist_cm; // desired distance in cm from ground or ceiling
uint32_t last_update_ms; // system time of last update to target_alt_cm
uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery
bool valid_for_logging; // true if target_alt_cm is valid for logging

2
ArduCopter/mode.cpp
View File

@ -549,7 +549,7 @@ void Mode::land_run_vertical_control(bool pause_descent)
}
// update altitude target and call position controller
pos_control->set_pos_target_z_from_climb_rate_cm(cmb_rate, ignore_descent_limit);
pos_control->land_at_climb_rate_cm(cmb_rate, ignore_descent_limit);
pos_control->update_z_controller();
}

9
ArduCopter/mode_althold.cpp
View File

@ -84,13 +84,14 @@ void ModeAltHold::run()
copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
#endif
// adjust climb rate using rangefinder
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
break;
}

7
ArduCopter/mode_auto.cpp
View File

@ -983,7 +983,12 @@ void ModeAuto::loiter_to_alt_run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
pos_control->update_z_controller();
}

2
ArduCopter/mode_autorotate.cpp
View File

@ -252,7 +252,7 @@ void ModeAutorotate::run()
_pitch_target -= _target_pitch_adjust*G_Dt;
}
// Set position controller
pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z, false);
pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z);
// Update controllers
pos_control->update_z_controller();

2
ArduCopter/mode_brake.cpp
View File

@ -59,7 +59,7 @@ void ModeBrake::run()
// call attitude controller
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false);
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
pos_control->update_z_controller();
if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {

9
ArduCopter/mode_circle.cpp
View File

@ -89,11 +89,6 @@ void ModeCircle::run()
// get pilot desired climb rate (or zero if in radio failsafe)
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
@ -104,7 +99,9 @@ void ModeCircle::run()
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run circle controller
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
// call attitude controller

9
ArduCopter/mode_flowhold.cpp
View File

@ -297,13 +297,14 @@ void ModeFlowHold::run()
case AltHold_Flying:
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
copter.pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
break;
}

2
ArduCopter/mode_guided.cpp
View File

@ -879,7 +879,7 @@ void ModeGuided::angle_control_run()
if (guided_angle_state.use_thrust) {
attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt);
} else {
pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false);
pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
pos_control->update_z_controller();
}
}

9
ArduCopter/mode_loiter.cpp
View File

@ -171,13 +171,14 @@ void ModeLoiter::run()
// call attitude controller
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
// adjust climb rate using rangefinder
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
break;
}

12
ArduCopter/mode_poshold.cpp
View File

@ -158,16 +158,14 @@ void ModePosHold::run()
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
}
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
break;
}

9
ArduCopter/mode_sport.cpp
View File

@ -105,13 +105,14 @@ void ModeSport::run()
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
break;
}

4
ArduCopter/mode_throw.cpp
View File

@ -173,7 +173,7 @@ void ModeThrow::run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
// call height controller
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false);
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
pos_control->update_z_controller();
break;
@ -193,7 +193,7 @@ void ModeThrow::run()
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
// call height controller
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false);
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
pos_control->update_z_controller();
break;

9
ArduCopter/mode_zigzag.cpp
View File

@ -376,13 +376,14 @@ void ModeZigZag::manual_control()
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
// adjust climb rate using rangefinder
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();
// Send the commanded climb rate to the position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
break;
}

90
ArduCopter/surface_tracking.cpp
View File

@ -1,67 +1,46 @@
#include "Copter.h"
// adjust_climb_rate - hold copter at the desired distance above the
// ground; returns climb rate (in cm/s) which should be passed to
// the position controller
float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
// update_surface_offset - manages the vertical offset of the position controller to follow the measured ground or ceiling
// level measured using the range finder.
void Copter::SurfaceTracking::update_surface_offset()
{
float offset_cm = 0.0;
#if RANGEFINDER_ENABLED == ENABLED
// check tracking state and that range finders are healthy
if ((surface == Surface::NONE) ||
((surface == Surface::GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
((surface == Surface::CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
return target_rate;
if (((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) ||
((surface == Surface::CEILING) && copter.rangefinder_up_ok() && (copter.rangefinder_up_state.glitch_count == 0))) {
// init based on tracking direction/state
RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
offset_cm = copter.inertial_nav.get_altitude() - dir * rf_state.alt_cm;
// reset target altitude if this controller has just been engaged
// target has been changed between upwards vs downwards
// or glitch has cleared
const uint32_t now = millis();
if ((now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) ||
reset_target ||
(last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
copter.pos_control->set_pos_offset_z_cm(offset_cm);
reset_target = false;
last_glitch_cleared_ms = rf_state.glitch_cleared_ms;
}
last_update_ms = now;
valid_for_logging = true;
} else {
copter.pos_control->set_pos_offset_z_cm(offset_cm);
}
// calculate current ekf based altitude error
const float current_alt_error = copter.pos_control->get_pos_target_z_cm() - copter.inertial_nav.get_altitude();
// init based on tracking direction/state
RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
// reset target altitude if this controller has just been engaged
// target has been changed between upwards vs downwards
// or glitch has cleared
const uint32_t now = millis();
if ((now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) ||
reset_target ||
(last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
target_dist_cm = rf_state.alt_cm + (dir * current_alt_error);
reset_target = false;
last_glitch_cleared_ms = rf_state.glitch_cleared_ms;\
}
last_update_ms = now;
// adjust rangefinder target alt if motors have not hit their limits
if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) {
target_dist_cm += dir * target_rate * copter.G_Dt;
}
valid_for_logging = true;
#if AC_AVOID_ENABLED == ENABLED
// upward facing terrain following never gets closer than avoidance margin
if (surface == Surface::CEILING) {
const float margin_cm = copter.avoid.enabled() ? copter.avoid.get_margin() * 100.0f : 0.0f;
target_dist_cm = MAX(target_dist_cm, margin_cm);
}
#endif
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
const float distance_error = (target_dist_cm - rf_state.alt_cm) - (dir * current_alt_error);
float velocity_correction = dir * distance_error * copter.g.rangefinder_gain;
velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);
// return combined pilot climb rate + rate to correct rangefinder alt error
return (target_rate + velocity_correction);
#else
return target_rate;
copter.pos_control->set_pos_offset_z_cm(offset_cm);
#endif
copter.pos_control->set_pos_offset_target_z_cm(offset_cm);
}
// get target altitude (in cm) above ground
// returns true if there is a valid target
bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
bool Copter::SurfaceTracking::get_target_alt_cm(float &target_alt_cm) const
{
// fail if we are not tracking downwards
if (surface != Surface::GROUND) {
@ -71,7 +50,7 @@ bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
return false;
}
_target_alt_cm = target_dist_cm;
target_alt_cm = (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm());
return true;
}
@ -82,7 +61,7 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
if (surface != Surface::GROUND) {
return;
}
target_dist_cm = _target_alt_cm;
copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_altitude() - _target_alt_cm);
last_update_ms = AP_HAL::millis();
}
@ -92,7 +71,8 @@ bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) co
return false;
}
target_dist = target_dist_cm * 0.01f;
const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
target_dist = dir * (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()) * 0.01f;
return true;
}