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

New ArducopterNG architecture 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@331 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-08-28 16:07:33 +00:00
parent bd2ba2fcc9
commit 93bcda0518
18 changed files with 3506 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
Arducopter/UserConfig.h
View File

@ -1,9 +1,15 @@
/* /*
ArduCopter v1.3 - August 2010 www.ArduCopter.com - www.DIYDrones.com
www.ArduCopter.com
Copyright (c) 2010. All rights reserved. Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter. An Open Source Arduino based multicopter.
File : UserConfig.h
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
This program is free software: you can redistribute it and/or modify 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 it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
@ -16,14 +22,19 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
* ************************************************************** *
/* ************************************************************* ChangeLog:
- 27-08-2010, New header layout
* ************************************************************** *
TODO: TODO:
- comment variables properly - List of thigs
- that still need to be done
************************************************************* */ * ************************************************************** */
@ -385,4 +396,4 @@ void writeUserConfig() {
writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR); writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR);
writeEEPROM(ch_aux_offset, ch_aux_offset_ADR); writeEEPROM(ch_aux_offset, ch_aux_offset_ADR);
writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR); writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR);
} }

254
ArducopterNG/ArduUser.h Normal file
View File

@ -0,0 +1,254 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : UserDefines.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
/*************************************************************/
// Safety & Security
// Arm & Disarm delays
#define ARM_DELAY 200 // milliseconds of how long you need to keep rudder to max right for arming motors
#define DISARM_DELAY 100 // milliseconds of how long you need to keep rudder to max left for disarming motors
/*************************************************************/
// AM Mode & Flight information
/* AM PIN Definitions */
/* Will be moved in future to AN extension ports */
/* due need to have PWM pins free for sonars and servos */
#define FR_LED 3 // Mega PE4 pin, OUT7
#define RE_LED 2 // Mega PE5 pin, OUT6
#define RI_LED 7 // Mega PH4 pin, OUT5
#define LE_LED 8 // Mega PH5 pin, OUT4
/* AM PIN Definitions - END */
/*************************************************************/
// Radio related definitions
// If you don't know these values, you can activate RADIO_TEST_MODE below
// and check your mid values
//#define RADIO_TEST_MODE
#define ROLL_MID 1500 // Radio Roll channel mid value
#define PITCH_MID 1500 // Radio Pitch channel mid value
#define YAW_MID 1500 // Radio Yaw channel mid value
#define THROTTLE_MID 1505 // Radio Throttle channel mid value
#define AUX_MID 1500
#define CHANN_CENTER 1500 // Channel center, legacy
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
// Following variables stored in EEPROM
float KP_QUAD_ROLL;
float KI_QUAD_ROLL;
float KD_QUAD_ROLL;
float KP_QUAD_PITCH;
float KI_QUAD_PITCH;
float KD_QUAD_PITCH;
float KP_QUAD_YAW;
float KI_QUAD_YAW;
float KD_QUAD_YAW;
float STABLE_MODE_KP_RATE;
float KP_GPS_ROLL;
float KI_GPS_ROLL;
float KD_GPS_ROLL;
float KP_GPS_PITCH;
float KI_GPS_PITCH;
float KD_GPS_PITCH;
float GPS_MAX_ANGLE;
float KP_ALTITUDE;
float KI_ALTITUDE;
float KD_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;
float ch_roll_slope = 1;
float ch_pitch_slope = 1;
float ch_throttle_slope = 1;
float ch_yaw_slope = 1;
float ch_aux_slope = 1;
float ch_aux2_slope = 1;
float ch_roll_offset = 0;
float ch_pitch_offset = 0;
float ch_throttle_offset = 0;
float ch_yaw_offset = 0;
float ch_aux_offset = 0;
float ch_aux2_offset = 0;
// This function call contains the default values that are set to the ArduCopter
// when a "Default EEPROM Value" command is sent through serial interface
void defaultUserConfig() {
KP_QUAD_ROLL = 1.8;
KI_QUAD_ROLL = 0.30; //0.4
KD_QUAD_ROLL = 0.4; //0.48
KP_QUAD_PITCH = 1.8;
KI_QUAD_PITCH = 0.30; //0.4
KD_QUAD_PITCH = 0.4; //0.48
KP_QUAD_YAW = 3.6;
KI_QUAD_YAW = 0.15;
KD_QUAD_YAW = 1.2;
STABLE_MODE_KP_RATE = 0.2; // New param for stable mode
KP_GPS_ROLL = 0.02;
KI_GPS_ROLL = 0.008;
KD_GPS_ROLL = 0.006;
KP_GPS_PITCH = 0.02;
KI_GPS_PITCH = 0.008;
KD_GPS_PITCH = 0.006;
GPS_MAX_ANGLE = 18;
KP_ALTITUDE = 0.8;
KI_ALTITUDE = 0.2;
KD_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.0014;
Ki_ROLLPITCH = 0.00000015;
Kp_YAW = 1.2;
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;
roll_mid = 1500;
pitch_mid = 1500;
yaw_mid = 1500;
ch_roll_slope = 1;
ch_pitch_slope = 1;
ch_throttle_slope = 1;
ch_yaw_slope = 1;
ch_aux_slope = 1;
ch_aux2_slope = 1;
ch_roll_offset = 0;
ch_pitch_offset = 0;
ch_throttle_offset = 0;
ch_yaw_offset = 0;
ch_aux_offset = 0;
ch_aux2_offset = 0;
}
// EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0
#define KI_QUAD_ROLL_ADR 8
#define KD_QUAD_ROLL_ADR 4
#define KP_QUAD_PITCH_ADR 12
#define KI_QUAD_PITCH_ADR 20
#define KD_QUAD_PITCH_ADR 16
#define KP_QUAD_YAW_ADR 24
#define KI_QUAD_YAW_ADR 32
#define KD_QUAD_YAW_ADR 28
#define STABLE_MODE_KP_RATE_ADR 36
#define KP_GPS_ROLL_ADR 40
#define KI_GPS_ROLL_ADR 48
#define KD_GPS_ROLL_ADR 44
#define KP_GPS_PITCH_ADR 52
#define KI_GPS_PITCH_ADR 60
#define KD_GPS_PITCH_ADR 56
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KI_ALTITUDE_ADR 76
#define KD_ALTITUDE_ADR 72
#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
#define CHROLL_MID 168
#define CHPITCH_MID 172
#define CHYAW_MID 176
#define ch_roll_slope_ADR 180
#define ch_pitch_slope_ADR 184
#define ch_throttle_slope_ADR 188
#define ch_yaw_slope_ADR 192
#define ch_aux_slope_ADR 196
#define ch_aux2_slope_ADR 200
#define ch_roll_offset_ADR 204
#define ch_pitch_offset_ADR 208
#define ch_throttle_offset_ADR 212
#define ch_yaw_offset_ADR 216
#define ch_aux_offset_ADR 220
#define ch_aux2_offset_ADR 224

391
ArducopterNG/Arducopter.h Normal file
View File

@ -0,0 +1,391 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Arducopter.h
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
#include "WProgram.h"
/* APM Hardware definitions */
#define LED_Yellow 36
#define LED_Red 35
#define LED_Green 37
#define RELE_pin 47
#define SW1_pin 41
#define SW2_pin 40
//#define VDIV1 AN1
//#define VDIV2 AN2
//#define VDIV3 AN3
//#define VDIV4 AN4
//#define AN5
//#define AN6
// Hardware Parameters
#define SLIDE_SWITCH_PIN 40
#define PUSHBUTTON_PIN 41
#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C
#define B_LED_PIN 36
#define C_LED_PIN 35
#define EE_LAST_LOG_PAGE 0xE00
#define EE_LAST_LOG_NUM 0xE02
#define EE_LOG_1_START 0xE04
// Serial ports
#define SERIAL0_BAUD 38400 // this is the main USB out 38400 57600 115200
#define SERIAL1_BAUD 115200
#define SERIAL2_BAUD 115200
#define SERIAL3_BAUD 115200
#ifdef Ser3
#define SerPr Serial3.print
#define SerPrln Serial3.println
#define SerRe Serial3.read
#define SerAv Serial3.available
#endif
#ifndef SerPr
#define Ser10
#endif
#ifdef Ser10
#define SerPr Serial.print
#define SerPrln Serial.println
#define SerRe Serial.read
#define SerAv Serial.available
#endif
/****************************************************/
/*Logging Stuff - These should be 1 (on) or 0 (off)*/
/****************************************************/
#define LOG_ATTITUDE 1 // Logs basic attitude info
#define LOG_GPS 1 // Logs GPS info
#define LOG_PM 1 // Logs IMU performance monitoring info£
#define LOG_CTUN 0 // Logs control loop tuning info
#define LOG_NTUN 0 // Logs navigation loop tuning info
#define LOG_MODE 1 // Logs mode changes
#define LOG_RAW 0 // Logs raw accel/gyro data
#define LOG_SEN 1 // Logs sensor data
// GCS Message ID's
#define MSG_ACKNOWLEDGE 0x00
#define MSG_HEARTBEAT 0x01
#define MSG_ATTITUDE 0x02
#define MSG_LOCATION 0x03
#define MSG_PRESSURE 0x04
#define MSG_STATUS_TEXT 0x05
#define MSG_PERF_REPORT 0x06
#define MSG_COMMAND 0x22
#define MSG_VALUE 0x32
#define MSG_PID 0x42
#define MSG_TRIMS 0x50
#define MSG_MINS 0x51
#define MSG_MAXS 0x52
#define MSG_IMU_OUT 0x53
#define SEVERITY_LOW 1
#define SEVERITY_MEDIUM 2
#define SEVERITY_HIGH 3
#define SEVERITY_CRITICAL 4
// Debug options - set only one of these options to 1 at a time, set the others to 0
#define DEBUG_SUBSYSTEM 0 // 0 = no debug
// 1 = Debug the Radio input
// 2 = Debug the Servo output
// 3 = Debug the Sensor input
// 4 = Debug the GPS input
// 5 = Debug the GPS input - RAW HEX OUTPUT
// 6 = Debug the IMU
// 7 = Debug the Control Switch
// 8 = Debug the Servo DIP switches
// 9 = Debug the Relay out
// 10 = Debug the Magnetometer
// 11 = Debug the ABS pressure sensor
// 12 = Debug the stored waypoints
// 13 = Debug the Throttle
// 14 = Debug the Radio Min Max
// 15 = Debug the EEPROM - Hex Dump
#define DEBUG_LEVEL SEVERITY_LOW
// SEVERITY_LOW
// SEVERITY_MEDIUM
// SEVERITY_HIGH
// SEVERITY_CRITICAL
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Hardware
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ
int SENSOR_SIGN[]={
1, -1, 1, -1, 1, 1, -1, -1, -1};
//{-1,1,-1,1,-1,1,-1,-1,-1};
/* APM Hardware definitions, END */
/* General definitions */
#define TRUE 1
#define FALSE 0
#define ON 1
#define OFF 0
// 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
// Tested value : 408
#define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
// Tested values :
#define Gyro_Gain_X 0.4 //X axis Gyro gain
#define Gyro_Gain_Y 0.41 //Y axis Gyro gain
#define Gyro_Gain_Z 0.41 //Z axis Gyro gain
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //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
/*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
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 gyro_temp;
float G_Dt=0.02; // Integration time for the gyros (DCM algorithm)
float Accel_Vector[3]= {0, 0, 0}; //Store the acceleration in a vector
float Accel_Vector_unfiltered[3]= {0, 0, 0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {0, 0, 0}; //Store the gyros rutn rate in a vector
float Omega_Vector[3]= {0, 0, 0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0, 0, 0}; //Omega Proportional correction
float Omega_I[3]= {0, 0, 0}; //Omega Integrator
float Omega[3]= {0, 0, 0};
//float Accel_magnitude;
//float Accel_weight;
float errorRollPitch[3] = {0, 0, 0};
float errorYaw[3] = {0, 0, 0};
float errorCourse = 0;
float COGX = 0; //Course overground X axis
float COGY = 1; //Course overground Y axis
float roll = 0;
float pitch = 0;
float yaw = 0;
unsigned int counter = 0;
float DCM_Matrix[3][3]= {
{ 1,0,0 },
{ 0,1,0 },
{ 0,0,1 }};
float Update_Matrix[3][3]={
{ 0,1,2 },
{ 3,4,5 },
{ 6,7,8 }}; //Gyros here
float Temporary_Matrix[3][3]={
{ 0,0,0 },
{ 0,0,0 },
{ 0,0,0 }};
// GPS variables
float speed_3d=0;
int GPS_ground_speed=0;
// Main timers
long timer=0;
long timer_old;
long GPS_timer;
long GPS_timer_old;
float GPS_Dt=0.2; // GPS Dt
// Attitude control variables
float command_rx_roll=0; // User commands
float command_rx_roll_old;
float command_rx_roll_diff;
float command_rx_pitch=0;
float command_rx_pitch_old;
float command_rx_pitch_diff;
float command_rx_yaw=0;
float command_rx_yaw_diff;
int control_roll; // PID control results
int control_pitch;
int control_yaw;
float K_aux;
// Attitude PID controls
float roll_I=0;
float roll_D;
float err_roll;
float pitch_I=0;
float pitch_D;
float err_pitch;
float yaw_I=0;
float yaw_D;
float err_yaw;
//Position control
#ifdef IsGPS
long target_longitude;
long target_lattitude;
byte target_position;
#endif
float gps_err_roll;
float gps_err_roll_old;
float gps_roll_D;
float gps_roll_I=0;
float gps_err_pitch;
float gps_err_pitch_old;
float gps_pitch_D;
float gps_pitch_I=0;
float command_gps_roll;
float command_gps_pitch;
//Altitude control
int Initial_Throttle;
int target_sonar_altitude;
int err_altitude;
int err_altitude_old;
float command_altitude;
float altitude_I;
float altitude_D;
//Pressure Sensor variables
#ifdef UseBMP
unsigned long abs_press = 0;
unsigned long abs_press_filt = 0;
unsigned long abs_press_gnd = 0;
int ground_temperature = 0; //
int temp_unfilt = 0;
long ground_alt = 0; // Ground altitude from gps at startup in centimeters
long press_alt = 0; // Pressure altitude
#endif
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define AIRSPEED_PIN 1 // Need to correct value
#define BATTERY_PIN 1 // Need to correct value
#define RELAY_PIN 47
#define LOW_VOLTAGE 11.4 // Pack voltage at which to trigger alarm
#define INPUT_VOLTAGE 5.2 // (Volts) voltage your power regulator is feeding your ArduPilot to have an accurate pressure and battery level readings. (you need a multimeter to measure and set this of course)
#define VOLT_DIV_RATIO 1.0 // Voltage divider ratio set with thru-hole resistor (see manual)
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage, initialized above threshold for filter
// Sonar variables
int Sonar_value=0;
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
int Sonar_Counter=0;
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
byte AP_mode = 2;
// Mode LED timers and variables, used to blink LED_Green
byte gled_status = HIGH;
long gled_timer;
int gled_speed;
long t0;
int num_iter;
float aux_debug;
// Radio definitions
int roll_mid;
int pitch_mid;
int yaw_mid;
int Neutro_yaw;
int ch_roll;
int ch_pitch;
int ch_throttle;
int ch_yaw;
int ch_aux;
int ch_aux2;
int frontMotor;
int backMotor;
int leftMotor;
int rightMotor;
byte motorArmed = 0;
int minThrottle = 0;
// Serial communication
char queryType;
long tlmTimer = 0;
// Arming/Disarming
uint8_t Arming_counter=0;
uint8_t Disarming_counter=0;
// Performance monitoring
// ----------------------
long perf_mon_timer = 0;
float imu_health = 0; //Metric based on accel gain deweighting
int G_Dt_max = 0; //Max main loop cycle time in milliseconds
byte gyro_sat_count = 0;
byte adc_constraints = 0;
byte renorm_sqrt_count = 0;
byte renorm_blowup_count = 0;
int gps_fix_count = 0;
byte gcs_messages_sent = 0;
// System Timers
// --------------
unsigned long fast_loopTimer = 0; // Time in miliseconds of main control loop
unsigned long medium_loopTimer = 0; // Time in miliseconds of navigation control loop
byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops
byte slow_loopCounter = 0; //
unsigned long deltaMiliSeconds = 0; // Delta Time in miliseconds
unsigned long dTnav = 0; // Delta Time in milliseconds for navigation computations
int mainLoop_count = 0;
unsigned long elapsedTime = 0; // for doing custom events
//unsigned int GPS_timer = 0;

199
ArducopterNG/Arducopter.pde Normal file
View File

@ -0,0 +1,199 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Arducopter.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
/* ********************************************************************** */
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
/* Mounting position : RC connectors pointing backwards */
/* This code use this libraries : */
/* APM_RC : Radio library (with InstantPWM) */
/* APM_ADC : External ADC library */
/* DataFlash : DataFlash log library */
/* APM_BMP085 : BMP085 barometer library */
/* APM_Compass : HMC5843 compass library [optional] */
/* GPS_UBLOX or GPS_NMEA or GPS_MTK : GPS library [optional] */
/* ********************************************************************** */
/* ********************************************************************** *
ChangeLog:
* *********************************************************************** *
TODO:
* *********************************************************************** */
/* ************************************************************ */
/* **************** MAIN PROGRAM - MODULES ******************** */
/* ************************************************************ */
/* User definable modules */
// Comment out with // modules that you are not using
//#define IsGPS // Do we have a GPS connected
//#define IsNEWMTEK // Do we have MTEK with new firmware
//#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
//#define IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port
//#define IsAM // Do we have motormount LED's. AM = Atraction Mode
// Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link
#define Ser0 // FTDI/USB Port Either one
//#define Ser3 // Telemetry port
//#define CONFIGURATOR // Do se use Configurator or normal text output over serial link
/**********************************************/
// Not in use yet, starting to work with battery monitors and pressure sensors.
// Added 19-08-2010
//#define UseAirspeed
//#define UseBMP
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!)
/**********************************************/
// Frame build condiguration
#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
/* ************************************************************ */
/* **************** MAIN PROGRAM - INCLUDES ******************* */
/* ************************************************************ */
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <APM_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <APM_Compass.h> // ArduPilot Mega Magnetometer Library
//#include "defines.h"
//#include "console.h"
#include <Wire.h> // I2C Communication library
#ifdef UseBMP
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#endif
//#include <GPS_IMU.h> // ArduPilot IMU/SIM GPS Library
#include <GPS_MTK.h> // ArduPilot MTK GPS Library
//#include <GPS_UBLOX.h> // ArduPilot Ublox GPS Library
//#include <GPS_NMEA.h> // ArduPilot NMEA GPS library
// EEPROM storage for user configurable values
#include <EEPROM.h> // EEPROM
#include "Arducopter.h"
#include "ArduUser.h"
/* Software version */
#define VER 1.40 // Current software version (only numeric values)
/* ************************************************************ */
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
/* ************************************************************ */
/* ************************************************************ */
/* **************** MAIN PROGRAM - SETUP ********************** */
/* ************************************************************ */
void setup() {
//APM_Init_IO();
APM_Init();
//APM_Init_ADC();
//APM_Init_Radio();
//APM_Init_Serial();
//APM_Init_xx
}
/* ************************************************************ */
/* ************** MAIN PROGRAM - MAIN LOOP ******************** */
/* ************************************************************ */
void loop() {
// We want this to execute at 500Hz if possible
// -------------------------------------------
if (millis()-fast_loopTimer > 5) {
deltaMiliSeconds = millis() - fast_loopTimer;
G_Dt = (float)deltaMiliSeconds / 1000.f;
fast_loopTimer = millis();
mainLoop_count++;
// Execute the fast loop
// ---------------------
// fast_loop();
// - PWM Updates
// - Stabilization
// - Altitude correction
// Execute the medium loop
// -----------------------
// medium_loop();
// - Radio read
// - GPS read
// - Drift correction
// Execute the slow loop
// -----------------------
// slow_loop();
// - Battery usage
// - GCS updates
// - Garbage management
if (millis()- perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
//send_message(MSG_PERF_REPORT);
#if LOG_PM
Log_Write_Performance();
#endif
resetPerfData();
}
}
}
}

213
ArducopterNG/Attitude.pde Normal file
View File

@ -0,0 +1,213 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Attitude.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
/* ************************************************************ */
//////////////////////////////////////////////////
// Function : Attitude_control_v2()
//
// Stable flight mode main algoritms
//
// Parameters:
// - none
//
// Returns : - none
//
// Alters :
// err_roll, roll_rate
//
// Relies :
// Radio input, Gyro
//
// STABLE MODE
// ROLL, PITCH and YAW PID controls...
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
void Attitude_control_v2()
{
float err_roll_rate;
float err_pitch_rate;
float roll_rate;
float pitch_rate;
// ROLL CONTROL
if (AP_mode == 2) // Normal Mode => Stabilization mode
err_roll = command_rx_roll - ToDeg(roll);
else
err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control
err_roll = constrain(err_roll, -25, 25); // to limit max roll command...
// New control term...
roll_rate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
err_roll_rate = ((ch_roll - roll_mid) >> 1) - roll_rate;
roll_I += err_roll * G_Dt;
roll_I = constrain(roll_I, -20, 20);
// D term implementation => two parts: gyro part and command part
// To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
// We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
roll_D = - roll_rate; // 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 + STABLE_MODE_KP_RATE * err_roll_rate;
// PITCH CONTROL
if (AP_mode==2) // Normal mode => Stabilization mode
err_pitch = command_rx_pitch - ToDeg(pitch);
else
err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control
err_pitch = constrain(err_pitch, -25, 25); // to limit max pitch command...
// New control term...
pitch_rate = ToDeg(Omega[1]);
err_pitch_rate = ((ch_pitch - pitch_mid) >> 1) - pitch_rate;
pitch_I += err_pitch * G_Dt;
pitch_I = constrain(pitch_I, -20, 20);
// D term
pitch_D = - pitch_rate;
// 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 + STABLE_MODE_KP_RATE * err_pitch_rate;
// YAW CONTROL
err_yaw = command_rx_yaw - ToDeg(yaw);
if (err_yaw > 180) // Normalize to -180,180
err_yaw -= 360;
else if(err_yaw < -180)
err_yaw += 360;
err_yaw = constrain(err_yaw, -60, 60); // to limit max yaw command...
yaw_I += err_yaw * G_Dt;
yaw_I = constrain(yaw_I, -20, 20);
yaw_D = - ToDeg(Omega[2]);
// PID control
control_yaw = KP_QUAD_YAW * err_yaw + KD_QUAD_YAW * yaw_D + KI_QUAD_YAW * yaw_I;
}
//////////////////////////////////////////////////
// Function : Rate_control()
//
// Acro mode main algoritms
//
// Parameters:
// - none
//
// Returns : - none
//
// Alters :
// err_roll, roll_rate
//
// Relies :
// Radio input, Gyro
//
// ACRO MODE
void Rate_control()
{
static float previousRollRate, previousPitchRate, previousYawRate;
float currentRollRate, currentPitchRate, currentYawRate;
// ROLL CONTROL
currentRollRate = read_adc(0); // I need a positive sign here
err_roll = ((ch_roll - roll_mid) * 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 - pitch_mid) * 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 - yaw_mid) * 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)
{
int diff_ch_old;
if (ch_old==0) // ch_old not initialized
return(ch);
diff_ch_old = ch - ch_old; // Difference with old reading
if (diff_ch_old < 0)
{
if (diff_ch_old <- 40)
return(ch_old - 40); // We limit the max difference between readings
}
else
{
if (diff_ch_old > 40)
return(ch_old + 40);
}
return((ch + ch_old) >> 1); // Small filtering
//return(ch);
}

261
ArducopterNG/DCM.pde Normal file
View File

@ -0,0 +1,261 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : DCM.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
/* ******* ADC functions ********************* */
// Read all the ADC channles
void Read_adc_raw(void)
{
int temp;
for (int i=0;i<6;i++)
AN[i] = APM_ADC.Ch(sensors[i]);
// Correction for non ratiometric sensor (test code)
gyro_temp = APM_ADC.Ch(3);
//AN[0] += 1500-temp;
//AN[1] += 1500-temp;
//AN[2] += 1500-temp;
}
// Returns an analog value with the offset
int read_adc(int select)
{
if (SENSOR_SIGN[select]<0)
return (AN_OFFSET[select]-AN[select]);
else
return (AN[select]-AN_OFFSET[select]);
}
/* ******************************************* */
/* ******* DCM IMU functions ********************* */
/**************************************************/
void Normalize(void)
{
float error=0;
float temporary[3][3];
float renorm=0;
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}
/**************************************************/
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
float errorCourse;
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
//*****Roll and Pitch***************
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
// errorRollPitch are in Accel ADC units
// Limit max errorRollPitch to limit max Omega_P and Omega_I
errorRollPitch[0] = constrain(errorRollPitch[0],-50,50);
errorRollPitch[1] = constrain(errorRollPitch[1],-50,50);
errorRollPitch[2] = constrain(errorRollPitch[2],-50,50);
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading
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.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
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
}
}
/**************************************************/
void Accel_adjust(void)
{
//Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
//Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
}
/**************************************************/
void Matrix_update(void)
{
Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
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
// Low pass filter on accelerometer data (to filter vibrations)
Accel_Vector[0]=Accel_Vector[0]*0.6 + (float)read_adc(3)*0.4; // acc x
Accel_Vector[1]=Accel_Vector[1]*0.6 + (float)read_adc(4)*0.4; // acc y
Accel_Vector[2]=Accel_Vector[2]*0.6 + (float)read_adc(5)*0.4; // 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
//Accel_adjust();//adjusting centrifugal acceleration. // Not used for quadcopter
#if OUTPUTMODE==1 // corrected mode
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#endif
#if OUTPUTMODE==0 // uncorrected data of the gyros (with drift)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
void Euler_angles(void)
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
pitch = -asin((Accel_Vector[0])/(float)GRAVITY); // asin(acc_x)
yaw = 0;
#else // Euler angles from DCM matrix
pitch = asin(-DCM_Matrix[2][0]);
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
#endif
}
// VECTOR FUNCTIONS
//Computes the dot product of two vectors
float Vector_Dot_Product(float vector1[3],float vector2[3])
{
float op=0;
for(int c=0; c<3; c++)
{
op+=vector1[c]*vector2[c];
}
return op;
}
//Computes the cross product of two vectors
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
{
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
}
//Multiply the vector by a scalar.
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
{
for(int c=0; c<3; c++)
{
vectorOut[c]=vectorIn[c]*scale2;
}
}
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
{
for(int c=0; c<3; c++)
{
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
}
}
/********* MATRIX FUNCTIONS *****************************************/
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
{
float op[3];
for(int x=0; x<3; x++)
{
for(int y=0; y<3; y++)
{
for(int w=0; w<3; w++)
{
op[w]=a[x][w]*b[w][y];
}
mat[x][y]=0;
mat[x][y]=op[0]+op[1]+op[2];
float test=mat[x][y];
}
}
}

546
ArducopterNG/Debug.pde Normal file
View File

@ -0,0 +1,546 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Debug.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
#if DEBUG_SUBSYSTEM == 1
void debug_subsystem()
{
Serial.println("Begin Debug: Radio Subsystem ");
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
Serial.print("Radio in ch1: ");
Serial.print(radio_in[CH_ROLL], DEC);
Serial.print("\tch2: ");
Serial.print(radio_in[CH_PITCH], DEC);
Serial.print("\tch3: ");
Serial.print(radio_in[CH_THROTTLE], DEC);
Serial.print("\tch4: ");
Serial.print(radio_in[CH_RUDDER], DEC);
Serial.print("\tch5: ");
Serial.print(radio_in[4], DEC);
Serial.print("\tch6: ");
Serial.print(radio_in[5], DEC);
Serial.print("\tch7: ");
Serial.print(radio_in[6], DEC);
Serial.print("\tch8: ");
Serial.println(radio_in[7], DEC);
}
}
#endif
#if DEBUG_SUBSYSTEM == 2
void debug_subsystem()
{
Serial.println("Begin Debug: Servo Subsystem ");
Serial.print("Reverse ROLL - CH1: ");
Serial.println(reverse_roll, DEC);
Serial.print("Reverse PITCH - CH2: ");
Serial.println(reverse_pitch, DEC);
Serial.print("Reverse THROTTLE - CH3: ");
Serial.println(REVERSE_THROTTLE, DEC);
Serial.print("Reverse RUDDER - CH4: ");
Serial.println(reverse_rudder, DEC);
// read the radio to set trims
// ---------------------------
trim_radio();
radio_max[0] = CH1_MAX;
radio_max[1] = CH2_MAX;
radio_max[2] = CH3_MAX;
radio_max[3] = CH4_MAX;
radio_max[4] = CH5_MAX;
radio_max[5] = CH6_MAX;
radio_max[6] = CH7_MAX;
radio_max[7] = CH8_MAX;
radio_min[0] = CH1_MIN;
radio_min[1] = CH2_MIN;
radio_min[2] = CH3_MIN;
radio_min[3] = CH4_MIN;
radio_min[4] = CH5_MIN;
radio_min[5] = CH6_MIN;
radio_min[6] = CH7_MIN;
radio_min[7] = CH8_MIN;
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
update_servo_switches();
servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45.0f * reverse_roll) / 500;
servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45.0f * reverse_roll) / 500;
servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45.0f * reverse_rudder) / 500;
// write out the servo PWM values
// ------------------------------
set_servos_4();
for(int y = 4; y < 8; y++) {
radio_out[y] = constrain(radio_in[y], radio_min[y], radio_max[y]);
APM_RC.OutputCh(y, radio_out[y]); // send to Servos
}
/*
Serial.print("Servo_out ch1: ");
Serial.print(servo_out[CH_ROLL], DEC);
Serial.print("\tch2: ");
Serial.print(servo_out[CH_PITCH], DEC);
Serial.print("\tch3: ");
Serial.print(servo_out[CH_THROTTLE], DEC);
Serial.print("\tch4: ");
Serial.print(servo_out[CH_RUDDER], DEC);
*/
///*
Serial.print("ch1: ");
Serial.print(radio_out[CH_ROLL], DEC);
Serial.print("\tch2: ");
Serial.print(radio_out[CH_PITCH], DEC);
Serial.print("\tch3: ");
Serial.print(radio_out[CH_THROTTLE], DEC);
Serial.print("\tch4: ");
Serial.print(radio_out[CH_RUDDER], DEC);
Serial.print("\tch5: ");
Serial.print(radio_out[4], DEC);
Serial.print("\tch6: ");
Serial.print(radio_out[5], DEC);
Serial.print("\tch7: ");
Serial.print(radio_out[6], DEC);
Serial.print("\tch8: ");
Serial.println(radio_out[7], DEC);
//*/
}
}
#endif
#if DEBUG_SUBSYSTEM == 3
void debug_subsystem()
{
Serial.println("Begin Debug: Analog Sensor Subsystem ");
Serial.print("AirSpeed sensor enabled: ");
Serial.println(AIRSPEED_SENSOR, DEC);
Serial.print("Enable Battery: ");
Serial.println(BATTERY_EVENT, DEC);
zero_airspeed();
Serial.print("Air pressure offset:");
Serial.println(airpressure_offset, DEC);
while(1){
delay(20);
read_z_sensor();
read_XY_sensors();
read_battery();
Serial.print("Analog IN:");
Serial.print(" 0:");
Serial.print(analog0, DEC);
Serial.print(", 1: ");
Serial.print(analog1, DEC);
Serial.print(", 2: ");
Serial.print(analog2, DEC);
Serial.print(", 3: ");
Serial.print(airpressure_raw, DEC);
Serial.print("\t\tSensors:");
Serial.print(" airSpeed:");
Serial.print(airspeed, DEC);
Serial.print("m \tbattV:");
Serial.print(battery_voltage, DEC);
Serial.println("volts ");
}
}
#endif
#if DEBUG_SUBSYSTEM == 4
void debug_subsystem()
{
Serial.println("Begin Debug: GPS Subsystem ");
while(1){
delay(333);
// Blink GPS LED if we don't have a fix
// ------------------------------------
//update_GPS_light();
GPS.Read();
if (GPS.NewData){
Serial.print("Lat:");
Serial.print(GPS.Lattitude, DEC);
Serial.print(" Lon:");
Serial.print(GPS.Longitude, DEC);
Serial.print(" Alt:");
Serial.print(GPS.Altitude / 100, DEC);
Serial.print("m, gs: ");
Serial.print(GPS.Ground_Speed / 100, DEC);
Serial.print(" COG:");
Serial.print(GPS.Ground_Course / 1000l);
Serial.print(" SAT:");
Serial.print(GPS.NumSats, DEC);
Serial.print(" FIX:");
Serial.print(GPS.Fix, DEC);
Serial.print(" TIM:");
Serial.print(GPS.Time);
Serial.println();
}
}
}
#endif
#if DEBUG_SUBSYSTEM == 5
void debug_subsystem()
{
Serial.println("Begin Debug: GPS Subsystem, RAW OUTPUT");
while(1){
if(Serial1.available() > 0) // Ok, let me see, the buffer is empty?
{
delay(60); // wait for it all
while(Serial1.available() > 0){
byte incoming = Serial1.read();
//Serial.print(incoming,DEC);
Serial.print(incoming, HEX); // will output Hex values
Serial.print(",");
}
Serial.println(" ");
}
}
}
#endif
#if DEBUG_SUBSYSTEM == 6
void debug_subsystem()
{
Serial.println("Begin Debug: IMU Subsystem ");
startup_IMU_ground();
while(1){
delay(20);
read_AHRS();
// We are using the IMU
// ---------------------
Serial.print(" roll:");
Serial.print(roll_sensor / 100, DEC);
Serial.print(" pitch:");
Serial.print(pitch_sensor / 100, DEC);
Serial.print(" yaw:");
Serial.println(yaw_sensor / 100, DEC);
}
}
#endif
#if DEBUG_SUBSYSTEM == 7
void debug_subsystem()
{
Serial.println("Begin Debug: Control Switch Test");
while(1){
delay(20);
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
switch(switchPosition)
{
case 1: // First position
Serial.println("Position 1");
break;
case 2: // middle position
Serial.println("Position 2");
break;
case 3: // last position
Serial.println("Position 3");
break;
case 4: // last position
Serial.println("Position 4");
break;
case 5: // last position
Serial.println("Position 5 - Software Manual");
break;
case 6: // last position
Serial.println("Position 6 - Hardware MUX, Manual");
break;
}
oldSwitchPosition = switchPosition;
}
}
}
#endif
#if DEBUG_SUBSYSTEM == 8
void debug_subsystem()
{
Serial.println("Begin Debug: Control Switch Test");
while(1){
delay(20);
update_servo_switches();
if (mix_mode == 0) {
Serial.print("Mix:standard ");
Serial.print("\t reverse_roll: ");
Serial.print(reverse_roll, DEC);
Serial.print("\t reverse_pitch: ");
Serial.print(reverse_pitch, DEC);
Serial.print("\t reverse_rudder: ");
Serial.println(reverse_rudder, DEC);
} else {
Serial.print("Mix:elevons ");
Serial.print("\t reverse_elevons: ");
Serial.print(reverse_elevons, DEC);
Serial.print("\t reverse_ch1_elevon: ");
Serial.print(reverse_ch1_elevon, DEC);
Serial.print("\t reverse_ch2_elevon: ");
Serial.println(reverse_ch2_elevon, DEC);
}
}
}
#endif
#if DEBUG_SUBSYSTEM == 9
void debug_subsystem()
{
Serial.println("Begin Debug: Relay");
pinMode(RELAY_PIN, OUTPUT);
while(1){
delay(3000);
Serial.println("Relay Position A");
PORTL |= B00000100;
delay(3000);
Serial.println("Relay Position B");
PORTL ^= B00000100;
}
}
#endif
#if DEBUG_SUBSYSTEM == 10
void debug_subsystem()
{
Serial.println("Begin Debug: Magnatometer");
while(1){
delay(3000);
}
}
#endif
#if DEBUG_SUBSYSTEM == 11
void debug_subsystem()
{
ground_alt = 0;
Serial.println("Begin Debug: Absolute Airpressure");
while(1){
delay(20);
read_airpressure();
Serial.print("Air Pressure Altitude: ");
Serial.print(press_alt / 100, DEC);
Serial.println("meters");
}
}
#endif
#if DEBUG_SUBSYSTEM == 12
void debug_subsystem()
{
ground_alt = 0;
Serial.println("Begin Debug: Display Waypoints");
delay(500);
eeprom_busy_wait();
uint8_t options = eeprom_read_byte((uint8_t *) EE_CONFIG);
eeprom_busy_wait();
int32_t hold = eeprom_read_dword((uint32_t *) EE_ALT_HOLD_HOME);
// save the alitude above home option
if(options & HOLD_ALT_ABOVE_HOME){
Serial.print("Hold this altitude over home: ");
Serial.print(hold / 100, DEC);
Serial.println(" meters");
}else{
Serial.println("Maintain current altitude ");
}
Serial.print("Number of Waypoints: ");
Serial.println(wp_total, DEC);
Serial.print("Hit radius for Waypoints: ");
Serial.println(wp_radius, DEC);
Serial.print("Loiter radius around Waypoints: ");
Serial.println(loiter_radius, DEC);
Serial.println(" ");
for(byte i = 0; i < wp_total; i++){
struct Location temp = get_wp_with_index(i);
print_waypoint(&temp, i);
}
while(1){
}
}
#endif
#if DEBUG_SUBSYSTEM == 13
void debug_subsystem()
{
Serial.println("Begin Debug: Throttle Subsystem ");
read_radio();
uint16_t low_pwm = radio_in[CH_THROTTLE];
uint16_t high_pwm = radio_in[CH_THROTTLE];
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
//update_throttle();
set_servos_4();
low_pwm = min(low_pwm, radio_in[CH_THROTTLE]);
high_pwm = max(high_pwm, radio_in[CH_THROTTLE]);
Serial.print("Radio_in: ");
Serial.print(radio_in[CH_THROTTLE], DEC);
Serial.print("\tPWM output: ");
Serial.print(radio_out[CH_THROTTLE], DEC);
Serial.print("\tThrottle: ");
Serial.print(servo_out[CH_THROTTLE], DEC);
Serial.print("\tPWM Min: ");
Serial.print(low_pwm, DEC);
Serial.print("\tPWM Max: ");
Serial.println(high_pwm, DEC);
}
}
#endif
#if DEBUG_SUBSYSTEM == 14
void debug_subsystem()
{
Serial.println("Begin Debug: Radio Min Max ");
uint16_t low_pwm[8];
uint16_t high_pwm[8];
uint8_t i;
for(i = 0; i < 100; i++){
delay(20);
read_radio();
}
for(i = 0; i < 8; i++){
radio_min[i] = 0;
radio_max[i] = 2400;
low_pwm[i] = radio_in[i];
high_pwm[i] = radio_in[i];
}
while(1){
delay(100);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
for(i = 0; i < 8; i++){
low_pwm[i] = min(low_pwm[i], radio_in[i]);
high_pwm[i] = max(high_pwm[i], radio_in[i]);
}
for(i = 0; i < 8; i++){
Serial.print("CH");
Serial.print(i + 1, DEC);
Serial.print(": ");
low_pwm[i] = min(low_pwm[i], radio_in[i]);
Serial.print(low_pwm[i], DEC);
Serial.print("|");
high_pwm[i] = max(high_pwm[i], radio_in[i]);
Serial.print(high_pwm[i], DEC);
Serial.print(" ");
}
Serial.println(" ");
}
}
#endif
#if DEBUG_SUBSYSTEM == 15
void debug_subsystem()
{
Serial.println("Begin Debug: EEPROM Dump ");
uint16_t temp;
for(int n = 0; n < 512; n++){
for(int i = 0; i < 4; i++){
temp = eeprom_read_word((uint16_t *) mem);
mem += 2;
Serial.print(temp, HEX);
Serial.print(" ");
}
Serial.print(" ");
for(int i = 0; i < 4; i++){
temp = eeprom_read_word((uint16_t *) mem);
mem += 2;
Serial.print(temp, HEX);
Serial.print(" ");
}
}
}
#endif

177
ArducopterNG/EEPROM.pde Normal file
View File

@ -0,0 +1,177 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : EEPROM.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
// 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);
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR);
KD_ALTITUDE = readEEPROM(KD_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);
roll_mid = readEEPROM(CHROLL_MID);
pitch_mid = readEEPROM(CHPITCH_MID);
yaw_mid = readEEPROM(CHYAW_MID);
ch_roll_slope = readEEPROM(ch_roll_slope_ADR);
ch_pitch_slope = readEEPROM(ch_pitch_slope_ADR);
ch_throttle_slope = readEEPROM(ch_throttle_slope_ADR);
ch_yaw_slope = readEEPROM(ch_yaw_slope_ADR);
ch_aux_slope = readEEPROM(ch_aux_slope_ADR);
ch_aux2_slope = readEEPROM(ch_aux2_slope_ADR);
ch_roll_offset = readEEPROM(ch_roll_offset_ADR);
ch_pitch_offset = readEEPROM(ch_pitch_offset_ADR);
ch_throttle_offset = readEEPROM(ch_throttle_offset_ADR);
ch_yaw_offset = readEEPROM(ch_yaw_offset_ADR);
ch_aux_offset = readEEPROM(ch_aux_offset_ADR);
ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR);
}
void writeUserConfig() {
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(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_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);
writeEEPROM(roll_mid, CHROLL_MID);
writeEEPROM(pitch_mid, CHPITCH_MID);
writeEEPROM(yaw_mid, CHYAW_MID);
writeEEPROM(ch_roll_slope, ch_roll_slope_ADR);
writeEEPROM(ch_pitch_slope, ch_pitch_slope_ADR);
writeEEPROM(ch_throttle_slope, ch_throttle_slope_ADR);
writeEEPROM(ch_yaw_slope, ch_yaw_slope_ADR);
writeEEPROM(ch_aux_slope, ch_aux_slope_ADR);
writeEEPROM(ch_aux2_slope, ch_aux2_slope_ADR);
writeEEPROM(ch_roll_offset, ch_roll_offset_ADR);
writeEEPROM(ch_pitch_offset, ch_pitch_offset_ADR);
writeEEPROM(ch_throttle_offset, ch_throttle_offset_ADR);
writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR);
writeEEPROM(ch_aux_offset, ch_aux_offset_ADR);
writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR);
}

36
ArducopterNG/Events.pde Normal file
View File

@ -0,0 +1,36 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Events.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */

81
ArducopterNG/Functions.pde Normal file
View File

@ -0,0 +1,81 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Functions.pde
Version : v1.0, Aug 28, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
// Flash those A,B,C LEDs on IMU Board
//
// Function: FullBlink(int, int);
// int 1 =
void FullBlink(int count, int blinkdelay) {
for(int i = 0; i <= count; i++) {
digitalWrite(LED_Green, HIGH);
digitalWrite(LED_Yellow, HIGH);
digitalWrite(LED_Red, HIGH);
delay(blinkdelay);
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
delay(blinkdelay);
}
}
void RunningLights(int LightStep) {
if(LightStep == 0) {
digitalWrite(LED_Green, HIGH);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
}
else if (LightStep == 1) {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, HIGH);
digitalWrite(LED_Red, LOW);
}
else {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, HIGH);
}
}
void LightsOff() {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
}

440
ArducopterNG/GCS.pde Normal file
View File

@ -0,0 +1,440 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : GCS.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
//
// Function : send_message()
//
// Parameters:
// byte severity - Debug level
// char str - Text to write
//
// Returns : - none
void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
{
if(severity >= DEBUG_LEVEL){
SerPr("MSG: ");
SerPrln(str);
}
}
//////////////////////////////////////////////////
// Function : readSerialCommand()
//
// Parameters:
// - none
//
// Returns : - none
//
void readSerialCommand() {
// Check for serial message
if (SerAv()) {
queryType = SerRe();
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();
STABLE_MODE_KP_RATE = 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
writeUserConfig();
break;
case 'Y': // Initialize EEPROM with default values
defaultUserConfig();
break;
case '1': // Receive transmitter calibration values
ch_roll_slope = readFloatSerial();
ch_roll_offset = readFloatSerial();
ch_pitch_slope = readFloatSerial();
ch_pitch_offset = readFloatSerial();
ch_yaw_slope = readFloatSerial();
ch_yaw_offset = readFloatSerial();
ch_throttle_slope = readFloatSerial();
ch_throttle_offset = readFloatSerial();
ch_aux_slope = readFloatSerial();
ch_aux_offset = readFloatSerial();
ch_aux2_slope = readFloatSerial();
ch_aux2_offset = readFloatSerial();
break;
}
}
}
void sendSerialTelemetry() {
float aux_float[3]; // used for sensor calibration
switch (queryType) {
case '=': // Reserved debug command to view any variable from Serial Monitor
/* SerPr("throttle =");
SerPrln(ch_throttle);
SerPr("control roll =");
SerPrln(control_roll-CHANN_CENTER);
SerPr("control pitch =");
SerPrln(control_pitch-CHANN_CENTER);
SerPr("control yaw =");
SerPrln(control_yaw-CHANN_CENTER);
SerPr("front left yaw =");
SerPrln(frontMotor);
SerPr("front right yaw =");
SerPrln(rightMotor);
SerPr("rear left yaw =");
SerPrln(leftMotor);
SerPr("rear right motor =");
SerPrln(backMotor);
SerPrln();
SerPr("current roll rate =");
SerPrln(read_adc(0));
SerPr("current pitch rate =");
SerPrln(read_adc(1));
SerPr("current yaw rate =");
SerPrln(read_adc(2));
SerPr("command rx yaw =");
SerPrln(command_rx_yaw);
SerPrln();
queryType = 'X';*/
SerPr(APM_RC.InputCh(0));
comma();
SerPr(ch_roll_slope);
comma();
SerPr(ch_roll_offset);
comma();
SerPrln(ch_roll);
break;
case 'B': // Send roll, pitch and yaw PID values
SerPr(KP_QUAD_ROLL, 3);
comma();
SerPr(KI_QUAD_ROLL, 3);
comma();
SerPr(KD_QUAD_ROLL, 3);
comma();
SerPr(KP_QUAD_PITCH, 3);
comma();
SerPr(KI_QUAD_PITCH, 3);
comma();
SerPr(KD_QUAD_PITCH, 3);
comma();
SerPr(KP_QUAD_YAW, 3);
comma();
SerPr(KI_QUAD_YAW, 3);
comma();
SerPr(KD_QUAD_YAW, 3);
comma();
SerPr(STABLE_MODE_KP_RATE, 3);
comma();
SerPrln(MAGNETOMETER, 3);
queryType = 'X';
break;
case 'D': // Send GPS PID
SerPr(KP_GPS_ROLL, 3);
comma();
SerPr(KI_GPS_ROLL, 3);
comma();
SerPr(KD_GPS_ROLL, 3);
comma();
SerPr(KP_GPS_PITCH, 3);
comma();
SerPr(KI_GPS_PITCH, 3);
comma();
SerPr(KD_GPS_PITCH, 3);
comma();
SerPr(GPS_MAX_ANGLE, 3);
comma();
SerPrln(GEOG_CORRECTION_FACTOR, 3);
queryType = 'X';
break;
case 'F': // Send altitude PID
SerPr(KP_ALTITUDE, 3);
comma();
SerPr(KI_ALTITUDE, 3);
comma();
SerPrln(KD_ALTITUDE, 3);
queryType = 'X';
break;
case 'H': // Send drift correction PID
SerPr(Kp_ROLLPITCH, 4);
comma();
SerPr(Ki_ROLLPITCH, 7);
comma();
SerPr(Kp_YAW, 4);
comma();
SerPrln(Ki_YAW, 6);
queryType = 'X';
break;
case 'J': // Send sensor offset
SerPr(gyro_offset_roll);
comma();
SerPr(gyro_offset_pitch);
comma();
SerPr(gyro_offset_yaw);
comma();
SerPr(acc_offset_x);
comma();
SerPr(acc_offset_y);
comma();
SerPrln(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
// RadioCalibration();
queryType = 'X';
break;
case 'N': // Send magnetometer config
queryType = 'X';
break;
case 'P': // Send rate control PID
SerPr(Kp_RateRoll, 3);
comma();
SerPr(Ki_RateRoll, 3);
comma();
SerPr(Kd_RateRoll, 3);
comma();
SerPr(Kp_RatePitch, 3);
comma();
SerPr(Ki_RatePitch, 3);
comma();
SerPr(Kd_RatePitch, 3);
comma();
SerPr(Kp_RateYaw, 3);
comma();
SerPr(Ki_RateYaw, 3);
comma();
SerPr(Kd_RateYaw, 3);
comma();
SerPrln(xmitFactor, 3);
queryType = 'X';
break;
case 'Q': // Send sensor data
SerPr(read_adc(0));
comma();
SerPr(read_adc(1));
comma();
SerPr(read_adc(2));
comma();
SerPr(read_adc(4));
comma();
SerPr(read_adc(3));
comma();
SerPr(read_adc(5));
comma();
SerPr(err_roll);
comma();
SerPr(err_pitch);
comma();
SerPr(degrees(roll));
comma();
SerPr(degrees(pitch));
comma();
SerPrln(degrees(yaw));
break;
case 'R': // Send raw sensor data
break;
case 'S': // Send all flight data
SerPr(timer-timer_old);
comma();
SerPr(read_adc(0));
comma();
SerPr(read_adc(1));
comma();
SerPr(read_adc(2));
comma();
SerPr(ch_throttle);
comma();
SerPr(control_roll);
comma();
SerPr(control_pitch);
comma();
SerPr(control_yaw);
comma();
SerPr(frontMotor); // Front Motor
comma();
SerPr(backMotor); // Back Motor
comma();
SerPr(rightMotor); // Right Motor
comma();
SerPr(leftMotor); // Left Motor
comma();
SerPr(read_adc(4));
comma();
SerPr(read_adc(3));
comma();
SerPrln(read_adc(5));
break;
case 'T': // Spare
break;
case 'U': // Send receiver values
SerPr(ch_roll); // Aileron
comma();
SerPr(ch_pitch); // Elevator
comma();
SerPr(ch_yaw); // Yaw
comma();
SerPr(ch_throttle); // Throttle
comma();
SerPr(ch_aux); // AUX1 (Mode)
comma();
SerPr(ch_aux2); // AUX2
comma();
SerPr(roll_mid); // Roll MID value
comma();
SerPr(pitch_mid); // Pitch MID value
comma();
SerPrln(yaw_mid); // Yaw MID Value
break;
case 'V': // Spare
break;
case 'X': // Stop sending messages
break;
case '!': // Send flight software version
SerPrln(VER);
queryType = 'X';
break;
case '2': // Send transmitter calibration values
SerPr(ch_roll_slope);
comma();
SerPr(ch_roll_offset);
comma();
SerPr(ch_pitch_slope);
comma();
SerPr(ch_pitch_offset);
comma();
SerPr(ch_yaw_slope);
comma();
SerPr(ch_yaw_offset);
comma();
SerPr(ch_throttle_slope);
comma();
SerPr(ch_throttle_offset);
comma();
SerPr(ch_aux_slope);
comma();
SerPr(ch_aux_offset);
comma();
SerPr(ch_aux2_slope);
comma();
SerPrln(ch_aux2_offset);
queryType = 'X';
break;
case '.': // Modify GPS settings, print directly to GPS Port
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
break;
}
}
void comma() {
SerPr(',');
}
// Used to read floating point values from the serial port
float readFloatSerial() {
byte index = 0;
byte timeout = 0;
char data[128] = "";
do {
if (SerAv() == 0) {
delay(10);
timeout++;
}
else {
data[index] = SerRe();
timeout = 0;
index++;
}
}
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
return atof(data);
}

516
ArducopterNG/Log.pde Normal file
View File

@ -0,0 +1,516 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Log.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs
#define HEAD_BYTE1 0xA3 // Decimal 163
#define HEAD_BYTE2 0x95 // Decimal 149
#define END_BYTE 0xBA // Decimal 186
#define LOG_ATTITUDE_MSG 0x01
#define LOG_GPS_MSG 0x02
#define LOG_MODE_MSG 0X03
#define LOG_CONTROL_TUNING_MSG 0X04
#define LOG_NAV_TUNING_MSG 0X05
#define LOG_PERFORMANCE_MSG 0X06
#define LOG_RAW_MSG 0x07
#define LOG_RADIO_MSG 0x08
#define LOG_SENSOR_MSG 0x09
// Function to display available logs and solicit action from the user.
// --------------------------------------------------------------------
void Print_Log_Menu(void)
{
int log_start;
int log_end;
eeprom_busy_wait();
byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); eeprom_busy_wait();
if (last_log_num == 0) {
Serial.println("No logs available for download");
} else {
Serial.print(last_log_num,DEC);
Serial.println(" logs available for download");
for(int i=1;i<last_log_num+1;i++) {
log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i-1)*0x02)); eeprom_busy_wait();
log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i)*0x02))-1; eeprom_busy_wait();
if (i == last_log_num) {
log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); eeprom_busy_wait();
}
Serial.print("Log number ");
Serial.print(i);
Serial.print(", start page ");
Serial.print(log_start);
Serial.print(", end page ");
Serial.println(log_end);
}
}
Serial.println(" ");
Serial.println("Input 1 <enter> to dump a log");
Serial.println("Input 2 <enter> to erase all logs");
Serial.println("Input 3 <enter> to leave log mode");
Serial.println(" ");
}
// Function to get desired action from the user.
// ---------------------------------------------
byte Get_User_Log_Command(void)
{
byte step=0;
byte char_1;
byte char_2;
byte char_3;
Serial.flush();
while(step<3)
{
if(Serial.available()){
char_1 = Serial.read();
char_2 = Serial.read();
char_3 = Serial.read();
if (char_1<0x30 || char_1>0x39) {
return 0;
} else if (char_2 == 0xFF) {
return (char_1 - 0x30);
} else if (char_2<0x30 || char_2>0x39) {
return 0;
} else if (char_2 != 0xFF) {
return 0;
} else {
return ((char_1 - 0x30)*10 + char_2 - 0x30);
}
} else {
delay(20);
}
}
}
// Write an attitude packet. Total length : 10 bytes
#if LOG_ATTITUDE
void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(log_roll);
DataFlash.WriteInt(log_pitch);
DataFlash.WriteInt(log_yaw);
DataFlash.WriteByte(END_BYTE);
}
#endif
#ifdef LOG_SEN
// Write a Sensor Raw data packet
void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_SENSOR_MSG);
DataFlash.WriteInt(s1);
DataFlash.WriteInt(s2);
DataFlash.WriteInt(s3);
DataFlash.WriteInt(s4);
DataFlash.WriteInt(s5);
DataFlash.WriteInt(s6);
DataFlash.WriteInt(s7);
DataFlash.WriteByte(END_BYTE);
}
#endif
#if LOG_PM
// Write a performance monitoring packet. Total length : 19 bytes
void Log_Write_Performance()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteLong(millis() - perf_mon_timer);
DataFlash.WriteInt(mainLoop_count);
DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(gyro_sat_count);
DataFlash.WriteByte(adc_constraints);
DataFlash.WriteByte(renorm_sqrt_count);
DataFlash.WriteByte(renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(imu_health*1000));
DataFlash.WriteByte(END_BYTE);
}
#endif
#if LOG_CTUN
// Write a control tuning packet. Total length : 22 bytes
void Log_Write_Control_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(servo_out[CH_ROLL]*100));
DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)roll_sensor);
DataFlash.WriteInt((int)(servo_out[CH_PITCH]*100));
DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)pitch_sensor);
DataFlash.WriteInt((int)(servo_out[CH_THROTTLE]*100));
DataFlash.WriteInt((int)(servo_out[CH_RUDDER]*100));
DataFlash.WriteInt((int)AN[5]);
DataFlash.WriteByte(END_BYTE);
}
#endif
#if LOG_NTUN
// Write a navigation tuning packet. Total length : 18 bytes
void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((int)yaw_sensor);
DataFlash.WriteInt((int)wp_distance);
DataFlash.WriteInt((int)target_bearing);
DataFlash.WriteInt((int)nav_bearing);
DataFlash.WriteInt(altitude_error);
DataFlash.WriteInt((int)airspeed_error);
DataFlash.WriteInt((int)(nav_gain_scaler*1000));
DataFlash.WriteByte(END_BYTE);
}
#endif
#if LOG_MODE
// Write a mode packet. Total length : 5 bytes
void Log_Write_Mode(byte mode)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_MODE_MSG);
DataFlash.WriteByte(mode);
DataFlash.WriteByte(END_BYTE);
}
#endif
#if LOG_GPS
// Write an GPS packet. Total length : 30 bytes
void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_mix_alt, long log_gps_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(log_Time);
DataFlash.WriteByte(log_Fix);
DataFlash.WriteByte(log_NumSats);
DataFlash.WriteLong(log_Lattitude);
DataFlash.WriteLong(log_Longitude);
DataFlash.WriteLong(log_mix_alt);
DataFlash.WriteLong(log_gps_alt);
DataFlash.WriteLong(log_Ground_Speed);
DataFlash.WriteLong(log_Ground_Course);
DataFlash.WriteByte(END_BYTE);
DataFlash.WriteByte(END_BYTE);
}
#endif
#if LOG_RAW
// Write an raw accel/gyro data packet. Total length : 28 bytes
void Log_Write_Raw()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
for(int i=0;i<6;i++)
DataFlash.WriteLong((long)(AN[i]*1000.0));
DataFlash.WriteByte(END_BYTE);
}
#endif
// Read an control tuning packet
void Log_Read_Control_Tuning()
{
float logvar;
Serial.print("CTUN:");
for (int y=1;y<10;y++) {
logvar = DataFlash.ReadInt();
if(y < 9) logvar = logvar/100.f;
Serial.print(logvar);
Serial.print(",");
}
Serial.println(" ");
}
// Read a nav tuning packet
void Log_Read_Nav_Tuning()
{
float logvar;
Serial.print("NTUN:");
for (int y=1;y<8;y++) {
logvar = DataFlash.ReadInt();
if(y > 6) logvar = logvar/1000.f;
Serial.print(logvar);
Serial.print(",");
}
Serial.println(" ");
}
// Read a performance packet
void Log_Read_Performance()
{
long pm_time;
int logvar;
Serial.print("PM:");
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
Serial.print(",");
for (int y=1;y<9;y++) {
if(y<3 || y>7) logvar = DataFlash.ReadInt();
else logvar = DataFlash.ReadByte();
if(y > 7) logvar = logvar/1000.f;
Serial.print(logvar);
Serial.print(",");
}
Serial.println(" ");
}
// Read an attitude packet
void Log_Read_Attitude()
{
int log_roll;
int log_pitch;
int log_yaw;
log_roll = DataFlash.ReadInt();
log_pitch = DataFlash.ReadInt();
log_yaw = DataFlash.ReadInt();
Serial.print("ATT:");
Serial.print(log_roll);
Serial.print(",");
Serial.print(log_pitch);
Serial.print(",");
Serial.print(log_yaw);
Serial.println();
}
// Read a mode packet
void Log_Read_Mode()
{
byte mode;
mode = DataFlash.ReadByte();
Serial.print("MOD:");
switch (mode) {
case 0:
Serial.println("Manual mode");
break;
case 1:
Serial.println("Stabilize mode");
break;
case 5:
Serial.println("Fly By Wire A mode");
break;
case 6:
Serial.println("Fly By Wire B mode");
break;
case 10:
Serial.println("AUTO mode");
break;
case 11:
Serial.println("RTL mode");
break;
case 12:
Serial.println("Loiter mode");
break;
case 98:
Serial.println("Air Start Complete");
break;
case 99:
Serial.println("Ground Start Complete");
break;
}
}
// Read a GPS packet
void Log_Read_GPS()
{
long log_Time;
byte log_Fix;
byte log_NumSats;
long log_Lattitude;
long log_Longitude;
long log_gps_alt;
long log_mix_alt;
float log_Ground_Speed;
float log_Ground_Course;
log_Time = DataFlash.ReadLong();
log_Fix = DataFlash.ReadByte();
log_NumSats = DataFlash.ReadByte();
log_Lattitude = DataFlash.ReadLong();
log_Longitude = DataFlash.ReadLong();
log_mix_alt = DataFlash.ReadLong();
log_gps_alt = DataFlash.ReadLong();
log_Ground_Speed = DataFlash.ReadLong();
log_Ground_Course = DataFlash.ReadLong();
Serial.print("GPS:");
Serial.print(log_Time);
Serial.print(",");
Serial.print((int)log_Fix);
Serial.print(",");
Serial.print((int)log_NumSats);
Serial.print(",");
Serial.print(log_Lattitude);
Serial.print(",");
Serial.print(log_Longitude);
Serial.print(",");
Serial.print(log_mix_alt/100.0);
Serial.print(",");
Serial.print(log_gps_alt/100.0);
Serial.print(",");
Serial.print(log_Ground_Speed/100.0f);
Serial.print(",");
Serial.print(log_Ground_Course/100.0); // This is incorrect!!!!!
Serial.println();
}
// Read a raw accel/gyro packet
void Log_Read_Raw()
{
float logvar;
Serial.print("RAW:");
for (int y=0;y<6;y++) {
logvar = DataFlash.ReadLong()/1000.f;
Serial.print(logvar);
Serial.print(",");
}
Serial.println(" ");
}
// Read the DataFlash log memory : Packet Parser
void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step=0;
int packet_count=0;
int flash_led = 1;
DataFlash.StartRead(start_page);
while (DataFlash.GetPage() < end_page)
{
data = DataFlash.ReadByte();
switch(log_step) //This is a state machine to read the packets
{
case 0:
if(data==HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data==HEAD_BYTE2) // Head byte 2
log_step++;
break;
case 2:
if(data==LOG_ATTITUDE_MSG){
Log_Read_Attitude();
log_step++;
}else if(data==LOG_MODE_MSG){
Log_Read_Mode();
log_step++;
}else if(data==LOG_CONTROL_TUNING_MSG){
Log_Read_Control_Tuning();
log_step++;
}else if(data==LOG_NAV_TUNING_MSG){
Log_Read_Nav_Tuning();
log_step++;
}else if(data==LOG_PERFORMANCE_MSG){
Log_Read_Performance();
log_step++;
}else if(data==LOG_RAW_MSG){
Log_Read_Raw();
log_step++;
}else {
if(data==LOG_GPS_MSG){
Log_Read_GPS();
log_step++;
} else {
Serial.print("Error Reading Packet: ");
Serial.print(packet_count);
log_step=0; // Restart, we have a problem...
}
}
break;
case 3:
if(data==END_BYTE){
packet_count++;
}else{
Serial.print("Error Reading END_BYTE ");
Serial.println(data,DEC);
}
log_step=0; // Restart sequence: new packet...
if(flash_led > 0) {
digitalWrite(A_LED_PIN,HIGH);
flash_led++;
if(flash_led>100) flash_led=-1;
} else {
digitalWrite(A_LED_PIN,LOW);
flash_led--;
if(flash_led<-100) flash_led=1;
}
break;
}
}
Serial.print("Number of packets read: ");
Serial.println(packet_count);
digitalWrite(A_LED_PIN,HIGH);
}

34
ArducopterNG/Mixer.pde Normal file
View File

@ -0,0 +1,34 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Mixer.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */

34
ArducopterNG/Motors.pde Normal file
View File

@ -0,0 +1,34 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Motors.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */

34
ArducopterNG/Navigation.pde Normal file
View File

@ -0,0 +1,34 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Navigation.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */

34
ArducopterNG/Radio.pde Normal file
View File

@ -0,0 +1,34 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Radio.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */

34
ArducopterNG/Sensors.pde Normal file
View File

@ -0,0 +1,34 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Sensors.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */

203
ArducopterNG/System.pde Normal file
View File

@ -0,0 +1,203 @@
/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : System.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
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/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
// General Initialization for all APM electronics
void APM_Init() {
int i, j; // Temporary variables used to count things
float aux_float[3];
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
pinMode(LED_Red,OUTPUT); //Red LED B (PC2)
pinMode(LED_Green,OUTPUT); //Green LED C (PC0)
pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0)
pinMode(RELE_pin,OUTPUT); // Rele output
digitalWrite(RELE_pin,LOW);
// delay(1000); // Wait until frame is not moving after initial power cord has connected
FullBlink(50,20);
APM_RC.Init(); // APM Radio initialization
APM_ADC.Init(); // APM ADC library initialization
DataFlash.Init(); // DataFlash log initialization
#ifdef IsGPS
GPS.Init(); // GPS Initialization
#ifdef IsNEWMTEK
delay(250);
// DIY Drones MTEK GPS needs binary sentences activated if you upgraded to latest firmware.
// If your GPS shows solid blue but LED C (Red) does not go on, your GPS is on NMEA mode
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
#endif
#endif
readUserConfig(); // Load user configurable items from EEPROM
// Safety measure for Channel mids
if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500;
if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
// 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
DataFlash.StartWrite(1); // Start a write session on page 1
Serial.begin(115200);
//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
while (digitalRead(SW1_pin)==0)
{
Serial.println("Entering Log Read Mode...");
Log_Read(1,1000);
delay(30000);
}
Read_adc_raw();
delay(20);
// Offset values for accels and gyros...
AN_OFFSET[3] = acc_offset_x;
AN_OFFSET[4] = acc_offset_y;
AN_OFFSET[5] = acc_offset_z;
aux_float[0] = gyro_offset_roll;
aux_float[1] = gyro_offset_pitch;
aux_float[2] = gyro_offset_yaw;
j = 0;
// Take the gyro offset values
for(i=0;i<300;i++)
{
Read_adc_raw();
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
{
aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
//Serial.print(AN[y]);
//Serial.print(",");
}
//Serial.println();
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
delay(10);
RunningLights(j);
// Runnings lights effect to let user know that we are taking mesurements
if((i % 5) == 0) j++;
if(j >= 3) j = 0;
}
// Switch off all ABC lights
LightsOff();
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]);
}
Serial.print("Yaw neutral value:");
// Serial.println(Neutro_yaw);
Serial.print(yaw_mid);
#endif
#ifdef RADIO_TEST_MODE // RADIO TEST MODE TO TEST RADIO CHANNELS
while(1)
{
if (APM_RC.GetState()==1)
{
Serial.print("AIL:");
Serial.print(APM_RC.InputCh(0));
Serial.print("ELE:");
Serial.print(APM_RC.InputCh(1));
Serial.print("THR:");
Serial.print(APM_RC.InputCh(2));
Serial.print("YAW:");
Serial.print(APM_RC.InputCh(3));
Serial.print("AUX(mode):");
Serial.print(APM_RC.InputCh(4));
Serial.print("AUX2:");
Serial.print(APM_RC.InputCh(5));
Serial.println();
delay(200);
}
}
#endif
delay(1000);
DataFlash.StartWrite(1); // Start a write session on page 1
timer = millis();
tlmTimer = millis();
Read_adc_raw(); // Initialize ADC readings...
delay(20);
#ifdef IsAM
// Switch Left & Right lights on
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
#endif
motorArmed = 0;
digitalWrite(LED_Green,HIGH); // Ready to go...
}
void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
gyro_sat_count = 0;
adc_constraints = 0;
renorm_sqrt_count = 0;
renorm_blowup_count = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
}