mirror of https://github.com/ArduPilot/ardupilot
Addition of position hold version 2 (builds up I term on log + lat instead of roll + pitch)
Addition of obstacle avoidance including new option in CLI menu to set PID values git-svn-id: https://arducopter.googlecode.com/svn/trunk@1228 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
b9e2dcd124
commit
0b7fd432b5
|
@ -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
|
||||
|
|
|
@ -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 <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <Wire.h> // I2C Communication library
|
||||
#include <EEPROM.h> // EEPROM
|
||||
#include <AP_RangeFinder.h> // RangeFinders (Sonars, IR Sensors)
|
||||
//#include <AP_GPS.h>
|
||||
#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()
|
|||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
@ -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();
|
||||
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() {
|
|||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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() {
|
|||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue