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.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
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
it under the terms of the GNU General Public License as published by
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
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* *************************************************************
* ************************************************************** *
ChangeLog:
- 27-08-2010, New header layout
* ************************************************************** *
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_aux_offset, ch_aux_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();
}