Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopterNG - added altitude hold using Sonar/Rangefinder

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1265 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-12-26 04:30:55 +00:00
parent 5d6c90b723
commit e93daf2546
9 changed files with 188 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArducopterNG/Arducopter.h
View File

@ -294,6 +294,7 @@ float gps_lon_I=0;
// object avoidance // object avoidance
float RF_roll_I=0; float RF_roll_I=0;
float RF_pitch_I=0; float RF_pitch_I=0;
float RF_throttle_I=0;
float command_RF_roll = 0; float command_RF_roll = 0;
float command_RF_pitch = 0; float command_RF_pitch = 0;
float command_RF_throttle = 0; float command_RF_throttle = 0;
@ -308,6 +309,7 @@ float command_altitude;
float altitude_I; float altitude_I;
float altitude_D; float altitude_D;
int ch_throttle_altitude_hold; int ch_throttle_altitude_hold;
int sonar_altitude_valid = 0;
//Pressure Sensor variables //Pressure Sensor variables
long target_baro_altitude; // Altitude in cm 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_MODE 1 // Logs mode changes
#define LOG_RAW 0 // Logs raw accel/gyro data #define LOG_RAW 0 // Logs raw accel/gyro data
#define LOG_SEN 1 // Logs sensor data #define LOG_SEN 1 // Logs sensor data
#define LOG_RANGEFINDER 0 // Logs data from range finders
// GCS Message ID's // GCS Message ID's
#define MSG_ACKNOWLEDGE 0x00 #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_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) 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 // This function call contains the default values that are set to the ArduCopter
// when a "Default EEPROM Value" command is sent through serial interface // when a "Default EEPROM Value" command is sent through serial interface
void defaultUserConfig() { void defaultUserConfig() {
@ -644,6 +652,9 @@ void defaultUserConfig() {
KD_RF_PITCH = 0.03; KD_RF_PITCH = 0.03;
RF_MAX_ANGLE = 10.0; RF_MAX_ANGLE = 10.0;
RF_SAFETY_ZONE = 120.0; // object avoidance will avoid objects within this range (in cm) 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 // EEPROM storage addresses
@ -712,13 +723,16 @@ void defaultUserConfig() {
#define mag_offset_z_ADR 248 #define mag_offset_z_ADR 248
#define MIN_THROTTLE_ADR 252 #define MIN_THROTTLE_ADR 252
#define KP_RF_ROLL_ADR 256 #define KP_RF_ROLL_ADR 256
#define KD_RF_ROLL_ADR 260 #define KI_RF_ROLL_ADR 260
#define KI_RF_ROLL_ADR 264 #define KD_RF_ROLL_ADR 264
#define KP_RF_PITCH_ADR 268 #define KP_RF_PITCH_ADR 268
#define KD_RF_PITCH_ADR 272 #define KI_RF_PITCH_ADR 272
#define KI_RF_PITCH_ADR 276 #define KD_RF_PITCH_ADR 276
#define RF_MAX_ANGLE_ADR 280 #define RF_MAX_ANGLE_ADR 280
#define RF_SAFETY_ZONE_ADR 284 #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_counter_ADR 238 // hmm should i move these?!? , 31-10-10, jp
//#define eeprom_checker_ADR 240 // this too... , 31-10-10, jp //#define eeprom_checker_ADR 240 // this too... , 31-10-10, jp

79
ArducopterNG/ArducopterNG.pde
View File

@ -58,11 +58,11 @@
#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator #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 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 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 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 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 #define CONFIGURATOR
@ -225,12 +225,7 @@
AP_ADC_ADS7844 adc; AP_ADC_ADS7844 adc;
APM_BMP085_Class APM_BMP085; APM_BMP085_Class APM_BMP085;
AP_Compass_HMC5843 AP_Compass; AP_Compass_HMC5843 AP_Compass;
#ifdef IsRANGEFINDER AP_RangeFinder_MaxsonarXL AP_RangeFinder_down; // Other possible sonar is AP_RangeFinder_MaxsonarLV
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
/* ************************************************************ */ /* ************************************************************ */
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */ /* ************* MAIN PROGRAM - DECLARATIONS ****************** */
@ -303,9 +298,6 @@ void setup() {
// Main loop // Main loop
void loop() void loop()
{ {
//int aux;
//int i;
//float aux_float;
currentTimeMicros = micros(); currentTimeMicros = micros();
currentTime = currentTimeMicros / 1000; currentTime = currentTimeMicros / 1000;
@ -313,7 +305,6 @@ void loop()
// Main loop at 200Hz (IMU + control) // Main loop at 200Hz (IMU + control)
if ((currentTime-mainLoop) > 5) // about 200Hz (every 5ms) if ((currentTime-mainLoop) > 5) // about 200Hz (every 5ms)
{ {
//G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!!
G_Dt = (currentTimeMicros-previousTimeMicros) * 0.000001; // Microseconds!!! G_Dt = (currentTimeMicros-previousTimeMicros) * 0.000001; // Microseconds!!!
mainLoop = currentTime; mainLoop = currentTime;
previousTimeMicros = currentTimeMicros; previousTimeMicros = currentTimeMicros;
@ -393,23 +384,7 @@ void loop()
} }
} }
#endif #endif
#ifdef UseBMP } else { // First time we enter in GPS position hold we capture the target position as the actual position
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
{
#ifdef IsGPS #ifdef IsGPS
if (GPS.Fix){ // We need a GPS Fix to capture the actual position... if (GPS.Fix){ // We need a GPS Fix to capture the actual position...
target_lattitude = GPS.Lattitude; target_lattitude = GPS.Lattitude;
@ -419,22 +394,60 @@ void loop()
#endif #endif
command_gps_roll=0; command_gps_roll=0;
command_gps_pitch=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; target_baro_altitude = press_alt;
Initial_Throttle = ch_throttle; Initial_Throttle = ch_throttle;
ch_throttle_altitude_hold = 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? #ifdef IsRANGEFINDER // Do we have Range Finders connected?
if( RF_new_data ) if( RF_new_data )
{ {
Obstacle_avoidance(RF_SAFETY_ZONE); if( sonar_altitude_valid ) {
RF_new_data = 0; // 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 #endif
}else{ }else{
digitalWrite(LED_Yellow,LOW); digitalWrite(LED_Yellow,LOW);
target_position=0; target_position=0;
target_baro_altitude=0;
target_sonar_altitude=0;
} }
} }
@ -492,8 +505,6 @@ void loop()
if (APM_BMP085.Read()){ if (APM_BMP085.Read()){
read_baro(); read_baro();
Baro_new_data = 1; Baro_new_data = 1;
//Serial.print("B ");
//Serial.println(press_alt);
} }
#endif #endif
#ifdef IsRANGEFINDER #ifdef IsRANGEFINDER

66
ArducopterNG/CLI.pde
View File

@ -85,7 +85,7 @@ void RunCLI () {
CALIB_Esc(); CALIB_Esc();
break; break;
case 'o': case 'o':
Set_Obstacle_Avoidance_PIDs(); Set_SonarAndObstacleAvoidance_PIDs();
break; break;
case 's': case 's':
Show_Settings(); Show_Settings();
@ -126,7 +126,7 @@ void Show_MainMenu() {
SerPrln(" e - ESC Throttle calibration (Works with official ArduCopter ESCs)"); SerPrln(" e - ESC Throttle calibration (Works with official ArduCopter ESCs)");
SerPrln(" i - Initialize and calibrate Accel offsets"); SerPrln(" i - Initialize and calibrate Accel offsets");
SerPrln(" m - Motor tester with AIL/ELE stick"); 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(" r - Reset to factory settings");
SerPrln(" t - Calibrate MIN Throttle value"); SerPrln(" t - Calibrate MIN Throttle value");
SerPrln(" s - Show settings"); SerPrln(" s - Show settings");
@ -556,14 +556,18 @@ void Show_Settings() {
SerPrln("+ mode"); SerPrln("+ mode");
} }
Show_Obstacle_Avoidance_PIDs() ; Show_SonarAndObstacleAvoidance_PIDs();
SerPrln(); SerPrln();
} }
// Display obstacle avoidance pids // Display obstacle avoidance pids
void Show_Obstacle_Avoidance_PIDs() { void Show_SonarAndObstacleAvoidance_PIDs() {
SerPri("\tSafetyZone: "); SerPri("\tSonar PID: ");
SerPri(KP_SONAR_ALTITUDE); cspc();
SerPri(KI_SONAR_ALTITUDE); cspc();
SerPrln(KD_SONAR_ALTITUDE);
SerPri("\tObstacle SafetyZone: ");
SerPrln(RF_SAFETY_ZONE); SerPrln(RF_SAFETY_ZONE);
SerPri("\tRoll PID: "); SerPri("\tRoll PID: ");
SerPri(KP_RF_ROLL); cspc(); SerPri(KP_RF_ROLL); cspc();
@ -580,24 +584,49 @@ void Show_Obstacle_Avoidance_PIDs() {
} }
// save RF pids to eeprom // save RF pids to eeprom
void Save_Obstacle_Avoidance_PIDs_toEEPROM() { void Save_SonarAndObstacleAvoidancePIDs_toEEPROM() {
writeEEPROM(KP_RF_ROLL,KP_RF_ROLL_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(KI_RF_ROLL,KI_RF_ROLL_ADR);
writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR);
writeEEPROM(KP_RF_PITCH,KP_RF_PITCH_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(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_MAX_ANGLE,RF_MAX_ANGLE_ADR);
writeEEPROM(RF_SAFETY_ZONE,RF_SAFETY_ZONE_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; float tempVal1, tempVal2, tempVal3;
int saveToEeprom = 0; int saveToEeprom = 0;
// Display current PID values // Display current PID values
SerPrln("Obstacle Avoidance:"); SerPrln("Sonar and Obstacle Avoidance:");
Show_Obstacle_Avoidance_PIDs(); 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(); SerPrln();
// SAFETY ZONE // SAFETY ZONE
@ -605,10 +634,11 @@ void Set_Obstacle_Avoidance_PIDs() {
SerPri("Enter Safety Zone (in cm) or 0 to skip: "); SerPri("Enter Safety Zone (in cm) or 0 to skip: ");
while( !SerAva() ); // wait until user presses a key while( !SerAva() ); // wait until user presses a key
tempVal1 = readFloatSerial(); tempVal1 = readFloatSerial();
if( tempVal1 >= 20 && tempVal1 <= 150 ) { if( tempVal1 >= 20 && tempVal1 <= 700 ) {
RF_SAFETY_ZONE = tempVal1; RF_SAFETY_ZONE = tempVal1;
SerPri("SafetyZone: "); SerPri("SafetyZone: ");
SerPrln(RF_SAFETY_ZONE); SerPri(RF_SAFETY_ZONE);
saveToEeprom = 1;
} }
SerPrln(); SerPrln();
@ -662,7 +692,7 @@ void Set_Obstacle_Avoidance_PIDs() {
while( !SerAva() ); // wait until user presses a key while( !SerAva() ); // wait until user presses a key
tempVal1 = readFloatSerial(); tempVal1 = readFloatSerial();
SerPrln(tempVal1); SerPrln(tempVal1);
if( tempVal1 > 0 ) { if( tempVal1 > 0 && tempVal1 < 90 ) {
RF_MAX_ANGLE = tempVal1; RF_MAX_ANGLE = tempVal1;
SerPrln(); SerPrln();
SerPri("MaxAngle: "); SerPri("MaxAngle: ");
@ -673,9 +703,15 @@ void Set_Obstacle_Avoidance_PIDs() {
// save to eeprom // save to eeprom
if( saveToEeprom == 1 ) { if( saveToEeprom == 1 ) {
Save_Obstacle_Avoidance_PIDs_toEEPROM(); SerPrln("Obstacle Avoidance:");
Show_SonarAndObstacleAvoidance_PIDs();
SerPrln();
Save_SonarAndObstacleAvoidancePIDs_toEEPROM();
SerPrln("Saved to EEPROM"); SerPrln("Saved to EEPROM");
SerPrln(); SerPrln();
}else{
SerPrln("No changes. Nothing saved to EEPROM");
SerPrln();
} }
} }

10
ArducopterNG/EEPROM.pde
View File

@ -131,6 +131,9 @@ void readUserConfig() {
KI_RF_PITCH = readEEPROM(KI_RF_PITCH_ADR); KI_RF_PITCH = readEEPROM(KI_RF_PITCH_ADR);
RF_MAX_ANGLE = readEEPROM(RF_MAX_ANGLE_ADR); RF_MAX_ANGLE = readEEPROM(RF_MAX_ANGLE_ADR);
RF_SAFETY_ZONE = readEEPROM(RF_SAFETY_ZONE_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() { void writeUserConfig() {
@ -203,11 +206,14 @@ void writeUserConfig() {
writeEEPROM(mag_offset_z, mag_offset_z_ADR); writeEEPROM(mag_offset_z, mag_offset_z_ADR);
writeEEPROM(MIN_THROTTLE, MIN_THROTTLE_ADR); writeEEPROM(MIN_THROTTLE, MIN_THROTTLE_ADR);
writeEEPROM(KP_RF_ROLL,KP_RF_ROLL_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(KI_RF_ROLL,KI_RF_ROLL_ADR);
writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR);
writeEEPROM(KP_RF_PITCH,KP_RF_PITCH_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(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_MAX_ANGLE,RF_MAX_ANGLE_ADR);
writeEEPROM(RF_SAFETY_ZONE,RF_SAFETY_ZONE_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);
} }

5
ArducopterNG/Log.pde
View File

@ -304,7 +304,7 @@ void Log_Write_Raw()
} }
#endif #endif
#ifdef LOG_RANGEFINDER #if LOG_RANGEFINDER
// Write a Sensor Raw data packet // Write a Sensor Raw data packet
void Log_Write_RangeFinder(int rf1, int rf2, int rf3,int rf4, int rf5, int rf6) 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(" "); SerPriln(" ");
} }
// Read a raw accel/gyro packet // Read a RangeFinder packet
void Log_Read_RangeFinder() void Log_Read_RangeFinder()
{ {
SerPri("RF:"); SerPri("RF:");
@ -551,7 +551,6 @@ void Log_Read_RangeFinder()
SerPri(DataFlash.ReadInt()); SerPri(DataFlash.ReadInt());
SerPri(","); SerPri(",");
SerPri(DataFlash.ReadInt()); SerPri(DataFlash.ReadInt());
SerPri(",");
SerPriln(" "); SerPriln(" ");
} }

2
ArducopterNG/Motors.pde
View File

@ -42,7 +42,7 @@ void motor_output()
byte throttle_mode=0; byte throttle_mode=0;
throttle = ch_throttle; throttle = ch_throttle;
#ifdef UseBMP #if defined(UseBMP) || defined(IsRANGEFINDER)
if (AP_mode == AP_AUTOMATIC_MODE) if (AP_mode == AP_AUTOMATIC_MODE)
{ {
throttle = ch_throttle_altitude_hold; throttle = ch_throttle_altitude_hold;

58
ArducopterNG/Navigation.pde
View File

@ -115,9 +115,9 @@ void Position_control_v2(long lat_dest, long lon_dest)
// add distance to I term // add distance to I term
gps_lon_I += gps_err_lon; 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 += 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 // calculate the ground speed - for D term
gps_lon_D = (gps_err_lon - gps_err_lon_old) / GPS_Dt; 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 = 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 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 // PITCH
gps_err_pitch = (-gps_err_lat * DCM_Matrix[0][0] - gps_err_lon * DCM_Matrix[1][0]); 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 = 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 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 #endif
} }
void Reset_I_terms_navigation() void Reset_I_terms_navigation()
{ {
altitude_I = 0;
gps_roll_I = 0; gps_roll_I = 0;
gps_pitch_I = 0; gps_pitch_I = 0;
gps_lon_I = 0; // for position hold ver 2 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; return command_altitude;
} }
/* ************************************************************ */
/* Altitude control... (based on sonar) */ /* Altitude control... (based on sonar) */
/* CONTROL PARAMETERS FOR SONAR ALTITUDE CONTROL (TEMPORATLY HERE) */ #define GdT_SONAR_ALTITUDE 0.05
#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 ALTITUDE_CONTROL_SONAR_OUTPUT_MIN 60 #define ALTITUDE_CONTROL_SONAR_OUTPUT_MIN 60
#define ALTITUDE_CONTROL_SONAR_OUTPUT_MAX 80 #define ALTITUDE_CONTROL_SONAR_OUTPUT_MAX 80
int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_altitude)
{
int command_altitude; int command_altitude;
err_altitude_old = err_altitude; err_altitude_old = err_altitude;
err_altitude = target_sonar_altitude - Sonar_altitude; err_altitude = target_sonar_altitude - Sonar_altitude;
altitude_D = (float)(err_altitude-err_altitude_old)/0.05; altitude_D = (float)(err_altitude-err_altitude_old)/GdT_SONAR_ALTITUDE;
altitude_I += (float)err_altitude*0.05; altitude_I += (float)err_altitude*GdT_SONAR_ALTITUDE;
altitude_I = constrain(altitude_I,-1000,1000); altitude_I = constrain(altitude_I,-1000,1000);
command_altitude = KP_SONAR_ALTITUDE*err_altitude + KD_SONAR_ALTITUDE*altitude_D + KI_SONAR_ALTITUDE*altitude_I; 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)); return (Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX));
} }
/* ************************************************************ */ /* ************************************************************ */
/* Obstacle avoidance routine */ /* Obstacle avoidance routine */
/* NOT YET FULLY IMPLEMENTED */
void Obstacle_avoidance(int safeDistance) void Obstacle_avoidance(int safeDistance)
{ {
#ifdef IsRANGEFINDER #ifdef IsRANGEFINDER
@ -220,28 +218,28 @@ void Obstacle_avoidance(int safeDistance)
int err_temp; int err_temp;
// front right // front right
err_temp = max(safeDistance - AP_RangeFinder_frontRight.distance,0); // err_temp = max(safeDistance - AP_RangeFinder_frontRight.distance,0);
RF_err_roll += err_temp * AP_RangeFinder_frontRight.orientation_x; // RF_err_roll += err_temp * AP_RangeFinder_frontRight.orientation_x;
RF_err_pitch += err_temp * AP_RangeFinder_frontRight.orientation_y; // RF_err_pitch += err_temp * AP_RangeFinder_frontRight.orientation_y;
RF_err_throttle += err_temp * AP_RangeFinder_frontRight.orientation_z; // RF_err_throttle += err_temp * AP_RangeFinder_frontRight.orientation_z;
// back right // back right
err_temp = max(safeDistance - AP_RangeFinder_backRight.distance,0); // err_temp = max(safeDistance - AP_RangeFinder_backRight.distance,0);
RF_err_roll += err_temp * AP_RangeFinder_backRight.orientation_x; // RF_err_roll += err_temp * AP_RangeFinder_backRight.orientation_x;
RF_err_pitch += err_temp * AP_RangeFinder_backRight.orientation_y; // RF_err_pitch += err_temp * AP_RangeFinder_backRight.orientation_y;
RF_err_throttle += err_temp * AP_RangeFinder_backRight.orientation_z; // RF_err_throttle += err_temp * AP_RangeFinder_backRight.orientation_z;
// back left // back left
err_temp = max(safeDistance - AP_RangeFinder_backLeft.distance,0); // err_temp = max(safeDistance - AP_RangeFinder_backLeft.distance,0);
RF_err_roll += err_temp * AP_RangeFinder_backLeft.orientation_x; // RF_err_roll += err_temp * AP_RangeFinder_backLeft.orientation_x;
RF_err_pitch += err_temp * AP_RangeFinder_backLeft.orientation_y; // RF_err_pitch += err_temp * AP_RangeFinder_backLeft.orientation_y;
RF_err_throttle += err_temp * AP_RangeFinder_backLeft.orientation_z; // RF_err_throttle += err_temp * AP_RangeFinder_backLeft.orientation_z;
// front left // front left
err_temp = max(safeDistance - AP_RangeFinder_frontLeft.distance,0); // err_temp = max(safeDistance - AP_RangeFinder_frontLeft.distance,0);
RF_err_roll += err_temp * AP_RangeFinder_frontLeft.orientation_x; // RF_err_roll += err_temp * AP_RangeFinder_frontLeft.orientation_x;
RF_err_pitch += err_temp * AP_RangeFinder_frontLeft.orientation_y; // RF_err_pitch += err_temp * AP_RangeFinder_frontLeft.orientation_y;
RF_err_throttle += err_temp * AP_RangeFinder_frontLeft.orientation_z; // RF_err_throttle += err_temp * AP_RangeFinder_frontLeft.orientation_z;
// ROLL - P term // ROLL - P term
RF_roll_P = RF_err_roll * KP_RF_ROLL; RF_roll_P = RF_err_roll * KP_RF_ROLL;

26
ArducopterNG/Sensors.pde
View File

@ -101,17 +101,27 @@ void read_baro(void)
#endif #endif
#ifdef IsRANGEFINDER #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() void read_RF_Sensors()
{ {
AP_RangeFinder_frontRight.read(); // AP_RangeFinder_frontRight.read();
AP_RangeFinder_backRight.read(); // AP_RangeFinder_backRight.read();
AP_RangeFinder_backLeft.read(); // AP_RangeFinder_backLeft.read();
AP_RangeFinder_frontLeft.read(); // AP_RangeFinder_frontLeft.read();
#ifdef LOG_RANGEFINDER // calculate altitude from down sensor
Log_Write_RangeFinder(AP_RangeFinder_frontRight.distance, AP_RangeFinder_backRight.distance, AP_RangeFinder_backLeft.distance,AP_RangeFinder_frontLeft.distance,0,0); AP_RangeFinder_down.read();
#endif // LOG_RANGEFINDER 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 #endif

20
ArducopterNG/System.pde
View File

@ -171,12 +171,11 @@ void APM_Init() {
#ifndef CONFIGURATOR #ifndef CONFIGURATOR
for(i=0;i<6;i++) for(i=0;i<6;i++)
{ {
Serial.print("AN[]:"); SerPri("AN[]:");
Serial.println(AN_OFFSET[i]); SerPrln(AN_OFFSET[i]);
} }
Serial.print("Yaw neutral value:"); SerPri("Yaw neutral value:");
// Serial.println(Neutro_yaw); SerPri(yaw_mid);
Serial.print(yaw_mid);
#endif #endif
#ifdef UseBMP #ifdef UseBMP
@ -184,17 +183,16 @@ void APM_Init() {
#endif #endif
#ifdef IsRANGEFINDER #ifdef IsRANGEFINDER
AP_RangeFinder_frontRight.init(AN2); AP_RangeFinder_frontRight.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT); AP_RangeFinder_down.init(AN1); AP_RangeFinder_down.set_orientation(AP_RANGEFINDER_ORIENTATION_DOWN);
AP_RangeFinder_backRight.init(AN3); AP_RangeFinder_backRight.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_RIGHT); //AP_RangeFinder_frontRight.init(AN5); AP_RangeFinder_frontRight.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT);
AP_RangeFinder_backLeft.init(AN4); AP_RangeFinder_backLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_LEFT); //AP_RangeFinder_backRight.init(AN4); AP_RangeFinder_backRight.set_orientation(AP_RANGEFINDER_ORIENTATION_BACK_RIGHT);
AP_RangeFinder_frontLeft.init(AN5); AP_RangeFinder_frontLeft.set_orientation(AP_RANGEFINDER_ORIENTATION_FRONT_LEFT); //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 #endif
delay(1000); delay(1000);
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
//timer = millis();
//tlmTimer = millis();
// initialise helicopter // initialise helicopter
#if AIRFRAME == HELI #if AIRFRAME == HELI