zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
mission-4.1.18
rmackay9@yahoo.com 14 years ago
parent
b9e2dcd124
commit
0b7fd432b5
8 changed files with 409 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 45
      ArducopterNG/Arducopter.h
  2. 39
      ArducopterNG/ArducopterNG.pde
  3. 123
      ArducopterNG/CLI.pde
  4. 21
      ArducopterNG/EEPROM.pde
  5. 42
      ArducopterNG/Log.pde
  6. 131
      ArducopterNG/Navigation.pde
  7. 15
      ArducopterNG/Sensors.pde
  8. 9
      ArducopterNG/System.pde

45
ArducopterNG/Arducopter.h
Unescape View File

@ -282,6 +282,22 @@ float gps_pitch_D; @@ -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; @@ -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() { @@ -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() { @@ -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

39
ArducopterNG/ArducopterNG.pde
Unescape View File

@ -58,6 +58,7 @@ @@ -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 @@ @@ -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 @@ @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -509,4 +532,4 @@ void loop()
}

123
ArducopterNG/CLI.pde
Unescape View File

@ -84,6 +84,9 @@ void RunCLI () { @@ -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() { @@ -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() { @@ -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() { @@ -581,4 +702,4 @@ void WaitSerialEnter() {

21
ArducopterNG/EEPROM.pde
Unescape View File

@ -123,7 +123,14 @@ void readUserConfig() { @@ -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() { @@ -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);
}

42
ArducopterNG/Log.pde
Unescape View File

@ -51,6 +51,7 @@ TODO: @@ -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() @@ -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() @@ -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) @@ -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);

131
ArducopterNG/Navigation.pde
Unescape View File

@ -97,11 +97,62 @@ void Position_control(long lat_dest, long lon_dest) @@ -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) @@ -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
}

15
ArducopterNG/Sensors.pde
Unescape View File

@ -99,4 +99,19 @@ void read_baro(void) @@ -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

9
ArducopterNG/System.pde
Unescape View File

@ -183,6 +183,13 @@ void APM_Init() { @@ -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() { @@ -202,4 +209,4 @@ void APM_Init() {
}

Write Preview
Loading…
Cancel
Save