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

New version from Ted´s Mikro branch. Added InstantPWM. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@38 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-07-04 20:15:20 +00:00
parent 830fa2b104
commit 1a8856d0d5
11 changed files with 842 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

329
ArduCopter.pde → Arducopter/Arducopter.pde
View File

@ -1,17 +1,20 @@
/* ********************************************************************** */ /* ********************************************************************** */
/* ArduCopter Quadcopter code */ /* ArduCopter Quadcopter code */
/* */ /* */
/* Code based on ArduIMU DCM code from Diydrones.com */ /* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */
/* IMU DCM code from Diydrones.com */
/* (Original ArduIMU code from Jordi Muñoz and William Premerlani) */ /* (Original ArduIMU code from Jordi Muñoz and William Premerlani) */
/* Ardupilot core code : from DIYDrones.com development team */ /* Ardupilot core code : from DIYDrones.com development team */
/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */ /* Authors : Arducopter development team */
/* Authors : Jose Julio, Ted Carancho (aeroquad), Jordi Muñoz, */ /* Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, */
/* Roberto Navoni, ... (Arcucopter team) */ /* Jani Hirvinen, Ken McEwans, Roberto Navoni, */
/* Date : 17-06-2010 */ /* Sandro Benigno, Chris Anderson */
/* Version : 1.1 beta */ /* Date : 04-07-2010 */
/* Version : 1.3 beta */
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ /* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
/* Mounting position : RC connectors pointing backwards */
/* This code use this libraries : */ /* This code use this libraries : */
/* APM_RC_QUAD : Radio library (adapted for quads) */ /* APM_RC : Radio library (with InstantPWM) */
/* APM_ADC : External ADC library */ /* APM_ADC : External ADC library */
/* DataFlash : DataFlash log library */ /* DataFlash : DataFlash log library */
/* APM_BMP085 : BMP085 barometer library */ /* APM_BMP085 : BMP085 barometer library */
@ -21,12 +24,16 @@
#include <Wire.h> #include <Wire.h>
#include <APM_ADC.h> #include <APM_ADC.h>
#include <APM_RC_QUAD.h> #include <APM_RC.h>
#include <DataFlash.h> #include <DataFlash.h>
#include <APM_Compass.h> #include <APM_Compass.h>
// Put your GPS library here: // Put your GPS library here:
#include <GPS_NMEA.h> // MTK GPS #include <GPS_NMEA.h> // MTK GPS
// EEPROM storage for user configurable values
#include <EEPROM.h>
#include "UserSettings.h"
/* APM Hardware definitions */ /* APM Hardware definitions */
#define LED_Yellow 36 #define LED_Yellow 36
#define LED_Red 35 #define LED_Red 35
@ -39,48 +46,6 @@
/* ***************************************************************************** */ /* ***************************************************************************** */
/* CONFIGURATION PART */ /* CONFIGURATION PART */
/* ***************************************************************************** */ /* ***************************************************************************** */
//Adjust this parameter for your lattitude
#define GEOG_CORRECTION_FACTOR 0.87 // cos(lattitude)
#define RADIO_TEST_MODE 0 // 0:Normal 1:Radio Test mode (to test radio channels)
#define MAGNETOMETER 1 // 0 : No magnetometer, 1: Magnetometer
// QuadCopter Attitude control PID GAINS
#define KP_QUAD_ROLL 1.8 // 1.5 //1.75
#define KD_QUAD_ROLL 0.48 //0.35 // 0.4 //Absolute max:0.85
#define KI_QUAD_ROLL 0.30 // 0.4 //0.45
#define KP_QUAD_PITCH 1.8
#define KD_QUAD_PITCH 0.48
#define KI_QUAD_PITCH 0.30 //0.4
#define KP_QUAD_YAW 3.6 // 3.8
#define KD_QUAD_YAW 1.2 // 1.3
#define KI_QUAD_YAW 0.15 // 0.15
#define KD_QUAD_COMMAND_PART 2.0 // for special KD implementation (in two parts). Higher values makes the quadcopter more responsive to user inputs
// Position control PID GAINS
#define KP_GPS_ROLL 0.012
#define KD_GPS_ROLL 0.005
#define KI_GPS_ROLL 0.004
#define KP_GPS_PITCH 0.012
#define KD_GPS_PITCH 0.005
#define KI_GPS_PITCH 0.004
#define GPS_MAX_ANGLE 10 // Maximun command roll and pitch angle from position control
// Altitude control PID GAINS
#define KP_ALTITUDE 0.8
#define KD_ALTITUDE 0.2
#define KI_ALTITUDE 0.2
// The IMU should be correctly adjusted : Gyro Gains and also initial IMU offsets:
// We have to take this values with the IMU flat (0º roll, 0º pitch)
#define acc_offset_x 2079
#define acc_offset_y 2050
#define acc_offset_z 2008 // We need to rotate the IMU exactly 90º to take this value
#define gyro_offset_roll 1659 //1650
#define gyro_offset_pitch 1618 //1690
#define gyro_offset_yaw 1673
// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step // ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
@ -100,11 +65,6 @@
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
#define Kp_ROLLPITCH 0.0032 //0.002 //0.003125 // Pitch&Roll Proportional Gain
#define Ki_ROLLPITCH 0.000001 //0.000005 //0.0000025 // Pitch&Roll Integrator Gain
#define Kp_YAW 1.5 // Yaw Porportional Gain
#define Ki_YAW 0.00005 //0.00005 // Yaw Integrator Gain
/*For debugging purposes*/ /*For debugging purposes*/
#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data #define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
@ -112,7 +72,7 @@
uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware
//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
int SENSOR_SIGN[]={-1,1,-1,1,-1,1,-1,-1,-1}; //{1,-1,-1,1,-1,1,-1,-1,-1} int SENSOR_SIGN[]={1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1};
int AN[6]; //array that store the 6 ADC channels int AN[6]; //array that store the 6 ADC channels
int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
@ -228,9 +188,25 @@ int ch_roll;
int ch_pitch; int ch_pitch;
int ch_throttle; int ch_throttle;
int ch_yaw; int ch_yaw;
int ch_aux;
int ch_aux2;
#define CHANN_CENTER 1500 #define CHANN_CENTER 1500
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun... #define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
// Motor variables
#define FLIGHT_MODE_+
//#define FLIGHT_MODE_X
int frontMotor;
int backMotor;
int leftMotor;
int rightMotor;
byte motorArmed = 0;
// Serial communication
#define CONFIGURATOR
char queryType;
long tlmTimer = 0;
/* ************************************************************ */ /* ************************************************************ */
/* Altitude control... (based on sonar) */ /* Altitude control... (based on sonar) */
void Altitude_control(int target_sonar_altitude) void Altitude_control(int target_sonar_altitude)
@ -303,6 +279,7 @@ void Attitude_control()
roll_D = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command) roll_D = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command)
// PID control // PID control
K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I; control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I;
// PITCH CONTROL // PITCH CONTROL
@ -319,6 +296,7 @@ void Attitude_control()
pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]); pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]);
// PID control // PID control
K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I; control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I;
// YAW CONTROL // YAW CONTROL
@ -339,6 +317,60 @@ void Attitude_control()
control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
} }
void Rate_control()
{
static float previousRollRate, previousPitchRate, previousYawRate;
float currentRollRate, currentPitchRate, currentYawRate;
// ROLL CONTROL
// NOTE: We need to test THIS !! Plaese CHECK
#ifdef FLIGHT_MODE_+
currentRollRate = read_adc(0); // I need a positive sign here
#endif
#ifdef FLIGHT_MODE_X
currentRollRate = -read_adc(0); // Original from Ted
#endif
err_roll = ((ch_roll-1500) * xmitFactor) - currentRollRate;
roll_I += err_roll*G_Dt;
roll_I = constrain(roll_I,-20,20);
roll_D = currentRollRate - previousRollRate;
previousRollRate = currentRollRate;
// PID control
control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I;
// PITCH CONTROL
currentPitchRate = read_adc(1);
err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate;
pitch_I += err_pitch*G_Dt;
pitch_I = constrain(pitch_I,-20,20);
pitch_D = currentPitchRate - previousPitchRate;
previousPitchRate = currentPitchRate;
// PID control
control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I;
// YAW CONTROL
currentYawRate = read_adc(2);
err_yaw = ((ch_yaw-1500)* xmitFactor) - currentYawRate;
yaw_I += err_yaw*G_Dt;
yaw_I = constrain(yaw_I,-20,20);
yaw_D = currentYawRate - previousYawRate;
previousYawRate = currentYawRate;
// PID control
K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain
control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I;
}
// Maximun slope filter for radio inputs... (limit max differences between readings) // Maximun slope filter for radio inputs... (limit max differences between readings)
int channel_filter(int ch, int ch_old) int channel_filter(int ch, int ch_old)
{ {
@ -379,27 +411,28 @@ void setup()
delay(250); delay(250);
APM_RC_QUAD.Init(); // APM Radio initialization APM_RC.Init(); // APM Radio initialization
APM_ADC.Init(); // APM ADC library initialization APM_ADC.Init(); // APM ADC library initialization
DataFlash.Init(); // DataFlash log initialization DataFlash.Init(); // DataFlash log initialization
GPS.Init(); // GPS Initialization GPS.Init(); // GPS Initialization
delay(100);
// RC channels Initialization (Quad motors)
APM_RC_QUAD.OutputCh(0,MIN_THROTTLE+15); // Motors stoped
APM_RC_QUAD.OutputCh(1,MIN_THROTTLE+15);
APM_RC_QUAD.OutputCh(2,MIN_THROTTLE+15);
APM_RC_QUAD.OutputCh(3,MIN_THROTTLE+15);
#if (MAGNETOMETER) readUserConfig(); // Load user configurable items from EEPROM
// RC channels Initialization (Quad motors)
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
APM_RC.OutputCh(1,MIN_THROTTLE);
APM_RC.OutputCh(2,MIN_THROTTLE);
APM_RC.OutputCh(3,MIN_THROTTLE);
if (MAGNETOMETER == 1)
APM_Compass.Init(); // I2C initialization APM_Compass.Init(); // I2C initialization
#endif
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
Serial.begin(57600); //Serial.begin(57600);
Serial.println(); Serial.begin(115200);
Serial.println("ArduCopter Quadcopter v1.0"); //Serial.println();
//Serial.println("ArduCopter Quadcopter v1.0");
// Check if we enable the DataFlash log Read Mode (switch) // Check if we enable the DataFlash log Read Mode (switch)
// If we press switch 1 at startup we read the Dataflash eeprom // If we press switch 1 at startup we read the Dataflash eeprom
@ -410,7 +443,7 @@ void setup()
delay(30000); delay(30000);
} }
delay(3000); //delay(3000);
Read_adc_raw(); Read_adc_raw();
delay(20); delay(20);
@ -424,7 +457,7 @@ void setup()
aux_float[2] = gyro_offset_yaw; aux_float[2] = gyro_offset_yaw;
// Take the gyro offset values // Take the gyro offset values
for(i=0;i<250;i++) for(i=0;i<300;i++)
{ {
Read_adc_raw(); Read_adc_raw();
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset. for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
@ -439,34 +472,36 @@ void setup()
} }
for(int y=0; y<=2; y++) for(int y=0; y<=2; y++)
AN_OFFSET[y]=aux_float[y]; AN_OFFSET[y]=aux_float[y];
Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
#ifndef CONFIGURATOR
for(i=0;i<6;i++) for(i=0;i<6;i++)
{ {
Serial.print("AN[]:"); Serial.print("AN[]:");
Serial.println(AN_OFFSET[i]); Serial.println(AN_OFFSET[i]);
} }
Neutro_yaw = APM_RC_QUAD.InputCh(3); // Take yaw neutral radio value
Serial.print("Yaw neutral value:"); Serial.print("Yaw neutral value:");
Serial.println(Neutro_yaw); Serial.println(Neutro_yaw);
#endif
#if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS #if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS
while(1) while(1)
{ {
if (APM_RC_QUAD.GetState()==1) if (APM_RC.GetState()==1)
{ {
Serial.print("AIL:"); Serial.print("AIL:");
Serial.print(APM_RC_QUAD.InputCh(0)); Serial.print(APM_RC.InputCh(0));
Serial.print("ELE:"); Serial.print("ELE:");
Serial.print(APM_RC_QUAD.InputCh(1)); Serial.print(APM_RC.InputCh(1));
Serial.print("THR:"); Serial.print("THR:");
Serial.print(APM_RC_QUAD.InputCh(2)); Serial.print(APM_RC.InputCh(2));
Serial.print("YAW:"); Serial.print("YAW:");
Serial.print(APM_RC_QUAD.InputCh(3)); Serial.print(APM_RC.InputCh(3));
Serial.print("AUX(mode):"); Serial.print("AUX(mode):");
Serial.print(APM_RC_QUAD.InputCh(4)); Serial.print(APM_RC.InputCh(4));
Serial.print("AUX2:"); Serial.print("AUX2:");
Serial.print(APM_RC_QUAD.InputCh(5)); Serial.print(APM_RC.InputCh(5));
Serial.println(); Serial.println();
delay(200); delay(200);
} }
@ -477,8 +512,10 @@ void setup()
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
timer = millis(); timer = millis();
tlmTimer = millis();
Read_adc_raw(); // Initialize ADC readings... Read_adc_raw(); // Initialize ADC readings...
delay(20); delay(20);
motorArmed = 0;
digitalWrite(LED_Green,HIGH); // Ready to go... digitalWrite(LED_Green,HIGH); // Ready to go...
} }
@ -494,7 +531,7 @@ void loop(){
int log_yaw; int log_yaw;
if((millis()-timer)>=14) // Main loop 70Hz if((millis()-timer)>=10) // Main loop 100Hz
{ {
counter++; counter++;
timer_old = timer; timer_old = timer;
@ -503,14 +540,14 @@ void loop(){
// IMU DCM Algorithm // IMU DCM Algorithm
Read_adc_raw(); Read_adc_raw();
#if (MAGNETOMETER) if (MAGNETOMETER == 1) {
if (counter > 8) // Read compass data at 10Hz... (7 loop runs) if (counter > 10) // Read compass data at 10Hz... (10 loop runs)
{ {
counter=0; counter=0;
APM_Compass.Read(); // Read magnetometer APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading APM_Compass.Calculate(roll,pitch); // Calculate heading
} }
#endif }
Matrix_update(); Matrix_update();
Normalize(); Normalize();
Drift_correction(); Drift_correction();
@ -521,34 +558,36 @@ void loop(){
log_roll = ToDeg(roll)*10; log_roll = ToDeg(roll)*10;
log_pitch = ToDeg(pitch)*10; log_pitch = ToDeg(pitch)*10;
log_yaw = ToDeg(yaw)*10; log_yaw = ToDeg(yaw)*10;
#ifndef CONFIGURATOR
Serial.print(log_roll); Serial.print(log_roll);
Serial.print(","); Serial.print(",");
Serial.print(log_pitch); Serial.print(log_pitch);
Serial.print(","); Serial.print(",");
Serial.print(log_yaw); Serial.print(log_yaw);
/*
for (int i=0;i<6;i++) for (int i=0;i<6;i++)
{ {
Serial.print(AN[i]); Serial.print(AN[i]);
Serial.print(","); Serial.print(",");
} }
*/ #endif
// Write Sensor raw data to DataFlash log // Write Sensor raw data to DataFlash log
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
// Write attitude to DataFlash log // Write attitude to DataFlash log
Log_Write_Attitude(log_roll,log_pitch,log_yaw); Log_Write_Attitude(log_roll,log_pitch,log_yaw);
if (APM_RC_QUAD.GetState()==1) // New radio frame? if (APM_RC.GetState()==1) // New radio frame?
{ {
// Commands from radio Rx... // Commands from radio Rx...
// Stick position defines the desired angle in roll, pitch and yaw // Stick position defines the desired angle in roll, pitch and yaw
ch_roll = channel_filter(APM_RC_QUAD.InputCh(0),ch_roll); ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll);
ch_pitch = channel_filter(APM_RC_QUAD.InputCh(1),ch_pitch); ch_pitch = channel_filter(APM_RC.InputCh(1),ch_pitch);
ch_throttle = channel_filter(APM_RC_QUAD.InputCh(2),ch_throttle); ch_throttle = channel_filter(APM_RC.InputCh(2),ch_throttle);
ch_yaw = channel_filter(APM_RC_QUAD.InputCh(3),ch_yaw); ch_yaw = channel_filter(APM_RC.InputCh(3),ch_yaw);
ch_aux = APM_RC.InputCh(4);
ch_aux2 = APM_RC.InputCh(5);
command_rx_roll_old = command_rx_roll; command_rx_roll_old = command_rx_roll;
command_rx_roll = (ch_roll-CHANN_CENTER)/12.0; command_rx_roll = (ch_roll-CHANN_CENTER)/12.0;
command_rx_roll_diff = command_rx_roll-command_rx_roll_old; command_rx_roll_diff = command_rx_roll-command_rx_roll_old;
@ -562,18 +601,20 @@ void loop(){
command_rx_yaw -= 360.0; command_rx_yaw -= 360.0;
else if (command_rx_yaw < -180) else if (command_rx_yaw < -180)
command_rx_yaw += 360.0; command_rx_yaw += 360.0;
// Read through comments in Attitude_control() if you wish to use transmitter to adjust P gains
// I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now] // I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now]
//K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2; //K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2;
K_aux = K_aux*0.8 + ((APM_RC_QUAD.InputCh(5)-1500)/300.0 + 1.7)*0.2; // /300 + 1.0 K_aux = K_aux*0.8 + ((ch_aux2-1500)/300.0 + 1.7)*0.2; // /300 + 1.0
if (K_aux < 0) if (K_aux < 0)
K_aux = 0; K_aux = 0;
//Serial.print(","); //Serial.print(",");
//Serial.print(K_aux); //Serial.print(K_aux);
// We read the Quad Mode from Channel 5 // We read the Quad Mode from Channel 5
if (APM_RC_QUAD.InputCh(4) < 1200) if (ch_aux < 1200)
{ {
AP_mode = 1; // Position hold mode (GPS position control) AP_mode = 1; // Position hold mode (GPS position control)
digitalWrite(LED_Yellow,HIGH); // Yellow LED On digitalWrite(LED_Yellow,HIGH); // Yellow LED On
@ -593,11 +634,13 @@ void loop(){
{ {
target_lattitude = GPS.Lattitude; target_lattitude = GPS.Lattitude;
target_longitude = GPS.Longitude; target_longitude = GPS.Longitude;
#ifndef CONFIGURATOR
Serial.println(); Serial.println();
Serial.print("* Target:"); Serial.print("* Target:");
Serial.print(target_longitude); Serial.print(target_longitude);
Serial.print(","); Serial.print(",");
Serial.println(target_lattitude); Serial.println(target_lattitude);
#endif
target_position=1; target_position=1;
//target_sonar_altitude = sonar_value; //target_sonar_altitude = sonar_value;
//Initial_Throttle = ch3; //Initial_Throttle = ch3;
@ -635,13 +678,6 @@ void loop(){
if ((target_position==1)&&(GPS.Fix)) if ((target_position==1)&&(GPS.Fix))
{ {
Position_control(target_lattitude,target_longitude); // Call position hold routine Position_control(target_lattitude,target_longitude); // Call position hold routine
/*
Serial.print("PC:");
Serial.print(command_gps_roll);
Serial.print(",");
Serial.print(command_gps_pitch);
Serial.println();
*/
} }
else else
{ {
@ -652,30 +688,83 @@ void loop(){
} }
} }
// Attitude control (Roll, Pitch, yaw) // Control methodology selected using AUX2
Attitude_control(); if (ch_aux2 < 1200)
Attitude_control();
else
{
Rate_control();
// Reset yaw, so if we change to stable mode we continue with the actual yaw direction
command_rx_yaw = ToDeg(yaw);
command_rx_yaw_diff = 0;
}
// Arm motor output
if (ch_throttle < 1100) {
control_yaw = 0;
if (ch_yaw > 1800)
motorArmed = 1;
if (ch_yaw < 1200)
motorArmed = 0;
}
// Quadcopter mix // Quadcopter mix
// Ask Jose if we still need this IF statement, and if we want to do an ESC calibration
if (ch_throttle > (MIN_THROTTLE+20)) // Minimun throttle to start control if (ch_throttle > (MIN_THROTTLE+20)) // Minimun throttle to start control
{ {
APM_RC_QUAD.OutputCh(0,ch_throttle - control_roll - control_yaw); // Right motor if (motorArmed == 1) {
APM_RC_QUAD.OutputCh(1,ch_throttle + control_roll - control_yaw); // Left motor #ifdef FLIGHT_MODE_+
APM_RC_QUAD.OutputCh(2,ch_throttle + control_pitch + control_yaw); // Front motor rightMotor = ch_throttle - control_roll - control_yaw;
APM_RC_QUAD.OutputCh(3,ch_throttle - control_pitch + control_yaw); // Back motor leftMotor = ch_throttle + control_roll - control_yaw;
frontMotor = ch_throttle + control_pitch + control_yaw;
backMotor = ch_throttle - control_pitch + control_yaw;
#endif
#ifdef FLIGHT_MODE_X
frontMotor = ch_throttle + control_roll - control_pitch - control_yaw; // front left motor
rightMotor = ch_throttle - control_roll - control_pitch + control_yaw; // front right motor
leftMotor = ch_throttle + control_roll + control_pitch + control_yaw; // rear left motor
backMotor = ch_throttle - control_roll + control_pitch - control_yaw; // rear right motor
#endif
}
if (motorArmed == 0) {
rightMotor = MIN_THROTTLE;
leftMotor = MIN_THROTTLE;
frontMotor = MIN_THROTTLE;
backMotor = MIN_THROTTLE;
}
APM_RC.OutputCh(0, rightMotor); // Right motor
APM_RC.OutputCh(1, leftMotor); // Left motor
APM_RC.OutputCh(2, frontMotor); // Front motor
APM_RC.OutputCh(3, backMotor); // Back motor
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
} }
else else
{ {
roll_I = 0; // reset I terms of PID controls roll_I = 0; // reset I terms of PID controls
pitch_I = 0; pitch_I = 0;
yaw_I = 0; yaw_I = 0;
APM_RC_QUAD.OutputCh(0,MIN_THROTTLE); // Motors stoped APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
APM_RC_QUAD.OutputCh(1,MIN_THROTTLE); APM_RC.OutputCh(1,MIN_THROTTLE);
APM_RC_QUAD.OutputCh(2,MIN_THROTTLE); APM_RC.OutputCh(2,MIN_THROTTLE);
APM_RC_QUAD.OutputCh(3,MIN_THROTTLE); APM_RC.OutputCh(3,MIN_THROTTLE);
// Initialize yaw command to actual yaw // InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
// Initialize yaw command to actual yaw when throttle is down...
command_rx_yaw = ToDeg(yaw); command_rx_yaw = ToDeg(yaw);
command_rx_yaw_diff = 0; command_rx_yaw_diff = 0;
} }
#ifndef CONFIGURATOR
Serial.println(); // Line END Serial.println(); // Line END
} #endif
}
#ifdef CONFIGURATOR
if((millis()-tlmTimer)>=100) {
readSerialCommand();
sendSerialTelemetry();
tlmTimer = millis();
}
#endif
} }

27
DCM.pde → Arducopter/DCM.pde
View File

@ -15,7 +15,7 @@ void Read_adc_raw(void)
} }
// Returns an analog value with the offset // Returns an analog value with the offset
float read_adc(int select) int read_adc(int select)
{ {
if (SENSOR_SIGN[select]<0) if (SENSOR_SIGN[select]<0)
return (AN_OFFSET[select]-AN[select]); return (AN_OFFSET[select]-AN[select]);
@ -65,10 +65,13 @@ void Drift_correction(void)
//*****Roll and Pitch*************** //*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector // Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); //Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. //Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0) // Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0)
Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1); // Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1);
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
//Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);
Accel_weight = 1.0;
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
@ -78,7 +81,7 @@ void Drift_correction(void)
//*****YAW*************** //*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading // We make the gyro YAW drift correction based on compass magnetic heading
#if (MAGNETOMETER) if (MAGNETOMETER == 1) {
errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
@ -87,7 +90,7 @@ void Drift_correction(void)
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
#endif }
} }
/**************************************************/ /**************************************************/
@ -104,14 +107,14 @@ void Matrix_update(void)
Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
//Accel_Vector[0]=read_adc(3); // acc x Accel_Vector[0]=read_adc(3); // acc x
//Accel_Vector[1]=read_adc(4); // acc y Accel_Vector[1]=read_adc(4); // acc y
//Accel_Vector[2]=read_adc(5); // acc z Accel_Vector[2]=read_adc(5); // acc z
// Low pass filter on accelerometer data (to filter vibrations) // Low pass filter on accelerometer data (to filter vibrations)
Accel_Vector[0]=Accel_Vector[0]*0.5 + (float)read_adc(3)*0.5; // acc x //Accel_Vector[0]=Accel_Vector[0]*0.5 + (float)read_adc(3)*0.5; // acc x
Accel_Vector[1]=Accel_Vector[1]*0.5 + (float)read_adc(4)*0.5; // acc y //Accel_Vector[1]=Accel_Vector[1]*0.5 + (float)read_adc(4)*0.5; // acc y
Accel_Vector[2]=Accel_Vector[2]*0.5 + (float)read_adc(5)*0.5; // acc z //Accel_Vector[2]=Accel_Vector[2]*0.5 + (float)read_adc(5)*0.5; // acc z
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional

0
Log.pde → Arducopter/Log.pde
View File

410
Arducopter/SerialCom.pde Normal file
View File

@ -0,0 +1,410 @@
/*
ArduCopter v1.2 - June 2010
www.ArduCopter.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
void readSerialCommand() {
// Check for serial message
if (Serial.available()) {
queryType = Serial.read();
switch (queryType) {
case 'A': // Stable PID
KP_QUAD_ROLL = readFloatSerial();
KI_QUAD_ROLL = readFloatSerial();
KD_QUAD_ROLL = readFloatSerial();
KP_QUAD_PITCH = readFloatSerial();
KI_QUAD_PITCH = readFloatSerial();
KD_QUAD_PITCH = readFloatSerial();
KP_QUAD_YAW = readFloatSerial();
KI_QUAD_YAW = readFloatSerial();
KD_QUAD_YAW = readFloatSerial();
KD_QUAD_COMMAND_PART = readFloatSerial();
MAGNETOMETER = readFloatSerial();
break;
case 'C': // Receive GPS PID
KP_GPS_ROLL = readFloatSerial();
KI_GPS_ROLL = readFloatSerial();
KD_GPS_ROLL = readFloatSerial();
KP_GPS_PITCH = readFloatSerial();
KI_GPS_PITCH = readFloatSerial();
KD_GPS_PITCH = readFloatSerial();
GPS_MAX_ANGLE = readFloatSerial();
GEOG_CORRECTION_FACTOR = readFloatSerial();
break;
case 'E': // Receive altitude PID
KP_ALTITUDE = readFloatSerial();
KD_ALTITUDE = readFloatSerial();
KI_ALTITUDE = readFloatSerial();
break;
case 'G': // Receive drift correction PID
Kp_ROLLPITCH = readFloatSerial();
Ki_ROLLPITCH = readFloatSerial();
Kp_YAW = readFloatSerial();
Ki_YAW = readFloatSerial();
break;
case 'I': // Receive sensor offset
gyro_offset_roll = readFloatSerial();
gyro_offset_pitch = readFloatSerial();
gyro_offset_yaw = readFloatSerial();
acc_offset_x = readFloatSerial();
acc_offset_y = readFloatSerial();
acc_offset_z = readFloatSerial();
break;
case 'K': // Spare
break;
case 'M': // Receive debug motor commands
frontMotor = readFloatSerial();
backMotor = readFloatSerial();
rightMotor = readFloatSerial();
leftMotor = readFloatSerial();
motorArmed = readFloatSerial();
break;
case 'O': // Rate Control PID
Kp_RateRoll = readFloatSerial();
Ki_RateRoll = readFloatSerial();
Kd_RateRoll = readFloatSerial();
Kp_RatePitch = readFloatSerial();
Ki_RatePitch = readFloatSerial();
Kd_RatePitch = readFloatSerial();
Kp_RateYaw = readFloatSerial();
Ki_RateYaw = readFloatSerial();
Kd_RateYaw = readFloatSerial();
xmitFactor = readFloatSerial();
break;
case 'W': // Write all user configurable values to EEPROM
writeEEPROM(KP_QUAD_ROLL, KP_QUAD_ROLL_ADR);
writeEEPROM(KD_QUAD_ROLL, KD_QUAD_ROLL_ADR);
writeEEPROM(KI_QUAD_ROLL, KI_QUAD_ROLL_ADR);
writeEEPROM(KP_QUAD_PITCH, KP_QUAD_PITCH_ADR);
writeEEPROM(KD_QUAD_PITCH, KD_QUAD_PITCH_ADR);
writeEEPROM(KI_QUAD_PITCH, KI_QUAD_PITCH_ADR);
writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR);
writeEEPROM(KD_QUAD_YAW, KD_QUAD_YAW_ADR);
writeEEPROM(KI_QUAD_YAW, KI_QUAD_YAW_ADR);
writeEEPROM(KD_QUAD_COMMAND_PART, KD_QUAD_COMMAND_PART_ADR);
writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR);
writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR);
writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR);
writeEEPROM(KP_GPS_PITCH, KP_GPS_PITCH_ADR);
writeEEPROM(KD_GPS_PITCH, KD_GPS_PITCH_ADR);
writeEEPROM(KI_GPS_PITCH, KI_GPS_PITCH_ADR);
writeEEPROM(GPS_MAX_ANGLE, GPS_MAX_ANGLE_ADR);
writeEEPROM(KP_ALTITUDE, KP_ALTITUDE_ADR);
writeEEPROM(KD_ALTITUDE, KD_ALTITUDE_ADR);
writeEEPROM(KI_ALTITUDE, KI_ALTITUDE_ADR);
writeEEPROM(acc_offset_x, acc_offset_x_ADR);
writeEEPROM(acc_offset_y, acc_offset_y_ADR);
writeEEPROM(acc_offset_z, acc_offset_z_ADR);
writeEEPROM(gyro_offset_roll, gyro_offset_roll_ADR);
writeEEPROM(gyro_offset_pitch, gyro_offset_pitch_ADR);
writeEEPROM(gyro_offset_yaw, gyro_offset_yaw_ADR);
writeEEPROM(Kp_ROLLPITCH, Kp_ROLLPITCH_ADR);
writeEEPROM(Ki_ROLLPITCH, Ki_ROLLPITCH_ADR);
writeEEPROM(Kp_YAW, Kp_YAW_ADR);
writeEEPROM(Ki_YAW, Ki_YAW_ADR);
writeEEPROM(GEOG_CORRECTION_FACTOR, GEOG_CORRECTION_FACTOR_ADR);
writeEEPROM(MAGNETOMETER, MAGNETOMETER_ADR);
writeEEPROM(Kp_RateRoll, KP_RATEROLL_ADR);
writeEEPROM(Ki_RateRoll, KI_RATEROLL_ADR);
writeEEPROM(Kd_RateRoll, KD_RATEROLL_ADR);
writeEEPROM(Kp_RatePitch, KP_RATEPITCH_ADR);
writeEEPROM(Ki_RatePitch, KI_RATEPITCH_ADR);
writeEEPROM(Kd_RatePitch, KD_RATEPITCH_ADR);
writeEEPROM(Kp_RateYaw, KP_RATEYAW_ADR);
writeEEPROM(Ki_RateYaw, KI_RATEYAW_ADR);
writeEEPROM(Kd_RateYaw, KD_RATEYAW_ADR);
writeEEPROM(xmitFactor, XMITFACTOR_ADR);
break;
case 'Y': // Initialize EEPROM with default values
KP_QUAD_ROLL = 1.8;
KD_QUAD_ROLL = 0.48;
KI_QUAD_ROLL = 0.40;
KP_QUAD_PITCH = 1.8;
KD_QUAD_PITCH = 0.48;
KI_QUAD_PITCH = 0.40;
KP_QUAD_YAW = 3.6;
KD_QUAD_YAW = 1.2;
KI_QUAD_YAW = 0.15;
KD_QUAD_COMMAND_PART = 4.0;
KP_GPS_ROLL = 0.012;
KD_GPS_ROLL = 0.005;
KI_GPS_ROLL = 0.004;
KP_GPS_PITCH = 0.012;
KD_GPS_PITCH = 0.005;
KI_GPS_PITCH = 0.004;
GPS_MAX_ANGLE = 10;
KP_ALTITUDE = 0.8;
KD_ALTITUDE = 0.2;
KI_ALTITUDE = 0.2;
acc_offset_x = 2073;
acc_offset_y = 2056;
acc_offset_z = 2010;
gyro_offset_roll = 1659;
gyro_offset_pitch = 1618;
gyro_offset_yaw = 1673;
Kp_ROLLPITCH = 0.002;
Ki_ROLLPITCH = 0.0000005;
Kp_YAW = 1.5;
Ki_YAW = 0.00005;
GEOG_CORRECTION_FACTOR = 0.87;
MAGNETOMETER = 0;
Kp_RateRoll = 0.6;
Ki_RateRoll = 0.1;
Kd_RateRoll = -0.8;
Kp_RatePitch = 0.6;
Ki_RatePitch = 0.1;
Kd_RatePitch = -0.8;
Kp_RateYaw = 1.6;
Ki_RateYaw = 0.3;
Kd_RateYaw = 0;
xmitFactor = 0.8;
break;
}
}
}
void sendSerialTelemetry() {
float aux_float[3]; // used for sensor calibration
switch (queryType) {
case '=': // Reserved debug command to view any variable from Serial Monitor
Serial.print("throttle =");Serial.println(ch_throttle);
Serial.print("control roll =");Serial.println(control_roll-CHANN_CENTER);
Serial.print("control pitch =");Serial.println(control_pitch-CHANN_CENTER);
Serial.print("control yaw =");Serial.println(control_yaw-CHANN_CENTER);
Serial.print("front left yaw =");Serial.println(frontMotor);
Serial.print("front right yaw =");Serial.println(rightMotor);
Serial.print("rear left yaw =");Serial.println(leftMotor);
Serial.print("rear right motor =");Serial.println(backMotor);Serial.println();
Serial.print("current roll rate =");Serial.println(read_adc(0));
Serial.print("current pitch rate =");Serial.println(read_adc(1));
Serial.print("current yaw rate =");Serial.println(read_adc(2));
Serial.print("command rx yaw =");Serial.println(command_rx_yaw);
Serial.println();
queryType = 'X';
break;
case 'B': // Send roll, pitch and yaw PID values
Serial.print(KP_QUAD_ROLL, 3);
comma();
Serial.print(KI_QUAD_ROLL, 3);
comma();
Serial.print(KD_QUAD_ROLL, 3);
comma();
Serial.print(KP_QUAD_PITCH, 3);
comma();
Serial.print(KI_QUAD_PITCH, 3);
comma();
Serial.print(KD_QUAD_PITCH, 3);
comma();
Serial.print(KP_QUAD_YAW, 3);
comma();
Serial.print(KI_QUAD_YAW, 3);
comma();
Serial.print(KD_QUAD_YAW, 3);
comma();
Serial.print(KD_QUAD_COMMAND_PART, 3);
comma();
Serial.println(MAGNETOMETER, 3);
queryType = 'X';
break;
case 'D': // Send GPS PID
Serial.print(KP_GPS_ROLL, 3);
comma();
Serial.print(KI_GPS_ROLL, 3);
comma();
Serial.print(KD_GPS_ROLL, 3);
comma();
Serial.print(KP_GPS_PITCH, 3);
comma();
Serial.print(KI_GPS_PITCH, 3);
comma();
Serial.print(KD_GPS_PITCH, 3);
comma();
Serial.print(GPS_MAX_ANGLE, 3);
comma();
Serial.println(GEOG_CORRECTION_FACTOR, 3);
queryType = 'X';
break;
case 'F': // Send altitude PID
Serial.print(KP_ALTITUDE, 3);
comma();
Serial.print(KI_ALTITUDE, 3);
comma();
Serial.println(KD_ALTITUDE, 3);
queryType = 'X';
break;
case 'H': // Send drift correction PID
Serial.print(Kp_ROLLPITCH, 4);
comma();
Serial.print(Ki_ROLLPITCH, 7);
comma();
Serial.print(Kp_YAW, 4);
comma();
Serial.println(Ki_YAW, 6);
queryType = 'X';
break;
case 'J': // Send sensor offset
Serial.print(gyro_offset_roll);
comma();
Serial.print(gyro_offset_pitch);
comma();
Serial.print(gyro_offset_yaw);
comma();
Serial.print(acc_offset_x);
comma();
Serial.print(acc_offset_y);
comma();
Serial.println(acc_offset_z);
AN_OFFSET[3] = acc_offset_x;
AN_OFFSET[4] = acc_offset_y;
AN_OFFSET[5] = acc_offset_z;
queryType = 'X';
break;
case 'L': // Spare
queryType = 'X';
break;
case 'N': // Send magnetometer config
queryType = 'X';
break;
case 'P': // Send rate control PID
Serial.print(Kp_RateRoll, 3);
comma();
Serial.print(Ki_RateRoll, 3);
comma();
Serial.print(Kd_RateRoll, 3);
comma();
Serial.print(Kp_RatePitch, 3);
comma();
Serial.print(Ki_RatePitch, 3);
comma();
Serial.print(Kd_RatePitch, 3);
comma();
Serial.print(Kp_RateYaw, 3);
comma();
Serial.print(Ki_RateYaw, 3);
comma();
Serial.print(Kd_RateYaw, 3);
comma();
Serial.println(xmitFactor, 3);
queryType = 'X';
break;
case 'Q': // Send sensor data
Serial.print(read_adc(0));
comma();
Serial.print(read_adc(1));
comma();
Serial.print(read_adc(2));
comma();
Serial.print(read_adc(4));
comma();
Serial.print(read_adc(3));
comma();
Serial.print(read_adc(5));
comma();
Serial.print(err_roll);
comma();
Serial.print(err_pitch);
comma();
Serial.print(degrees(-roll));
comma();
Serial.print(degrees(-pitch));
comma();
Serial.println(degrees(yaw));
break;
case 'R': // Send raw sensor data
break;
case 'S': // Send all flight data
Serial.print(timer-timer_old);
comma();
Serial.print(read_adc(0));
comma();
Serial.print(read_adc(1));
comma();
Serial.print(read_adc(2));
comma();
Serial.print(ch_throttle);
comma();
Serial.print(control_roll);
comma();
Serial.print(control_pitch);
comma();
Serial.print(control_yaw);
comma();
Serial.print(frontMotor); // Front Motor
comma();
Serial.print(backMotor); // Back Motor
comma();
Serial.print(rightMotor); // Right Motor
comma();
Serial.print(leftMotor); // Left Motor
comma();
Serial.print(read_adc(4));
comma();
Serial.print(read_adc(3));
comma();
Serial.println(read_adc(5));
break;
case 'T': // Spare
break;
case 'U': // Send receiver values
Serial.print(ch_roll); // Aileron
comma();
Serial.print(ch_pitch); // Elevator
comma();
Serial.print(ch_yaw); // Yaw
comma();
Serial.print(ch_throttle); // Throttle
comma();
Serial.print(ch_aux); // AUX1 (Mode)
comma();
Serial.println(ch_aux2); // AUX2
break;
case 'V': // Spare
break;
case 'X': // Stop sending messages
break;
case '!': // Send flight software version
Serial.println("1.2");
queryType = 'X';
break;
}
}
// Used to read floating point values from the serial port
float readFloatSerial() {
byte index = 0;
byte timeout = 0;
char data[128] = "";
do {
if (Serial.available() == 0) {
delay(10);
timeout++;
}
else {
data[index] = Serial.read();
timeout = 0;
index++;
}
} while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
return atof(data);
}
void comma() {
Serial.print(',');
}

177
Arducopter/UserSettings.h Normal file
View File

@ -0,0 +1,177 @@
/*
ArduCopter v1.2
www.ArduCopter.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "WProgram.h"
// Following variables stored in EEPROM
float KP_QUAD_ROLL;
float KD_QUAD_ROLL;
float KI_QUAD_ROLL;
float KP_QUAD_PITCH;
float KD_QUAD_PITCH;
float KI_QUAD_PITCH;
float KP_QUAD_YAW;
float KD_QUAD_YAW;
float KI_QUAD_YAW;
float KD_QUAD_COMMAND_PART;
float KP_GPS_ROLL;
float KD_GPS_ROLL;
float KI_GPS_ROLL;
float KP_GPS_PITCH;
float KD_GPS_PITCH;
float KI_GPS_PITCH;
float GPS_MAX_ANGLE;
float KP_ALTITUDE;
float KD_ALTITUDE;
float KI_ALTITUDE;
int acc_offset_x;
int acc_offset_y;
int acc_offset_z;
int gyro_offset_roll;
int gyro_offset_pitch;
int gyro_offset_yaw;
float Kp_ROLLPITCH;
float Ki_ROLLPITCH;
float Kp_YAW;
float Ki_YAW;
float GEOG_CORRECTION_FACTOR;
int MAGNETOMETER;
float Kp_RateRoll;
float Ki_RateRoll;
float Kd_RateRoll;
float Kp_RatePitch;
float Ki_RatePitch;
float Kd_RatePitch;
float Kp_RateYaw;
float Ki_RateYaw;
float Kd_RateYaw;
float xmitFactor;
// EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0
#define KD_QUAD_ROLL_ADR 4
#define KI_QUAD_ROLL_ADR 8
#define KP_QUAD_PITCH_ADR 12
#define KD_QUAD_PITCH_ADR 16
#define KI_QUAD_PITCH_ADR 20
#define KP_QUAD_YAW_ADR 24
#define KD_QUAD_YAW_ADR 28
#define KI_QUAD_YAW_ADR 32
#define KD_QUAD_COMMAND_PART_ADR 36
#define KP_GPS_ROLL_ADR 40
#define KD_GPS_ROLL_ADR 44
#define KI_GPS_ROLL_ADR 48
#define KP_GPS_PITCH_ADR 52
#define KD_GPS_PITCH_ADR 56
#define KI_GPS_PITCH_ADR 60
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KD_ALTITUDE_ADR 72
#define KI_ALTITUDE_ADR 76
#define acc_offset_x_ADR 80
#define acc_offset_y_ADR 84
#define acc_offset_z_ADR 88
#define gyro_offset_roll_ADR 92
#define gyro_offset_pitch_ADR 96
#define gyro_offset_yaw_ADR 100
#define Kp_ROLLPITCH_ADR 104
#define Ki_ROLLPITCH_ADR 108
#define Kp_YAW_ADR 112
#define Ki_YAW_ADR 116
#define GEOG_CORRECTION_FACTOR_ADR 120
#define MAGNETOMETER_ADR 124
#define XMITFACTOR_ADR 128
#define KP_RATEROLL_ADR 132
#define KI_RATEROLL_ADR 136
#define KD_RATEROLL_ADR 140
#define KP_RATEPITCH_ADR 144
#define KI_RATEPITCH_ADR 148
#define KD_RATEPITCH_ADR 152
#define KP_RATEYAW_ADR 156
#define KI_RATEYAW_ADR 160
#define KD_RATEYAW_ADR 164
// Utilities for writing and reading from the EEPROM
float readEEPROM(int address) {
union floatStore {
byte floatByte[4];
float floatVal;
} floatOut;
for (int i = 0; i < 4; i++)
floatOut.floatByte[i] = EEPROM.read(address + i);
return floatOut.floatVal;
}
void writeEEPROM(float value, int address) {
union floatStore {
byte floatByte[4];
float floatVal;
} floatIn;
floatIn.floatVal = value;
for (int i = 0; i < 4; i++)
EEPROM.write(address + i, floatIn.floatByte[i]);
}
void readUserConfig() {
KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
KD_QUAD_COMMAND_PART = readEEPROM(KD_QUAD_COMMAND_PART_ADR);
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR);
acc_offset_x = readEEPROM(acc_offset_x_ADR);
acc_offset_y = readEEPROM(acc_offset_y_ADR);
acc_offset_z = readEEPROM(acc_offset_z_ADR);
gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR);
gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR);
gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR);
Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR);
Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR);
Kp_YAW = readEEPROM(Kp_YAW_ADR);
Ki_YAW = readEEPROM(Ki_YAW_ADR);
GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR);
MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR);
Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR);
Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR);
Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR);
Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR);
Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR);
Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR);
Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR);
Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR);
Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR);
xmitFactor = readEEPROM(XMITFACTOR_ADR);
}

0
Vector.pde → Arducopter/Vector.pde
View File

4
Install.txt
View File

@ -3,9 +3,7 @@ Install notes
------------- -------------
To install the libraries: To install the libraries:
- copy Library Directories to your \arduino\hardware\lirbaries\ directory - copy Library Directories to your \arduino\hardware\libraries\ or arduino\libraries directory
- Restart arduino IDE - Restart arduino IDE
Each library comes with a simple example. You can find the examples in menu File->Examples Each library comes with a simple example. You can find the examples in menu File->Examples
This code works with Ardupilot Mega Hardware and Beta sensor shield (FOXTRAP2).

8
libraries/APM_Compass/APM_Compass.cpp
View File

@ -23,7 +23,8 @@
Read() : Read Sensor data Read() : Read Sensor data
To do : Calibration of the sensor, code optimization To do : Calibration of the sensor, code optimization
Mount position : Big capacitor pointing forward, connector backward Mount position : UPDATED
Big capacitor pointing backward, connector forward
*/ */
extern "C" { extern "C" {
@ -74,8 +75,8 @@ void APM_Compass_Class::Read()
if (i==6) // All bytes received? if (i==6) // All bytes received?
{ {
// MSB byte first, then LSB, X,Y,Z // MSB byte first, then LSB, X,Y,Z
Mag_X = ((((int)buff[0]) << 8) | buff[1]); // X axis Mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis
Mag_Y = -((((int)buff[2]) << 8) | buff[3]); // Y axis Mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis
Mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis Mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis
} }
} }
@ -105,6 +106,5 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
} }
// make one instance for the user to use // make one instance for the user to use
APM_Compass_Class APM_Compass; APM_Compass_Class APM_Compass;

20
libraries/APM_RC/APM_RC.cpp
View File

@ -163,5 +163,25 @@ unsigned char APM_RC_Class::GetState(void)
return(radio_status); return(radio_status);
} }
// InstantPWM implementation
// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use
void APM_RC_Class::Force_Out0_Out1(void)
{
if (TCNT5>5000) // We take care that there are not a pulse in the output
TCNT5=39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000
}
// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use
void APM_RC_Class::Force_Out2_Out3(void)
{
if (TCNT1>5000)
TCNT1=39990;
}
// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use
void APM_RC_Class::Force_Out6_Out7(void)
{
if (TCNT3>5000)
TCNT3=39990;
}
// make one instance for the user to use // make one instance for the user to use
APM_RC_Class APM_RC; APM_RC_Class APM_RC;

3
libraries/APM_RC/APM_RC.h
View File

@ -14,6 +14,9 @@ class APM_RC_Class
void OutputCh(unsigned char ch, int pwm); void OutputCh(unsigned char ch, int pwm);
int InputCh(unsigned char ch); int InputCh(unsigned char ch);
unsigned char GetState(); unsigned char GetState();
void Force_Out0_Out1(void);
void Force_Out2_Out3(void);
void Force_Out6_Out7(void);
}; };
extern APM_RC_Class APM_RC; extern APM_RC_Class APM_RC;

3
libraries/APM_RC/keywords.txt
View File

@ -3,3 +3,6 @@ begin KEYWORD2
InputCh KEYWORD2 InputCh KEYWORD2
OutputCh KEYWORD2 OutputCh KEYWORD2
GetState KEYWORD2 GetState KEYWORD2
Force_Out0_Out1 KEYWORD2
Force_Out2_Out3 KEYWORD2
Force_Out6_Out7 KEYWORD2