aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: added get_throttle_althold_with_slew to allow slower altitude target changes

Browse Source 
Improved surface tracking by using slewed althold controller
Reduced sonar mode filter to just 3 elements to reduce lag but at the possible consequence of allowing sonar noise to creep through for people with margin sonar set-ups.
This commit is contained in:
rmackay9 2013-01-08 16:41:07 +09:00 committed by Andrew Tridgell
parent f683cff9e2
commit 21b6c78d12
4 changed files with 94 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ArduCopter/ArduCopter.pde
View File

@ -315,9 +315,8 @@ GCS_MAVLINK gcs3;
// SONAR selection
////////////////////////////////////////////////////////////////////////////////
//
ModeFilterInt16_Size3 sonar_mode_filter(1);
#if CONFIG_SONAR == ENABLED
ModeFilterInt16_Size5 sonar_mode_filter(2);
AP_HAL::AnalogSource *sonar_analog_source;
AP_RangeFinder_MaxsonarXL *sonar;
#endif
@ -640,6 +639,8 @@ static float current_total1;
////////////////////////////////////////////////////////////////////////////////
// Altitude
////////////////////////////////////////////////////////////////////////////////
// The (throttle) controller desired altitude in cm
static float controller_desired_alt;
// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP
static int32_t altitude_error;
// The cm/s we are moving up or down based on sensor data - Positive = UP
@ -650,7 +651,7 @@ static int16_t climb_rate_error;
static int16_t climb_rate;
// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
static int16_t sonar_alt;
static bool sonar_alt_ok; // true if we can trust the altitude from the sonar
static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar
// The climb_rate as reported by sonar in cm/s
static int16_t sonar_rate;
// The altitude as reported by Baro in cm – Values can be quite high
@ -1786,13 +1787,17 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
break;
case THROTTLE_STABILIZED_RATE:
case THROTTLE_DIRECT_ALT:
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
throttle_initialised = true;
break;
case THROTTLE_HOLD:
case THROTTLE_AUTO:
case THROTTLE_SURFACE_TRACKING:
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
set_new_altitude(current_loc.alt); // by default hold the current altitude
if ( throttle_mode < THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
@ -1803,19 +1808,12 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_LAND:
set_land_complete(false); // mark landing as incomplete
land_detector = 0; // A counter that goes up if our climb rate stalls out.
set_new_altitude(0); // Set a new target altitude
throttle_initialised = true;
break;
case THROTTLE_SURFACE_TRACKING:
if( g.sonar_enabled ) {
set_new_altitude(current_loc.alt); // by default hold the current altitude
if ( throttle_mode < THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
}
throttle_initialised = true;
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
// Set target altitude to LAND_START_ALT if we are high, below this altitude the get_throttle_rate_stabilized will take care of setting the next_WP.alt
if (current_loc.alt >= LAND_START_ALT) {
set_new_altitude(LAND_START_ALT);
}
// To-Do: handle the case where the sonar is not enabled
throttle_initialised = true;
break;
default:
@ -1954,7 +1952,7 @@ void update_throttle_mode(void)
altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS
}else{
int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in);
get_throttle_althold(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
get_throttle_althold_with_slew(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
}
break;
@ -1967,7 +1965,7 @@ void update_throttle_mode(void)
case THROTTLE_AUTO:
// auto pilot altitude controller with target altitude held in next_WP.alt
if(motors.auto_armed() == true) {
get_throttle_althold(next_WP.alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
get_throttle_althold_with_slew(next_WP.alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
}
break;
@ -1979,7 +1977,7 @@ void update_throttle_mode(void)
case THROTTLE_SURFACE_TRACKING:
// surface tracking with sonar or other rangefinder plus pilot input of climb rate
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
if( sonar_alt_ok ) {
if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
// if sonar is ok, use surface tracking
get_throttle_surface_tracking(pilot_climb_rate);
}else{

85
ArduCopter/Attitude.pde
View File

@ -787,15 +787,24 @@ static int16_t
get_throttle_accel(int16_t z_target_accel)
{
static float z_accel_error = 0; // The acceleration error in cm.
static uint32_t last_call_ms = 0; // the last time this controller was called
int32_t p,i,d; // used to capture pid values for logging
int16_t output;
float z_accel_meas;
uint32_t now = millis();
// Calculate Earth Frame Z acceleration
z_accel_meas = -(ahrs.get_accel_ef().z + GRAVITY_MSS) * 100;
// calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error);
// reset target altitude if this controller has just been engaged
if( now - last_call_ms > 100 ) {
// Reset Filter
z_accel_error = 0;
} else {
// calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error);
}
last_call_ms = now;
// separately calculate p, i, d values for logging
p = g.pid_throttle_accel.get_p(z_accel_error);
@ -916,12 +925,21 @@ static int32_t get_pilot_desired_direct_alt(int16_t throttle_control)
static void
get_throttle_rate(int16_t z_target_speed)
{
static uint32_t last_call_ms = 0;
static float z_rate_error = 0; // The velocity error in cm.
int32_t p,i,d; // used to capture pid values for logging
int16_t output; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors
uint32_t now = millis();
// calculate rate error and filter with cut off frequency of 2 Hz
z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - climb_rate) - z_rate_error);
// reset target altitude if this controller has just been engaged
if( now - last_call_ms > 100 ) {
// Reset Filter
z_rate_error = 0;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - climb_rate) - z_rate_error);
}
last_call_ms = now;
// separately calculate p, i, d values for logging
p = g.pid_throttle.get_p(z_rate_error);
@ -980,9 +998,9 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
if( g.pi_alt_hold.kP() != 0 ) {
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( alt_error > 2*linear_distance ) {
desired_rate = sqrt(2*250*(alt_error-linear_distance));
desired_rate = safe_sqrt(2*250*(alt_error-linear_distance));
}else if( alt_error < -2*linear_distance ) {
desired_rate = -sqrt(2*250*(-alt_error-linear_distance));
desired_rate = -safe_sqrt(2*250*(-alt_error-linear_distance));
}else{
desired_rate = g.pi_alt_hold.get_p(alt_error);
}
@ -1001,44 +1019,45 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
// TO-DO: enabled PID logging for this controller
}
// get_throttle_althold_with_slew - altitude controller with slew to avoid step changes in altitude target
// calls normal althold controller which updates accel based throttle controller targets
static void
get_throttle_althold_with_slew(int16_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
// limit target altitude change
controller_desired_alt += constrain(target_alt-controller_desired_alt, min_climb_rate*0.02, max_climb_rate*0.02);
// do not let target altitude get too far from current altitude
controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
get_throttle_althold(controller_desired_alt, min_climb_rate-250, max_climb_rate+250); // 250 is added to give head room to alt hold controller
}
// get_throttle_rate_stabilized - rate controller with additional 'stabilizer'
// 'stabilizer' ensure desired rate is being met
// calls normal throttle rate controller which updates accel based throttle controller targets
static void
get_throttle_rate_stabilized(int16_t target_rate)
{
static float target_alt = 0; // The desired altitude in cm.
static uint32_t last_call_ms = 0;
uint32_t now = millis();
// reset target altitude if this controller has just been engaged
if( now - last_call_ms > 1000 ) {
target_alt = current_loc.alt;
}
last_call_ms = millis();
target_alt += target_rate * 0.02;
controller_desired_alt += target_rate * 0.02;
// do not let target altitude get too far from current altitude
target_alt = constrain(target_alt,current_loc.alt-750,current_loc.alt+750);
controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
set_new_altitude(target_alt);
set_new_altitude(controller_desired_alt);
get_throttle_althold(target_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
get_throttle_althold(controller_desired_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
}
// get_throttle_land - high level landing logic
// sends the desired acceleration in the accel based throttle controller
// called at 50hz
#define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent
#define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete.
static void
get_throttle_land()
{
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
if (current_loc.alt >= LAND_START_ALT && !(g.sonar_enabled && sonar_alt_ok)) {
get_throttle_althold(LAND_START_ALT, g.auto_velocity_z_min, -abs(g.land_speed));
if (current_loc.alt >= LAND_START_ALT && !(g.sonar_enabled && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
get_throttle_althold_with_slew(LAND_START_ALT, g.auto_velocity_z_min, -abs(g.land_speed));
}else{
get_throttle_rate_stabilized(-abs(g.land_speed));
@ -1069,23 +1088,29 @@ get_throttle_surface_tracking(int16_t target_rate)
{
static float target_sonar_alt = 0; // The desired altitude in cm above the ground
static uint32_t last_call_ms = 0;
float distance_error;
float sonar_induced_slew_rate;
uint32_t now = millis();
// reset target altitude if this controller has just been engaged
if( now - last_call_ms > 1000 ) {
target_sonar_alt = sonar_alt + next_WP.alt - current_loc.alt;
if( now - last_call_ms > 200 ) {
target_sonar_alt = sonar_alt + controller_desired_alt - current_loc.alt;
}
last_call_ms = millis();
last_call_ms = now;
target_sonar_alt += target_rate * 0.02;
distance_error = (target_sonar_alt-sonar_alt);
sonar_induced_slew_rate = constrain(fabs(THR_SURFACE_TRACKING_P * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX);
// do not let target altitude get too far from current altitude above ground
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
target_sonar_alt = constrain(target_sonar_alt,sonar_alt-750,sonar_alt+750);
set_new_altitude(current_loc.alt+(target_sonar_alt-sonar_alt));
controller_desired_alt = current_loc.alt+(target_sonar_alt-sonar_alt);
set_new_altitude(controller_desired_alt);
get_throttle_althold(next_WP.alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
get_throttle_althold_with_slew(controller_desired_alt, target_rate-sonar_induced_slew_rate, target_rate+sonar_induced_slew_rate); // VELZ_MAX limits how quickly we react
}
/*

19
ArduCopter/config.h
View File

@ -311,6 +311,18 @@
# define CONFIG_SONAR ENABLED
#endif
#ifndef SONAR_ALT_HEALTH_MAX
# define SONAR_ALT_HEALTH_MAX 3 // number of good reads that indicates a healthy sonar
#endif
#ifndef THR_SURFACE_TRACKING_P
# define THR_SURFACE_TRACKING_P 0.2 // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
#endif
#ifndef THR_SURFACE_TRACKING_VELZ_MAX
# define THR_SURFACE_TRACKING_VELZ_MAX 30 // max speed number of good reads that indicates a healthy sonar
#endif
//////////////////////////////////////////////////////////////////////////////
// Channel 7 default option
//
@ -504,7 +516,12 @@
#ifndef LAND_SPEED
# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s
#endif
#ifndef LAND_START_ALT
# define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent
#endif
#ifndef LAND_DETECTOR_TRIGGER
# define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete.
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

8
ArduCopter/sensors.pde
View File

@ -37,10 +37,12 @@ static int16_t read_sonar(void)
#if CONFIG_SONAR == ENABLED
int16_t temp_alt = sonar->read();
if(temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70) {
sonar_alt_ok = true;
if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70) {
if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) {
sonar_alt_health++;
}
}else{
sonar_alt_ok = false;
sonar_alt_health = 0;
}
#if SONAR_TILT_CORRECTION == 1