mirror of https://github.com/ArduPilot/ardupilot
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
parent
830fa2b104
commit
1a8856d0d5
|
|
@ -1,17 +1,20 @@
|
|||
/* ********************************************************************** */
|
||||
/* 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) */
|
||||
/* Ardupilot core code : from DIYDrones.com development team */
|
||||
/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */
|
||||
/* Authors : Jose Julio, Ted Carancho (aeroquad), Jordi Muñoz, */
|
||||
/* Roberto Navoni, ... (Arcucopter team) */
|
||||
/* Date : 17-06-2010 */
|
||||
/* Version : 1.1 beta */
|
||||
/* Authors : Arducopter development team */
|
||||
/* Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, */
|
||||
/* Jani Hirvinen, Ken McEwans, Roberto Navoni, */
|
||||
/* Sandro Benigno, Chris Anderson */
|
||||
/* Date : 04-07-2010 */
|
||||
/* Version : 1.3 beta */
|
||||
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
|
||||
/* Mounting position : RC connectors pointing backwards */
|
||||
/* This code use this libraries : */
|
||||
/* APM_RC_QUAD : Radio library (adapted for quads) */
|
||||
/* APM_RC : Radio library (with InstantPWM) */
|
||||
/* APM_ADC : External ADC library */
|
||||
/* DataFlash : DataFlash log library */
|
||||
/* APM_BMP085 : BMP085 barometer library */
|
||||
|
|
@ -21,12 +24,16 @@
|
|||
|
||||
#include <Wire.h>
|
||||
#include <APM_ADC.h>
|
||||
#include <APM_RC_QUAD.h>
|
||||
#include <APM_RC.h>
|
||||
#include <DataFlash.h>
|
||||
#include <APM_Compass.h>
|
||||
// Put your GPS library here:
|
||||
#include <GPS_NMEA.h> // MTK GPS
|
||||
|
||||
// EEPROM storage for user configurable values
|
||||
#include <EEPROM.h>
|
||||
#include "UserSettings.h"
|
||||
|
||||
/* APM Hardware definitions */
|
||||
#define LED_Yellow 36
|
||||
#define LED_Red 35
|
||||
|
|
@ -39,48 +46,6 @@
|
|||
/* ***************************************************************************** */
|
||||
/* 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
|
||||
// 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_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*/
|
||||
#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
|
||||
|
||||
//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_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
|
||||
|
|
@ -228,9 +188,25 @@ int ch_roll;
|
|||
int ch_pitch;
|
||||
int ch_throttle;
|
||||
int ch_yaw;
|
||||
int ch_aux;
|
||||
int ch_aux2;
|
||||
#define CHANN_CENTER 1500
|
||||
#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) */
|
||||
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)
|
||||
|
||||
// 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;
|
||||
|
||||
// PITCH CONTROL
|
||||
|
|
@ -319,6 +296,7 @@ void Attitude_control()
|
|||
pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]);
|
||||
|
||||
// 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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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)
|
||||
int channel_filter(int ch, int ch_old)
|
||||
{
|
||||
|
|
@ -379,27 +411,28 @@ void setup()
|
|||
|
||||
delay(250);
|
||||
|
||||
APM_RC_QUAD.Init(); // APM Radio initialization
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
APM_ADC.Init(); // APM ADC library initialization
|
||||
DataFlash.Init(); // DataFlash log 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
|
||||
#endif
|
||||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
|
||||
Serial.begin(57600);
|
||||
Serial.println();
|
||||
Serial.println("ArduCopter Quadcopter v1.0");
|
||||
//Serial.begin(57600);
|
||||
Serial.begin(115200);
|
||||
//Serial.println();
|
||||
//Serial.println("ArduCopter Quadcopter v1.0");
|
||||
|
||||
// Check if we enable the DataFlash log Read Mode (switch)
|
||||
// If we press switch 1 at startup we read the Dataflash eeprom
|
||||
|
|
@ -410,7 +443,7 @@ void setup()
|
|||
delay(30000);
|
||||
}
|
||||
|
||||
delay(3000);
|
||||
//delay(3000);
|
||||
|
||||
Read_adc_raw();
|
||||
delay(20);
|
||||
|
|
@ -424,7 +457,7 @@ void setup()
|
|||
aux_float[2] = gyro_offset_yaw;
|
||||
|
||||
// Take the gyro offset values
|
||||
for(i=0;i<250;i++)
|
||||
for(i=0;i<300;i++)
|
||||
{
|
||||
Read_adc_raw();
|
||||
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++)
|
||||
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++)
|
||||
{
|
||||
Serial.print("AN[]:");
|
||||
Serial.println(AN_OFFSET[i]);
|
||||
}
|
||||
|
||||
Neutro_yaw = APM_RC_QUAD.InputCh(3); // Take yaw neutral radio value
|
||||
Serial.print("Yaw neutral value:");
|
||||
Serial.println(Neutro_yaw);
|
||||
#endif
|
||||
|
||||
#if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS
|
||||
while(1)
|
||||
{
|
||||
if (APM_RC_QUAD.GetState()==1)
|
||||
if (APM_RC.GetState()==1)
|
||||
{
|
||||
Serial.print("AIL:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(0));
|
||||
Serial.print(APM_RC.InputCh(0));
|
||||
Serial.print("ELE:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(1));
|
||||
Serial.print(APM_RC.InputCh(1));
|
||||
Serial.print("THR:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(2));
|
||||
Serial.print(APM_RC.InputCh(2));
|
||||
Serial.print("YAW:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(3));
|
||||
Serial.print(APM_RC.InputCh(3));
|
||||
Serial.print("AUX(mode):");
|
||||
Serial.print(APM_RC_QUAD.InputCh(4));
|
||||
Serial.print(APM_RC.InputCh(4));
|
||||
Serial.print("AUX2:");
|
||||
Serial.print(APM_RC_QUAD.InputCh(5));
|
||||
Serial.print(APM_RC.InputCh(5));
|
||||
Serial.println();
|
||||
delay(200);
|
||||
}
|
||||
|
|
@ -477,8 +512,10 @@ void setup()
|
|||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
timer = millis();
|
||||
tlmTimer = millis();
|
||||
Read_adc_raw(); // Initialize ADC readings...
|
||||
delay(20);
|
||||
motorArmed = 0;
|
||||
digitalWrite(LED_Green,HIGH); // Ready to go...
|
||||
}
|
||||
|
||||
|
|
@ -494,7 +531,7 @@ void loop(){
|
|||
int log_yaw;
|
||||
|
||||
|
||||
if((millis()-timer)>=14) // Main loop 70Hz
|
||||
if((millis()-timer)>=10) // Main loop 100Hz
|
||||
{
|
||||
counter++;
|
||||
timer_old = timer;
|
||||
|
|
@ -503,14 +540,14 @@ void loop(){
|
|||
|
||||
// IMU DCM Algorithm
|
||||
Read_adc_raw();
|
||||
#if (MAGNETOMETER)
|
||||
if (counter > 8) // Read compass data at 10Hz... (7 loop runs)
|
||||
{
|
||||
counter=0;
|
||||
APM_Compass.Read(); // Read magnetometer
|
||||
APM_Compass.Calculate(roll,pitch); // Calculate heading
|
||||
}
|
||||
#endif
|
||||
if (MAGNETOMETER == 1) {
|
||||
if (counter > 10) // Read compass data at 10Hz... (10 loop runs)
|
||||
{
|
||||
counter=0;
|
||||
APM_Compass.Read(); // Read magnetometer
|
||||
APM_Compass.Calculate(roll,pitch); // Calculate heading
|
||||
}
|
||||
}
|
||||
Matrix_update();
|
||||
Normalize();
|
||||
Drift_correction();
|
||||
|
|
@ -521,34 +558,36 @@ void loop(){
|
|||
log_roll = ToDeg(roll)*10;
|
||||
log_pitch = ToDeg(pitch)*10;
|
||||
log_yaw = ToDeg(yaw)*10;
|
||||
|
||||
|
||||
#ifndef CONFIGURATOR
|
||||
Serial.print(log_roll);
|
||||
Serial.print(",");
|
||||
Serial.print(log_pitch);
|
||||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
|
||||
/*
|
||||
for (int i=0;i<6;i++)
|
||||
{
|
||||
Serial.print(AN[i]);
|
||||
Serial.print(",");
|
||||
}
|
||||
*/
|
||||
#endif
|
||||
|
||||
// Write Sensor raw data to DataFlash log
|
||||
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
|
||||
// Write attitude to DataFlash log
|
||||
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...
|
||||
// Stick position defines the desired angle in roll, pitch and yaw
|
||||
ch_roll = channel_filter(APM_RC_QUAD.InputCh(0),ch_roll);
|
||||
ch_pitch = channel_filter(APM_RC_QUAD.InputCh(1),ch_pitch);
|
||||
ch_throttle = channel_filter(APM_RC_QUAD.InputCh(2),ch_throttle);
|
||||
ch_yaw = channel_filter(APM_RC_QUAD.InputCh(3),ch_yaw);
|
||||
ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll);
|
||||
ch_pitch = channel_filter(APM_RC.InputCh(1),ch_pitch);
|
||||
ch_throttle = channel_filter(APM_RC.InputCh(2),ch_throttle);
|
||||
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 = (ch_roll-CHANN_CENTER)/12.0;
|
||||
command_rx_roll_diff = command_rx_roll-command_rx_roll_old;
|
||||
|
|
@ -562,18 +601,20 @@ void loop(){
|
|||
command_rx_yaw -= 360.0;
|
||||
else if (command_rx_yaw < -180)
|
||||
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]
|
||||
//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)
|
||||
K_aux = 0;
|
||||
|
||||
//Serial.print(",");
|
||||
//Serial.print(K_aux);
|
||||
|
||||
|
||||
// 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)
|
||||
digitalWrite(LED_Yellow,HIGH); // Yellow LED On
|
||||
|
|
@ -593,11 +634,13 @@ void loop(){
|
|||
{
|
||||
target_lattitude = GPS.Lattitude;
|
||||
target_longitude = GPS.Longitude;
|
||||
#ifndef CONFIGURATOR
|
||||
Serial.println();
|
||||
Serial.print("* Target:");
|
||||
Serial.print(target_longitude);
|
||||
Serial.print(",");
|
||||
Serial.println(target_lattitude);
|
||||
#endif
|
||||
target_position=1;
|
||||
//target_sonar_altitude = sonar_value;
|
||||
//Initial_Throttle = ch3;
|
||||
|
|
@ -635,13 +678,6 @@ void loop(){
|
|||
if ((target_position==1)&&(GPS.Fix))
|
||||
{
|
||||
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
|
||||
{
|
||||
|
|
@ -652,30 +688,83 @@ void loop(){
|
|||
}
|
||||
}
|
||||
|
||||
// Attitude control (Roll, Pitch, yaw)
|
||||
Attitude_control();
|
||||
// Control methodology selected using AUX2
|
||||
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
|
||||
// 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
|
||||
{
|
||||
APM_RC_QUAD.OutputCh(0,ch_throttle - control_roll - control_yaw); // Right motor
|
||||
APM_RC_QUAD.OutputCh(1,ch_throttle + control_roll - control_yaw); // Left motor
|
||||
APM_RC_QUAD.OutputCh(2,ch_throttle + control_pitch + control_yaw); // Front motor
|
||||
APM_RC_QUAD.OutputCh(3,ch_throttle - control_pitch + control_yaw); // Back motor
|
||||
if (motorArmed == 1) {
|
||||
#ifdef FLIGHT_MODE_+
|
||||
rightMotor = ch_throttle - control_roll - control_yaw;
|
||||
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
|
||||
{
|
||||
roll_I = 0; // reset I terms of PID controls
|
||||
pitch_I = 0;
|
||||
yaw_I = 0;
|
||||
APM_RC_QUAD.OutputCh(0,MIN_THROTTLE); // Motors stoped
|
||||
APM_RC_QUAD.OutputCh(1,MIN_THROTTLE);
|
||||
APM_RC_QUAD.OutputCh(2,MIN_THROTTLE);
|
||||
APM_RC_QUAD.OutputCh(3,MIN_THROTTLE);
|
||||
// Initialize yaw command to actual yaw
|
||||
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);
|
||||
// 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_diff = 0;
|
||||
}
|
||||
#ifndef CONFIGURATOR
|
||||
Serial.println(); // Line END
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#ifdef CONFIGURATOR
|
||||
if((millis()-tlmTimer)>=100) {
|
||||
readSerialCommand();
|
||||
sendSerialTelemetry();
|
||||
tlmTimer = millis();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
@ -15,7 +15,7 @@ void Read_adc_raw(void)
|
|||
}
|
||||
|
||||
// Returns an analog value with the offset
|
||||
float read_adc(int select)
|
||||
int read_adc(int select)
|
||||
{
|
||||
if (SENSOR_SIGN[select]<0)
|
||||
return (AN_OFFSET[select]-AN[select]);
|
||||
|
|
@ -65,10 +65,13 @@ void Drift_correction(void)
|
|||
//*****Roll and Pitch***************
|
||||
|
||||
// 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 = Accel_magnitude / GRAVITY; // Scale to gravity.
|
||||
//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.
|
||||
// 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_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
|
||||
|
|
@ -78,7 +81,7 @@ void Drift_correction(void)
|
|||
|
||||
//*****YAW***************
|
||||
// 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
|
||||
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_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[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
|
||||
|
||||
//Accel_Vector[0]=read_adc(3); // acc x
|
||||
//Accel_Vector[1]=read_adc(4); // acc y
|
||||
//Accel_Vector[2]=read_adc(5); // acc z
|
||||
Accel_Vector[0]=read_adc(3); // acc x
|
||||
Accel_Vector[1]=read_adc(4); // acc y
|
||||
Accel_Vector[2]=read_adc(5); // acc z
|
||||
|
||||
// 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[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[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[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_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional
|
||||
|
|
@ -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(',');
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -3,9 +3,7 @@ Install notes
|
|||
-------------
|
||||
|
||||
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
|
||||
|
||||
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).
|
||||
|
|
@ -23,7 +23,8 @@
|
|||
Read() : Read Sensor data
|
||||
|
||||
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" {
|
||||
|
|
@ -74,8 +75,8 @@ void APM_Compass_Class::Read()
|
|||
if (i==6) // All bytes received?
|
||||
{
|
||||
// MSB byte first, then LSB, X,Y,Z
|
||||
Mag_X = ((((int)buff[0]) << 8) | buff[1]); // X axis
|
||||
Mag_Y = -((((int)buff[2]) << 8) | buff[3]); // Y axis
|
||||
Mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis
|
||||
Mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y 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
|
||||
APM_Compass_Class APM_Compass;
|
||||
|
|
@ -163,5 +163,25 @@ unsigned char APM_RC_Class::GetState(void)
|
|||
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
|
||||
APM_RC_Class APM_RC;
|
||||
|
|
@ -14,6 +14,9 @@ class APM_RC_Class
|
|||
void OutputCh(unsigned char ch, int pwm);
|
||||
int InputCh(unsigned char ch);
|
||||
unsigned char GetState();
|
||||
void Force_Out0_Out1(void);
|
||||
void Force_Out2_Out3(void);
|
||||
void Force_Out6_Out7(void);
|
||||
};
|
||||
|
||||
extern APM_RC_Class APM_RC;
|
||||
|
|
|
|||
|
|
@ -3,3 +3,6 @@ begin KEYWORD2
|
|||
InputCh KEYWORD2
|
||||
OutputCh KEYWORD2
|
||||
GetState KEYWORD2
|
||||
Force_Out0_Out1 KEYWORD2
|
||||
Force_Out2_Out3 KEYWORD2
|
||||
Force_Out6_Out7 KEYWORD2
|
||||
|
|
|
|||
Loading…
Reference in New Issue