mirror of https://github.com/ArduPilot/ardupilot
ArduCopterNG - fixes to sonar/barometer altitude hold. Fixes bugs including:
1. bad values from sonar were allowed to feed into altitude PID control leading to little hops 2. barometer altitude hold was disabled if sonar wasn't present git-svn-id: https://arducopter.googlecode.com/svn/trunk@1420 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
e47352e941
commit
153da15579
|
@ -325,6 +325,7 @@ float baro_altitude_D;
|
|||
int target_sonar_altitude; // target altitude in cm
|
||||
long press_sonar_altitude = 0;
|
||||
int sonar_status = SONAR_STATUS_BAD; // indicates if sonar values can be trusted
|
||||
int sonar_valid_count = 0; // from -5 ~ 5 -ve = number of invalid readings, +ve = number of valid readings (in a row)
|
||||
long sonar_threshold; // threshold at which we should transfer control to barometer (normally 80% of sonar's max distance)
|
||||
byte sonar_new_data = 0;
|
||||
float sonar_altitude_I;
|
||||
|
|
|
@ -410,22 +410,25 @@ void loop()
|
|||
}
|
||||
|
||||
// Switch on altitude control if we have a barometer or Sonar
|
||||
#if defined(UseBMP) || defined(IsSONAR)
|
||||
#if (defined(UseBMP) || defined(IsSONAR))
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_NONE )
|
||||
{
|
||||
// by default turn on altitude hold using barometer
|
||||
#ifdef UseBMP
|
||||
altitude_control_method = ALTITUDE_CONTROL_BARO;
|
||||
target_baro_altitude = press_baro_altitude;
|
||||
baro_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control
|
||||
if( press_baro_altitude != 0 )
|
||||
{
|
||||
altitude_control_method = ALTITUDE_CONTROL_BARO;
|
||||
target_baro_altitude = press_baro_altitude;
|
||||
baro_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control
|
||||
}
|
||||
#endif
|
||||
|
||||
// use sonar if it's available
|
||||
#ifdef IsSONAR
|
||||
if( sonar_status == SONAR_STATUS_OK )
|
||||
if( sonar_status == SONAR_STATUS_OK && press_sonar_altitude != 0 )
|
||||
{
|
||||
altitude_control_method = ALTITUDE_CONTROL_SONAR;
|
||||
target_sonar_altitude = press_sonar_altitude;
|
||||
target_sonar_altitude = constrain(press_sonar_altitude,AP_RangeFinder_down.min_distance*3,sonar_threshold);
|
||||
}
|
||||
sonar_altitude_I = 0; // don't carry over any I values from previous times user may have switched on altitude control
|
||||
#endif
|
||||
|
@ -440,22 +443,22 @@ void loop()
|
|||
if( sonar_new_data ) // if new sonar data has arrived
|
||||
{
|
||||
// Allow switching between sonar and barometer
|
||||
#if defined(UseBMP)
|
||||
#ifdef UseBMP
|
||||
|
||||
// if SONAR become invalid switch to barometer
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_SONAR && sonar_status == SONAR_STATUS_BAD )
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_SONAR && sonar_valid_count <= -3 )
|
||||
{
|
||||
// next target barometer altitude to current barometer altitude + user's desired change over last sonar altitude (i.e. keeps up the momentum)
|
||||
altitude_control_method = ALTITUDE_CONTROL_BARO;
|
||||
target_baro_altitude = press_baro_altitude + constrain((target_sonar_altitude - press_sonar_altitude),-50,50);
|
||||
target_baro_altitude = press_baro_altitude;// + constrain((target_sonar_altitude - press_sonar_altitude),-50,50);
|
||||
}
|
||||
|
||||
// if SONAR becomes valid switch to sonar control
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_BARO && sonar_status == SONAR_STATUS_OK )
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_BARO && sonar_valid_count >= 3 )
|
||||
{
|
||||
altitude_control_method = ALTITUDE_CONTROL_SONAR;
|
||||
if( target_sonar_altitude == 0 ) { // if target sonar altitude hasn't been intialised before..
|
||||
target_sonar_altitude = press_sonar_altitude + constrain((target_baro_altitude - press_baro_altitude),-50,50); // maybe this should just use the user's last valid target sonar altitude
|
||||
target_sonar_altitude = press_sonar_altitude;// + constrain((target_baro_altitude - press_baro_altitude),-50,50); // maybe this should just use the user's last valid target sonar altitude
|
||||
}
|
||||
// ensure target altitude is reasonable
|
||||
target_sonar_altitude = constrain(target_sonar_altitude,AP_RangeFinder_down.min_distance*3,sonar_threshold);
|
||||
|
@ -534,11 +537,6 @@ void loop()
|
|||
heli_moveSwashPlate();
|
||||
#endif
|
||||
|
||||
#ifdef IsSONAR
|
||||
// read altitude from Sonar at 60Hz but only process every 3rd value (=20hz)
|
||||
read_Sonar();
|
||||
#endif
|
||||
|
||||
// Each of the six cases executes at 10Hz
|
||||
switch (medium_loopCounter){
|
||||
case 0: // Magnetometer reading (10Hz)
|
||||
|
@ -560,6 +558,7 @@ void loop()
|
|||
}
|
||||
#endif
|
||||
#ifdef IsSONAR
|
||||
read_Sonar();
|
||||
sonar_new_data = 1; // process sonar values at 20Hz
|
||||
#endif
|
||||
#ifdef IsRANGEFINDER
|
||||
|
@ -588,6 +587,7 @@ void loop()
|
|||
}
|
||||
#endif
|
||||
#ifdef IsSONAR
|
||||
read_Sonar();
|
||||
sonar_new_data = 1; // process sonar values at 20Hz
|
||||
#endif
|
||||
#ifdef IsRANGEFINDER
|
||||
|
|
|
@ -42,7 +42,7 @@ void motor_output()
|
|||
byte throttle_mode=0;
|
||||
|
||||
throttle = ch_throttle;
|
||||
#if defined(UseBMP) || defined(IsRANGEFINDER)
|
||||
#if (defined(UseBMP) || defined(IsSONAR))
|
||||
if (AP_mode == AP_AUTOMATIC_MODE)
|
||||
{
|
||||
throttle = ch_throttle_altitude_hold;
|
||||
|
|
|
@ -170,11 +170,12 @@ int Altitude_control_baro(int altitude, int target_altitude)
|
|||
|
||||
err_altitude_old = err_altitude;
|
||||
err_altitude = target_altitude - altitude;
|
||||
baro_altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz
|
||||
baro_altitude_I += (float)err_altitude*0.05;
|
||||
baro_altitude_I = constrain(baro_altitude_I,-140,140);
|
||||
baro_altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz
|
||||
command_altitude = KP_ALTITUDE*err_altitude + KD_ALTITUDE*baro_altitude_D + KI_ALTITUDE*baro_altitude_I;
|
||||
command_altitude = initial_throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX);
|
||||
Log_Write_PID(5,KP_ALTITUDE*err_altitude,KI_ALTITUDE*baro_altitude_I,KD_ALTITUDE*baro_altitude_D,command_altitude);
|
||||
return command_altitude;
|
||||
}
|
||||
|
||||
|
@ -183,18 +184,21 @@ int Altitude_control_baro(int altitude, int target_altitude)
|
|||
#define GdT_SONAR_ALTITUDE 0.05
|
||||
#define ALTITUDE_CONTROL_SONAR_OUTPUT_MIN 60
|
||||
#define ALTITUDE_CONTROL_SONAR_OUTPUT_MAX 80
|
||||
int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_altitude)
|
||||
int Altitude_control_Sonar(int altitude, int target_altitude)
|
||||
{
|
||||
static int err_altitude = 0;
|
||||
int command_altitude;
|
||||
int err_altitude_old;
|
||||
|
||||
err_altitude_old = err_altitude;
|
||||
err_altitude = target_sonar_altitude - Sonar_altitude;
|
||||
sonar_altitude_D = (float)(err_altitude-err_altitude_old)/GdT_SONAR_ALTITUDE;
|
||||
err_altitude = target_altitude - altitude;
|
||||
sonar_altitude_I += (float)err_altitude*GdT_SONAR_ALTITUDE;
|
||||
sonar_altitude_I = constrain(sonar_altitude_I,-1000,1000);
|
||||
command_altitude = KP_SONAR_ALTITUDE*err_altitude + KD_SONAR_ALTITUDE*sonar_altitude_D + KI_SONAR_ALTITUDE*sonar_altitude_I;
|
||||
sonar_altitude_D = (float)(err_altitude-err_altitude_old)/GdT_SONAR_ALTITUDE;
|
||||
command_altitude = KP_SONAR_ALTITUDE*err_altitude + KI_SONAR_ALTITUDE*sonar_altitude_I + KD_SONAR_ALTITUDE*sonar_altitude_D ;
|
||||
command_altitude = initial_throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX);
|
||||
Log_Write_PID(4,KP_SONAR_ALTITUDE*err_altitude,KI_SONAR_ALTITUDE*sonar_altitude_I,KD_SONAR_ALTITUDE*sonar_altitude_D,command_altitude);
|
||||
return (initial_throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX));
|
||||
return command_altitude;
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
|
|
|
@ -104,33 +104,31 @@ void read_baro(void)
|
|||
/* This function reads in the values from the attached range finders (currently only downward pointing sonar) */
|
||||
void read_Sonar()
|
||||
{
|
||||
// -ve = number of invalid readings, +ve = number of valid readings (in a row)
|
||||
static int sonar_valid_count = 0;
|
||||
|
||||
// calculate altitude from down sensor
|
||||
AP_RangeFinder_down.read();
|
||||
if( AP_RangeFinder_down.distance >= AP_RangeFinder_down.max_distance * 0.8 ) {
|
||||
// if we get too many invalid distances, mark sonar as invalid
|
||||
if( sonar_status == SONAR_STATUS_OK )
|
||||
{
|
||||
sonar_valid_count--;
|
||||
if( sonar_valid_count <= -5 )
|
||||
sonar_status = SONAR_STATUS_BAD;
|
||||
}
|
||||
}else{
|
||||
if( sonar_status == SONAR_STATUS_BAD )
|
||||
{
|
||||
sonar_valid_count++;
|
||||
if( sonar_valid_count >= 5 )
|
||||
sonar_status = SONAR_STATUS_OK;
|
||||
}else {
|
||||
press_sonar_altitude = DCM_Matrix[2][2] * AP_RangeFinder_down.distance;\
|
||||
|
||||
// deal with the unusual case that we're up-side-down
|
||||
if( press_sonar_altitude < 0 )
|
||||
press_sonar_altitude = 0;
|
||||
}
|
||||
// translate into an altitude
|
||||
press_sonar_altitude = DCM_Matrix[2][2] * AP_RangeFinder_down.distance;
|
||||
|
||||
// deal with the unusual case that we're up-side-down
|
||||
if( press_sonar_altitude < 0 )
|
||||
press_sonar_altitude = 0;
|
||||
|
||||
// set sonar status to OK and update sonar_valid_count which shows reliability of sonar (i.e. are we out of range?)
|
||||
if( AP_RangeFinder_down.distance > sonar_threshold ) {
|
||||
sonar_status = SONAR_STATUS_BAD;
|
||||
if( sonar_valid_count > 0 )
|
||||
sonar_valid_count = -1;
|
||||
else
|
||||
sonar_valid_count--;
|
||||
}else{
|
||||
sonar_status = SONAR_STATUS_OK;
|
||||
if( sonar_valid_count < 0 )
|
||||
sonar_valid_count = 1;
|
||||
else
|
||||
sonar_valid_count++;
|
||||
}
|
||||
sonar_valid_count = constrain(sonar_valid_count,-10,10);
|
||||
|
||||
#if LOG_RANGEFINDER && !defined(IsRANGEFINDER)
|
||||
Log_Write_RangeFinder(AP_RangeFinder_down.distance,0,0,0,0,0);
|
||||
|
|
|
@ -185,7 +185,9 @@ void APM_Init() {
|
|||
// Sonar for Altitude hold
|
||||
#ifdef IsSONAR
|
||||
AP_RangeFinder_down.init(AP_RANGEFINDER_PITOT_TUBE, &adc); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN);
|
||||
//AP_RangeFinder_down.init(AN5); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN);
|
||||
sonar_threshold = AP_RangeFinder_down.max_distance * 0.8;
|
||||
sonar_status = SONAR_STATUS_OK; // assume sonar is ok to start with
|
||||
#endif
|
||||
|
||||
// RangeFinders for obstacle avoidance
|
||||
|
|
Loading…
Reference in New Issue