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

Copter: move surface tracking variables into structure

This commit is contained in:
Randy Mackay 2019-04-11 12:20:43 +09:00
parent 33a57361bd
commit 89eb876faf
5 changed files with 29 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopter/Attitude.cpp
View File

@ -129,7 +129,6 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
return target_rate;
}
static uint32_t last_call_ms = 0;
const float current_alt = inertial_nav.get_altitude();
const float current_alt_target = pos_control->get_alt_target();
float distance_error;
@ -137,17 +136,17 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
uint32_t now = millis();
target_rangefinder_alt_used = true;
surface_tracking.valid_for_logging = true;
// reset target altitude if this controller has just been engaged
if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;
if (now - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
surface_tracking.target_alt_cm = rangefinder_state.alt_cm + current_alt_target - current_alt;
}
last_call_ms = now;
surface_tracking.last_update_ms = now;
// adjust rangefinder target alt if motors have not hit their limits
if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !motors->limit.throttle_upper)) {
target_rangefinder_alt += target_rate * G_Dt;
surface_tracking.target_alt_cm += target_rate * G_Dt;
}
/*
@ -158,7 +157,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
row. When that happens we reset the target altitude to the new
reading
*/
int32_t glitch_cm = rangefinder_state.alt_cm - target_rangefinder_alt;
int32_t glitch_cm = rangefinder_state.alt_cm - surface_tracking.target_alt_cm;
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) {
@ -168,7 +167,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
}
if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
// shift to the new rangefinder reading
target_rangefinder_alt = rangefinder_state.alt_cm;
surface_tracking.target_alt_cm = rangefinder_state.alt_cm;
rangefinder_state.glitch_count = 0;
}
if (rangefinder_state.glitch_count != 0) {
@ -177,9 +176,9 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
}
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
distance_error = (surface_tracking.target_alt_cm - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
velocity_correction = distance_error * g.rangefinder_gain;
velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
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);

8
ArduCopter/Copter.h
View File

@ -247,6 +247,12 @@ private:
int8_t glitch_count;
} rangefinder_state = { false, false, 0, 0 };
struct {
float target_alt_cm; // desired altitude in cm above the ground
uint32_t last_update_ms; // system time of last update to target_alt_cm
bool valid_for_logging; // true if target_alt_cm is valid for logging
} surface_tracking;
#if RPM_ENABLED == ENABLED
AP_RPM rpm_sensor;
#endif
@ -402,8 +408,6 @@ private:
#endif
// Altitude
float target_rangefinder_alt; // desired altitude in cm above the ground
bool target_rangefinder_alt_used; // true if mode is using target_rangefinder_alt
int32_t baro_alt; // barometer altitude in cm above home
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests

10
ArduCopter/Log.cpp
View File

@ -39,11 +39,11 @@ void Copter::Log_Write_Control_Tuning()
target_climb_rate_cms = pos_control->get_vel_target_z();
}
float _target_rangefinder_alt;
if (target_rangefinder_alt_used) {
_target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m
float surface_tracking_target_alt;
if (surface_tracking.valid_for_logging) {
surface_tracking_target_alt = surface_tracking.target_alt_cm * 0.01f; // cm->m
} else {
_target_rangefinder_alt = logger.quiet_nan();
surface_tracking_target_alt = logger.quiet_nan();
}
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
@ -55,7 +55,7 @@ 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 : _target_rangefinder_alt,
desired_rangefinder_alt : surface_tracking_target_alt,
rangefinder_alt : rangefinder_state.alt_cm,
terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms,

12
ArduCopter/config.h
View File

@ -88,16 +88,20 @@
# define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder
#endif
#ifndef RANGEFINDER_TIMEOUT_MS
# define RANGEFINDER_TIMEOUT_MS 1000 // rangefinder filter reset if no updates from sensor in 1 second
#endif
#ifndef RANGEFINDER_GAIN_DEFAULT
# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
#endif
#ifndef THR_SURFACE_TRACKING_VELZ_MAX
# define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
#ifndef SURFACE_TRACKING_VELZ_MAX
# define SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
#endif
#ifndef RANGEFINDER_TIMEOUT_MS
# define RANGEFINDER_TIMEOUT_MS 1000 // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
#ifndef SURFACE_TRACKING_TIMEOUT_MS
# define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
#endif
#ifndef RANGEFINDER_WPNAV_FILT_HZ

2
ArduCopter/mode.cpp
View File

@ -265,7 +265,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
// called at 100hz or more
void Copter::update_flight_mode()
{
target_rangefinder_alt_used = false;
surface_tracking.valid_for_logging = false; // invalidate surface tracking alt, flight mode will set to true if used
flightmode->run();
}