mirror of https://github.com/ArduPilot/ardupilot
ArduCopterNG - added altitude hold using Sonar/Rangefinder
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1265 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com
parent
5d6c90b723
commit
e93daf2546
|
|
@ -294,6 +294,7 @@ float gps_lon_I=0;
|
|||
// object avoidance
|
||||
float RF_roll_I=0;
|
||||
float RF_pitch_I=0;
|
||||
float RF_throttle_I=0;
|
||||
float command_RF_roll = 0;
|
||||
float command_RF_pitch = 0;
|
||||
float command_RF_throttle = 0;
|
||||
|
|
@ -308,6 +309,7 @@ float command_altitude;
|
|||
float altitude_I;
|
||||
float altitude_D;
|
||||
int ch_throttle_altitude_hold;
|
||||
int sonar_altitude_valid = 0;
|
||||
|
||||
//Pressure Sensor variables
|
||||
long target_baro_altitude; // Altitude in cm
|
||||
|
|
@ -419,6 +421,7 @@ unsigned long elapsedTime = 0; // for doing custom events
|
|||
#define LOG_MODE 1 // Logs mode changes
|
||||
#define LOG_RAW 0 // Logs raw accel/gyro data
|
||||
#define LOG_SEN 1 // Logs sensor data
|
||||
#define LOG_RANGEFINDER 0 // Logs data from range finders
|
||||
|
||||
// GCS Message ID's
|
||||
#define MSG_ACKNOWLEDGE 0x00
|
||||
|
|
@ -569,6 +572,11 @@ float KI_RF_PITCH;
|
|||
float RF_MAX_ANGLE; // Maximun command roll and pitch angle from obstacle avoiding control
|
||||
float RF_SAFETY_ZONE; // object avoidance will move away from objects within this distance (in cm)
|
||||
|
||||
// sonar for altitude hold
|
||||
float KP_SONAR_ALTITUDE;
|
||||
float KI_SONAR_ALTITUDE;
|
||||
float KD_SONAR_ALTITUDE;
|
||||
|
||||
// This function call contains the default values that are set to the ArduCopter
|
||||
// when a "Default EEPROM Value" command is sent through serial interface
|
||||
void defaultUserConfig() {
|
||||
|
|
@ -644,6 +652,9 @@ void defaultUserConfig() {
|
|||
KD_RF_PITCH = 0.03;
|
||||
RF_MAX_ANGLE = 10.0;
|
||||
RF_SAFETY_ZONE = 120.0; // object avoidance will avoid objects within this range (in cm)
|
||||
KP_SONAR_ALTITUDE = 0.8;
|
||||
KI_SONAR_ALTITUDE = 0.3;
|
||||
KD_SONAR_ALTITUDE = 0.7;
|
||||
}
|
||||
|
||||
// EEPROM storage addresses
|
||||
|
|
@ -712,13 +723,16 @@ void defaultUserConfig() {
|
|||
#define mag_offset_z_ADR 248
|
||||
#define MIN_THROTTLE_ADR 252
|
||||
#define KP_RF_ROLL_ADR 256
|
||||
#define KD_RF_ROLL_ADR 260
|
||||
#define KI_RF_ROLL_ADR 264
|
||||
#define KI_RF_ROLL_ADR 260
|
||||
#define KD_RF_ROLL_ADR 264
|
||||
#define KP_RF_PITCH_ADR 268
|
||||
#define KD_RF_PITCH_ADR 272
|
||||
#define KI_RF_PITCH_ADR 276
|
||||
#define KI_RF_PITCH_ADR 272
|
||||
#define KD_RF_PITCH_ADR 276
|
||||
#define RF_MAX_ANGLE_ADR 280
|
||||
#define RF_SAFETY_ZONE_ADR 284
|
||||
#define KP_SONAR_ALTITUDE_ADR 288
|
||||
#define KI_SONAR_ALTITUDE_ADR 292
|
||||
#define KD_SONAR_ALTITUDE_ADR 296
|
||||
|
||||
//#define eeprom_counter_ADR 238 // hmm should i move these?!? , 31-10-10, jp
|
||||
//#define eeprom_checker_ADR 240 // this too... , 31-10-10, jp
|
||||
|
|
|
|||
|
|
@ -58,11 +58,11 @@
|
|||
#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
|
||||
//#define IsAM // Do we have motormount LED's. AM = Atraction Mode
|
||||
//#define IsCAM // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h
|
||||
//#define IsRANGEFINDER // Do we have Range Finders connected
|
||||
|
||||
//#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10
|
||||
#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 CONFIGURATOR
|
||||
|
||||
|
|
@ -225,12 +225,7 @@
|
|||
AP_ADC_ADS7844 adc;
|
||||
APM_BMP085_Class APM_BMP085;
|
||||
AP_Compass_HMC5843 AP_Compass;
|
||||
#ifdef IsRANGEFINDER
|
||||
AP_RangeFinder_SharpGP2Y AP_RangeFinder_frontRight;
|
||||
AP_RangeFinder_SharpGP2Y AP_RangeFinder_backRight;
|
||||
AP_RangeFinder_SharpGP2Y AP_RangeFinder_backLeft;
|
||||
AP_RangeFinder_SharpGP2Y AP_RangeFinder_frontLeft;
|
||||
#endif
|
||||
AP_RangeFinder_MaxsonarXL AP_RangeFinder_down; // Other possible sonar is AP_RangeFinder_MaxsonarLV
|
||||
|
||||
/* ************************************************************ */
|
||||
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
|
||||
|
|
@ -303,9 +298,6 @@ void setup() {
|
|||
// Main loop
|
||||
void loop()
|
||||
{
|
||||
//int aux;
|
||||
//int i;
|
||||
//float aux_float;
|
||||
|
||||
currentTimeMicros = micros();
|
||||
currentTime = currentTimeMicros / 1000;
|
||||
|
|
@ -313,7 +305,6 @@ void loop()
|
|||
// Main loop at 200Hz (IMU + control)
|
||||
if ((currentTime-mainLoop) > 5) // about 200Hz (every 5ms)
|
||||
{
|
||||
//G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!!
|
||||
G_Dt = (currentTimeMicros-previousTimeMicros) * 0.000001; // Microseconds!!!
|
||||
mainLoop = currentTime;
|
||||
previousTimeMicros = currentTimeMicros;
|
||||
|
|
@ -393,23 +384,7 @@ void loop()
|
|||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef UseBMP
|
||||
if (Baro_new_data) // New altitude data?
|
||||
{
|
||||
ch_throttle_altitude_hold = Altitude_control_baro(press_alt,target_baro_altitude); // Altitude control
|
||||
Baro_new_data=0;
|
||||
if (abs(ch_throttle-Initial_Throttle)>100) // Change in stick position => altitude ascend/descend rate control
|
||||
target_baro_altitude += (ch_throttle-Initial_Throttle)/25;
|
||||
//Serial.print(Initial_Throttle);
|
||||
//Serial.print(" ");
|
||||
//Serial.print(ch_throttle);
|
||||
//Serial.print(" ");
|
||||
//Serial.println(target_baro_altitude);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else // First time we enter in GPS position hold we capture the target position as the actual position
|
||||
{
|
||||
} else { // First time we enter in GPS position hold we capture the target position as the actual position
|
||||
#ifdef IsGPS
|
||||
if (GPS.Fix){ // We need a GPS Fix to capture the actual position...
|
||||
target_lattitude = GPS.Lattitude;
|
||||
|
|
@ -419,22 +394,60 @@ void loop()
|
|||
#endif
|
||||
command_gps_roll=0;
|
||||
command_gps_pitch=0;
|
||||
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
|
||||
}
|
||||
|
||||
// Barometer Altitude control
|
||||
#ifdef UseBMP
|
||||
if( Baro_new_data ) // 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;
|
||||
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
|
||||
altitude_I = 0;
|
||||
}
|
||||
// obstacle avoidance - comes on with autopilot
|
||||
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
|
||||
|
||||
// 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
|
||||
}
|
||||
#endif
|
||||
|
||||
// Sonar Altitude control + object avoidance
|
||||
#ifdef IsRANGEFINDER // Do we have Range Finders connected?
|
||||
if( RF_new_data )
|
||||
{
|
||||
Obstacle_avoidance(RF_SAFETY_ZONE);
|
||||
RF_new_data = 0;
|
||||
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;
|
||||
}
|
||||
Obstacle_avoidance(RF_SAFETY_ZONE); // main obstacle avoidance function
|
||||
RF_new_data = 0; // record that we have consumed the rangefinder data
|
||||
}
|
||||
#endif
|
||||
}else{
|
||||
digitalWrite(LED_Yellow,LOW);
|
||||
target_position=0;
|
||||
target_baro_altitude=0;
|
||||
target_sonar_altitude=0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -492,8 +505,6 @@ void loop()
|
|||
if (APM_BMP085.Read()){
|
||||
read_baro();
|
||||
Baro_new_data = 1;
|
||||
//Serial.print("B ");
|
||||
//Serial.println(press_alt);
|
||||
}
|
||||
#endif
|
||||
#ifdef IsRANGEFINDER
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ void RunCLI () {
|
|||
CALIB_Esc();
|
||||
break;
|
||||
case 'o':
|
||||
Set_Obstacle_Avoidance_PIDs();
|
||||
Set_SonarAndObstacleAvoidance_PIDs();
|
||||
break;
|
||||
case 's':
|
||||
Show_Settings();
|
||||
|
|
@ -126,7 +126,7 @@ void Show_MainMenu() {
|
|||
SerPrln(" e - ESC Throttle calibration (Works with official ArduCopter ESCs)");
|
||||
SerPrln(" i - Initialize and calibrate Accel offsets");
|
||||
SerPrln(" m - Motor tester with AIL/ELE stick");
|
||||
SerPrln(" o - Show/Save obstacle avoidance PIDs");
|
||||
SerPrln(" o - Show/Save sonar & obstacle avoidance PIDs");
|
||||
SerPrln(" r - Reset to factory settings");
|
||||
SerPrln(" t - Calibrate MIN Throttle value");
|
||||
SerPrln(" s - Show settings");
|
||||
|
|
@ -556,14 +556,18 @@ void Show_Settings() {
|
|||
SerPrln("+ mode");
|
||||
}
|
||||
|
||||
Show_Obstacle_Avoidance_PIDs() ;
|
||||
Show_SonarAndObstacleAvoidance_PIDs();
|
||||
|
||||
SerPrln();
|
||||
}
|
||||
|
||||
// Display obstacle avoidance pids
|
||||
void Show_Obstacle_Avoidance_PIDs() {
|
||||
SerPri("\tSafetyZone: ");
|
||||
void Show_SonarAndObstacleAvoidance_PIDs() {
|
||||
SerPri("\tSonar PID: ");
|
||||
SerPri(KP_SONAR_ALTITUDE); cspc();
|
||||
SerPri(KI_SONAR_ALTITUDE); cspc();
|
||||
SerPrln(KD_SONAR_ALTITUDE);
|
||||
SerPri("\tObstacle SafetyZone: ");
|
||||
SerPrln(RF_SAFETY_ZONE);
|
||||
SerPri("\tRoll PID: ");
|
||||
SerPri(KP_RF_ROLL); cspc();
|
||||
|
|
@ -580,24 +584,49 @@ void Show_Obstacle_Avoidance_PIDs() {
|
|||
}
|
||||
|
||||
// save RF pids to eeprom
|
||||
void Save_Obstacle_Avoidance_PIDs_toEEPROM() {
|
||||
void Save_SonarAndObstacleAvoidancePIDs_toEEPROM() {
|
||||
writeEEPROM(KP_RF_ROLL,KP_RF_ROLL_ADR);
|
||||
writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR);
|
||||
writeEEPROM(KI_RF_ROLL,KI_RF_ROLL_ADR);
|
||||
writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR);
|
||||
writeEEPROM(KP_RF_PITCH,KP_RF_PITCH_ADR);
|
||||
writeEEPROM(KD_RF_PITCH,KD_RF_PITCH_ADR);
|
||||
writeEEPROM(KI_RF_PITCH,KI_RF_PITCH_ADR);
|
||||
writeEEPROM(KD_RF_PITCH,KD_RF_PITCH_ADR);
|
||||
writeEEPROM(RF_MAX_ANGLE,RF_MAX_ANGLE_ADR);
|
||||
writeEEPROM(RF_SAFETY_ZONE,RF_SAFETY_ZONE_ADR);
|
||||
writeEEPROM(KP_SONAR_ALTITUDE,KP_SONAR_ALTITUDE_ADR);
|
||||
writeEEPROM(KI_SONAR_ALTITUDE,KI_SONAR_ALTITUDE_ADR);
|
||||
writeEEPROM(KD_SONAR_ALTITUDE,KD_SONAR_ALTITUDE_ADR);
|
||||
}
|
||||
|
||||
//
|
||||
void Set_Obstacle_Avoidance_PIDs() {
|
||||
void Set_SonarAndObstacleAvoidance_PIDs() {
|
||||
float tempVal1, tempVal2, tempVal3;
|
||||
int saveToEeprom = 0;
|
||||
// Display current PID values
|
||||
SerPrln("Obstacle Avoidance:");
|
||||
Show_Obstacle_Avoidance_PIDs();
|
||||
SerPrln("Sonar and Obstacle Avoidance:");
|
||||
Show_SonarAndObstacleAvoidance_PIDs();
|
||||
SerPrln();
|
||||
|
||||
// SONAR PIDs
|
||||
SerFlu();
|
||||
SerPri("Enter Sonar P;I;D; values or 0 to skip: ");
|
||||
while( !SerAva() ); // wait until user presses a key
|
||||
tempVal1 = readFloatSerial();
|
||||
tempVal2 = readFloatSerial();
|
||||
tempVal3 = readFloatSerial();
|
||||
if( tempVal1 != 0 || tempVal2 != 0 || tempVal3 != 0 ) {
|
||||
KP_SONAR_ALTITUDE = tempVal1;
|
||||
KI_SONAR_ALTITUDE = tempVal2;
|
||||
KD_SONAR_ALTITUDE = tempVal3;
|
||||
SerPrln();
|
||||
SerPri("P:");
|
||||
SerPri(KP_SONAR_ALTITUDE);
|
||||
SerPri("\tI:");
|
||||
SerPri(KI_SONAR_ALTITUDE);
|
||||
SerPri("\tD:");
|
||||
SerPri(KD_SONAR_ALTITUDE);
|
||||
saveToEeprom = 1;
|
||||
}
|
||||
SerPrln();
|
||||
|
||||
// SAFETY ZONE
|
||||
|
|
@ -605,10 +634,11 @@ void Set_Obstacle_Avoidance_PIDs() {
|
|||
SerPri("Enter Safety Zone (in cm) or 0 to skip: ");
|
||||
while( !SerAva() ); // wait until user presses a key
|
||||
tempVal1 = readFloatSerial();
|
||||
if( tempVal1 >= 20 && tempVal1 <= 150 ) {
|
||||
if( tempVal1 >= 20 && tempVal1 <= 700 ) {
|
||||
RF_SAFETY_ZONE = tempVal1;
|
||||
SerPri("SafetyZone: ");
|
||||
SerPrln(RF_SAFETY_ZONE);
|
||||
SerPri(RF_SAFETY_ZONE);
|
||||
saveToEeprom = 1;
|
||||
}
|
||||
SerPrln();
|
||||
|
||||
|
|
@ -662,7 +692,7 @@ void Set_Obstacle_Avoidance_PIDs() {
|
|||
while( !SerAva() ); // wait until user presses a key
|
||||
tempVal1 = readFloatSerial();
|
||||
SerPrln(tempVal1);
|
||||
if( tempVal1 > 0 ) {
|
||||
if( tempVal1 > 0 && tempVal1 < 90 ) {
|
||||
RF_MAX_ANGLE = tempVal1;
|
||||
SerPrln();
|
||||
SerPri("MaxAngle: ");
|
||||
|
|
@ -673,9 +703,15 @@ void Set_Obstacle_Avoidance_PIDs() {
|
|||
|
||||
// save to eeprom
|
||||
if( saveToEeprom == 1 ) {
|
||||
Save_Obstacle_Avoidance_PIDs_toEEPROM();
|
||||
SerPrln("Obstacle Avoidance:");
|
||||
Show_SonarAndObstacleAvoidance_PIDs();
|
||||
SerPrln();
|
||||
Save_SonarAndObstacleAvoidancePIDs_toEEPROM();
|
||||
SerPrln("Saved to EEPROM");
|
||||
SerPrln();
|
||||
}else{
|
||||
SerPrln("No changes. Nothing saved to EEPROM");
|
||||
SerPrln();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -131,6 +131,9 @@ void readUserConfig() {
|
|||
KI_RF_PITCH = readEEPROM(KI_RF_PITCH_ADR);
|
||||
RF_MAX_ANGLE = readEEPROM(RF_MAX_ANGLE_ADR);
|
||||
RF_SAFETY_ZONE = readEEPROM(RF_SAFETY_ZONE_ADR);
|
||||
KP_SONAR_ALTITUDE = readEEPROM(KP_SONAR_ALTITUDE_ADR);
|
||||
KI_SONAR_ALTITUDE = readEEPROM(KI_SONAR_ALTITUDE_ADR);
|
||||
KD_SONAR_ALTITUDE = readEEPROM(KD_SONAR_ALTITUDE_ADR);
|
||||
}
|
||||
|
||||
void writeUserConfig() {
|
||||
|
|
@ -203,11 +206,14 @@ void writeUserConfig() {
|
|||
writeEEPROM(mag_offset_z, mag_offset_z_ADR);
|
||||
writeEEPROM(MIN_THROTTLE, MIN_THROTTLE_ADR);
|
||||
writeEEPROM(KP_RF_ROLL,KP_RF_ROLL_ADR);
|
||||
writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR);
|
||||
writeEEPROM(KI_RF_ROLL,KI_RF_ROLL_ADR);
|
||||
writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR);
|
||||
writeEEPROM(KP_RF_PITCH,KP_RF_PITCH_ADR);
|
||||
writeEEPROM(KD_RF_PITCH,KD_RF_PITCH_ADR);
|
||||
writeEEPROM(KI_RF_PITCH,KI_RF_PITCH_ADR);
|
||||
writeEEPROM(KD_RF_PITCH,KD_RF_PITCH_ADR);
|
||||
writeEEPROM(RF_MAX_ANGLE,RF_MAX_ANGLE_ADR);
|
||||
writeEEPROM(RF_SAFETY_ZONE,RF_SAFETY_ZONE_ADR);
|
||||
writeEEPROM(KP_SONAR_ALTITUDE,KP_SONAR_ALTITUDE_ADR);
|
||||
writeEEPROM(KI_SONAR_ALTITUDE,KI_SONAR_ALTITUDE_ADR);
|
||||
writeEEPROM(KD_SONAR_ALTITUDE,KD_SONAR_ALTITUDE_ADR);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -304,7 +304,7 @@ void Log_Write_Raw()
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef LOG_RANGEFINDER
|
||||
#if LOG_RANGEFINDER
|
||||
// Write a Sensor Raw data packet
|
||||
void Log_Write_RangeFinder(int rf1, int rf2, int rf3,int rf4, int rf5, int rf6)
|
||||
{
|
||||
|
|
@ -536,7 +536,7 @@ void Log_Read_Raw()
|
|||
SerPriln(" ");
|
||||
}
|
||||
|
||||
// Read a raw accel/gyro packet
|
||||
// Read a RangeFinder packet
|
||||
void Log_Read_RangeFinder()
|
||||
{
|
||||
SerPri("RF:");
|
||||
|
|
@ -551,7 +551,6 @@ void Log_Read_RangeFinder()
|
|||
SerPri(DataFlash.ReadInt());
|
||||
SerPri(",");
|
||||
SerPri(DataFlash.ReadInt());
|
||||
SerPri(",");
|
||||
SerPriln(" ");
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ void motor_output()
|
|||
byte throttle_mode=0;
|
||||
|
||||
throttle = ch_throttle;
|
||||
#ifdef UseBMP
|
||||
#if defined(UseBMP) || defined(IsRANGEFINDER)
|
||||
if (AP_mode == AP_AUTOMATIC_MODE)
|
||||
{
|
||||
throttle = ch_throttle_altitude_hold;
|
||||
|
|
|
|||
|
|
@ -115,9 +115,9 @@ void Position_control_v2(long lat_dest, long lon_dest)
|
|||
|
||||
// add distance to I term
|
||||
gps_lon_I += gps_err_lon;
|
||||
gps_lon_I = constrain(gps_lon_I,-800,800); // don't let I get too big
|
||||
gps_lon_I = constrain(gps_lon_I,-1200,1200); // don't let I get too big
|
||||
gps_lat_I += gps_err_lat;
|
||||
gps_lat_I = constrain(gps_lat_I,-800,800);
|
||||
gps_lat_I = constrain(gps_lat_I,-1200,1200);
|
||||
|
||||
// calculate the ground speed - for D term
|
||||
gps_lon_D = (gps_err_lon - gps_err_lon_old) / GPS_Dt;
|
||||
|
|
@ -132,7 +132,7 @@ void Position_control_v2(long lat_dest, long lon_dest)
|
|||
|
||||
command_gps_roll = KP_GPS_ROLL * gps_err_roll + KD_GPS_ROLL * gps_roll_D + KI_GPS_ROLL * gps_roll_I;
|
||||
command_gps_roll = constrain(command_gps_roll, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
|
||||
//Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll*10,KI_GPS_ROLL*gps_roll_I*10,KD_GPS_ROLL*gps_roll_D*10,command_gps_roll*10);
|
||||
Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll,KI_GPS_ROLL*gps_roll_I,KD_GPS_ROLL*gps_roll_D,command_gps_roll);
|
||||
|
||||
// PITCH
|
||||
gps_err_pitch = (-gps_err_lat * DCM_Matrix[0][0] - gps_err_lon * DCM_Matrix[1][0]);
|
||||
|
|
@ -141,14 +141,13 @@ void Position_control_v2(long lat_dest, long lon_dest)
|
|||
|
||||
command_gps_pitch = KP_GPS_PITCH * gps_err_pitch + KD_GPS_PITCH * gps_pitch_D + KI_GPS_PITCH * gps_pitch_I;
|
||||
command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
|
||||
//Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10);
|
||||
Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch,KI_GPS_PITCH*gps_pitch_I,KD_GPS_PITCH*gps_pitch_D,command_gps_pitch);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void Reset_I_terms_navigation()
|
||||
{
|
||||
altitude_I = 0;
|
||||
gps_roll_I = 0;
|
||||
gps_pitch_I = 0;
|
||||
gps_lon_I = 0; // for position hold ver 2
|
||||
|
|
@ -179,29 +178,28 @@ int Altitude_control_baro(int altitude, int target_altitude)
|
|||
return command_altitude;
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
/* Altitude control... (based on sonar) */
|
||||
/* CONTROL PARAMETERS FOR SONAR ALTITUDE CONTROL (TEMPORATLY HERE) */
|
||||
#define KP_SONAR_ALTITUDE 0.8
|
||||
#define KD_SONAR_ALTITUDE 0.7
|
||||
#define KI_SONAR_ALTITUDE 0.3
|
||||
int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_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 command_altitude;
|
||||
|
||||
err_altitude_old = err_altitude;
|
||||
err_altitude = target_sonar_altitude - Sonar_altitude;
|
||||
altitude_D = (float)(err_altitude-err_altitude_old)/0.05;
|
||||
altitude_I += (float)err_altitude*0.05;
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
/* Obstacle avoidance routine */
|
||||
/* NOT YET FULLY IMPLEMENTED */
|
||||
void Obstacle_avoidance(int safeDistance)
|
||||
{
|
||||
#ifdef IsRANGEFINDER
|
||||
|
|
@ -220,28 +218,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;
|
||||
|
|
|
|||
|
|
@ -101,17 +101,27 @@ void read_baro(void)
|
|||
#endif
|
||||
|
||||
#ifdef IsRANGEFINDER
|
||||
/* This function read IR data, convert to cm units and filter the data */
|
||||
/* 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();
|
||||
|
||||
#ifdef LOG_RANGEFINDER
|
||||
Log_Write_RangeFinder(AP_RangeFinder_frontRight.distance, AP_RangeFinder_backRight.distance, AP_RangeFinder_backLeft.distance,AP_RangeFinder_frontLeft.distance,0,0);
|
||||
#endif // LOG_RANGEFINDER
|
||||
// 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);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -171,12 +171,11 @@ void APM_Init() {
|
|||
#ifndef CONFIGURATOR
|
||||
for(i=0;i<6;i++)
|
||||
{
|
||||
Serial.print("AN[]:");
|
||||
Serial.println(AN_OFFSET[i]);
|
||||
SerPri("AN[]:");
|
||||
SerPrln(AN_OFFSET[i]);
|
||||
}
|
||||
Serial.print("Yaw neutral value:");
|
||||
// Serial.println(Neutro_yaw);
|
||||
Serial.print(yaw_mid);
|
||||
SerPri("Yaw neutral value:");
|
||||
SerPri(yaw_mid);
|
||||
#endif
|
||||
|
||||
#ifdef UseBMP
|
||||
|
|
@ -184,17 +183,16 @@ void APM_Init() {
|
|||
#endif
|
||||
|
||||
#ifdef IsRANGEFINDER
|
||||
AP_RangeFinder_frontRight.init(AN2); AP_RangeFinder_frontRight.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT);
|
||||
AP_RangeFinder_backRight.init(AN3); AP_RangeFinder_backRight.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_RIGHT);
|
||||
AP_RangeFinder_backLeft.init(AN4); AP_RangeFinder_backLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_LEFT);
|
||||
AP_RangeFinder_frontLeft.init(AN5); AP_RangeFinder_frontLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_LEFT);
|
||||
AP_RangeFinder_down.init(AN1); 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);
|
||||
#endif
|
||||
|
||||
delay(1000);
|
||||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
//timer = millis();
|
||||
//tlmTimer = millis();
|
||||
|
||||
// initialise helicopter
|
||||
#if AIRFRAME == HELI
|
||||
|
|
|
|||
Loading…
Reference in New Issue