Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add upward facing surface tracking

Browse Source
This commit is contained in:
Randy Mackay 2019-08-24 10:59:22 +09:00
parent 6be4adbe02
commit 1c4f47f882
5 changed files with 170 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/Copter.h
View File

@ -282,7 +282,7 @@ private:
AP_InertialSensor ins;
RangeFinder rangefinder;
struct {
struct RangeFinderState {
bool enabled:1;
bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder
@ -291,7 +291,7 @@ private:
int16_t alt_cm_glitch_protected; // last glitch protected altitude
int8_t glitch_count; // non-zero number indicates rangefinder is glitching
uint32_t glitch_cleared_ms; // system time glitch cleared
} rangefinder_state;
} rangefinder_state, rangefinder_up_state;
class SurfaceTracking {
public:
@ -302,15 +302,26 @@ private:
bool get_target_alt_cm(float &target_alt_cm) const;
void set_target_alt_cm(float target_alt_cm);
// get target altitude (in cm) for logging purposes
float logging_target_alt() const;
// get target and actual distances (in m) for logging purposes
bool get_dist_for_logging(float &target_dist, float &actual_dist) const;
void invalidate_for_logging() { valid_for_logging = false; }
// surface tracking states
enum class SurfaceTrackingState {
SURFACE_TRACKING_DISABLED = 0,
SURFACE_TRACKING_GROUND = 1,
SURFACE_TRACKING_CEILING = 2
};
// set direction
void set_state(SurfaceTrackingState state);
private:
float target_alt_cm; // desired altitude in cm above the ground
SurfaceTrackingState tracking_state = SurfaceTrackingState::SURFACE_TRACKING_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
bool reset_target; // true if target should be reset because of change in tracking_state
} surface_tracking;
#if RPM_ENABLED == ENABLED
@ -829,6 +840,7 @@ private:
void init_rangefinder(void);
void read_rangefinder(void);
bool rangefinder_alt_ok();
bool rangefinder_up_ok();
void rpm_update();
void init_optflow();
void update_optical_flow(void);

15
ArduCopter/Log.cpp
View File

@ -16,7 +16,7 @@ struct PACKED log_Control_Tuning {
float inav_alt;
int32_t baro_alt;
float desired_rangefinder_alt;
int16_t rangefinder_alt;
float rangefinder_alt;
float terr_alt;
int16_t target_climb_rate;
int16_t climb_rate;
@ -39,6 +39,13 @@ void Copter::Log_Write_Control_Tuning()
target_climb_rate_cms = pos_control->get_vel_target_z();
}
// get surface tracking alts
float desired_rangefinder_alt, rangefinder_alt;
if (!surface_tracking.get_dist_for_logging(desired_rangefinder_alt, rangefinder_alt)) {
desired_rangefinder_alt = AP::logger().quiet_nan();
rangefinder_alt = AP::logger().quiet_nan();;
}
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(),
@ -49,8 +56,8 @@ void Copter::Log_Write_Control_Tuning()
desired_alt : des_alt_m,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt,
desired_rangefinder_alt : surface_tracking.logging_target_alt(),
rangefinder_alt : rangefinder_state.alt_cm,
desired_rangefinder_alt : desired_rangefinder_alt,
rangefinder_alt : rangefinder_alt,
terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms,
climb_rate : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
@ -386,7 +393,7 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },
"CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),

15
ArduCopter/RC_Channel.cpp
View File

@ -77,6 +77,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_
case AUX_FUNC::PRECISION_LOITER:
case AUX_FUNC::INVERTED:
case AUX_FUNC::WINCH_ENABLE:
case AUX_FUNC::SURFACE_TRACKING:
do_aux_function(ch_option, ch_flag);
break;
// the following functions do not need to be initialised:
@ -541,6 +542,20 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
#endif
break;
case AUX_FUNC::SURFACE_TRACKING:
switch (ch_flag) {
case LOW:
copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_GROUND);
break;
case MIDDLE:
copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_DISABLED);
break;
case HIGH:
copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_CEILING);
break;
}
break;
default:
RC_Channel::do_aux_function(ch_option, ch_flag);
break;

113
ArduCopter/sensors.cpp
View File

@ -17,6 +17,10 @@ void Copter::init_rangefinder(void)
rangefinder.init(ROTATION_PITCH_270);
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
// upward facing range finder
rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
#endif
}
@ -26,62 +30,81 @@ void Copter::read_rangefinder(void)
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.update();
rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX));
#if RANGEFINDER_TILT_CORRECTION == ENABLED
const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
#else
const float tile_correction = 1.0f;
#endif
int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
// iterate through downward and upward facing lidar
struct {
RangeFinderState &state;
enum Rotation orientation;
} rngfnd[2] = { {rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}};
#if RANGEFINDER_TILT_CORRECTION == ENABLED
// correct alt for angle of the rangefinder
temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
#endif
for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++) {
// local variables to make accessing simpler
RangeFinderState &rf_state = rngfnd[i].state;
enum Rotation rf_orient = rngfnd[i].orientation;
// tilt corrected but unfiltered, not glitch protected alt
rangefinder_state.alt_cm = temp_alt;
// update health
rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::RangeFinder_Good) &&
(rangefinder.range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX));
// glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
// are considered a glitch and glitch_count becomes non-zero
// glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
// glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
const int32_t glitch_cm = rangefinder_state.alt_cm - rangefinder_state.alt_cm_glitch_protected;
if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
rangefinder_state.glitch_count = MAX(rangefinder_state.glitch_count+1, 1);
} else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
rangefinder_state.glitch_count = MIN(rangefinder_state.glitch_count-1, -1);
} else {
rangefinder_state.glitch_count = 0;
rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
}
if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
// clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
rangefinder_state.glitch_count = 0;
rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
rangefinder_state.glitch_cleared_ms = AP_HAL::millis();
}
// tilt corrected but unfiltered, not glitch protected alt
rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient);
// filter rangefinder for use by AC_WPNav
uint32_t now = AP_HAL::millis();
const bool timed_out = now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
if (rangefinder_state.alt_healthy) {
if (timed_out) {
// reset filter if we haven't used it within the last second
rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
// glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
// are considered a glitch and glitch_count becomes non-zero
// glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
// glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
const int32_t glitch_cm = rf_state.alt_cm - rf_state.alt_cm_glitch_protected;
if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1);
} else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1);
} else {
rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.02f);
rf_state.glitch_count = 0;
rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
}
if (abs(rf_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
// clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
rf_state.glitch_count = 0;
rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
rf_state.glitch_cleared_ms = AP_HAL::millis();
}
rangefinder_state.last_healthy_ms = now;
}
// send rangefinder altitude and health to waypoint navigation library
if (rangefinder_state.alt_healthy || timed_out) {
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
// filter rangefinder altitude
uint32_t now = AP_HAL::millis();
const bool timed_out = now - rf_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
if (rf_state.alt_healthy) {
if (timed_out) {
// reset filter if we haven't used it within the last second
rf_state.alt_cm_filt.reset(rf_state.alt_cm);
} else {
rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.02f);
}
rf_state.last_healthy_ms = now;
}
// send downward facing lidar altitude and health to waypoint navigation library
if (rf_orient == ROTATION_PITCH_270) {
if (rangefinder_state.alt_healthy || timed_out) {
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
}
}
}
#else
// downward facing rangefinder
rangefinder_state.enabled = false;
rangefinder_state.alt_healthy = false;
rangefinder_state.alt_cm = 0;
// upward facing rangefinder
rangefinder_up_state.enabled = false;
rangefinder_up_state.alt_healthy = false;
rangefinder_up_state.alt_cm = 0;
#endif
}
@ -91,6 +114,12 @@ bool Copter::rangefinder_alt_ok()
return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
}
// return true if rangefinder_alt can be used
bool Copter::rangefinder_up_ok()
{
return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
}
/*
update RPM sensors
*/

78
ArduCopter/surface_tracking.cpp
View File

@ -6,39 +6,42 @@
float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
{
#if RANGEFINDER_ENABLED == ENABLED
// if rangefinder alt is not ok or glitching, do not do surface tracking
if (!copter.rangefinder_alt_ok() ||
(copter.rangefinder_state.glitch_count != 0)) {
// check tracking state and that range finders are healthy
if ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED) ||
((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
return target_rate;
}
// calculate current ekf based altitude error
const float current_alt_error = copter.pos_control->get_alt_target() - copter.inertial_nav.get_altitude();
// init based on tracking direction/state
RangeFinderState &rf_state = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
const float dir = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_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) {
target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error;
last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
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_alt_cm += target_rate * copter.G_Dt;
target_dist_cm += dir * target_rate * copter.G_Dt;
}
valid_for_logging = true;
// handle glitch recovery by resetting target
if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) {
// shift to the new rangefinder reading
target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error;
last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
}
// 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_alt_cm - copter.rangefinder_state.alt_cm) - current_alt_error;
float velocity_correction = distance_error * copter.g.rangefinder_gain;
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
@ -52,25 +55,56 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
// returns true if there is a valid target
bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
{
// fail if we are not tracking downwards
if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
return false;
}
// check target has been updated recently
if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
return false;
}
_target_alt_cm = target_alt_cm;
_target_alt_cm = target_dist_cm;
return true;
}
// set target altitude (in cm) above ground
void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
{
target_alt_cm = _target_alt_cm;
// fail if we are not tracking downwards
if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
return;
}
target_dist_cm = _target_alt_cm;
last_update_ms = AP_HAL::millis();
}
float Copter::SurfaceTracking::logging_target_alt() const
bool Copter::SurfaceTracking::get_dist_for_logging(float &target_dist, float &actual_dist) const
{
if (!valid_for_logging) {
return AP::logger().quiet_nan();
if (!valid_for_logging || (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED)) {
return false;
}
return target_alt_cm * 0.01f; // cm->m
target_dist = target_dist_cm * 0.01f;
actual_dist = ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f;
return true;
}
// set direction
void Copter::SurfaceTracking::set_state(SurfaceTrackingState state)
{
if (tracking_state == state) {
return;
}
// check we have a range finder in the correct direction
if ((state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder");
AP_Notify::events.user_mode_change_failed = 1;
return;
}
if ((state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder");
AP_Notify::events.user_mode_change_failed = 1;
return;
}
tracking_state = state;
reset_target = true;
}