diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index 3c00aa8d49..7aa776c4b9 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -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 diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 23f641f8e2..ba710f980c 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -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,17 +298,13 @@ void setup() { // Main loop void loop() { - //int aux; - //int i; - //float aux_float; - + currentTimeMicros = micros(); currentTime = currentTimeMicros / 1000; // 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; - 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) } - // obstacle avoidance - comes on with autopilot + + // 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; + 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 + + // 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 diff --git a/ArducopterNG/CLI.pde b/ArducopterNG/CLI.pde index bf23c2927e..b282750016 100644 --- a/ArducopterNG/CLI.pde +++ b/ArducopterNG/CLI.pde @@ -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,35 +584,61 @@ 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(KI_RF_ROLL,KI_RF_ROLL_ADR); writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR); - writeEEPROM(KI_RF_ROLL,KI_RF_ROLL_ADR); writeEEPROM(KP_RF_PITCH,KP_RF_PITCH_ADR); + writeEEPROM(KI_RF_PITCH,KI_RF_PITCH_ADR); writeEEPROM(KD_RF_PITCH,KD_RF_PITCH_ADR); - writeEEPROM(KI_RF_PITCH,KI_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 SerFlu(); 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(); } } diff --git a/ArducopterNG/EEPROM.pde b/ArducopterNG/EEPROM.pde index 407e7a2182..2f0d9d10e9 100644 --- a/ArducopterNG/EEPROM.pde +++ b/ArducopterNG/EEPROM.pde @@ -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(KI_RF_PITCH,KI_RF_PITCH_ADR); writeEEPROM(KD_RF_PITCH,KD_RF_PITCH_ADR); - writeEEPROM(KI_RF_PITCH,KI_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); } diff --git a/ArducopterNG/Log.pde b/ArducopterNG/Log.pde index 92a873e6ab..58d2468436 100644 --- a/ArducopterNG/Log.pde +++ b/ArducopterNG/Log.pde @@ -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:"); @@ -550,8 +550,7 @@ void Log_Read_RangeFinder() SerPri(","); SerPri(DataFlash.ReadInt()); SerPri(","); - SerPri(DataFlash.ReadInt()); - SerPri(","); + SerPri(DataFlash.ReadInt()); SerPriln(" "); } diff --git a/ArducopterNG/Motors.pde b/ArducopterNG/Motors.pde index aaede5940c..b477658f25 100644 --- a/ArducopterNG/Motors.pde +++ b/ArducopterNG/Motors.pde @@ -42,12 +42,12 @@ 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; throttle_mode=1; - } + } #endif if ((throttle_mode==0)&&(ch_throttle < (MIN_THROTTLE + 100))) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely) @@ -103,4 +103,4 @@ void motor_output() //#endif } - + diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde index 801cca6db4..cea43e2e44 100644 --- a/ArducopterNG/Navigation.pde +++ b/ArducopterNG/Navigation.pde @@ -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 +#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) { - #define ALTITUDE_CONTROL_SONAR_OUTPUT_MIN 60 - #define ALTITUDE_CONTROL_SONAR_OUTPUT_MAX 80 - 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; diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde index ee91b77c32..ffcfca61f4 100644 --- a/ArducopterNG/Sensors.pde +++ b/ArducopterNG/Sensors.pde @@ -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 diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index 94ce9d9cb7..1ba193f992 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -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