Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
rmackay9@yahoo.com 2010-12-22 13:38:54 +00:00
parent b9e2dcd124
commit 0b7fd432b5
8 changed files with 411 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArducopterNG/Arducopter.h
View File

@ -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

39
ArducopterNG/ArducopterNG.pde
View 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()
}

127
ArducopterNG/CLI.pde
View File

@ -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() {

21
ArducopterNG/EEPROM.pde
View File

@ -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);
}

42
ArducopterNG/Log.pde
View File

@ -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);

131
ArducopterNG/Navigation.pde
View File

@ -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
}

15
ArducopterNG/Sensors.pde
View File

@ -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
View File

@ -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() {
}