mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: first implementation of surface tracking using sonar
Note: not yet enabled as part of any flight mode
This commit is contained in:
parent
07a3a7d4bb
commit
b9dbdadc80
|
@ -673,6 +673,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
|
||||
// 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
|
||||
|
@ -1797,6 +1798,17 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
|
|||
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;
|
||||
}
|
||||
// To-Do: handle the case where the sonar is not enabled
|
||||
break;
|
||||
|
||||
default:
|
||||
// To-Do: log an error message to the dataflash or tlogs instead of printing to the serial port
|
||||
cliSerial->printf_P(PSTR("Unsupported throttle mode: %d!!"),new_throttle_mode);
|
||||
|
@ -1953,6 +1965,18 @@ void update_throttle_mode(void)
|
|||
// landing throttle controller
|
||||
get_throttle_land();
|
||||
break;
|
||||
|
||||
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 is ok, use surface tracking
|
||||
get_throttle_surface_tracking(pilot_climb_rate);
|
||||
}else{
|
||||
// if no sonar fall back stabilize rate controller
|
||||
get_throttle_rate_stabilized(pilot_climb_rate);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -1052,8 +1052,8 @@ get_throttle_rate_stabilized(int16_t target_rate)
|
|||
static void
|
||||
get_throttle_land()
|
||||
{
|
||||
// if we are above 10m perform regular alt hold descent
|
||||
if (current_loc.alt >= LAND_START_ALT) {
|
||||
// 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));
|
||||
}else{
|
||||
get_throttle_rate_stabilized(-abs(g.land_speed));
|
||||
|
@ -1078,6 +1078,32 @@ get_throttle_land()
|
|||
}
|
||||
}
|
||||
|
||||
// get_throttle_surface_tracking - hold copter at the desired distance above the ground
|
||||
// updates accel based throttle controller targets
|
||||
static void
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
last_call_ms = millis();
|
||||
|
||||
target_sonar_alt += target_rate * 0.02;
|
||||
|
||||
// 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));
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
/*
|
||||
* reset all I integrators
|
||||
*/
|
||||
|
|
|
@ -39,6 +39,7 @@
|
|||
#define THROTTLE_HOLD 6 // alt hold plus pilot input of climb rate
|
||||
#define THROTTLE_AUTO 7 // auto pilot altitude controller with target altitude held in next_WP.alt
|
||||
#define THROTTLE_LAND 8 // landing throttle controller
|
||||
#define THROTTLE_SURFACE_TRACKING 9 // ground tracking with sonar or other rangefinder
|
||||
|
||||
|
||||
// active altitude sensor
|
||||
|
|
|
@ -37,6 +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;
|
||||
}else{
|
||||
sonar_alt_ok = false;
|
||||
}
|
||||
|
||||
#if SONAR_TILT_CORRECTION == 1
|
||||
// correct alt for angle of the sonar
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
|
|
Loading…
Reference in New Issue