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

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:
rmackay9@yahoo.com 2011-01-04 14:38:17 +00:00
parent e47352e941
commit 153da15579
6 changed files with 54 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArducopterNG/Arducopter.h
View File

@ -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;

38
ArducopterNG/ArducopterNG.pde
View File

@ -410,29 +410,32 @@ 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
// capture current throttle to use as base for altitude control
initial_throttle = ch_throttle;
ch_throttle_altitude_hold = ch_throttle;
ch_throttle_altitude_hold = ch_throttle;
}
// Sonar Altitude Control
@ -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);
@ -481,7 +484,7 @@ void loop()
#if !defined(UseBMP) // limit the upper altitude if no barometer used
if( target_sonar_altitude > sonar_threshold)
target_sonar_altitude = sonar_threshold;
#endif
#endif
}
}
sonar_new_data = 0; // record that we've consumed the sonar data
@ -517,7 +520,7 @@ void loop()
target_position=0;
if( altitude_control_method != ALTITUDE_CONTROL_NONE )
{
altitude_control_method = ALTITUDE_CONTROL_NONE; // turn off altitude control
altitude_control_method = ALTITUDE_CONTROL_NONE; // turn off altitude control
}
} // if (AP_mode == AP_AUTOMATIC_MODE)
}
@ -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

2
ArducopterNG/Motors.pde
View File

@ -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;

16
ArducopterNG/Navigation.pde
View File

@ -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;
}
/* ************************************************************ */

44
ArducopterNG/Sensors.pde
View File

@ -103,34 +103,32 @@ void read_baro(void)
#ifdef IsSONAR
/* 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;
}
// 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{
if( sonar_status == SONAR_STATUS_BAD )
{
sonar_status = SONAR_STATUS_OK;
if( sonar_valid_count < 0 )
sonar_valid_count = 1;
else
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;
}
}
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);

2
ArducopterNG/System.pde
View File

@ -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