ArduCopterNG -
#1 - changed GPS hold to ver2 which builds up PID's I term on the long + lat instead of roll+pitch. #2 - added ability to use Sonar and Barometer at the same time for altitude control (it uses Sonar if available, but if not uses Barometer). git-svn-id: https://arducopter.googlecode.com/svn/trunk@1405 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com
parent
95674b8518
commit
d119da451e
@ -179,6 +179,13 @@ int SENSOR_SIGN[]={
|
||||
/*For debugging purposes*/
|
||||
#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
|
||||
|
||||
// Altitude control methods
|
||||
#define ALTITUDE_CONTROL_NONE 0
|
||||
#define ALTITUDE_CONTROL_BARO 1
|
||||
#define ALTITUDE_CONTROL_SONAR 2
|
||||
#define SONAR_STATUS_BAD 0
|
||||
#define SONAR_STATUS_OK 1
|
||||
|
||||
int AN[6]; //array that store the 6 ADC channels
|
||||
int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
|
||||
int gyro_temp;
|
||||
@ -300,22 +307,28 @@ float command_RF_pitch = 0;
|
||||
float command_RF_throttle = 0;
|
||||
byte RF_new_data = 0;
|
||||
|
||||
//Altitude control
|
||||
int Initial_Throttle;
|
||||
int target_sonar_altitude;
|
||||
//Altitude control variables
|
||||
int altitude_control_method = ALTITUDE_CONTROL_NONE; // switch to indicate whether we are using Sonar or Barometer
|
||||
int initial_throttle; // the base throttle value used for the control PIDs. captured when user switched into autopilot
|
||||
int err_altitude;
|
||||
int err_altitude_old;
|
||||
float command_altitude;
|
||||
float altitude_I;
|
||||
float altitude_D;
|
||||
int ch_throttle_altitude_hold;
|
||||
int sonar_altitude_valid = 0;
|
||||
int ch_throttle_altitude_hold; // throttle value passed to motor_output function
|
||||
|
||||
//Pressure Sensor variables
|
||||
long target_baro_altitude; // Altitude in cm
|
||||
long press_alt = 0;
|
||||
byte Baro_new_data = 0;
|
||||
//Barometer Sensor variables
|
||||
long target_baro_altitude; // target altitude in cm
|
||||
long press_baro_altitude = 0;
|
||||
byte baro_new_data = 0;
|
||||
float baro_altitude_I;
|
||||
float baro_altitude_D;
|
||||
|
||||
// Sonar Sensor variables
|
||||
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
|
||||
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;
|
||||
float sonar_altitude_D;
|
||||
|
||||
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
|
||||
|
||||
@ -329,13 +342,6 @@ byte Baro_new_data = 0;
|
||||
|
||||
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage, initialized above threshold for filter
|
||||
|
||||
|
||||
// Sonar variables
|
||||
int Sonar_value=0;
|
||||
int Sonar_Counter=0;
|
||||
|
||||
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
|
||||
|
||||
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
|
||||
byte AP_mode = 2;
|
||||
//byte cam_mode = 0; // moved to general settings, 31-10-10, jp
|
||||
@ -590,12 +596,12 @@ void defaultUserConfig() {
|
||||
KI_QUAD_YAW = 0.15;
|
||||
STABLE_MODE_KP_RATE_YAW = 2.4;
|
||||
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
|
||||
KP_GPS_ROLL = 0.013;
|
||||
KI_GPS_ROLL = 0.005;
|
||||
KD_GPS_ROLL = 0.012;
|
||||
KP_GPS_PITCH = 0.013;
|
||||
KI_GPS_PITCH = 0.005;
|
||||
KD_GPS_PITCH = 0.01;
|
||||
KP_GPS_ROLL = 0.012; //0.013;
|
||||
KI_GPS_ROLL = 0.001; //0.005;
|
||||
KD_GPS_ROLL = 0.015; //0.012;
|
||||
KP_GPS_PITCH = 0.010; //0.013;
|
||||
KI_GPS_PITCH = 0.001; //0.005;
|
||||
KD_GPS_PITCH = 0.015; //0.01;
|
||||
GPS_MAX_ANGLE = 22;
|
||||
KP_ALTITUDE = 0.08;
|
||||
KI_ALTITUDE = 0.05;
|
||||
|
||||
@ -60,9 +60,10 @@
|
||||
//#define IsCAM // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h
|
||||
|
||||
//#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10
|
||||
#define UseBMP // Use pressure sensor
|
||||
#define UseBMP // Use pressure sensor
|
||||
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
|
||||
//#define IsRANGEFINDER // are we using a Sonar for altitude hold? use this or "UseBMP" not both!
|
||||
//#define IsSONAR // are we using a Sonar for altitude hold? use this or "UseBMP" not both!
|
||||
//#define IsRANGEFINDER // are we using range finders for obstacle avoidance?
|
||||
|
||||
#define CONFIGURATOR
|
||||
|
||||
@ -225,7 +226,16 @@
|
||||
AP_ADC_ADS7844 adc;
|
||||
APM_BMP085_Class APM_BMP085;
|
||||
AP_Compass_HMC5843 AP_Compass;
|
||||
AP_RangeFinder_MaxsonarXL AP_RangeFinder_down; // Other possible sonar is AP_RangeFinder_MaxsonarLV
|
||||
#ifdef IsSONAR
|
||||
AP_RangeFinder_MaxsonarXL AP_RangeFinder_down; // Default sonar for altitude hold
|
||||
//AP_RangeFinder_MaxsonarLV AP_RangeFinder_down; // Alternative sonar is AP_RangeFinder_MaxsonarLV
|
||||
#endif
|
||||
#ifdef IsRANGEFINDER
|
||||
AP_RangeFinder_MaxsonarLV AP_RangeFinder_frontRight;
|
||||
AP_RangeFinder_MaxsonarLV AP_RangeFinder_backRight;
|
||||
AP_RangeFinder_MaxsonarLV AP_RangeFinder_backLeft;
|
||||
AP_RangeFinder_MaxsonarLV AP_RangeFinder_frontLeft;
|
||||
#endif
|
||||
|
||||
/* ************************************************************ */
|
||||
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
|
||||
@ -289,6 +299,8 @@ void setup() {
|
||||
// * slow rate loop (10Hz)
|
||||
// magnetometer
|
||||
// barometer (20Hz)
|
||||
// sonar (20Hz)
|
||||
// obstacle avoidance (20Hz)
|
||||
// external command/telemetry
|
||||
// Battery monitor
|
||||
|
||||
@ -339,7 +351,7 @@ void loop()
|
||||
}else{ // Automatic mode : GPS position hold mode
|
||||
#if AIRFRAME == QUAD
|
||||
Attitude_control_v3(command_rx_roll+command_gps_roll+command_RF_roll,command_rx_pitch+command_gps_pitch+command_RF_pitch,command_rx_yaw);
|
||||
#endif
|
||||
#endif
|
||||
#if AIRFRAME == HELI
|
||||
heli_attitude_control(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_collective,command_rx_yaw);
|
||||
#endif
|
||||
@ -366,22 +378,22 @@ void loop()
|
||||
if (AP_mode == AP_AUTOMATIC_MODE)
|
||||
{
|
||||
digitalWrite(LED_Yellow,HIGH); // Yellow LED ON : GPS Position Hold MODE
|
||||
|
||||
// Do GPS Position hold (lattitude & longitude)
|
||||
if (target_position)
|
||||
{
|
||||
#ifdef IsGPS
|
||||
if (GPS.NewData) // New GPS info?
|
||||
{
|
||||
if (GPS.Fix)
|
||||
{
|
||||
{
|
||||
read_GPS_data(); // In Navigation.pde
|
||||
Position_control(target_lattitude,target_longitude); // Call GPS position hold routine
|
||||
//Position_control_v2(target_lattitude,target_longitude); // V2 of GPS Position holdCall GPS position hold routine
|
||||
}
|
||||
else
|
||||
{
|
||||
//Position_control(target_lattitude,target_longitude); // Call GPS position hold routine
|
||||
Position_control_v2(target_lattitude,target_longitude); // V2 of GPS Position holdCall GPS position hold routine
|
||||
}else{
|
||||
command_gps_roll=0;
|
||||
command_gps_pitch=0;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
} else { // First time we enter in GPS position hold we capture the target position as the actual position
|
||||
@ -397,48 +409,105 @@ void loop()
|
||||
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
|
||||
}
|
||||
|
||||
// Switch on altitude control if we have a barometer or Sonar
|
||||
#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
|
||||
#endif
|
||||
|
||||
// use sonar if it's available
|
||||
#ifdef IsSONAR
|
||||
if( sonar_status == SONAR_STATUS_OK )
|
||||
{
|
||||
altitude_control_method = ALTITUDE_CONTROL_SONAR;
|
||||
target_sonar_altitude = press_sonar_altitude;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
// Sonar Altitude Control
|
||||
#ifdef IsSONAR
|
||||
if( sonar_new_data ) // if new sonar data has arrived
|
||||
{
|
||||
// Allow switching between sonar and barometer
|
||||
#if defined(UseBMP)
|
||||
|
||||
// if SONAR become invalid switch to barometer
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_SONAR && sonar_status == SONAR_STATUS_BAD )
|
||||
{
|
||||
// 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);
|
||||
}
|
||||
|
||||
// if SONAR becomes valid switch to sonar control
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_BARO && sonar_status == SONAR_STATUS_OK )
|
||||
{
|
||||
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
|
||||
}
|
||||
// ensure target altitude is reasonable
|
||||
target_sonar_altitude = constrain(target_sonar_altitude,AP_RangeFinder_down.min_distance*3,sonar_threshold);
|
||||
}
|
||||
#endif // defined(UseBMP)
|
||||
|
||||
// main Sonar control
|
||||
if( altitude_control_method == ALTITUDE_CONTROL_SONAR )
|
||||
{
|
||||
if( sonar_status == SONAR_STATUS_OK ) {
|
||||
ch_throttle_altitude_hold = Altitude_control_Sonar(press_sonar_altitude,target_sonar_altitude); // calculate throttle to maintain altitude
|
||||
} else {
|
||||
// if sonar_altitude becomes invalid we return control to user temporarily
|
||||
ch_throttle_altitude_hold = ch_throttle;
|
||||
}
|
||||
|
||||
// modify the target altitude if user moves stick more than 100 up or down
|
||||
if( abs(ch_throttle-initial_throttle)>100 )
|
||||
{
|
||||
target_sonar_altitude += (ch_throttle-initial_throttle)/25;
|
||||
if( target_sonar_altitude < AP_RangeFinder_down.min_distance*3 )
|
||||
target_sonar_altitude = AP_RangeFinder_down.min_distance*3;
|
||||
#if !defined(UseBMP) // limit the upper altitude if no barometer used
|
||||
if( target_sonar_altitude > sonar_threshold)
|
||||
target_sonar_altitude = sonar_threshold;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
sonar_new_data = 0; // record that we've consumed the sonar data
|
||||
} // new sonar data
|
||||
#endif // #ifdef IsSONAR
|
||||
|
||||
// Barometer Altitude control
|
||||
#ifdef UseBMP
|
||||
if( Baro_new_data ) // New altitude data?
|
||||
if( baro_new_data && altitude_control_method == ALTITUDE_CONTROL_BARO ) // New altitude data?
|
||||
{
|
||||
// if it's the first time we're entering baro hold, grab some initial values
|
||||
if( target_baro_altitude == 0 ) {
|
||||
target_baro_altitude = press_alt;
|
||||
Initial_Throttle = ch_throttle;
|
||||
ch_throttle_altitude_hold = ch_throttle;
|
||||
altitude_I = 0;
|
||||
}
|
||||
ch_throttle_altitude_hold = Altitude_control_baro(press_alt,target_baro_altitude); // calculate throttle to maintain altitude
|
||||
Baro_new_data=0; // record that we have consumed the new data
|
||||
ch_throttle_altitude_hold = Altitude_control_baro(press_baro_altitude,target_baro_altitude); // calculate throttle to maintain altitude
|
||||
baro_new_data=0; // record that we have consumed the new data
|
||||
|
||||
// modify the target altitude if user moves stick more than 100 up or down
|
||||
if (abs(ch_throttle-Initial_Throttle)>100)
|
||||
target_baro_altitude += (ch_throttle-Initial_Throttle)/25; // Change in stick position => altitude ascend/descend rate control
|
||||
if (abs(ch_throttle-initial_throttle)>100)
|
||||
{
|
||||
target_baro_altitude += (ch_throttle-initial_throttle)/25; // Change in stick position => altitude ascend/descend rate control
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif // defined(UseBMP) || defined(IsSONAR)
|
||||
|
||||
// Sonar Altitude control + object avoidance
|
||||
#ifdef IsRANGEFINDER // Do we have Range Finders connected?
|
||||
if( RF_new_data )
|
||||
{
|
||||
if( sonar_altitude_valid ) {
|
||||
// if it's the first time we're entering sonar altitude hold, grab some initial values
|
||||
if( target_sonar_altitude == 0 ) {
|
||||
target_sonar_altitude = press_alt;
|
||||
Initial_Throttle = ch_throttle;
|
||||
ch_throttle_altitude_hold = ch_throttle;
|
||||
}
|
||||
ch_throttle_altitude_hold = Altitude_control_Sonar(press_alt,target_sonar_altitude); // calculate throttle to maintain altitude
|
||||
|
||||
// modify the target altitude if user moves stick more than 100 up or down
|
||||
if (abs(ch_throttle-Initial_Throttle)>100) { // Change in stick position => altitude ascend/descend rate control
|
||||
target_sonar_altitude += (ch_throttle-Initial_Throttle)/25;
|
||||
target_sonar_altitude = constrain(target_sonar_altitude,AP_RangeFinder_down.min_distance*2,AP_RangeFinder_down.max_distance*0.8);
|
||||
}
|
||||
}else{
|
||||
// if sonar_altitude becomes invalid we return control to user
|
||||
ch_throttle_altitude_hold = ch_throttle;
|
||||
}
|
||||
// Object avoidance
|
||||
#ifdef IsRANGEFINDER
|
||||
if( RF_new_data )
|
||||
{
|
||||
Obstacle_avoidance(RF_SAFETY_ZONE); // main obstacle avoidance function
|
||||
RF_new_data = 0; // record that we have consumed the rangefinder data
|
||||
}
|
||||
@ -446,10 +515,12 @@ void loop()
|
||||
}else{
|
||||
digitalWrite(LED_Yellow,LOW);
|
||||
target_position=0;
|
||||
target_baro_altitude=0;
|
||||
target_sonar_altitude=0;
|
||||
}
|
||||
}
|
||||
if( altitude_control_method != ALTITUDE_CONTROL_NONE )
|
||||
{
|
||||
altitude_control_method = ALTITUDE_CONTROL_NONE; // turn off altitude control
|
||||
}
|
||||
} // if (AP_mode == AP_AUTOMATIC_MODE)
|
||||
}
|
||||
|
||||
// Medium loop (about 60Hz)
|
||||
if ((currentTime-mediumLoop)>=17){
|
||||
@ -457,11 +528,17 @@ void loop()
|
||||
#ifdef IsGPS
|
||||
GPS.Read(); // Read GPS data
|
||||
#endif
|
||||
|
||||
|
||||
#if AIRFRAME == HELI
|
||||
// Send output commands to heli swashplate...
|
||||
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)
|
||||
@ -479,9 +556,12 @@ void loop()
|
||||
#ifdef UseBMP
|
||||
if (APM_BMP085.Read()){
|
||||
read_baro();
|
||||
Baro_new_data = 1;
|
||||
baro_new_data = 1;
|
||||
}
|
||||
#endif
|
||||
#ifdef IsSONAR
|
||||
sonar_new_data = 1; // process sonar values at 20Hz
|
||||
#endif
|
||||
#ifdef IsRANGEFINDER
|
||||
read_RF_Sensors();
|
||||
RF_new_data = 1;
|
||||
@ -494,7 +574,7 @@ void loop()
|
||||
#endif
|
||||
break;
|
||||
case 3: // Read serial telemetry (10Hz)
|
||||
medium_loopCounter++;
|
||||
medium_loopCounter++;
|
||||
#ifdef CONFIGURATOR
|
||||
readSerialCommand();
|
||||
#endif
|
||||
@ -504,16 +584,19 @@ void loop()
|
||||
#ifdef UseBMP
|
||||
if (APM_BMP085.Read()){
|
||||
read_baro();
|
||||
Baro_new_data = 1;
|
||||
baro_new_data = 1;
|
||||
}
|
||||
#endif
|
||||
#ifdef IsSONAR
|
||||
sonar_new_data = 1; // process sonar values at 20Hz
|
||||
#endif
|
||||
#ifdef IsRANGEFINDER
|
||||
read_RF_Sensors();
|
||||
RF_new_data = 1;
|
||||
#endif
|
||||
break;
|
||||
case 5: // Battery monitor (10Hz)
|
||||
medium_loopCounter=0;
|
||||
medium_loopCounter=0;
|
||||
#if BATTERY_EVENT == 1
|
||||
read_battery(); // Battery monitor
|
||||
#endif
|
||||
|
||||
@ -170,11 +170,11 @@ int Altitude_control_baro(int altitude, int target_altitude)
|
||||
|
||||
err_altitude_old = err_altitude;
|
||||
err_altitude = target_altitude - altitude;
|
||||
altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz
|
||||
altitude_I += (float)err_altitude*0.05;
|
||||
altitude_I = constrain(altitude_I,-140,140);
|
||||
command_altitude = KP_ALTITUDE*err_altitude + KD_ALTITUDE*altitude_D + KI_ALTITUDE*altitude_I;
|
||||
command_altitude = Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX);
|
||||
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);
|
||||
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);
|
||||
return command_altitude;
|
||||
}
|
||||
|
||||
@ -189,20 +189,19 @@ int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_altitude)
|
||||
|
||||
err_altitude_old = err_altitude;
|
||||
err_altitude = target_sonar_altitude - Sonar_altitude;
|
||||
altitude_D = (float)(err_altitude-err_altitude_old)/GdT_SONAR_ALTITUDE;
|
||||
altitude_I += (float)err_altitude*GdT_SONAR_ALTITUDE;
|
||||
altitude_I = constrain(altitude_I,-1000,1000);
|
||||
command_altitude = KP_SONAR_ALTITUDE*err_altitude + KD_SONAR_ALTITUDE*altitude_D + KI_SONAR_ALTITUDE*altitude_I;
|
||||
Log_Write_PID(4,KP_SONAR_ALTITUDE*err_altitude,KI_SONAR_ALTITUDE*altitude_I,KD_SONAR_ALTITUDE*altitude_D,command_altitude);
|
||||
return (Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX));
|
||||
sonar_altitude_D = (float)(err_altitude-err_altitude_old)/GdT_SONAR_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;
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
/* Obstacle avoidance routine */
|
||||
/* NOT YET FULLY IMPLEMENTED */
|
||||
#ifdef IsRANGEFINDER
|
||||
void Obstacle_avoidance(int safeDistance)
|
||||
{
|
||||
#ifdef IsRANGEFINDER
|
||||
int RF_err_roll = 0;
|
||||
int RF_err_pitch = 0;
|
||||
int RF_err_throttle = 0;
|
||||
@ -218,28 +217,28 @@ void Obstacle_avoidance(int safeDistance)
|
||||
int err_temp;
|
||||
|
||||
// front right
|
||||
// err_temp = max(safeDistance - AP_RangeFinder_frontRight.distance,0);
|
||||
// RF_err_roll += err_temp * AP_RangeFinder_frontRight.orientation_x;
|
||||
// RF_err_pitch += err_temp * AP_RangeFinder_frontRight.orientation_y;
|
||||
// RF_err_throttle += err_temp * AP_RangeFinder_frontRight.orientation_z;
|
||||
err_temp = max(safeDistance - AP_RangeFinder_frontRight.distance,0);
|
||||
RF_err_roll += err_temp * AP_RangeFinder_frontRight.orientation_x;
|
||||
RF_err_pitch += err_temp * AP_RangeFinder_frontRight.orientation_y;
|
||||
RF_err_throttle += err_temp * AP_RangeFinder_frontRight.orientation_z;
|
||||
|
||||
// back right
|
||||
// err_temp = max(safeDistance - AP_RangeFinder_backRight.distance,0);
|
||||
// RF_err_roll += err_temp * AP_RangeFinder_backRight.orientation_x;
|
||||
// RF_err_pitch += err_temp * AP_RangeFinder_backRight.orientation_y;
|
||||
// RF_err_throttle += err_temp * AP_RangeFinder_backRight.orientation_z;
|
||||
err_temp = max(safeDistance - AP_RangeFinder_backRight.distance,0);
|
||||
RF_err_roll += err_temp * AP_RangeFinder_backRight.orientation_x;
|
||||
RF_err_pitch += err_temp * AP_RangeFinder_backRight.orientation_y;
|
||||
RF_err_throttle += err_temp * AP_RangeFinder_backRight.orientation_z;
|
||||
|
||||
// back left
|
||||
// err_temp = max(safeDistance - AP_RangeFinder_backLeft.distance,0);
|
||||
// RF_err_roll += err_temp * AP_RangeFinder_backLeft.orientation_x;
|
||||
// RF_err_pitch += err_temp * AP_RangeFinder_backLeft.orientation_y;
|
||||
// RF_err_throttle += err_temp * AP_RangeFinder_backLeft.orientation_z;
|
||||
err_temp = max(safeDistance - AP_RangeFinder_backLeft.distance,0);
|
||||
RF_err_roll += err_temp * AP_RangeFinder_backLeft.orientation_x;
|
||||
RF_err_pitch += err_temp * AP_RangeFinder_backLeft.orientation_y;
|
||||
RF_err_throttle += err_temp * AP_RangeFinder_backLeft.orientation_z;
|
||||
|
||||
// front left
|
||||
// err_temp = max(safeDistance - AP_RangeFinder_frontLeft.distance,0);
|
||||
// RF_err_roll += err_temp * AP_RangeFinder_frontLeft.orientation_x;
|
||||
// RF_err_pitch += err_temp * AP_RangeFinder_frontLeft.orientation_y;
|
||||
// RF_err_throttle += err_temp * AP_RangeFinder_frontLeft.orientation_z;
|
||||
err_temp = max(safeDistance - AP_RangeFinder_frontLeft.distance,0);
|
||||
RF_err_roll += err_temp * AP_RangeFinder_frontLeft.orientation_x;
|
||||
RF_err_pitch += err_temp * AP_RangeFinder_frontLeft.orientation_y;
|
||||
RF_err_throttle += err_temp * AP_RangeFinder_frontLeft.orientation_z;
|
||||
|
||||
// ROLL - P term
|
||||
RF_roll_P = RF_err_roll * KP_RF_ROLL;
|
||||
@ -271,11 +270,6 @@ void Obstacle_avoidance(int safeDistance)
|
||||
|
||||
// THROTTLE - not yet implemented
|
||||
command_RF_throttle = 0;
|
||||
|
||||
#else
|
||||
command_RF_roll = 0;
|
||||
command_RF_pitch = 0;
|
||||
command_RF_throttle = 0;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -93,35 +93,63 @@ void read_baro(void)
|
||||
//tempPresAlt = pow(tempPresAlt, 0.190284);
|
||||
//press_alt = (1.0 - tempPresAlt) * 145366.45;
|
||||
tempPresAlt = pow(tempPresAlt, 0.190295);
|
||||
if (press_alt == 0)
|
||||
press_alt = (1.0 - tempPresAlt) * 4433000; // Altitude in cm
|
||||
if (press_baro_altitude == 0)
|
||||
press_baro_altitude = (1.0 - tempPresAlt) * 4433000; // Altitude in cm
|
||||
else
|
||||
press_alt = press_alt * 0.75 + ((1.0 - tempPresAlt) * 4433000)*0.25; // Altitude in cm (filtered)
|
||||
press_baro_altitude = press_baro_altitude * 0.75 + ((1.0 - tempPresAlt) * 4433000)*0.25; // Altitude in cm (filtered)
|
||||
}
|
||||
#endif
|
||||
|
||||
#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;
|
||||
}
|
||||
}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;
|
||||
}
|
||||
}
|
||||
|
||||
#if LOG_RANGEFINDER && !defined(IsRANGEFINDER)
|
||||
Log_Write_RangeFinder(AP_RangeFinder_down.distance,0,0,0,0,0);
|
||||
#endif
|
||||
}
|
||||
#endif // IsSONAR
|
||||
|
||||
#ifdef IsRANGEFINDER
|
||||
/* This function reads in the values from the attached range finders (currently only downward pointing sonar) */
|
||||
void read_RF_Sensors()
|
||||
{
|
||||
// AP_RangeFinder_frontRight.read();
|
||||
// AP_RangeFinder_backRight.read();
|
||||
// AP_RangeFinder_backLeft.read();
|
||||
// AP_RangeFinder_frontLeft.read();
|
||||
AP_RangeFinder_frontRight.read();
|
||||
AP_RangeFinder_backRight.read();
|
||||
AP_RangeFinder_backLeft.read();
|
||||
AP_RangeFinder_frontLeft.read();
|
||||
|
||||
// calculate altitude from down sensor
|
||||
AP_RangeFinder_down.read();
|
||||
if( AP_RangeFinder_down.distance < AP_RangeFinder_down.min_distance || AP_RangeFinder_down.distance >= AP_RangeFinder_down.max_distance * 0.8 ) {
|
||||
sonar_altitude_valid = 0;
|
||||
press_alt = 0;
|
||||
}else{
|
||||
sonar_altitude_valid = 1;
|
||||
press_alt = DCM_Matrix[2][2] * AP_RangeFinder_down.distance;
|
||||
}
|
||||
|
||||
#if LOG_RANGEFINDER
|
||||
//Log_Write_RangeFinder(AP_RangeFinder_frontRight.distance, AP_RangeFinder_backRight.distance, AP_RangeFinder_backLeft.distance,AP_RangeFinder_frontLeft.distance,AP_RangeFinder_down.distance,0);
|
||||
#if LOG_RANGEFINDER
|
||||
Log_Write_RangeFinder(AP_RangeFinder_down.distance, AP_RangeFinder_frontRight.distance, AP_RangeFinder_backRight.distance, AP_RangeFinder_backLeft.distance,AP_RangeFinder_frontLeft.distance,0);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
#endif // IsRANGEFINDER
|
||||
|
||||
|
||||
@ -182,12 +182,18 @@ void APM_Init() {
|
||||
APM_BMP085.Init(FALSE);
|
||||
#endif
|
||||
|
||||
#ifdef IsRANGEFINDER
|
||||
AP_RangeFinder_down.init(AN5); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN);
|
||||
//AP_RangeFinder_frontRight.init(AN5); AP_RangeFinder_frontRight.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT);
|
||||
//AP_RangeFinder_backRight.init(AN4); AP_RangeFinder_backRight.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_RIGHT);
|
||||
//AP_RangeFinder_backLeft.init(AN3); AP_RangeFinder_backLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_LEFT);
|
||||
//AP_RangeFinder_frontLeft.init(AN2); AP_RangeFinder_frontLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_LEFT);
|
||||
// Sonar for Altitude hold
|
||||
#ifdef IsSONAR
|
||||
AP_RangeFinder_down.init(AP_RANGEFINDER_PITOT_TUBE, &adc); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN);
|
||||
sonar_threshold = AP_RangeFinder_down.max_distance * 0.8;
|
||||
#endif
|
||||
|
||||
// RangeFinders for obstacle avoidance
|
||||
#ifdef IsRANGEFINDER
|
||||
AP_RangeFinder_frontRight.init(AN5); AP_RangeFinder_frontRight.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT);
|
||||
AP_RangeFinder_backRight.init(AN4); AP_RangeFinder_backRight.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_RIGHT);
|
||||
AP_RangeFinder_backLeft.init(AN3); AP_RangeFinder_backLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_LEFT);
|
||||
AP_RangeFinder_frontLeft.init(AN2); AP_RangeFinder_frontLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_LEFT);
|
||||
#endif
|
||||
|
||||
delay(1000);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user