From 93bcda0518e212baca05496220b39c601ad98d57 Mon Sep 17 00:00:00 2001 From: jphelirc Date: Sat, 28 Aug 2010 16:07:33 +0000 Subject: [PATCH] New ArducopterNG architecture git-svn-id: https://arducopter.googlecode.com/svn/trunk@331 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- Arducopter/UserConfig.h | 27 +- ArducopterNG/ArduUser.h | 254 +++++++++++++++++ ArducopterNG/Arducopter.h | 391 ++++++++++++++++++++++++++ ArducopterNG/Arducopter.pde | 199 +++++++++++++ ArducopterNG/Attitude.pde | 213 ++++++++++++++ ArducopterNG/DCM.pde | 261 +++++++++++++++++ ArducopterNG/Debug.pde | 546 ++++++++++++++++++++++++++++++++++++ ArducopterNG/EEPROM.pde | 177 ++++++++++++ ArducopterNG/Events.pde | 36 +++ ArducopterNG/Functions.pde | 81 ++++++ ArducopterNG/GCS.pde | 440 +++++++++++++++++++++++++++++ ArducopterNG/Log.pde | 516 ++++++++++++++++++++++++++++++++++ ArducopterNG/Mixer.pde | 34 +++ ArducopterNG/Motors.pde | 34 +++ ArducopterNG/Navigation.pde | 34 +++ ArducopterNG/Radio.pde | 34 +++ ArducopterNG/Sensors.pde | 34 +++ ArducopterNG/System.pde | 203 ++++++++++++++ 18 files changed, 3506 insertions(+), 8 deletions(-) create mode 100644 ArducopterNG/ArduUser.h create mode 100644 ArducopterNG/Arducopter.h create mode 100644 ArducopterNG/Arducopter.pde create mode 100644 ArducopterNG/Attitude.pde create mode 100644 ArducopterNG/DCM.pde create mode 100644 ArducopterNG/Debug.pde create mode 100644 ArducopterNG/EEPROM.pde create mode 100644 ArducopterNG/Events.pde create mode 100644 ArducopterNG/Functions.pde create mode 100644 ArducopterNG/GCS.pde create mode 100644 ArducopterNG/Log.pde create mode 100644 ArducopterNG/Mixer.pde create mode 100644 ArducopterNG/Motors.pde create mode 100644 ArducopterNG/Navigation.pde create mode 100644 ArducopterNG/Radio.pde create mode 100644 ArducopterNG/Sensors.pde create mode 100644 ArducopterNG/System.pde diff --git a/Arducopter/UserConfig.h b/Arducopter/UserConfig.h index 1598a7ec08..5f46966e9e 100644 --- a/Arducopter/UserConfig.h +++ b/Arducopter/UserConfig.h @@ -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 . -*/ - -/* ************************************************************* + +* ************************************************************** * +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); -} +} diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h new file mode 100644 index 0000000000..4461033c7c --- /dev/null +++ b/ArducopterNG/ArduUser.h @@ -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 . + +* ************************************************************** * +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 + + diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h new file mode 100644 index 0000000000..20c8939699 --- /dev/null +++ b/ArducopterNG/Arducopter.h @@ -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 . + +* ************************************************************** * +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; + + diff --git a/ArducopterNG/Arducopter.pde b/ArducopterNG/Arducopter.pde new file mode 100644 index 0000000000..60b64ab60a --- /dev/null +++ b/ArducopterNG/Arducopter.pde @@ -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 . + +/* ********************************************************************** */ +/* 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 +#include +#include +#include +#include // ArduPilot Mega RC Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega BMP085 Library +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Magnetometer Library +//#include "defines.h" +//#include "console.h" +#include // I2C Communication library + +#ifdef UseBMP +#include // ArduPilot Mega BMP085 Library +#endif + +//#include // ArduPilot IMU/SIM GPS Library +#include // ArduPilot MTK GPS Library +//#include // ArduPilot Ublox GPS Library +//#include // ArduPilot NMEA GPS library + +// EEPROM storage for user configurable values +#include // 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(); + } + } + } +} + diff --git a/ArducopterNG/Attitude.pde b/ArducopterNG/Attitude.pde new file mode 100644 index 0000000000..ba437f479f --- /dev/null +++ b/ArducopterNG/Attitude.pde @@ -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 . + +* ************************************************************** * +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); +} diff --git a/ArducopterNG/DCM.pde b/ArducopterNG/DCM.pde new file mode 100644 index 0000000000..a8c28a9005 --- /dev/null +++ b/ArducopterNG/DCM.pde @@ -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 . + +* ************************************************************** * +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]; + } + } +} + + diff --git a/ArducopterNG/Debug.pde b/ArducopterNG/Debug.pde new file mode 100644 index 0000000000..2e2a6acdf1 --- /dev/null +++ b/ArducopterNG/Debug.pde @@ -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 . + +* ************************************************************** * +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 diff --git a/ArducopterNG/EEPROM.pde b/ArducopterNG/EEPROM.pde new file mode 100644 index 0000000000..ad67d566a7 --- /dev/null +++ b/ArducopterNG/EEPROM.pde @@ -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 . + +* ************************************************************** * +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); +} diff --git a/ArducopterNG/Events.pde b/ArducopterNG/Events.pde new file mode 100644 index 0000000000..ed1bec4389 --- /dev/null +++ b/ArducopterNG/Events.pde @@ -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 . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ + + diff --git a/ArducopterNG/Functions.pde b/ArducopterNG/Functions.pde new file mode 100644 index 0000000000..a0bf48b6d6 --- /dev/null +++ b/ArducopterNG/Functions.pde @@ -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 . + +* ************************************************************** * +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); + +} diff --git a/ArducopterNG/GCS.pde b/ArducopterNG/GCS.pde new file mode 100644 index 0000000000..7b457718b4 --- /dev/null +++ b/ArducopterNG/GCS.pde @@ -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 . + +* ************************************************************** * +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); +} diff --git a/ArducopterNG/Log.pde b/ArducopterNG/Log.pde new file mode 100644 index 0000000000..8c3073e322 --- /dev/null +++ b/ArducopterNG/Log.pde @@ -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 . + +* ************************************************************** * +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 to dump a log"); + Serial.println("Input 2 to erase all logs"); + Serial.println("Input 3 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); +} + + diff --git a/ArducopterNG/Mixer.pde b/ArducopterNG/Mixer.pde new file mode 100644 index 0000000000..f65ec8fd64 --- /dev/null +++ b/ArducopterNG/Mixer.pde @@ -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 . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ diff --git a/ArducopterNG/Motors.pde b/ArducopterNG/Motors.pde new file mode 100644 index 0000000000..8f6386fcd2 --- /dev/null +++ b/ArducopterNG/Motors.pde @@ -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 . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde new file mode 100644 index 0000000000..8ac6ccef15 --- /dev/null +++ b/ArducopterNG/Navigation.pde @@ -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 . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ diff --git a/ArducopterNG/Radio.pde b/ArducopterNG/Radio.pde new file mode 100644 index 0000000000..05c7b7c76d --- /dev/null +++ b/ArducopterNG/Radio.pde @@ -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 . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde new file mode 100644 index 0000000000..5a8bb468bf --- /dev/null +++ b/ArducopterNG/Sensors.pde @@ -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 . + +* ************************************************************** * +ChangeLog: + + +* ************************************************************** * +TODO: + + +* ************************************************************** */ diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde new file mode 100644 index 0000000000..be916cf01a --- /dev/null +++ b/ArducopterNG/System.pde @@ -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 . + +* ************************************************************** * +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(); +} + +