diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index 73aa8adaa4..3c00aa8d49 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -282,6 +282,22 @@ float gps_pitch_D; float gps_pitch_I=0; float command_gps_roll; float command_gps_pitch; +float gps_err_lat; +float gps_err_lat_old; +float gps_lat_D; +float gps_lat_I=0; +float gps_err_lon; +float gps_err_lon_old; +float gps_lon_D; +float gps_lon_I=0; + +// object avoidance +float RF_roll_I=0; +float RF_pitch_I=0; +float command_RF_roll = 0; +float command_RF_pitch = 0; +float command_RF_throttle = 0; +byte RF_new_data = 0; //Altitude control int Initial_Throttle; @@ -543,6 +559,16 @@ int MIN_THROTTLE; //float eeprom_counter; // reserved for eeprom write counter, 31-10-10, jp //float eeprom_checker; // reserved for eeprom checksums, 01-11-10, jp +// obstacle avoidance +float KP_RF_ROLL; +float KD_RF_ROLL; +float KI_RF_ROLL; +float KP_RF_PITCH; +float KD_RF_PITCH; +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) + // 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() { @@ -610,6 +636,14 @@ void defaultUserConfig() { mag_offset_y = 0; mag_offset_z = 0; MIN_THROTTLE = 1040; // used to be #define but now in EEPROM + KP_RF_ROLL = 0.10; + KI_RF_ROLL = 0.00; + KD_RF_ROLL = 0.03; + KP_RF_PITCH = 0.10; + KI_RF_PITCH = 0.00; + 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) } // EEPROM storage addresses @@ -677,11 +711,18 @@ void defaultUserConfig() { #define mag_offset_y_ADR 244 #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 KP_RF_PITCH_ADR 268 +#define KD_RF_PITCH_ADR 272 +#define KI_RF_PITCH_ADR 276 +#define RF_MAX_ANGLE_ADR 280 +#define RF_SAFETY_ZONE_ADR 284 //#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 -// end of file +// end of file diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index cd56c8fa8c..e3a3697dd2 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -58,6 +58,7 @@ #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 @@ -200,6 +201,7 @@ #include // ArduPilot Mega Magnetometer Library #include // I2C Communication library #include // EEPROM +#include // RangeFinders (Sonars, IR Sensors) //#include #include "Arducopter.h" #include "ArduUser.h" @@ -223,6 +225,12 @@ 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 /* ************************************************************ */ /* ************* MAIN PROGRAM - DECLARATIONS ****************** */ @@ -339,7 +347,7 @@ void loop() #endif }else{ // Automatic mode : GPS position hold mode #if AIRFRAME == QUAD - Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw); + 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 #if AIRFRAME == HELI heli_attitude_control(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_collective,command_rx_yaw); @@ -363,7 +371,7 @@ void loop() if(!SW_DIP2) camera_output(); #endif - // Autopilot mode functions + // Autopilot mode functions - GPS Hold, Altitude Hold + object avoidance if (AP_mode == AP_AUTOMATIC_MODE) { digitalWrite(LED_Yellow,HIGH); // Yellow LED ON : GPS Position Hold MODE @@ -376,6 +384,7 @@ void loop() { 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 { @@ -415,12 +424,18 @@ void loop() ch_throttle_altitude_hold = ch_throttle; Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde) } - } - else + // obstacle avoidance - comes on with autopilot + #ifdef IsRANGEFINDER // Do we have Range Finders connected? + if( RF_new_data ) { + Obstacle_avoidance(RF_SAFETY_ZONE); + RF_new_data = 0; + } + #endif + }else{ digitalWrite(LED_Yellow,LOW); target_position=0; - } + } } // Medium loop (about 60Hz) @@ -446,13 +461,17 @@ void loop() } #endif break; - case 1: // Barometer reading (2x10Hz = 20Hz) + case 1: // Barometer + RangeFinder reading (2x10Hz = 20Hz) medium_loopCounter++; #ifdef UseBMP if (APM_BMP085.Read()){ read_baro(); Baro_new_data = 1; } +#endif +#ifdef IsRANGEFINDER + read_RF_Sensors(); + RF_new_data = 1; #endif break; case 2: // Send serial telemetry (10Hz) @@ -467,7 +486,7 @@ void loop() readSerialCommand(); #endif break; - case 4: // second Barometer reading (2x10Hz = 20Hz) + case 4: // second Barometer + RangeFinder reading (2x10Hz = 20Hz) medium_loopCounter++; #ifdef UseBMP if (APM_BMP085.Read()){ @@ -476,6 +495,10 @@ void loop() //Serial.print("B "); //Serial.println(press_alt); } +#endif +#ifdef IsRANGEFINDER + read_RF_Sensors(); + RF_new_data = 1; #endif break; case 5: // Battery monitor (10Hz) @@ -509,4 +532,4 @@ void loop() } - + diff --git a/ArducopterNG/CLI.pde b/ArducopterNG/CLI.pde index 2ab6ef4a91..bf23c2927e 100644 --- a/ArducopterNG/CLI.pde +++ b/ArducopterNG/CLI.pde @@ -84,6 +84,9 @@ void RunCLI () { case 'e': CALIB_Esc(); break; + case 'o': + Set_Obstacle_Avoidance_PIDs(); + break; case 's': Show_Settings(); break; @@ -123,6 +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(" r - Reset to factory settings"); SerPrln(" t - Calibrate MIN Throttle value"); SerPrln(" s - Show settings"); @@ -551,12 +555,129 @@ void Show_Settings() { else { SerPrln("+ mode"); } + + Show_Obstacle_Avoidance_PIDs() ; SerPrln(); +} + +// Display obstacle avoidance pids +void Show_Obstacle_Avoidance_PIDs() { + SerPri("\tSafetyZone: "); + SerPrln(RF_SAFETY_ZONE); + SerPri("\tRoll PID: "); + SerPri(KP_RF_ROLL); cspc(); + SerPri(KI_RF_ROLL); cspc(); + SerPrln(KD_RF_ROLL); + SerPri("\tPitch PID: "); + SerPri(KP_RF_PITCH); cspc(); + SerPri(KI_RF_PITCH); cspc(); + SerPri(KD_RF_PITCH); SerPrln(); + SerPri("\tMaxAngle: "); + SerPri(RF_MAX_ANGLE); + SerPrln(); +} +// save RF pids to eeprom +void Save_Obstacle_Avoidance_PIDs_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(KP_RF_PITCH,KP_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); } +// +void Set_Obstacle_Avoidance_PIDs() { + float tempVal1, tempVal2, tempVal3; + int saveToEeprom = 0; + // Display current PID values + SerPrln("Obstacle Avoidance:"); + Show_Obstacle_Avoidance_PIDs(); + 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 ) { + RF_SAFETY_ZONE = tempVal1; + SerPri("SafetyZone: "); + SerPrln(RF_SAFETY_ZONE); + } + SerPrln(); + + // ROLL PIDs + SerFlu(); + SerPri("Enter Roll 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_RF_ROLL = tempVal1; + KI_RF_ROLL = tempVal2; + KD_RF_ROLL = tempVal3; + SerPrln(); + SerPri("P:"); + SerPri(KP_RF_ROLL); + SerPri("\tI:"); + SerPri(KI_RF_ROLL); + SerPri("\tD:"); + SerPri(KD_RF_ROLL); + saveToEeprom = 1; + } + SerPrln(); + + // PITCH PIDs + SerFlu(); + SerPri("Enter Pitch 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_RF_PITCH = tempVal1; + KI_RF_PITCH = tempVal2; + KD_RF_PITCH = tempVal3; + SerPrln(); + SerPri("P:"); + SerPri(KP_RF_PITCH); + SerPri("\tI:"); + SerPri(KI_RF_PITCH); + SerPri("\tD:"); + SerPri(KD_RF_PITCH); + saveToEeprom = 1; + } + SerPrln(); + + // Max Angle + SerFlu(); + SerPri("Enter Max Angle or 0 to skip: "); + while( !SerAva() ); // wait until user presses a key + tempVal1 = readFloatSerial(); + SerPrln(tempVal1); + if( tempVal1 > 0 ) { + RF_MAX_ANGLE = tempVal1; + SerPrln(); + SerPri("MaxAngle: "); + SerPri(RF_MAX_ANGLE); + saveToEeprom = 1; + } + SerPrln(); + + // save to eeprom + if( saveToEeprom == 1 ) { + Save_Obstacle_Avoidance_PIDs_toEEPROM(); + SerPrln("Saved to EEPROM"); + SerPrln(); + } +} void cspc() { SerPri(", "); @@ -581,4 +702,4 @@ void WaitSerialEnter() { - + diff --git a/ArducopterNG/EEPROM.pde b/ArducopterNG/EEPROM.pde index 7c2ec5cd4c..407e7a2182 100644 --- a/ArducopterNG/EEPROM.pde +++ b/ArducopterNG/EEPROM.pde @@ -123,7 +123,14 @@ void readUserConfig() { mag_offset_y = readEEPROM(mag_offset_y_ADR); mag_offset_z = readEEPROM(mag_offset_z_ADR); MIN_THROTTLE = readEEPROM(MIN_THROTTLE_ADR); - + KP_RF_ROLL = readEEPROM(KP_RF_ROLL_ADR); + KD_RF_ROLL = readEEPROM(KD_RF_ROLL_ADR); + KI_RF_ROLL = readEEPROM(KI_RF_ROLL_ADR); + KP_RF_PITCH = readEEPROM(KP_RF_PITCH_ADR); + KD_RF_PITCH = readEEPROM(KD_RF_PITCH_ADR); + KI_RF_PITCH = readEEPROM(KI_RF_PITCH_ADR); + RF_MAX_ANGLE = readEEPROM(RF_MAX_ANGLE_ADR); + RF_SAFETY_ZONE = readEEPROM(RF_SAFETY_ZONE_ADR); } void writeUserConfig() { @@ -195,6 +202,12 @@ void writeUserConfig() { writeEEPROM(mag_offset_y, mag_offset_y_ADR); 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(KP_RF_PITCH,KP_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); +} diff --git a/ArducopterNG/Log.pde b/ArducopterNG/Log.pde index e808137d33..92a873e6ab 100644 --- a/ArducopterNG/Log.pde +++ b/ArducopterNG/Log.pde @@ -51,6 +51,7 @@ TODO: #define LOG_RADIO_MSG 0x08 #define LOG_SENSOR_MSG 0x09 #define LOG_PID_MSG 0x0A +#define LOG_RANGEFINDER_MSG 0x0B #define LOG_MAX_ERRORS 50 // when reading logs, give up after 50 sequential failures to find HEADBYTE1 @@ -303,6 +304,23 @@ void Log_Write_Raw() } #endif +#ifdef LOG_RANGEFINDER +// Write a Sensor Raw data packet +void Log_Write_RangeFinder(int rf1, int rf2, int rf3,int rf4, int rf5, int rf6) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_RANGEFINDER_MSG); + DataFlash.WriteInt(rf1); + DataFlash.WriteInt(rf2); + DataFlash.WriteInt(rf3); + DataFlash.WriteInt(rf4); + DataFlash.WriteInt(rf5); + DataFlash.WriteInt(rf6); + DataFlash.WriteByte(END_BYTE); +} +#endif + // Read an control tuning packet void Log_Read_Control_Tuning() @@ -518,6 +536,25 @@ void Log_Read_Raw() SerPriln(" "); } +// Read a raw accel/gyro packet +void Log_Read_RangeFinder() +{ + SerPri("RF:"); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPri(DataFlash.ReadInt()); + SerPri(","); + SerPriln(" "); +} + // Read the DataFlash log memory : Packet Parser void Log_Read(int start_page, int end_page) { @@ -584,6 +621,11 @@ void Log_Read(int start_page, int end_page) }else if(data==LOG_PID_MSG){ Log_Read_PID(); log_step++; + + }else if(data==LOG_RANGEFINDER_MSG) { + Log_Read_RangeFinder(); + log_step++; + }else{ SerPri("Error Reading Packet: "); SerPri(packet_count); diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde index 08a7476fe0..801cca6db4 100644 --- a/ArducopterNG/Navigation.pde +++ b/ArducopterNG/Navigation.pde @@ -97,11 +97,62 @@ void Position_control(long lat_dest, long lon_dest) #endif } +/* GPS based Position control Version 2 - builds up I and D term using lat/lon instead of roll/pitch*/ +void Position_control_v2(long lat_dest, long lon_dest) +{ +#ifdef IsGPS + //If we have not calculated GEOG_CORRECTION_FACTOR we calculate it here as cos(lattitude) + if (GEOG_CORRECTION_FACTOR==0) + GEOG_CORRECTION_FACTOR = cos(ToRad(GPS.Lattitude/10000000.0)); + + // store old lat & lon diff for d term? + gps_err_lon_old = gps_err_lon; + gps_err_lat_old = gps_err_lat; + + // calculate distance from target - for P term + gps_err_lon = (float)(lon_dest - GPS.Longitude) * GEOG_CORRECTION_FACTOR; + gps_err_lat = lat_dest - GPS.Lattitude; + + // 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_lat_I += gps_err_lat; + gps_lat_I = constrain(gps_lat_I,-800,800); + + // calculate the ground speed - for D term + gps_lon_D = (gps_err_lon - gps_err_lon_old) / GPS_Dt; + gps_lat_D = (gps_err_lat - gps_err_lat_old) / GPS_Dt; + + // Now separate lat & lon PID terms into roll & pitch components + // ROLL + //Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0] [This simplification is valid for low roll angles] + gps_err_roll = (gps_err_lon * DCM_Matrix[0][0] - gps_err_lat * DCM_Matrix[1][0]); + gps_roll_I = (gps_lon_I * DCM_Matrix[0][0] - gps_lat_I * DCM_Matrix[1][0]); + gps_roll_D = (gps_lon_D * DCM_Matrix[0][0] - gps_lat_D * DCM_Matrix[1][0]); + + 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); + + // PITCH + gps_err_pitch = (-gps_err_lat * DCM_Matrix[0][0] - gps_err_lon * DCM_Matrix[1][0]); + gps_pitch_I = (-gps_lat_I * DCM_Matrix[0][0] - gps_lon_I * DCM_Matrix[1][0]); + gps_pitch_D = (-gps_lat_D * DCM_Matrix[0][0] - gps_lon_D * DCM_Matrix[1][0]); + + 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); + +#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 + gps_lat_I = 0; } /* ************************************************************ */ @@ -149,4 +200,84 @@ int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_altitude) return (Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX)); } +/* ************************************************************ */ +/* Obstacle avoidance routine */ +void Obstacle_avoidance(int safeDistance) +{ +#ifdef IsRANGEFINDER + int RF_err_roll = 0; + int RF_err_pitch = 0; + int RF_err_throttle = 0; + float RF_roll_P; + float RF_roll_D; + float RF_pitch_P; + float RF_pitch_D; + float RF_throttle_P; + float RF_throttle_D; + static int RF_err_roll_old; + static int RF_err_pitch_old; + static int RF_err_throttle_old; + 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; + + // 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; + // 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; + + // 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; + + // ROLL - P term + RF_roll_P = RF_err_roll * KP_RF_ROLL; + RF_roll_P = constrain(RF_roll_P,-RF_MAX_ANGLE,RF_MAX_ANGLE); + // ROLL - I term + RF_roll_I += RF_err_roll * 0.05 * KI_RF_ROLL; + RF_roll_I = constrain(RF_roll_I,-RF_MAX_ANGLE/2,RF_MAX_ANGLE/2); + // ROLL - D term + RF_roll_D = (RF_err_roll-RF_err_roll_old) / 0.05 * KD_RF_ROLL; // RF_IR frequency is 20Hz (50ms) + RF_roll_D = constrain(RF_roll_D,-RF_MAX_ANGLE/2,RF_MAX_ANGLE/2); + RF_err_roll_old = RF_err_roll; + // ROLL - full comand + command_RF_roll = RF_roll_P + RF_roll_I + RF_roll_D; + command_RF_roll = constrain(command_RF_roll,-RF_MAX_ANGLE,RF_MAX_ANGLE); // Limit max command + + // PITCH - P term + RF_pitch_P = RF_err_pitch * KP_RF_PITCH; + RF_pitch_P = constrain(RF_pitch_P,-RF_MAX_ANGLE,RF_MAX_ANGLE); + // PITCH - I term + RF_pitch_I += RF_err_pitch * 0.05 * KI_RF_PITCH; + RF_pitch_I = constrain(RF_pitch_I,-RF_MAX_ANGLE/2,RF_MAX_ANGLE/2); + // PITCH - D term + RF_pitch_D = (RF_err_pitch-RF_err_pitch_old) / 0.05 * KD_RF_PITCH; // RF_IR frequency is 20Hz (50ms) + RF_pitch_D = constrain(RF_pitch_D,-RF_MAX_ANGLE/2,RF_MAX_ANGLE/2); + RF_err_pitch_old = RF_err_pitch; + // PITCH - full comand + command_RF_pitch = RF_pitch_P + RF_pitch_I + RF_pitch_D; + command_RF_pitch = constrain(command_RF_pitch,-RF_MAX_ANGLE,RF_MAX_ANGLE); // Limit max command + + // THROTTLE - not yet implemented + command_RF_throttle = 0; + +#else + command_RF_roll = 0; + command_RF_pitch = 0; + command_RF_throttle = 0; +#endif +} + diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde index bdea00f971..ee91b77c32 100644 --- a/ArducopterNG/Sensors.pde +++ b/ArducopterNG/Sensors.pde @@ -99,4 +99,19 @@ void read_baro(void) press_alt = press_alt * 0.75 + ((1.0 - tempPresAlt) * 4433000)*0.25; // Altitude in cm (filtered) } #endif + +#ifdef IsRANGEFINDER +/* This function read IR data, convert to cm units and filter the data */ +void read_RF_Sensors() +{ + 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 +} +#endif diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index f3a1782331..94ce9d9cb7 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -183,6 +183,13 @@ void APM_Init() { APM_BMP085.Init(FALSE); #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); +#endif + delay(1000); DataFlash.StartWrite(1); // Start a write session on page 1 @@ -202,4 +209,4 @@ void APM_Init() { } - +