mirror of https://github.com/ArduPilot/ardupilot
New ArducopterNG architecture
git-svn-id: https://arducopter.googlecode.com/svn/trunk@331 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
bd2ba2fcc9
commit
93bcda0518
|
|
@ -1,9 +1,15 @@
|
|||
/*
|
||||
ArduCopter v1.3 - August 2010
|
||||
www.ArduCopter.com
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : UserConfig.h
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
|
|
@ -16,14 +22,19 @@
|
|||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* *************************************************************
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
- 27-08-2010, New header layout
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
- comment variables properly
|
||||
- List of thigs
|
||||
- that still need to be done
|
||||
|
||||
************************************************************* */
|
||||
* ************************************************************** */
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,254 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : UserDefines.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
|
||||
/*************************************************************/
|
||||
// Safety & Security
|
||||
|
||||
// Arm & Disarm delays
|
||||
#define ARM_DELAY 200 // milliseconds of how long you need to keep rudder to max right for arming motors
|
||||
#define DISARM_DELAY 100 // milliseconds of how long you need to keep rudder to max left for disarming motors
|
||||
|
||||
|
||||
/*************************************************************/
|
||||
// AM Mode & Flight information
|
||||
|
||||
/* AM PIN Definitions */
|
||||
/* Will be moved in future to AN extension ports */
|
||||
/* due need to have PWM pins free for sonars and servos */
|
||||
|
||||
#define FR_LED 3 // Mega PE4 pin, OUT7
|
||||
#define RE_LED 2 // Mega PE5 pin, OUT6
|
||||
#define RI_LED 7 // Mega PH4 pin, OUT5
|
||||
#define LE_LED 8 // Mega PH5 pin, OUT4
|
||||
|
||||
/* AM PIN Definitions - END */
|
||||
|
||||
|
||||
/*************************************************************/
|
||||
// Radio related definitions
|
||||
|
||||
// If you don't know these values, you can activate RADIO_TEST_MODE below
|
||||
// and check your mid values
|
||||
|
||||
//#define RADIO_TEST_MODE
|
||||
|
||||
#define ROLL_MID 1500 // Radio Roll channel mid value
|
||||
#define PITCH_MID 1500 // Radio Pitch channel mid value
|
||||
#define YAW_MID 1500 // Radio Yaw channel mid value
|
||||
#define THROTTLE_MID 1505 // Radio Throttle channel mid value
|
||||
#define AUX_MID 1500
|
||||
|
||||
#define CHANN_CENTER 1500 // Channel center, legacy
|
||||
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
|
||||
|
||||
// Following variables stored in EEPROM
|
||||
float KP_QUAD_ROLL;
|
||||
float KI_QUAD_ROLL;
|
||||
float KD_QUAD_ROLL;
|
||||
float KP_QUAD_PITCH;
|
||||
float KI_QUAD_PITCH;
|
||||
float KD_QUAD_PITCH;
|
||||
float KP_QUAD_YAW;
|
||||
float KI_QUAD_YAW;
|
||||
float KD_QUAD_YAW;
|
||||
float STABLE_MODE_KP_RATE;
|
||||
float KP_GPS_ROLL;
|
||||
float KI_GPS_ROLL;
|
||||
float KD_GPS_ROLL;
|
||||
float KP_GPS_PITCH;
|
||||
float KI_GPS_PITCH;
|
||||
float KD_GPS_PITCH;
|
||||
float GPS_MAX_ANGLE;
|
||||
float KP_ALTITUDE;
|
||||
float KI_ALTITUDE;
|
||||
float KD_ALTITUDE;
|
||||
int acc_offset_x;
|
||||
int acc_offset_y;
|
||||
int acc_offset_z;
|
||||
int gyro_offset_roll;
|
||||
int gyro_offset_pitch;
|
||||
int gyro_offset_yaw;
|
||||
float Kp_ROLLPITCH;
|
||||
float Ki_ROLLPITCH;
|
||||
float Kp_YAW;
|
||||
float Ki_YAW;
|
||||
float GEOG_CORRECTION_FACTOR;
|
||||
int MAGNETOMETER;
|
||||
float Kp_RateRoll;
|
||||
float Ki_RateRoll;
|
||||
float Kd_RateRoll;
|
||||
float Kp_RatePitch;
|
||||
float Ki_RatePitch;
|
||||
float Kd_RatePitch;
|
||||
float Kp_RateYaw;
|
||||
float Ki_RateYaw;
|
||||
float Kd_RateYaw;
|
||||
float xmitFactor;
|
||||
float ch_roll_slope = 1;
|
||||
float ch_pitch_slope = 1;
|
||||
float ch_throttle_slope = 1;
|
||||
float ch_yaw_slope = 1;
|
||||
float ch_aux_slope = 1;
|
||||
float ch_aux2_slope = 1;
|
||||
float ch_roll_offset = 0;
|
||||
float ch_pitch_offset = 0;
|
||||
float ch_throttle_offset = 0;
|
||||
float ch_yaw_offset = 0;
|
||||
float ch_aux_offset = 0;
|
||||
float ch_aux2_offset = 0;
|
||||
|
||||
// This function call contains the default values that are set to the ArduCopter
|
||||
// when a "Default EEPROM Value" command is sent through serial interface
|
||||
void defaultUserConfig() {
|
||||
KP_QUAD_ROLL = 1.8;
|
||||
KI_QUAD_ROLL = 0.30; //0.4
|
||||
KD_QUAD_ROLL = 0.4; //0.48
|
||||
KP_QUAD_PITCH = 1.8;
|
||||
KI_QUAD_PITCH = 0.30; //0.4
|
||||
KD_QUAD_PITCH = 0.4; //0.48
|
||||
KP_QUAD_YAW = 3.6;
|
||||
KI_QUAD_YAW = 0.15;
|
||||
KD_QUAD_YAW = 1.2;
|
||||
STABLE_MODE_KP_RATE = 0.2; // New param for stable mode
|
||||
KP_GPS_ROLL = 0.02;
|
||||
KI_GPS_ROLL = 0.008;
|
||||
KD_GPS_ROLL = 0.006;
|
||||
KP_GPS_PITCH = 0.02;
|
||||
KI_GPS_PITCH = 0.008;
|
||||
KD_GPS_PITCH = 0.006;
|
||||
GPS_MAX_ANGLE = 18;
|
||||
KP_ALTITUDE = 0.8;
|
||||
KI_ALTITUDE = 0.2;
|
||||
KD_ALTITUDE = 0.2;
|
||||
acc_offset_x = 2073;
|
||||
acc_offset_y = 2056;
|
||||
acc_offset_z = 2010;
|
||||
gyro_offset_roll = 1659;
|
||||
gyro_offset_pitch = 1618;
|
||||
gyro_offset_yaw = 1673;
|
||||
Kp_ROLLPITCH = 0.0014;
|
||||
Ki_ROLLPITCH = 0.00000015;
|
||||
Kp_YAW = 1.2;
|
||||
Ki_YAW = 0.00005;
|
||||
GEOG_CORRECTION_FACTOR = 0.87;
|
||||
MAGNETOMETER = 0;
|
||||
Kp_RateRoll = 0.6;
|
||||
Ki_RateRoll = 0.1;
|
||||
Kd_RateRoll = -0.8;
|
||||
Kp_RatePitch = 0.6;
|
||||
Ki_RatePitch = 0.1;
|
||||
Kd_RatePitch = -0.8;
|
||||
Kp_RateYaw = 1.6;
|
||||
Ki_RateYaw = 0.3;
|
||||
Kd_RateYaw = 0;
|
||||
xmitFactor = 0.8;
|
||||
roll_mid = 1500;
|
||||
pitch_mid = 1500;
|
||||
yaw_mid = 1500;
|
||||
ch_roll_slope = 1;
|
||||
ch_pitch_slope = 1;
|
||||
ch_throttle_slope = 1;
|
||||
ch_yaw_slope = 1;
|
||||
ch_aux_slope = 1;
|
||||
ch_aux2_slope = 1;
|
||||
ch_roll_offset = 0;
|
||||
ch_pitch_offset = 0;
|
||||
ch_throttle_offset = 0;
|
||||
ch_yaw_offset = 0;
|
||||
ch_aux_offset = 0;
|
||||
ch_aux2_offset = 0;
|
||||
}
|
||||
|
||||
// EEPROM storage addresses
|
||||
#define KP_QUAD_ROLL_ADR 0
|
||||
#define KI_QUAD_ROLL_ADR 8
|
||||
#define KD_QUAD_ROLL_ADR 4
|
||||
#define KP_QUAD_PITCH_ADR 12
|
||||
#define KI_QUAD_PITCH_ADR 20
|
||||
#define KD_QUAD_PITCH_ADR 16
|
||||
#define KP_QUAD_YAW_ADR 24
|
||||
#define KI_QUAD_YAW_ADR 32
|
||||
#define KD_QUAD_YAW_ADR 28
|
||||
#define STABLE_MODE_KP_RATE_ADR 36
|
||||
#define KP_GPS_ROLL_ADR 40
|
||||
#define KI_GPS_ROLL_ADR 48
|
||||
#define KD_GPS_ROLL_ADR 44
|
||||
#define KP_GPS_PITCH_ADR 52
|
||||
#define KI_GPS_PITCH_ADR 60
|
||||
#define KD_GPS_PITCH_ADR 56
|
||||
#define GPS_MAX_ANGLE_ADR 64
|
||||
#define KP_ALTITUDE_ADR 68
|
||||
#define KI_ALTITUDE_ADR 76
|
||||
#define KD_ALTITUDE_ADR 72
|
||||
#define acc_offset_x_ADR 80
|
||||
#define acc_offset_y_ADR 84
|
||||
#define acc_offset_z_ADR 88
|
||||
#define gyro_offset_roll_ADR 92
|
||||
#define gyro_offset_pitch_ADR 96
|
||||
#define gyro_offset_yaw_ADR 100
|
||||
#define Kp_ROLLPITCH_ADR 104
|
||||
#define Ki_ROLLPITCH_ADR 108
|
||||
#define Kp_YAW_ADR 112
|
||||
#define Ki_YAW_ADR 116
|
||||
#define GEOG_CORRECTION_FACTOR_ADR 120
|
||||
#define MAGNETOMETER_ADR 124
|
||||
#define XMITFACTOR_ADR 128
|
||||
#define KP_RATEROLL_ADR 132
|
||||
#define KI_RATEROLL_ADR 136
|
||||
#define KD_RATEROLL_ADR 140
|
||||
#define KP_RATEPITCH_ADR 144
|
||||
#define KI_RATEPITCH_ADR 148
|
||||
#define KD_RATEPITCH_ADR 152
|
||||
#define KP_RATEYAW_ADR 156
|
||||
#define KI_RATEYAW_ADR 160
|
||||
#define KD_RATEYAW_ADR 164
|
||||
#define CHROLL_MID 168
|
||||
#define CHPITCH_MID 172
|
||||
#define CHYAW_MID 176
|
||||
#define ch_roll_slope_ADR 180
|
||||
#define ch_pitch_slope_ADR 184
|
||||
#define ch_throttle_slope_ADR 188
|
||||
#define ch_yaw_slope_ADR 192
|
||||
#define ch_aux_slope_ADR 196
|
||||
#define ch_aux2_slope_ADR 200
|
||||
#define ch_roll_offset_ADR 204
|
||||
#define ch_pitch_offset_ADR 208
|
||||
#define ch_throttle_offset_ADR 212
|
||||
#define ch_yaw_offset_ADR 216
|
||||
#define ch_aux_offset_ADR 220
|
||||
#define ch_aux2_offset_ADR 224
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,391 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Arducopter.h
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
#include "WProgram.h"
|
||||
|
||||
|
||||
/* APM Hardware definitions */
|
||||
#define LED_Yellow 36
|
||||
#define LED_Red 35
|
||||
#define LED_Green 37
|
||||
#define RELE_pin 47
|
||||
#define SW1_pin 41
|
||||
#define SW2_pin 40
|
||||
|
||||
//#define VDIV1 AN1
|
||||
//#define VDIV2 AN2
|
||||
//#define VDIV3 AN3
|
||||
//#define VDIV4 AN4
|
||||
|
||||
//#define AN5
|
||||
//#define AN6
|
||||
|
||||
// Hardware Parameters
|
||||
#define SLIDE_SWITCH_PIN 40
|
||||
#define PUSHBUTTON_PIN 41
|
||||
|
||||
#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C
|
||||
#define B_LED_PIN 36
|
||||
#define C_LED_PIN 35
|
||||
|
||||
|
||||
#define EE_LAST_LOG_PAGE 0xE00
|
||||
#define EE_LAST_LOG_NUM 0xE02
|
||||
#define EE_LOG_1_START 0xE04
|
||||
|
||||
// Serial ports
|
||||
#define SERIAL0_BAUD 38400 // this is the main USB out 38400 57600 115200
|
||||
#define SERIAL1_BAUD 115200
|
||||
#define SERIAL2_BAUD 115200
|
||||
#define SERIAL3_BAUD 115200
|
||||
|
||||
|
||||
#ifdef Ser3
|
||||
#define SerPr Serial3.print
|
||||
#define SerPrln Serial3.println
|
||||
#define SerRe Serial3.read
|
||||
#define SerAv Serial3.available
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef SerPr
|
||||
#define Ser10
|
||||
#endif
|
||||
#ifdef Ser10
|
||||
#define SerPr Serial.print
|
||||
#define SerPrln Serial.println
|
||||
#define SerRe Serial.read
|
||||
#define SerAv Serial.available
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
/****************************************************/
|
||||
/*Logging Stuff - These should be 1 (on) or 0 (off)*/
|
||||
/****************************************************/
|
||||
#define LOG_ATTITUDE 1 // Logs basic attitude info
|
||||
#define LOG_GPS 1 // Logs GPS info
|
||||
#define LOG_PM 1 // Logs IMU performance monitoring info£
|
||||
#define LOG_CTUN 0 // Logs control loop tuning info
|
||||
#define LOG_NTUN 0 // Logs navigation loop tuning info
|
||||
#define LOG_MODE 1 // Logs mode changes
|
||||
#define LOG_RAW 0 // Logs raw accel/gyro data
|
||||
#define LOG_SEN 1 // Logs sensor data
|
||||
|
||||
// GCS Message ID's
|
||||
#define MSG_ACKNOWLEDGE 0x00
|
||||
#define MSG_HEARTBEAT 0x01
|
||||
#define MSG_ATTITUDE 0x02
|
||||
#define MSG_LOCATION 0x03
|
||||
#define MSG_PRESSURE 0x04
|
||||
#define MSG_STATUS_TEXT 0x05
|
||||
#define MSG_PERF_REPORT 0x06
|
||||
#define MSG_COMMAND 0x22
|
||||
#define MSG_VALUE 0x32
|
||||
#define MSG_PID 0x42
|
||||
#define MSG_TRIMS 0x50
|
||||
#define MSG_MINS 0x51
|
||||
#define MSG_MAXS 0x52
|
||||
#define MSG_IMU_OUT 0x53
|
||||
|
||||
#define SEVERITY_LOW 1
|
||||
#define SEVERITY_MEDIUM 2
|
||||
#define SEVERITY_HIGH 3
|
||||
#define SEVERITY_CRITICAL 4
|
||||
|
||||
// Debug options - set only one of these options to 1 at a time, set the others to 0
|
||||
#define DEBUG_SUBSYSTEM 0 // 0 = no debug
|
||||
// 1 = Debug the Radio input
|
||||
// 2 = Debug the Servo output
|
||||
// 3 = Debug the Sensor input
|
||||
// 4 = Debug the GPS input
|
||||
// 5 = Debug the GPS input - RAW HEX OUTPUT
|
||||
// 6 = Debug the IMU
|
||||
// 7 = Debug the Control Switch
|
||||
// 8 = Debug the Servo DIP switches
|
||||
// 9 = Debug the Relay out
|
||||
// 10 = Debug the Magnetometer
|
||||
// 11 = Debug the ABS pressure sensor
|
||||
// 12 = Debug the stored waypoints
|
||||
// 13 = Debug the Throttle
|
||||
// 14 = Debug the Radio Min Max
|
||||
// 15 = Debug the EEPROM - Hex Dump
|
||||
|
||||
|
||||
#define DEBUG_LEVEL SEVERITY_LOW
|
||||
// SEVERITY_LOW
|
||||
// SEVERITY_MEDIUM
|
||||
// SEVERITY_HIGH
|
||||
// SEVERITY_CRITICAL
|
||||
|
||||
|
||||
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
|
||||
uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Hardware
|
||||
|
||||
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ
|
||||
int SENSOR_SIGN[]={
|
||||
1, -1, 1, -1, 1, 1, -1, -1, -1};
|
||||
//{-1,1,-1,1,-1,1,-1,-1,-1};
|
||||
/* APM Hardware definitions, END */
|
||||
|
||||
/* General definitions */
|
||||
|
||||
#define TRUE 1
|
||||
#define FALSE 0
|
||||
#define ON 1
|
||||
#define OFF 0
|
||||
|
||||
|
||||
// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
|
||||
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
|
||||
// Tested value : 408
|
||||
#define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer
|
||||
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
|
||||
|
||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
|
||||
// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
|
||||
// Tested values :
|
||||
#define Gyro_Gain_X 0.4 //X axis Gyro gain
|
||||
#define Gyro_Gain_Y 0.41 //Y axis Gyro gain
|
||||
#define Gyro_Gain_Z 0.41 //Z axis Gyro gain
|
||||
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
|
||||
/*For debugging purposes*/
|
||||
#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
|
||||
|
||||
int AN[6]; //array that store the 6 ADC channels
|
||||
int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
|
||||
int gyro_temp;
|
||||
|
||||
|
||||
float G_Dt=0.02; // Integration time for the gyros (DCM algorithm)
|
||||
float Accel_Vector[3]= {0, 0, 0}; //Store the acceleration in a vector
|
||||
float Accel_Vector_unfiltered[3]= {0, 0, 0}; //Store the acceleration in a vector
|
||||
float Gyro_Vector[3]= {0, 0, 0}; //Store the gyros rutn rate in a vector
|
||||
float Omega_Vector[3]= {0, 0, 0}; //Corrected Gyro_Vector data
|
||||
float Omega_P[3]= {0, 0, 0}; //Omega Proportional correction
|
||||
float Omega_I[3]= {0, 0, 0}; //Omega Integrator
|
||||
float Omega[3]= {0, 0, 0};
|
||||
//float Accel_magnitude;
|
||||
//float Accel_weight;
|
||||
|
||||
float errorRollPitch[3] = {0, 0, 0};
|
||||
float errorYaw[3] = {0, 0, 0};
|
||||
float errorCourse = 0;
|
||||
float COGX = 0; //Course overground X axis
|
||||
float COGY = 1; //Course overground Y axis
|
||||
|
||||
float roll = 0;
|
||||
float pitch = 0;
|
||||
float yaw = 0;
|
||||
|
||||
unsigned int counter = 0;
|
||||
|
||||
float DCM_Matrix[3][3]= {
|
||||
{ 1,0,0 },
|
||||
{ 0,1,0 },
|
||||
{ 0,0,1 }};
|
||||
float Update_Matrix[3][3]={
|
||||
{ 0,1,2 },
|
||||
{ 3,4,5 },
|
||||
{ 6,7,8 }}; //Gyros here
|
||||
|
||||
float Temporary_Matrix[3][3]={
|
||||
{ 0,0,0 },
|
||||
{ 0,0,0 },
|
||||
{ 0,0,0 }};
|
||||
|
||||
// GPS variables
|
||||
float speed_3d=0;
|
||||
int GPS_ground_speed=0;
|
||||
|
||||
// Main timers
|
||||
long timer=0;
|
||||
long timer_old;
|
||||
long GPS_timer;
|
||||
long GPS_timer_old;
|
||||
float GPS_Dt=0.2; // GPS Dt
|
||||
|
||||
// Attitude control variables
|
||||
float command_rx_roll=0; // User commands
|
||||
float command_rx_roll_old;
|
||||
float command_rx_roll_diff;
|
||||
float command_rx_pitch=0;
|
||||
float command_rx_pitch_old;
|
||||
float command_rx_pitch_diff;
|
||||
float command_rx_yaw=0;
|
||||
float command_rx_yaw_diff;
|
||||
int control_roll; // PID control results
|
||||
int control_pitch;
|
||||
int control_yaw;
|
||||
float K_aux;
|
||||
|
||||
// Attitude PID controls
|
||||
float roll_I=0;
|
||||
float roll_D;
|
||||
float err_roll;
|
||||
float pitch_I=0;
|
||||
float pitch_D;
|
||||
float err_pitch;
|
||||
float yaw_I=0;
|
||||
float yaw_D;
|
||||
float err_yaw;
|
||||
|
||||
//Position control
|
||||
#ifdef IsGPS
|
||||
long target_longitude;
|
||||
long target_lattitude;
|
||||
byte target_position;
|
||||
#endif
|
||||
float gps_err_roll;
|
||||
float gps_err_roll_old;
|
||||
float gps_roll_D;
|
||||
float gps_roll_I=0;
|
||||
float gps_err_pitch;
|
||||
float gps_err_pitch_old;
|
||||
float gps_pitch_D;
|
||||
float gps_pitch_I=0;
|
||||
float command_gps_roll;
|
||||
float command_gps_pitch;
|
||||
|
||||
//Altitude control
|
||||
int Initial_Throttle;
|
||||
int target_sonar_altitude;
|
||||
int err_altitude;
|
||||
int err_altitude_old;
|
||||
float command_altitude;
|
||||
float altitude_I;
|
||||
float altitude_D;
|
||||
|
||||
//Pressure Sensor variables
|
||||
#ifdef UseBMP
|
||||
unsigned long abs_press = 0;
|
||||
unsigned long abs_press_filt = 0;
|
||||
unsigned long abs_press_gnd = 0;
|
||||
int ground_temperature = 0; //
|
||||
int temp_unfilt = 0;
|
||||
long ground_alt = 0; // Ground altitude from gps at startup in centimeters
|
||||
long press_alt = 0; // Pressure altitude
|
||||
#endif
|
||||
|
||||
|
||||
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
|
||||
|
||||
#define AIRSPEED_PIN 1 // Need to correct value
|
||||
#define BATTERY_PIN 1 // Need to correct value
|
||||
#define RELAY_PIN 47
|
||||
#define LOW_VOLTAGE 11.4 // Pack voltage at which to trigger alarm
|
||||
#define INPUT_VOLTAGE 5.2 // (Volts) voltage your power regulator is feeding your ArduPilot to have an accurate pressure and battery level readings. (you need a multimeter to measure and set this of course)
|
||||
#define VOLT_DIV_RATIO 1.0 // Voltage divider ratio set with thru-hole resistor (see manual)
|
||||
|
||||
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage, initialized above threshold for filter
|
||||
|
||||
|
||||
// Sonar variables
|
||||
int Sonar_value=0;
|
||||
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
|
||||
int Sonar_Counter=0;
|
||||
|
||||
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
|
||||
byte AP_mode = 2;
|
||||
|
||||
// Mode LED timers and variables, used to blink LED_Green
|
||||
byte gled_status = HIGH;
|
||||
long gled_timer;
|
||||
int gled_speed;
|
||||
|
||||
long t0;
|
||||
int num_iter;
|
||||
float aux_debug;
|
||||
|
||||
// Radio definitions
|
||||
int roll_mid;
|
||||
int pitch_mid;
|
||||
int yaw_mid;
|
||||
|
||||
int Neutro_yaw;
|
||||
int ch_roll;
|
||||
int ch_pitch;
|
||||
int ch_throttle;
|
||||
int ch_yaw;
|
||||
int ch_aux;
|
||||
int ch_aux2;
|
||||
|
||||
int frontMotor;
|
||||
int backMotor;
|
||||
int leftMotor;
|
||||
int rightMotor;
|
||||
byte motorArmed = 0;
|
||||
int minThrottle = 0;
|
||||
|
||||
// Serial communication
|
||||
char queryType;
|
||||
long tlmTimer = 0;
|
||||
|
||||
// Arming/Disarming
|
||||
uint8_t Arming_counter=0;
|
||||
uint8_t Disarming_counter=0;
|
||||
|
||||
// Performance monitoring
|
||||
// ----------------------
|
||||
long perf_mon_timer = 0;
|
||||
float imu_health = 0; //Metric based on accel gain deweighting
|
||||
int G_Dt_max = 0; //Max main loop cycle time in milliseconds
|
||||
byte gyro_sat_count = 0;
|
||||
byte adc_constraints = 0;
|
||||
byte renorm_sqrt_count = 0;
|
||||
byte renorm_blowup_count = 0;
|
||||
int gps_fix_count = 0;
|
||||
|
||||
byte gcs_messages_sent = 0;
|
||||
|
||||
// System Timers
|
||||
// --------------
|
||||
unsigned long fast_loopTimer = 0; // Time in miliseconds of main control loop
|
||||
unsigned long medium_loopTimer = 0; // Time in miliseconds of navigation control loop
|
||||
byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops
|
||||
byte slow_loopCounter = 0; //
|
||||
unsigned long deltaMiliSeconds = 0; // Delta Time in miliseconds
|
||||
unsigned long dTnav = 0; // Delta Time in milliseconds for navigation computations
|
||||
int mainLoop_count = 0;
|
||||
unsigned long elapsedTime = 0; // for doing custom events
|
||||
//unsigned int GPS_timer = 0;
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,199 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Arducopter.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
/* ********************************************************************** */
|
||||
/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */
|
||||
/* Mounting position : RC connectors pointing backwards */
|
||||
/* This code use this libraries : */
|
||||
/* APM_RC : Radio library (with InstantPWM) */
|
||||
/* APM_ADC : External ADC library */
|
||||
/* DataFlash : DataFlash log library */
|
||||
/* APM_BMP085 : BMP085 barometer library */
|
||||
/* APM_Compass : HMC5843 compass library [optional] */
|
||||
/* GPS_UBLOX or GPS_NMEA or GPS_MTK : GPS library [optional] */
|
||||
/* ********************************************************************** */
|
||||
|
||||
/* ********************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* *********************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* *********************************************************************** */
|
||||
|
||||
|
||||
/* ************************************************************ */
|
||||
/* **************** MAIN PROGRAM - MODULES ******************** */
|
||||
/* ************************************************************ */
|
||||
|
||||
/* User definable modules */
|
||||
|
||||
// Comment out with // modules that you are not using
|
||||
|
||||
//#define IsGPS // Do we have a GPS connected
|
||||
//#define IsNEWMTEK // Do we have MTEK with new firmware
|
||||
//#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
|
||||
//#define IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port
|
||||
//#define IsAM // Do we have motormount LED's. AM = Atraction Mode
|
||||
|
||||
|
||||
// Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link
|
||||
#define Ser0 // FTDI/USB Port Either one
|
||||
//#define Ser3 // Telemetry port
|
||||
|
||||
//#define CONFIGURATOR // Do se use Configurator or normal text output over serial link
|
||||
|
||||
|
||||
/**********************************************/
|
||||
// Not in use yet, starting to work with battery monitors and pressure sensors.
|
||||
// Added 19-08-2010
|
||||
|
||||
//#define UseAirspeed
|
||||
//#define UseBMP
|
||||
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!)
|
||||
|
||||
/**********************************************/
|
||||
|
||||
|
||||
// Frame build condiguration
|
||||
#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
|
||||
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************ */
|
||||
/* **************** MAIN PROGRAM - INCLUDES ******************* */
|
||||
/* ************************************************************ */
|
||||
|
||||
#include <avr/io.h>
|
||||
#include <avr/eeprom.h>
|
||||
#include <avr/pgmspace.h>
|
||||
#include <math.h>
|
||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||
#include <APM_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
|
||||
#include <APM_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
//#include "defines.h"
|
||||
//#include "console.h"
|
||||
#include <Wire.h> // I2C Communication library
|
||||
|
||||
#ifdef UseBMP
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
#endif
|
||||
|
||||
//#include <GPS_IMU.h> // ArduPilot IMU/SIM GPS Library
|
||||
#include <GPS_MTK.h> // ArduPilot MTK GPS Library
|
||||
//#include <GPS_UBLOX.h> // ArduPilot Ublox GPS Library
|
||||
//#include <GPS_NMEA.h> // ArduPilot NMEA GPS library
|
||||
|
||||
// EEPROM storage for user configurable values
|
||||
#include <EEPROM.h> // EEPROM
|
||||
#include "Arducopter.h"
|
||||
#include "ArduUser.h"
|
||||
|
||||
/* Software version */
|
||||
#define VER 1.40 // Current software version (only numeric values)
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************ */
|
||||
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
|
||||
/* ************************************************************ */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************ */
|
||||
/* **************** MAIN PROGRAM - SETUP ********************** */
|
||||
/* ************************************************************ */
|
||||
void setup() {
|
||||
|
||||
//APM_Init_IO();
|
||||
APM_Init();
|
||||
|
||||
//APM_Init_ADC();
|
||||
//APM_Init_Radio();
|
||||
//APM_Init_Serial();
|
||||
//APM_Init_xx
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************ */
|
||||
/* ************** MAIN PROGRAM - MAIN LOOP ******************** */
|
||||
/* ************************************************************ */
|
||||
void loop() {
|
||||
// We want this to execute at 500Hz if possible
|
||||
// -------------------------------------------
|
||||
if (millis()-fast_loopTimer > 5) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f;
|
||||
fast_loopTimer = millis();
|
||||
mainLoop_count++;
|
||||
|
||||
// Execute the fast loop
|
||||
// ---------------------
|
||||
// fast_loop();
|
||||
// - PWM Updates
|
||||
// - Stabilization
|
||||
// - Altitude correction
|
||||
|
||||
// Execute the medium loop
|
||||
// -----------------------
|
||||
// medium_loop();
|
||||
// - Radio read
|
||||
// - GPS read
|
||||
// - Drift correction
|
||||
|
||||
// Execute the slow loop
|
||||
// -----------------------
|
||||
// slow_loop();
|
||||
// - Battery usage
|
||||
// - GCS updates
|
||||
// - Garbage management
|
||||
|
||||
if (millis()- perf_mon_timer > 20000) {
|
||||
if (mainLoop_count != 0) {
|
||||
|
||||
//send_message(MSG_PERF_REPORT);
|
||||
#if LOG_PM
|
||||
Log_Write_Performance();
|
||||
#endif
|
||||
resetPerfData();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,213 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Attitude.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
/* ************************************************************ */
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
// Function : Attitude_control_v2()
|
||||
//
|
||||
// Stable flight mode main algoritms
|
||||
//
|
||||
// Parameters:
|
||||
// - none
|
||||
//
|
||||
// Returns : - none
|
||||
//
|
||||
// Alters :
|
||||
// err_roll, roll_rate
|
||||
//
|
||||
// Relies :
|
||||
// Radio input, Gyro
|
||||
//
|
||||
|
||||
// STABLE MODE
|
||||
// ROLL, PITCH and YAW PID controls...
|
||||
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
|
||||
void Attitude_control_v2()
|
||||
{
|
||||
float err_roll_rate;
|
||||
float err_pitch_rate;
|
||||
float roll_rate;
|
||||
float pitch_rate;
|
||||
|
||||
// ROLL CONTROL
|
||||
if (AP_mode == 2) // Normal Mode => Stabilization mode
|
||||
err_roll = command_rx_roll - ToDeg(roll);
|
||||
else
|
||||
err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control
|
||||
|
||||
err_roll = constrain(err_roll, -25, 25); // to limit max roll command...
|
||||
|
||||
// New control term...
|
||||
roll_rate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
|
||||
err_roll_rate = ((ch_roll - roll_mid) >> 1) - roll_rate;
|
||||
|
||||
roll_I += err_roll * G_Dt;
|
||||
roll_I = constrain(roll_I, -20, 20);
|
||||
// D term implementation => two parts: gyro part and command part
|
||||
// To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
|
||||
// We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
|
||||
roll_D = - roll_rate; // Take into account Angular velocity of the stick (command)
|
||||
|
||||
// PID control
|
||||
K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
|
||||
control_roll = K_aux * err_roll + KD_QUAD_ROLL * roll_D + KI_QUAD_ROLL * roll_I + STABLE_MODE_KP_RATE * err_roll_rate;
|
||||
|
||||
// PITCH CONTROL
|
||||
if (AP_mode==2) // Normal mode => Stabilization mode
|
||||
err_pitch = command_rx_pitch - ToDeg(pitch);
|
||||
else
|
||||
err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control
|
||||
|
||||
err_pitch = constrain(err_pitch, -25, 25); // to limit max pitch command...
|
||||
|
||||
// New control term...
|
||||
pitch_rate = ToDeg(Omega[1]);
|
||||
err_pitch_rate = ((ch_pitch - pitch_mid) >> 1) - pitch_rate;
|
||||
|
||||
pitch_I += err_pitch * G_Dt;
|
||||
pitch_I = constrain(pitch_I, -20, 20);
|
||||
// D term
|
||||
pitch_D = - pitch_rate;
|
||||
|
||||
// PID control
|
||||
K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
|
||||
control_pitch = K_aux * err_pitch + KD_QUAD_PITCH * pitch_D + KI_QUAD_PITCH * pitch_I + STABLE_MODE_KP_RATE * err_pitch_rate;
|
||||
|
||||
// YAW CONTROL
|
||||
err_yaw = command_rx_yaw - ToDeg(yaw);
|
||||
if (err_yaw > 180) // Normalize to -180,180
|
||||
err_yaw -= 360;
|
||||
else if(err_yaw < -180)
|
||||
err_yaw += 360;
|
||||
|
||||
err_yaw = constrain(err_yaw, -60, 60); // to limit max yaw command...
|
||||
|
||||
yaw_I += err_yaw * G_Dt;
|
||||
yaw_I = constrain(yaw_I, -20, 20);
|
||||
yaw_D = - ToDeg(Omega[2]);
|
||||
|
||||
// PID control
|
||||
control_yaw = KP_QUAD_YAW * err_yaw + KD_QUAD_YAW * yaw_D + KI_QUAD_YAW * yaw_I;
|
||||
}
|
||||
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
// Function : Rate_control()
|
||||
//
|
||||
// Acro mode main algoritms
|
||||
//
|
||||
// Parameters:
|
||||
// - none
|
||||
//
|
||||
// Returns : - none
|
||||
//
|
||||
// Alters :
|
||||
// err_roll, roll_rate
|
||||
//
|
||||
// Relies :
|
||||
// Radio input, Gyro
|
||||
//
|
||||
|
||||
|
||||
// ACRO MODE
|
||||
void Rate_control()
|
||||
{
|
||||
static float previousRollRate, previousPitchRate, previousYawRate;
|
||||
float currentRollRate, currentPitchRate, currentYawRate;
|
||||
|
||||
// ROLL CONTROL
|
||||
currentRollRate = read_adc(0); // I need a positive sign here
|
||||
|
||||
err_roll = ((ch_roll - roll_mid) * xmitFactor) - currentRollRate;
|
||||
|
||||
roll_I += err_roll * G_Dt;
|
||||
roll_I = constrain(roll_I, -20, 20);
|
||||
|
||||
roll_D = currentRollRate - previousRollRate;
|
||||
previousRollRate = currentRollRate;
|
||||
|
||||
// PID control
|
||||
control_roll = Kp_RateRoll * err_roll + Kd_RateRoll * roll_D + Ki_RateRoll * roll_I;
|
||||
|
||||
// PITCH CONTROL
|
||||
currentPitchRate = read_adc(1);
|
||||
err_pitch = ((ch_pitch - pitch_mid) * xmitFactor) - currentPitchRate;
|
||||
|
||||
pitch_I += err_pitch*G_Dt;
|
||||
pitch_I = constrain(pitch_I,-20,20);
|
||||
|
||||
pitch_D = currentPitchRate - previousPitchRate;
|
||||
previousPitchRate = currentPitchRate;
|
||||
|
||||
// PID control
|
||||
control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I;
|
||||
|
||||
// YAW CONTROL
|
||||
currentYawRate = read_adc(2);
|
||||
err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate;
|
||||
|
||||
yaw_I += err_yaw*G_Dt;
|
||||
yaw_I = constrain(yaw_I, -20, 20);
|
||||
|
||||
yaw_D = currentYawRate - previousYawRate;
|
||||
previousYawRate = currentYawRate;
|
||||
|
||||
// PID control
|
||||
K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain
|
||||
control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I;
|
||||
}
|
||||
|
||||
// Maximun slope filter for radio inputs... (limit max differences between readings)
|
||||
int channel_filter(int ch, int ch_old)
|
||||
{
|
||||
int diff_ch_old;
|
||||
|
||||
if (ch_old==0) // ch_old not initialized
|
||||
return(ch);
|
||||
diff_ch_old = ch - ch_old; // Difference with old reading
|
||||
if (diff_ch_old < 0)
|
||||
{
|
||||
if (diff_ch_old <- 40)
|
||||
return(ch_old - 40); // We limit the max difference between readings
|
||||
}
|
||||
else
|
||||
{
|
||||
if (diff_ch_old > 40)
|
||||
return(ch_old + 40);
|
||||
}
|
||||
return((ch + ch_old) >> 1); // Small filtering
|
||||
//return(ch);
|
||||
}
|
||||
|
|
@ -0,0 +1,261 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : DCM.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
/* ******* ADC functions ********************* */
|
||||
// Read all the ADC channles
|
||||
void Read_adc_raw(void)
|
||||
{
|
||||
int temp;
|
||||
|
||||
for (int i=0;i<6;i++)
|
||||
AN[i] = APM_ADC.Ch(sensors[i]);
|
||||
|
||||
// Correction for non ratiometric sensor (test code)
|
||||
gyro_temp = APM_ADC.Ch(3);
|
||||
//AN[0] += 1500-temp;
|
||||
//AN[1] += 1500-temp;
|
||||
//AN[2] += 1500-temp;
|
||||
}
|
||||
|
||||
// Returns an analog value with the offset
|
||||
int read_adc(int select)
|
||||
{
|
||||
if (SENSOR_SIGN[select]<0)
|
||||
return (AN_OFFSET[select]-AN[select]);
|
||||
else
|
||||
return (AN[select]-AN_OFFSET[select]);
|
||||
}
|
||||
/* ******************************************* */
|
||||
|
||||
/* ******* DCM IMU functions ********************* */
|
||||
/**************************************************/
|
||||
void Normalize(void)
|
||||
{
|
||||
float error=0;
|
||||
float temporary[3][3];
|
||||
float renorm=0;
|
||||
|
||||
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
|
||||
|
||||
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
|
||||
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
|
||||
|
||||
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
|
||||
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
|
||||
|
||||
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
|
||||
|
||||
renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
|
||||
renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
|
||||
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
|
||||
|
||||
renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
|
||||
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
void Drift_correction(void)
|
||||
{
|
||||
//Compensation the Roll, Pitch and Yaw drift.
|
||||
float errorCourse;
|
||||
static float Scaled_Omega_P[3];
|
||||
static float Scaled_Omega_I[3];
|
||||
|
||||
//*****Roll and Pitch***************
|
||||
|
||||
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
|
||||
// errorRollPitch are in Accel ADC units
|
||||
// Limit max errorRollPitch to limit max Omega_P and Omega_I
|
||||
errorRollPitch[0] = constrain(errorRollPitch[0],-50,50);
|
||||
errorRollPitch[1] = constrain(errorRollPitch[1],-50,50);
|
||||
errorRollPitch[2] = constrain(errorRollPitch[2],-50,50);
|
||||
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH);
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
|
||||
|
||||
//*****YAW***************
|
||||
// We make the gyro YAW drift correction based on compass magnetic heading
|
||||
if (MAGNETOMETER == 1) {
|
||||
errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
}
|
||||
|
||||
}
|
||||
/**************************************************/
|
||||
void Accel_adjust(void)
|
||||
{
|
||||
//Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
//Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Matrix_update(void)
|
||||
{
|
||||
Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
|
||||
Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
|
||||
Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
|
||||
|
||||
//Accel_Vector[0]=read_adc(3); // acc x
|
||||
//Accel_Vector[1]=read_adc(4); // acc y
|
||||
//Accel_Vector[2]=read_adc(5); // acc z
|
||||
|
||||
// Low pass filter on accelerometer data (to filter vibrations)
|
||||
Accel_Vector[0]=Accel_Vector[0]*0.6 + (float)read_adc(3)*0.4; // acc x
|
||||
Accel_Vector[1]=Accel_Vector[1]*0.6 + (float)read_adc(4)*0.4; // acc y
|
||||
Accel_Vector[2]=Accel_Vector[2]*0.6 + (float)read_adc(5)*0.4; // acc z
|
||||
|
||||
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
|
||||
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional
|
||||
|
||||
//Accel_adjust();//adjusting centrifugal acceleration. // Not used for quadcopter
|
||||
|
||||
#if OUTPUTMODE==1 // corrected mode
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
|
||||
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
|
||||
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
#if OUTPUTMODE==0 // uncorrected data of the gyros (with drift)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
|
||||
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
|
||||
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
||||
|
||||
for(int x=0; x<3; x++) //Matrix Addition (update)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Euler_angles(void)
|
||||
{
|
||||
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
|
||||
roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
|
||||
pitch = -asin((Accel_Vector[0])/(float)GRAVITY); // asin(acc_x)
|
||||
yaw = 0;
|
||||
#else // Euler angles from DCM matrix
|
||||
pitch = asin(-DCM_Matrix[2][0]);
|
||||
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
// VECTOR FUNCTIONS
|
||||
//Computes the dot product of two vectors
|
||||
float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
float op=0;
|
||||
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
op+=vector1[c]*vector2[c];
|
||||
}
|
||||
|
||||
return op;
|
||||
}
|
||||
|
||||
//Computes the cross product of two vectors
|
||||
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
//Multiply the vector by a scalar.
|
||||
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn[c]*scale2;
|
||||
}
|
||||
}
|
||||
|
||||
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
|
||||
}
|
||||
}
|
||||
|
||||
/********* MATRIX FUNCTIONS *****************************************/
|
||||
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
|
||||
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=0;
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
|
||||
float test=mat[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,546 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Debug.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 1
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Radio Subsystem ");
|
||||
while(1){
|
||||
delay(20);
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
Serial.print("Radio in ch1: ");
|
||||
Serial.print(radio_in[CH_ROLL], DEC);
|
||||
Serial.print("\tch2: ");
|
||||
Serial.print(radio_in[CH_PITCH], DEC);
|
||||
Serial.print("\tch3: ");
|
||||
Serial.print(radio_in[CH_THROTTLE], DEC);
|
||||
Serial.print("\tch4: ");
|
||||
Serial.print(radio_in[CH_RUDDER], DEC);
|
||||
Serial.print("\tch5: ");
|
||||
Serial.print(radio_in[4], DEC);
|
||||
Serial.print("\tch6: ");
|
||||
Serial.print(radio_in[5], DEC);
|
||||
Serial.print("\tch7: ");
|
||||
Serial.print(radio_in[6], DEC);
|
||||
Serial.print("\tch8: ");
|
||||
Serial.println(radio_in[7], DEC);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 2
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Servo Subsystem ");
|
||||
Serial.print("Reverse ROLL - CH1: ");
|
||||
Serial.println(reverse_roll, DEC);
|
||||
Serial.print("Reverse PITCH - CH2: ");
|
||||
Serial.println(reverse_pitch, DEC);
|
||||
Serial.print("Reverse THROTTLE - CH3: ");
|
||||
Serial.println(REVERSE_THROTTLE, DEC);
|
||||
Serial.print("Reverse RUDDER - CH4: ");
|
||||
Serial.println(reverse_rudder, DEC);
|
||||
|
||||
// read the radio to set trims
|
||||
// ---------------------------
|
||||
trim_radio();
|
||||
|
||||
radio_max[0] = CH1_MAX;
|
||||
radio_max[1] = CH2_MAX;
|
||||
radio_max[2] = CH3_MAX;
|
||||
radio_max[3] = CH4_MAX;
|
||||
radio_max[4] = CH5_MAX;
|
||||
radio_max[5] = CH6_MAX;
|
||||
radio_max[6] = CH7_MAX;
|
||||
radio_max[7] = CH8_MAX;
|
||||
radio_min[0] = CH1_MIN;
|
||||
radio_min[1] = CH2_MIN;
|
||||
radio_min[2] = CH3_MIN;
|
||||
radio_min[3] = CH4_MIN;
|
||||
radio_min[4] = CH5_MIN;
|
||||
radio_min[5] = CH6_MIN;
|
||||
radio_min[6] = CH7_MIN;
|
||||
radio_min[7] = CH8_MIN;
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
update_servo_switches();
|
||||
|
||||
servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45.0f * reverse_roll) / 500;
|
||||
servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45.0f * reverse_roll) / 500;
|
||||
servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45.0f * reverse_rudder) / 500;
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
set_servos_4();
|
||||
|
||||
|
||||
for(int y = 4; y < 8; y++) {
|
||||
radio_out[y] = constrain(radio_in[y], radio_min[y], radio_max[y]);
|
||||
APM_RC.OutputCh(y, radio_out[y]); // send to Servos
|
||||
}
|
||||
|
||||
/*
|
||||
Serial.print("Servo_out ch1: ");
|
||||
Serial.print(servo_out[CH_ROLL], DEC);
|
||||
Serial.print("\tch2: ");
|
||||
Serial.print(servo_out[CH_PITCH], DEC);
|
||||
Serial.print("\tch3: ");
|
||||
Serial.print(servo_out[CH_THROTTLE], DEC);
|
||||
Serial.print("\tch4: ");
|
||||
Serial.print(servo_out[CH_RUDDER], DEC);
|
||||
*/
|
||||
///*
|
||||
Serial.print("ch1: ");
|
||||
Serial.print(radio_out[CH_ROLL], DEC);
|
||||
Serial.print("\tch2: ");
|
||||
Serial.print(radio_out[CH_PITCH], DEC);
|
||||
Serial.print("\tch3: ");
|
||||
Serial.print(radio_out[CH_THROTTLE], DEC);
|
||||
Serial.print("\tch4: ");
|
||||
Serial.print(radio_out[CH_RUDDER], DEC);
|
||||
Serial.print("\tch5: ");
|
||||
Serial.print(radio_out[4], DEC);
|
||||
Serial.print("\tch6: ");
|
||||
Serial.print(radio_out[5], DEC);
|
||||
Serial.print("\tch7: ");
|
||||
Serial.print(radio_out[6], DEC);
|
||||
Serial.print("\tch8: ");
|
||||
Serial.println(radio_out[7], DEC);
|
||||
|
||||
//*/
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 3
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Analog Sensor Subsystem ");
|
||||
|
||||
Serial.print("AirSpeed sensor enabled: ");
|
||||
Serial.println(AIRSPEED_SENSOR, DEC);
|
||||
|
||||
Serial.print("Enable Battery: ");
|
||||
Serial.println(BATTERY_EVENT, DEC);
|
||||
zero_airspeed();
|
||||
|
||||
Serial.print("Air pressure offset:");
|
||||
Serial.println(airpressure_offset, DEC);
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
read_z_sensor();
|
||||
read_XY_sensors();
|
||||
read_battery();
|
||||
|
||||
Serial.print("Analog IN:");
|
||||
Serial.print(" 0:");
|
||||
Serial.print(analog0, DEC);
|
||||
Serial.print(", 1: ");
|
||||
Serial.print(analog1, DEC);
|
||||
Serial.print(", 2: ");
|
||||
Serial.print(analog2, DEC);
|
||||
Serial.print(", 3: ");
|
||||
Serial.print(airpressure_raw, DEC);
|
||||
|
||||
Serial.print("\t\tSensors:");
|
||||
Serial.print(" airSpeed:");
|
||||
Serial.print(airspeed, DEC);
|
||||
Serial.print("m \tbattV:");
|
||||
Serial.print(battery_voltage, DEC);
|
||||
Serial.println("volts ");
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 4
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: GPS Subsystem ");
|
||||
|
||||
while(1){
|
||||
delay(333);
|
||||
|
||||
// Blink GPS LED if we don't have a fix
|
||||
// ------------------------------------
|
||||
//update_GPS_light();
|
||||
|
||||
GPS.Read();
|
||||
|
||||
if (GPS.NewData){
|
||||
Serial.print("Lat:");
|
||||
Serial.print(GPS.Lattitude, DEC);
|
||||
Serial.print(" Lon:");
|
||||
Serial.print(GPS.Longitude, DEC);
|
||||
Serial.print(" Alt:");
|
||||
Serial.print(GPS.Altitude / 100, DEC);
|
||||
Serial.print("m, gs: ");
|
||||
Serial.print(GPS.Ground_Speed / 100, DEC);
|
||||
Serial.print(" COG:");
|
||||
Serial.print(GPS.Ground_Course / 1000l);
|
||||
Serial.print(" SAT:");
|
||||
Serial.print(GPS.NumSats, DEC);
|
||||
Serial.print(" FIX:");
|
||||
Serial.print(GPS.Fix, DEC);
|
||||
Serial.print(" TIM:");
|
||||
Serial.print(GPS.Time);
|
||||
Serial.println();
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 5
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: GPS Subsystem, RAW OUTPUT");
|
||||
|
||||
while(1){
|
||||
if(Serial1.available() > 0) // Ok, let me see, the buffer is empty?
|
||||
{
|
||||
|
||||
delay(60); // wait for it all
|
||||
while(Serial1.available() > 0){
|
||||
byte incoming = Serial1.read();
|
||||
//Serial.print(incoming,DEC);
|
||||
Serial.print(incoming, HEX); // will output Hex values
|
||||
Serial.print(",");
|
||||
}
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 6
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: IMU Subsystem ");
|
||||
startup_IMU_ground();
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
read_AHRS();
|
||||
|
||||
// We are using the IMU
|
||||
// ---------------------
|
||||
Serial.print(" roll:");
|
||||
Serial.print(roll_sensor / 100, DEC);
|
||||
Serial.print(" pitch:");
|
||||
Serial.print(pitch_sensor / 100, DEC);
|
||||
Serial.print(" yaw:");
|
||||
Serial.println(yaw_sensor / 100, DEC);
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 7
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Control Switch Test");
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
byte switchPosition = readSwitch();
|
||||
if (oldSwitchPosition != switchPosition){
|
||||
|
||||
switch(switchPosition)
|
||||
{
|
||||
case 1: // First position
|
||||
Serial.println("Position 1");
|
||||
|
||||
break;
|
||||
|
||||
case 2: // middle position
|
||||
Serial.println("Position 2");
|
||||
break;
|
||||
|
||||
case 3: // last position
|
||||
Serial.println("Position 3");
|
||||
break;
|
||||
|
||||
case 4: // last position
|
||||
Serial.println("Position 4");
|
||||
break;
|
||||
|
||||
case 5: // last position
|
||||
Serial.println("Position 5 - Software Manual");
|
||||
break;
|
||||
|
||||
case 6: // last position
|
||||
Serial.println("Position 6 - Hardware MUX, Manual");
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
oldSwitchPosition = switchPosition;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 8
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Control Switch Test");
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
update_servo_switches();
|
||||
if (mix_mode == 0) {
|
||||
Serial.print("Mix:standard ");
|
||||
Serial.print("\t reverse_roll: ");
|
||||
Serial.print(reverse_roll, DEC);
|
||||
Serial.print("\t reverse_pitch: ");
|
||||
Serial.print(reverse_pitch, DEC);
|
||||
Serial.print("\t reverse_rudder: ");
|
||||
Serial.println(reverse_rudder, DEC);
|
||||
} else {
|
||||
Serial.print("Mix:elevons ");
|
||||
Serial.print("\t reverse_elevons: ");
|
||||
Serial.print(reverse_elevons, DEC);
|
||||
Serial.print("\t reverse_ch1_elevon: ");
|
||||
Serial.print(reverse_ch1_elevon, DEC);
|
||||
Serial.print("\t reverse_ch2_elevon: ");
|
||||
Serial.println(reverse_ch2_elevon, DEC);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 9
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Relay");
|
||||
pinMode(RELAY_PIN, OUTPUT);
|
||||
|
||||
while(1){
|
||||
delay(3000);
|
||||
|
||||
Serial.println("Relay Position A");
|
||||
PORTL |= B00000100;
|
||||
delay(3000);
|
||||
|
||||
Serial.println("Relay Position B");
|
||||
PORTL ^= B00000100;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 10
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Magnatometer");
|
||||
|
||||
while(1){
|
||||
delay(3000);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 11
|
||||
void debug_subsystem()
|
||||
{
|
||||
ground_alt = 0;
|
||||
Serial.println("Begin Debug: Absolute Airpressure");
|
||||
while(1){
|
||||
delay(20);
|
||||
read_airpressure();
|
||||
Serial.print("Air Pressure Altitude: ");
|
||||
Serial.print(press_alt / 100, DEC);
|
||||
Serial.println("meters");
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 12
|
||||
void debug_subsystem()
|
||||
{
|
||||
ground_alt = 0;
|
||||
Serial.println("Begin Debug: Display Waypoints");
|
||||
delay(500);
|
||||
|
||||
eeprom_busy_wait();
|
||||
uint8_t options = eeprom_read_byte((uint8_t *) EE_CONFIG);
|
||||
|
||||
eeprom_busy_wait();
|
||||
int32_t hold = eeprom_read_dword((uint32_t *) EE_ALT_HOLD_HOME);
|
||||
|
||||
// save the alitude above home option
|
||||
if(options & HOLD_ALT_ABOVE_HOME){
|
||||
Serial.print("Hold this altitude over home: ");
|
||||
Serial.print(hold / 100, DEC);
|
||||
Serial.println(" meters");
|
||||
}else{
|
||||
Serial.println("Maintain current altitude ");
|
||||
}
|
||||
|
||||
Serial.print("Number of Waypoints: ");
|
||||
Serial.println(wp_total, DEC);
|
||||
|
||||
Serial.print("Hit radius for Waypoints: ");
|
||||
Serial.println(wp_radius, DEC);
|
||||
|
||||
Serial.print("Loiter radius around Waypoints: ");
|
||||
Serial.println(loiter_radius, DEC);
|
||||
Serial.println(" ");
|
||||
|
||||
for(byte i = 0; i < wp_total; i++){
|
||||
struct Location temp = get_wp_with_index(i);
|
||||
print_waypoint(&temp, i);
|
||||
}
|
||||
|
||||
while(1){
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 13
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Throttle Subsystem ");
|
||||
read_radio();
|
||||
|
||||
uint16_t low_pwm = radio_in[CH_THROTTLE];
|
||||
uint16_t high_pwm = radio_in[CH_THROTTLE];
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
//update_throttle();
|
||||
set_servos_4();
|
||||
low_pwm = min(low_pwm, radio_in[CH_THROTTLE]);
|
||||
high_pwm = max(high_pwm, radio_in[CH_THROTTLE]);
|
||||
|
||||
Serial.print("Radio_in: ");
|
||||
Serial.print(radio_in[CH_THROTTLE], DEC);
|
||||
Serial.print("\tPWM output: ");
|
||||
Serial.print(radio_out[CH_THROTTLE], DEC);
|
||||
Serial.print("\tThrottle: ");
|
||||
Serial.print(servo_out[CH_THROTTLE], DEC);
|
||||
Serial.print("\tPWM Min: ");
|
||||
Serial.print(low_pwm, DEC);
|
||||
Serial.print("\tPWM Max: ");
|
||||
Serial.println(high_pwm, DEC);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 14
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: Radio Min Max ");
|
||||
uint16_t low_pwm[8];
|
||||
uint16_t high_pwm[8];
|
||||
uint8_t i;
|
||||
|
||||
for(i = 0; i < 100; i++){
|
||||
delay(20);
|
||||
read_radio();
|
||||
}
|
||||
|
||||
for(i = 0; i < 8; i++){
|
||||
radio_min[i] = 0;
|
||||
radio_max[i] = 2400;
|
||||
low_pwm[i] = radio_in[i];
|
||||
high_pwm[i] = radio_in[i];
|
||||
}
|
||||
|
||||
while(1){
|
||||
delay(100);
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
for(i = 0; i < 8; i++){
|
||||
low_pwm[i] = min(low_pwm[i], radio_in[i]);
|
||||
high_pwm[i] = max(high_pwm[i], radio_in[i]);
|
||||
}
|
||||
|
||||
for(i = 0; i < 8; i++){
|
||||
Serial.print("CH");
|
||||
Serial.print(i + 1, DEC);
|
||||
Serial.print(": ");
|
||||
low_pwm[i] = min(low_pwm[i], radio_in[i]);
|
||||
Serial.print(low_pwm[i], DEC);
|
||||
Serial.print("|");
|
||||
high_pwm[i] = max(high_pwm[i], radio_in[i]);
|
||||
Serial.print(high_pwm[i], DEC);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println(" ");
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#if DEBUG_SUBSYSTEM == 15
|
||||
void debug_subsystem()
|
||||
{
|
||||
Serial.println("Begin Debug: EEPROM Dump ");
|
||||
uint16_t temp;
|
||||
for(int n = 0; n < 512; n++){
|
||||
for(int i = 0; i < 4; i++){
|
||||
temp = eeprom_read_word((uint16_t *) mem);
|
||||
mem += 2;
|
||||
Serial.print(temp, HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.print(" ");
|
||||
for(int i = 0; i < 4; i++){
|
||||
temp = eeprom_read_word((uint16_t *) mem);
|
||||
mem += 2;
|
||||
Serial.print(temp, HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,177 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : EEPROM.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
// Utilities for writing and reading from the EEPROM
|
||||
float readEEPROM(int address) {
|
||||
union floatStore {
|
||||
byte floatByte[4];
|
||||
float floatVal;
|
||||
} floatOut;
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
floatOut.floatByte[i] = EEPROM.read(address + i);
|
||||
return floatOut.floatVal;
|
||||
}
|
||||
|
||||
void writeEEPROM(float value, int address) {
|
||||
union floatStore {
|
||||
byte floatByte[4];
|
||||
float floatVal;
|
||||
} floatIn;
|
||||
|
||||
floatIn.floatVal = value;
|
||||
for (int i = 0; i < 4; i++)
|
||||
EEPROM.write(address + i, floatIn.floatByte[i]);
|
||||
}
|
||||
|
||||
void readUserConfig() {
|
||||
KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
|
||||
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
|
||||
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
|
||||
KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
|
||||
KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
|
||||
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
|
||||
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
|
||||
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
|
||||
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
|
||||
STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
|
||||
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
|
||||
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
|
||||
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
|
||||
KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR);
|
||||
KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR);
|
||||
KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR);
|
||||
GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR);
|
||||
KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR);
|
||||
KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR);
|
||||
KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR);
|
||||
acc_offset_x = readEEPROM(acc_offset_x_ADR);
|
||||
acc_offset_y = readEEPROM(acc_offset_y_ADR);
|
||||
acc_offset_z = readEEPROM(acc_offset_z_ADR);
|
||||
gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR);
|
||||
gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR);
|
||||
gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR);
|
||||
Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR);
|
||||
Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR);
|
||||
Kp_YAW = readEEPROM(Kp_YAW_ADR);
|
||||
Ki_YAW = readEEPROM(Ki_YAW_ADR);
|
||||
GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR);
|
||||
MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR);
|
||||
Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR);
|
||||
Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR);
|
||||
Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR);
|
||||
Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR);
|
||||
Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR);
|
||||
Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR);
|
||||
Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR);
|
||||
Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR);
|
||||
Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR);
|
||||
xmitFactor = readEEPROM(XMITFACTOR_ADR);
|
||||
roll_mid = readEEPROM(CHROLL_MID);
|
||||
pitch_mid = readEEPROM(CHPITCH_MID);
|
||||
yaw_mid = readEEPROM(CHYAW_MID);
|
||||
ch_roll_slope = readEEPROM(ch_roll_slope_ADR);
|
||||
ch_pitch_slope = readEEPROM(ch_pitch_slope_ADR);
|
||||
ch_throttle_slope = readEEPROM(ch_throttle_slope_ADR);
|
||||
ch_yaw_slope = readEEPROM(ch_yaw_slope_ADR);
|
||||
ch_aux_slope = readEEPROM(ch_aux_slope_ADR);
|
||||
ch_aux2_slope = readEEPROM(ch_aux2_slope_ADR);
|
||||
ch_roll_offset = readEEPROM(ch_roll_offset_ADR);
|
||||
ch_pitch_offset = readEEPROM(ch_pitch_offset_ADR);
|
||||
ch_throttle_offset = readEEPROM(ch_throttle_offset_ADR);
|
||||
ch_yaw_offset = readEEPROM(ch_yaw_offset_ADR);
|
||||
ch_aux_offset = readEEPROM(ch_aux_offset_ADR);
|
||||
ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR);
|
||||
}
|
||||
|
||||
void writeUserConfig() {
|
||||
writeEEPROM(KP_QUAD_ROLL, KP_QUAD_ROLL_ADR);
|
||||
writeEEPROM(KD_QUAD_ROLL, KD_QUAD_ROLL_ADR);
|
||||
writeEEPROM(KI_QUAD_ROLL, KI_QUAD_ROLL_ADR);
|
||||
writeEEPROM(KP_QUAD_PITCH, KP_QUAD_PITCH_ADR);
|
||||
writeEEPROM(KD_QUAD_PITCH, KD_QUAD_PITCH_ADR);
|
||||
writeEEPROM(KI_QUAD_PITCH, KI_QUAD_PITCH_ADR);
|
||||
writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR);
|
||||
writeEEPROM(KD_QUAD_YAW, KD_QUAD_YAW_ADR);
|
||||
writeEEPROM(KI_QUAD_YAW, KI_QUAD_YAW_ADR);
|
||||
writeEEPROM(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR);
|
||||
writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR);
|
||||
writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR);
|
||||
writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR);
|
||||
writeEEPROM(KP_GPS_PITCH, KP_GPS_PITCH_ADR);
|
||||
writeEEPROM(KD_GPS_PITCH, KD_GPS_PITCH_ADR);
|
||||
writeEEPROM(KI_GPS_PITCH, KI_GPS_PITCH_ADR);
|
||||
writeEEPROM(GPS_MAX_ANGLE, GPS_MAX_ANGLE_ADR);
|
||||
writeEEPROM(KP_ALTITUDE, KP_ALTITUDE_ADR);
|
||||
writeEEPROM(KD_ALTITUDE, KD_ALTITUDE_ADR);
|
||||
writeEEPROM(KI_ALTITUDE, KI_ALTITUDE_ADR);
|
||||
writeEEPROM(acc_offset_x, acc_offset_x_ADR);
|
||||
writeEEPROM(acc_offset_y, acc_offset_y_ADR);
|
||||
writeEEPROM(acc_offset_z, acc_offset_z_ADR);
|
||||
writeEEPROM(gyro_offset_roll, gyro_offset_roll_ADR);
|
||||
writeEEPROM(gyro_offset_pitch, gyro_offset_pitch_ADR);
|
||||
writeEEPROM(gyro_offset_yaw, gyro_offset_yaw_ADR);
|
||||
writeEEPROM(Kp_ROLLPITCH, Kp_ROLLPITCH_ADR);
|
||||
writeEEPROM(Ki_ROLLPITCH, Ki_ROLLPITCH_ADR);
|
||||
writeEEPROM(Kp_YAW, Kp_YAW_ADR);
|
||||
writeEEPROM(Ki_YAW, Ki_YAW_ADR);
|
||||
writeEEPROM(GEOG_CORRECTION_FACTOR, GEOG_CORRECTION_FACTOR_ADR);
|
||||
writeEEPROM(MAGNETOMETER, MAGNETOMETER_ADR);
|
||||
writeEEPROM(Kp_RateRoll, KP_RATEROLL_ADR);
|
||||
writeEEPROM(Ki_RateRoll, KI_RATEROLL_ADR);
|
||||
writeEEPROM(Kd_RateRoll, KD_RATEROLL_ADR);
|
||||
writeEEPROM(Kp_RatePitch, KP_RATEPITCH_ADR);
|
||||
writeEEPROM(Ki_RatePitch, KI_RATEPITCH_ADR);
|
||||
writeEEPROM(Kd_RatePitch, KD_RATEPITCH_ADR);
|
||||
writeEEPROM(Kp_RateYaw, KP_RATEYAW_ADR);
|
||||
writeEEPROM(Ki_RateYaw, KI_RATEYAW_ADR);
|
||||
writeEEPROM(Kd_RateYaw, KD_RATEYAW_ADR);
|
||||
writeEEPROM(xmitFactor, XMITFACTOR_ADR);
|
||||
writeEEPROM(roll_mid, CHROLL_MID);
|
||||
writeEEPROM(pitch_mid, CHPITCH_MID);
|
||||
writeEEPROM(yaw_mid, CHYAW_MID);
|
||||
writeEEPROM(ch_roll_slope, ch_roll_slope_ADR);
|
||||
writeEEPROM(ch_pitch_slope, ch_pitch_slope_ADR);
|
||||
writeEEPROM(ch_throttle_slope, ch_throttle_slope_ADR);
|
||||
writeEEPROM(ch_yaw_slope, ch_yaw_slope_ADR);
|
||||
writeEEPROM(ch_aux_slope, ch_aux_slope_ADR);
|
||||
writeEEPROM(ch_aux2_slope, ch_aux2_slope_ADR);
|
||||
writeEEPROM(ch_roll_offset, ch_roll_offset_ADR);
|
||||
writeEEPROM(ch_pitch_offset, ch_pitch_offset_ADR);
|
||||
writeEEPROM(ch_throttle_offset, ch_throttle_offset_ADR);
|
||||
writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR);
|
||||
writeEEPROM(ch_aux_offset, ch_aux_offset_ADR);
|
||||
writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR);
|
||||
}
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Events.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Functions.pde
|
||||
Version : v1.0, Aug 28, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
|
||||
// Flash those A,B,C LEDs on IMU Board
|
||||
//
|
||||
// Function: FullBlink(int, int);
|
||||
// int 1 =
|
||||
void FullBlink(int count, int blinkdelay) {
|
||||
for(int i = 0; i <= count; i++) {
|
||||
digitalWrite(LED_Green, HIGH);
|
||||
digitalWrite(LED_Yellow, HIGH);
|
||||
digitalWrite(LED_Red, HIGH);
|
||||
delay(blinkdelay);
|
||||
digitalWrite(LED_Green, LOW);
|
||||
digitalWrite(LED_Yellow, LOW);
|
||||
digitalWrite(LED_Red, LOW);
|
||||
delay(blinkdelay);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void RunningLights(int LightStep) {
|
||||
|
||||
if(LightStep == 0) {
|
||||
digitalWrite(LED_Green, HIGH);
|
||||
digitalWrite(LED_Yellow, LOW);
|
||||
digitalWrite(LED_Red, LOW);
|
||||
}
|
||||
else if (LightStep == 1) {
|
||||
digitalWrite(LED_Green, LOW);
|
||||
digitalWrite(LED_Yellow, HIGH);
|
||||
digitalWrite(LED_Red, LOW);
|
||||
}
|
||||
else {
|
||||
digitalWrite(LED_Green, LOW);
|
||||
digitalWrite(LED_Yellow, LOW);
|
||||
digitalWrite(LED_Red, HIGH);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void LightsOff() {
|
||||
|
||||
digitalWrite(LED_Green, LOW);
|
||||
digitalWrite(LED_Yellow, LOW);
|
||||
digitalWrite(LED_Red, LOW);
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,440 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : GCS.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
//
|
||||
// Function : send_message()
|
||||
//
|
||||
// Parameters:
|
||||
// byte severity - Debug level
|
||||
// char str - Text to write
|
||||
//
|
||||
// Returns : - none
|
||||
|
||||
void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
|
||||
{
|
||||
if(severity >= DEBUG_LEVEL){
|
||||
SerPr("MSG: ");
|
||||
SerPrln(str);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
// Function : readSerialCommand()
|
||||
//
|
||||
// Parameters:
|
||||
// - none
|
||||
//
|
||||
// Returns : - none
|
||||
//
|
||||
void readSerialCommand() {
|
||||
// Check for serial message
|
||||
if (SerAv()) {
|
||||
queryType = SerRe();
|
||||
switch (queryType) {
|
||||
case 'A': // Stable PID
|
||||
KP_QUAD_ROLL = readFloatSerial();
|
||||
KI_QUAD_ROLL = readFloatSerial();
|
||||
KD_QUAD_ROLL = readFloatSerial();
|
||||
KP_QUAD_PITCH = readFloatSerial();
|
||||
KI_QUAD_PITCH = readFloatSerial();
|
||||
KD_QUAD_PITCH = readFloatSerial();
|
||||
KP_QUAD_YAW = readFloatSerial();
|
||||
KI_QUAD_YAW = readFloatSerial();
|
||||
KD_QUAD_YAW = readFloatSerial();
|
||||
STABLE_MODE_KP_RATE = readFloatSerial();
|
||||
MAGNETOMETER = readFloatSerial();
|
||||
break;
|
||||
case 'C': // Receive GPS PID
|
||||
KP_GPS_ROLL = readFloatSerial();
|
||||
KI_GPS_ROLL = readFloatSerial();
|
||||
KD_GPS_ROLL = readFloatSerial();
|
||||
KP_GPS_PITCH = readFloatSerial();
|
||||
KI_GPS_PITCH = readFloatSerial();
|
||||
KD_GPS_PITCH = readFloatSerial();
|
||||
GPS_MAX_ANGLE = readFloatSerial();
|
||||
GEOG_CORRECTION_FACTOR = readFloatSerial();
|
||||
break;
|
||||
case 'E': // Receive altitude PID
|
||||
KP_ALTITUDE = readFloatSerial();
|
||||
KD_ALTITUDE = readFloatSerial();
|
||||
KI_ALTITUDE = readFloatSerial();
|
||||
break;
|
||||
case 'G': // Receive drift correction PID
|
||||
Kp_ROLLPITCH = readFloatSerial();
|
||||
Ki_ROLLPITCH = readFloatSerial();
|
||||
Kp_YAW = readFloatSerial();
|
||||
Ki_YAW = readFloatSerial();
|
||||
break;
|
||||
case 'I': // Receive sensor offset
|
||||
gyro_offset_roll = readFloatSerial();
|
||||
gyro_offset_pitch = readFloatSerial();
|
||||
gyro_offset_yaw = readFloatSerial();
|
||||
acc_offset_x = readFloatSerial();
|
||||
acc_offset_y = readFloatSerial();
|
||||
acc_offset_z = readFloatSerial();
|
||||
break;
|
||||
case 'K': // Spare
|
||||
break;
|
||||
case 'M': // Receive debug motor commands
|
||||
frontMotor = readFloatSerial();
|
||||
backMotor = readFloatSerial();
|
||||
rightMotor = readFloatSerial();
|
||||
leftMotor = readFloatSerial();
|
||||
motorArmed = readFloatSerial();
|
||||
break;
|
||||
case 'O': // Rate Control PID
|
||||
Kp_RateRoll = readFloatSerial();
|
||||
Ki_RateRoll = readFloatSerial();
|
||||
Kd_RateRoll = readFloatSerial();
|
||||
Kp_RatePitch = readFloatSerial();
|
||||
Ki_RatePitch = readFloatSerial();
|
||||
Kd_RatePitch = readFloatSerial();
|
||||
Kp_RateYaw = readFloatSerial();
|
||||
Ki_RateYaw = readFloatSerial();
|
||||
Kd_RateYaw = readFloatSerial();
|
||||
xmitFactor = readFloatSerial();
|
||||
break;
|
||||
case 'W': // Write all user configurable values to EEPROM
|
||||
writeUserConfig();
|
||||
break;
|
||||
case 'Y': // Initialize EEPROM with default values
|
||||
defaultUserConfig();
|
||||
break;
|
||||
case '1': // Receive transmitter calibration values
|
||||
ch_roll_slope = readFloatSerial();
|
||||
ch_roll_offset = readFloatSerial();
|
||||
ch_pitch_slope = readFloatSerial();
|
||||
ch_pitch_offset = readFloatSerial();
|
||||
ch_yaw_slope = readFloatSerial();
|
||||
ch_yaw_offset = readFloatSerial();
|
||||
ch_throttle_slope = readFloatSerial();
|
||||
ch_throttle_offset = readFloatSerial();
|
||||
ch_aux_slope = readFloatSerial();
|
||||
ch_aux_offset = readFloatSerial();
|
||||
ch_aux2_slope = readFloatSerial();
|
||||
ch_aux2_offset = readFloatSerial();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void sendSerialTelemetry() {
|
||||
float aux_float[3]; // used for sensor calibration
|
||||
switch (queryType) {
|
||||
case '=': // Reserved debug command to view any variable from Serial Monitor
|
||||
/* SerPr("throttle =");
|
||||
SerPrln(ch_throttle);
|
||||
SerPr("control roll =");
|
||||
SerPrln(control_roll-CHANN_CENTER);
|
||||
SerPr("control pitch =");
|
||||
SerPrln(control_pitch-CHANN_CENTER);
|
||||
SerPr("control yaw =");
|
||||
SerPrln(control_yaw-CHANN_CENTER);
|
||||
SerPr("front left yaw =");
|
||||
SerPrln(frontMotor);
|
||||
SerPr("front right yaw =");
|
||||
SerPrln(rightMotor);
|
||||
SerPr("rear left yaw =");
|
||||
SerPrln(leftMotor);
|
||||
SerPr("rear right motor =");
|
||||
SerPrln(backMotor);
|
||||
SerPrln();
|
||||
|
||||
SerPr("current roll rate =");
|
||||
SerPrln(read_adc(0));
|
||||
SerPr("current pitch rate =");
|
||||
SerPrln(read_adc(1));
|
||||
SerPr("current yaw rate =");
|
||||
SerPrln(read_adc(2));
|
||||
SerPr("command rx yaw =");
|
||||
SerPrln(command_rx_yaw);
|
||||
SerPrln();
|
||||
queryType = 'X';*/
|
||||
SerPr(APM_RC.InputCh(0));
|
||||
comma();
|
||||
SerPr(ch_roll_slope);
|
||||
comma();
|
||||
SerPr(ch_roll_offset);
|
||||
comma();
|
||||
SerPrln(ch_roll);
|
||||
break;
|
||||
case 'B': // Send roll, pitch and yaw PID values
|
||||
SerPr(KP_QUAD_ROLL, 3);
|
||||
comma();
|
||||
SerPr(KI_QUAD_ROLL, 3);
|
||||
comma();
|
||||
SerPr(KD_QUAD_ROLL, 3);
|
||||
comma();
|
||||
SerPr(KP_QUAD_PITCH, 3);
|
||||
comma();
|
||||
SerPr(KI_QUAD_PITCH, 3);
|
||||
comma();
|
||||
SerPr(KD_QUAD_PITCH, 3);
|
||||
comma();
|
||||
SerPr(KP_QUAD_YAW, 3);
|
||||
comma();
|
||||
SerPr(KI_QUAD_YAW, 3);
|
||||
comma();
|
||||
SerPr(KD_QUAD_YAW, 3);
|
||||
comma();
|
||||
SerPr(STABLE_MODE_KP_RATE, 3);
|
||||
comma();
|
||||
SerPrln(MAGNETOMETER, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'D': // Send GPS PID
|
||||
SerPr(KP_GPS_ROLL, 3);
|
||||
comma();
|
||||
SerPr(KI_GPS_ROLL, 3);
|
||||
comma();
|
||||
SerPr(KD_GPS_ROLL, 3);
|
||||
comma();
|
||||
SerPr(KP_GPS_PITCH, 3);
|
||||
comma();
|
||||
SerPr(KI_GPS_PITCH, 3);
|
||||
comma();
|
||||
SerPr(KD_GPS_PITCH, 3);
|
||||
comma();
|
||||
SerPr(GPS_MAX_ANGLE, 3);
|
||||
comma();
|
||||
SerPrln(GEOG_CORRECTION_FACTOR, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'F': // Send altitude PID
|
||||
SerPr(KP_ALTITUDE, 3);
|
||||
comma();
|
||||
SerPr(KI_ALTITUDE, 3);
|
||||
comma();
|
||||
SerPrln(KD_ALTITUDE, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'H': // Send drift correction PID
|
||||
SerPr(Kp_ROLLPITCH, 4);
|
||||
comma();
|
||||
SerPr(Ki_ROLLPITCH, 7);
|
||||
comma();
|
||||
SerPr(Kp_YAW, 4);
|
||||
comma();
|
||||
SerPrln(Ki_YAW, 6);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'J': // Send sensor offset
|
||||
SerPr(gyro_offset_roll);
|
||||
comma();
|
||||
SerPr(gyro_offset_pitch);
|
||||
comma();
|
||||
SerPr(gyro_offset_yaw);
|
||||
comma();
|
||||
SerPr(acc_offset_x);
|
||||
comma();
|
||||
SerPr(acc_offset_y);
|
||||
comma();
|
||||
SerPrln(acc_offset_z);
|
||||
AN_OFFSET[3] = acc_offset_x;
|
||||
AN_OFFSET[4] = acc_offset_y;
|
||||
AN_OFFSET[5] = acc_offset_z;
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'L': // Spare
|
||||
// RadioCalibration();
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'N': // Send magnetometer config
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'P': // Send rate control PID
|
||||
SerPr(Kp_RateRoll, 3);
|
||||
comma();
|
||||
SerPr(Ki_RateRoll, 3);
|
||||
comma();
|
||||
SerPr(Kd_RateRoll, 3);
|
||||
comma();
|
||||
SerPr(Kp_RatePitch, 3);
|
||||
comma();
|
||||
SerPr(Ki_RatePitch, 3);
|
||||
comma();
|
||||
SerPr(Kd_RatePitch, 3);
|
||||
comma();
|
||||
SerPr(Kp_RateYaw, 3);
|
||||
comma();
|
||||
SerPr(Ki_RateYaw, 3);
|
||||
comma();
|
||||
SerPr(Kd_RateYaw, 3);
|
||||
comma();
|
||||
SerPrln(xmitFactor, 3);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case 'Q': // Send sensor data
|
||||
SerPr(read_adc(0));
|
||||
comma();
|
||||
SerPr(read_adc(1));
|
||||
comma();
|
||||
SerPr(read_adc(2));
|
||||
comma();
|
||||
SerPr(read_adc(4));
|
||||
comma();
|
||||
SerPr(read_adc(3));
|
||||
comma();
|
||||
SerPr(read_adc(5));
|
||||
comma();
|
||||
SerPr(err_roll);
|
||||
comma();
|
||||
SerPr(err_pitch);
|
||||
comma();
|
||||
SerPr(degrees(roll));
|
||||
comma();
|
||||
SerPr(degrees(pitch));
|
||||
comma();
|
||||
SerPrln(degrees(yaw));
|
||||
break;
|
||||
case 'R': // Send raw sensor data
|
||||
break;
|
||||
case 'S': // Send all flight data
|
||||
SerPr(timer-timer_old);
|
||||
comma();
|
||||
SerPr(read_adc(0));
|
||||
comma();
|
||||
SerPr(read_adc(1));
|
||||
comma();
|
||||
SerPr(read_adc(2));
|
||||
comma();
|
||||
SerPr(ch_throttle);
|
||||
comma();
|
||||
SerPr(control_roll);
|
||||
comma();
|
||||
SerPr(control_pitch);
|
||||
comma();
|
||||
SerPr(control_yaw);
|
||||
comma();
|
||||
SerPr(frontMotor); // Front Motor
|
||||
comma();
|
||||
SerPr(backMotor); // Back Motor
|
||||
comma();
|
||||
SerPr(rightMotor); // Right Motor
|
||||
comma();
|
||||
SerPr(leftMotor); // Left Motor
|
||||
comma();
|
||||
SerPr(read_adc(4));
|
||||
comma();
|
||||
SerPr(read_adc(3));
|
||||
comma();
|
||||
SerPrln(read_adc(5));
|
||||
break;
|
||||
case 'T': // Spare
|
||||
break;
|
||||
case 'U': // Send receiver values
|
||||
SerPr(ch_roll); // Aileron
|
||||
comma();
|
||||
SerPr(ch_pitch); // Elevator
|
||||
comma();
|
||||
SerPr(ch_yaw); // Yaw
|
||||
comma();
|
||||
SerPr(ch_throttle); // Throttle
|
||||
comma();
|
||||
SerPr(ch_aux); // AUX1 (Mode)
|
||||
comma();
|
||||
SerPr(ch_aux2); // AUX2
|
||||
comma();
|
||||
SerPr(roll_mid); // Roll MID value
|
||||
comma();
|
||||
SerPr(pitch_mid); // Pitch MID value
|
||||
comma();
|
||||
SerPrln(yaw_mid); // Yaw MID Value
|
||||
break;
|
||||
case 'V': // Spare
|
||||
break;
|
||||
case 'X': // Stop sending messages
|
||||
break;
|
||||
case '!': // Send flight software version
|
||||
SerPrln(VER);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case '2': // Send transmitter calibration values
|
||||
SerPr(ch_roll_slope);
|
||||
comma();
|
||||
SerPr(ch_roll_offset);
|
||||
comma();
|
||||
SerPr(ch_pitch_slope);
|
||||
comma();
|
||||
SerPr(ch_pitch_offset);
|
||||
comma();
|
||||
SerPr(ch_yaw_slope);
|
||||
comma();
|
||||
SerPr(ch_yaw_offset);
|
||||
comma();
|
||||
SerPr(ch_throttle_slope);
|
||||
comma();
|
||||
SerPr(ch_throttle_offset);
|
||||
comma();
|
||||
SerPr(ch_aux_slope);
|
||||
comma();
|
||||
SerPr(ch_aux_offset);
|
||||
comma();
|
||||
SerPr(ch_aux2_slope);
|
||||
comma();
|
||||
SerPrln(ch_aux2_offset);
|
||||
queryType = 'X';
|
||||
break;
|
||||
case '.': // Modify GPS settings, print directly to GPS Port
|
||||
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void comma() {
|
||||
SerPr(',');
|
||||
}
|
||||
|
||||
|
||||
// Used to read floating point values from the serial port
|
||||
float readFloatSerial() {
|
||||
byte index = 0;
|
||||
byte timeout = 0;
|
||||
char data[128] = "";
|
||||
|
||||
do {
|
||||
if (SerAv() == 0) {
|
||||
delay(10);
|
||||
timeout++;
|
||||
}
|
||||
else {
|
||||
data[index] = SerRe();
|
||||
timeout = 0;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128));
|
||||
return atof(data);
|
||||
}
|
||||
|
|
@ -0,0 +1,516 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Log.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
|
||||
// Code to Write and Read packets from DataFlash log memory
|
||||
// Code to interact with the user to dump or erase logs
|
||||
|
||||
#define HEAD_BYTE1 0xA3 // Decimal 163
|
||||
#define HEAD_BYTE2 0x95 // Decimal 149
|
||||
#define END_BYTE 0xBA // Decimal 186
|
||||
|
||||
#define LOG_ATTITUDE_MSG 0x01
|
||||
#define LOG_GPS_MSG 0x02
|
||||
#define LOG_MODE_MSG 0X03
|
||||
#define LOG_CONTROL_TUNING_MSG 0X04
|
||||
#define LOG_NAV_TUNING_MSG 0X05
|
||||
#define LOG_PERFORMANCE_MSG 0X06
|
||||
#define LOG_RAW_MSG 0x07
|
||||
#define LOG_RADIO_MSG 0x08
|
||||
#define LOG_SENSOR_MSG 0x09
|
||||
|
||||
|
||||
// Function to display available logs and solicit action from the user.
|
||||
// --------------------------------------------------------------------
|
||||
void Print_Log_Menu(void)
|
||||
{
|
||||
int log_start;
|
||||
int log_end;
|
||||
eeprom_busy_wait();
|
||||
byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); eeprom_busy_wait();
|
||||
if (last_log_num == 0) {
|
||||
Serial.println("No logs available for download");
|
||||
} else {
|
||||
Serial.print(last_log_num,DEC);
|
||||
Serial.println(" logs available for download");
|
||||
for(int i=1;i<last_log_num+1;i++) {
|
||||
log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i-1)*0x02)); eeprom_busy_wait();
|
||||
log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i)*0x02))-1; eeprom_busy_wait();
|
||||
if (i == last_log_num) {
|
||||
log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); eeprom_busy_wait();
|
||||
}
|
||||
Serial.print("Log number ");
|
||||
Serial.print(i);
|
||||
Serial.print(", start page ");
|
||||
Serial.print(log_start);
|
||||
Serial.print(", end page ");
|
||||
Serial.println(log_end);
|
||||
}
|
||||
}
|
||||
Serial.println(" ");
|
||||
Serial.println("Input 1 <enter> to dump a log");
|
||||
Serial.println("Input 2 <enter> to erase all logs");
|
||||
Serial.println("Input 3 <enter> to leave log mode");
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
|
||||
// Function to get desired action from the user.
|
||||
// ---------------------------------------------
|
||||
byte Get_User_Log_Command(void)
|
||||
{
|
||||
|
||||
byte step=0;
|
||||
byte char_1;
|
||||
byte char_2;
|
||||
byte char_3;
|
||||
|
||||
Serial.flush();
|
||||
|
||||
while(step<3)
|
||||
{
|
||||
if(Serial.available()){
|
||||
char_1 = Serial.read();
|
||||
char_2 = Serial.read();
|
||||
char_3 = Serial.read();
|
||||
|
||||
if (char_1<0x30 || char_1>0x39) {
|
||||
return 0;
|
||||
} else if (char_2 == 0xFF) {
|
||||
return (char_1 - 0x30);
|
||||
} else if (char_2<0x30 || char_2>0x39) {
|
||||
return 0;
|
||||
} else if (char_2 != 0xFF) {
|
||||
return 0;
|
||||
} else {
|
||||
return ((char_1 - 0x30)*10 + char_2 - 0x30);
|
||||
}
|
||||
|
||||
} else {
|
||||
delay(20);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Write an attitude packet. Total length : 10 bytes
|
||||
#if LOG_ATTITUDE
|
||||
void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
|
||||
DataFlash.WriteInt(log_roll);
|
||||
DataFlash.WriteInt(log_pitch);
|
||||
DataFlash.WriteInt(log_yaw);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LOG_SEN
|
||||
// Write a Sensor Raw data packet
|
||||
void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_SENSOR_MSG);
|
||||
DataFlash.WriteInt(s1);
|
||||
DataFlash.WriteInt(s2);
|
||||
DataFlash.WriteInt(s3);
|
||||
DataFlash.WriteInt(s4);
|
||||
DataFlash.WriteInt(s5);
|
||||
DataFlash.WriteInt(s6);
|
||||
DataFlash.WriteInt(s7);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if LOG_PM
|
||||
// Write a performance monitoring packet. Total length : 19 bytes
|
||||
void Log_Write_Performance()
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
|
||||
DataFlash.WriteLong(millis() - perf_mon_timer);
|
||||
DataFlash.WriteInt(mainLoop_count);
|
||||
DataFlash.WriteInt(G_Dt_max);
|
||||
DataFlash.WriteByte(gyro_sat_count);
|
||||
DataFlash.WriteByte(adc_constraints);
|
||||
DataFlash.WriteByte(renorm_sqrt_count);
|
||||
DataFlash.WriteByte(renorm_blowup_count);
|
||||
DataFlash.WriteByte(gps_fix_count);
|
||||
DataFlash.WriteInt((int)(imu_health*1000));
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if LOG_CTUN
|
||||
// Write a control tuning packet. Total length : 22 bytes
|
||||
void Log_Write_Control_Tuning()
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
||||
DataFlash.WriteInt((int)(servo_out[CH_ROLL]*100));
|
||||
DataFlash.WriteInt((int)nav_roll);
|
||||
DataFlash.WriteInt((int)roll_sensor);
|
||||
DataFlash.WriteInt((int)(servo_out[CH_PITCH]*100));
|
||||
DataFlash.WriteInt((int)nav_pitch);
|
||||
DataFlash.WriteInt((int)pitch_sensor);
|
||||
DataFlash.WriteInt((int)(servo_out[CH_THROTTLE]*100));
|
||||
DataFlash.WriteInt((int)(servo_out[CH_RUDDER]*100));
|
||||
DataFlash.WriteInt((int)AN[5]);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if LOG_NTUN
|
||||
// Write a navigation tuning packet. Total length : 18 bytes
|
||||
void Log_Write_Nav_Tuning()
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
|
||||
DataFlash.WriteInt((int)yaw_sensor);
|
||||
DataFlash.WriteInt((int)wp_distance);
|
||||
DataFlash.WriteInt((int)target_bearing);
|
||||
DataFlash.WriteInt((int)nav_bearing);
|
||||
DataFlash.WriteInt(altitude_error);
|
||||
DataFlash.WriteInt((int)airspeed_error);
|
||||
DataFlash.WriteInt((int)(nav_gain_scaler*1000));
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if LOG_MODE
|
||||
// Write a mode packet. Total length : 5 bytes
|
||||
void Log_Write_Mode(byte mode)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_MODE_MSG);
|
||||
DataFlash.WriteByte(mode);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if LOG_GPS
|
||||
// Write an GPS packet. Total length : 30 bytes
|
||||
void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_mix_alt, long log_gps_alt,
|
||||
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_GPS_MSG);
|
||||
DataFlash.WriteLong(log_Time);
|
||||
DataFlash.WriteByte(log_Fix);
|
||||
DataFlash.WriteByte(log_NumSats);
|
||||
DataFlash.WriteLong(log_Lattitude);
|
||||
DataFlash.WriteLong(log_Longitude);
|
||||
DataFlash.WriteLong(log_mix_alt);
|
||||
DataFlash.WriteLong(log_gps_alt);
|
||||
DataFlash.WriteLong(log_Ground_Speed);
|
||||
DataFlash.WriteLong(log_Ground_Course);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if LOG_RAW
|
||||
// Write an raw accel/gyro data packet. Total length : 28 bytes
|
||||
void Log_Write_Raw()
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_RAW_MSG);
|
||||
for(int i=0;i<6;i++)
|
||||
DataFlash.WriteLong((long)(AN[i]*1000.0));
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// Read an control tuning packet
|
||||
void Log_Read_Control_Tuning()
|
||||
{
|
||||
float logvar;
|
||||
|
||||
Serial.print("CTUN:");
|
||||
for (int y=1;y<10;y++) {
|
||||
logvar = DataFlash.ReadInt();
|
||||
if(y < 9) logvar = logvar/100.f;
|
||||
Serial.print(logvar);
|
||||
Serial.print(",");
|
||||
}
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
// Read a nav tuning packet
|
||||
void Log_Read_Nav_Tuning()
|
||||
{
|
||||
float logvar;
|
||||
|
||||
Serial.print("NTUN:");
|
||||
for (int y=1;y<8;y++) {
|
||||
logvar = DataFlash.ReadInt();
|
||||
if(y > 6) logvar = logvar/1000.f;
|
||||
Serial.print(logvar);
|
||||
Serial.print(",");
|
||||
}
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
// Read a performance packet
|
||||
void Log_Read_Performance()
|
||||
{
|
||||
long pm_time;
|
||||
int logvar;
|
||||
|
||||
Serial.print("PM:");
|
||||
pm_time = DataFlash.ReadLong();
|
||||
Serial.print(pm_time);
|
||||
Serial.print(",");
|
||||
for (int y=1;y<9;y++) {
|
||||
if(y<3 || y>7) logvar = DataFlash.ReadInt();
|
||||
else logvar = DataFlash.ReadByte();
|
||||
if(y > 7) logvar = logvar/1000.f;
|
||||
Serial.print(logvar);
|
||||
Serial.print(",");
|
||||
}
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
// Read an attitude packet
|
||||
void Log_Read_Attitude()
|
||||
{
|
||||
int log_roll;
|
||||
int log_pitch;
|
||||
int log_yaw;
|
||||
|
||||
log_roll = DataFlash.ReadInt();
|
||||
log_pitch = DataFlash.ReadInt();
|
||||
log_yaw = DataFlash.ReadInt();
|
||||
Serial.print("ATT:");
|
||||
Serial.print(log_roll);
|
||||
Serial.print(",");
|
||||
Serial.print(log_pitch);
|
||||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
// Read a mode packet
|
||||
void Log_Read_Mode()
|
||||
{
|
||||
byte mode;
|
||||
|
||||
mode = DataFlash.ReadByte();
|
||||
Serial.print("MOD:");
|
||||
switch (mode) {
|
||||
case 0:
|
||||
Serial.println("Manual mode");
|
||||
break;
|
||||
case 1:
|
||||
Serial.println("Stabilize mode");
|
||||
break;
|
||||
case 5:
|
||||
Serial.println("Fly By Wire A mode");
|
||||
break;
|
||||
case 6:
|
||||
Serial.println("Fly By Wire B mode");
|
||||
break;
|
||||
case 10:
|
||||
Serial.println("AUTO mode");
|
||||
break;
|
||||
case 11:
|
||||
Serial.println("RTL mode");
|
||||
break;
|
||||
case 12:
|
||||
Serial.println("Loiter mode");
|
||||
break;
|
||||
case 98:
|
||||
Serial.println("Air Start Complete");
|
||||
break;
|
||||
case 99:
|
||||
Serial.println("Ground Start Complete");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Read a GPS packet
|
||||
void Log_Read_GPS()
|
||||
{
|
||||
long log_Time;
|
||||
byte log_Fix;
|
||||
byte log_NumSats;
|
||||
long log_Lattitude;
|
||||
long log_Longitude;
|
||||
long log_gps_alt;
|
||||
long log_mix_alt;
|
||||
float log_Ground_Speed;
|
||||
float log_Ground_Course;
|
||||
|
||||
log_Time = DataFlash.ReadLong();
|
||||
log_Fix = DataFlash.ReadByte();
|
||||
log_NumSats = DataFlash.ReadByte();
|
||||
log_Lattitude = DataFlash.ReadLong();
|
||||
log_Longitude = DataFlash.ReadLong();
|
||||
log_mix_alt = DataFlash.ReadLong();
|
||||
log_gps_alt = DataFlash.ReadLong();
|
||||
log_Ground_Speed = DataFlash.ReadLong();
|
||||
log_Ground_Course = DataFlash.ReadLong();
|
||||
|
||||
Serial.print("GPS:");
|
||||
Serial.print(log_Time);
|
||||
Serial.print(",");
|
||||
Serial.print((int)log_Fix);
|
||||
Serial.print(",");
|
||||
Serial.print((int)log_NumSats);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Lattitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Longitude);
|
||||
Serial.print(",");
|
||||
Serial.print(log_mix_alt/100.0);
|
||||
Serial.print(",");
|
||||
Serial.print(log_gps_alt/100.0);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Ground_Speed/100.0f);
|
||||
Serial.print(",");
|
||||
Serial.print(log_Ground_Course/100.0); // This is incorrect!!!!!
|
||||
Serial.println();
|
||||
|
||||
}
|
||||
|
||||
// Read a raw accel/gyro packet
|
||||
void Log_Read_Raw()
|
||||
{
|
||||
float logvar;
|
||||
|
||||
Serial.print("RAW:");
|
||||
for (int y=0;y<6;y++) {
|
||||
logvar = DataFlash.ReadLong()/1000.f;
|
||||
Serial.print(logvar);
|
||||
Serial.print(",");
|
||||
}
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
// Read the DataFlash log memory : Packet Parser
|
||||
void Log_Read(int start_page, int end_page)
|
||||
{
|
||||
byte data;
|
||||
byte log_step=0;
|
||||
int packet_count=0;
|
||||
int flash_led = 1;
|
||||
|
||||
DataFlash.StartRead(start_page);
|
||||
while (DataFlash.GetPage() < end_page)
|
||||
{
|
||||
data = DataFlash.ReadByte();
|
||||
switch(log_step) //This is a state machine to read the packets
|
||||
{
|
||||
case 0:
|
||||
if(data==HEAD_BYTE1) // Head byte 1
|
||||
log_step++;
|
||||
break;
|
||||
case 1:
|
||||
if(data==HEAD_BYTE2) // Head byte 2
|
||||
log_step++;
|
||||
break;
|
||||
case 2:
|
||||
if(data==LOG_ATTITUDE_MSG){
|
||||
Log_Read_Attitude();
|
||||
log_step++;
|
||||
|
||||
}else if(data==LOG_MODE_MSG){
|
||||
Log_Read_Mode();
|
||||
log_step++;
|
||||
|
||||
}else if(data==LOG_CONTROL_TUNING_MSG){
|
||||
Log_Read_Control_Tuning();
|
||||
log_step++;
|
||||
|
||||
}else if(data==LOG_NAV_TUNING_MSG){
|
||||
Log_Read_Nav_Tuning();
|
||||
log_step++;
|
||||
|
||||
}else if(data==LOG_PERFORMANCE_MSG){
|
||||
Log_Read_Performance();
|
||||
log_step++;
|
||||
|
||||
}else if(data==LOG_RAW_MSG){
|
||||
Log_Read_Raw();
|
||||
log_step++;
|
||||
}else {
|
||||
if(data==LOG_GPS_MSG){
|
||||
Log_Read_GPS();
|
||||
log_step++;
|
||||
} else {
|
||||
Serial.print("Error Reading Packet: ");
|
||||
Serial.print(packet_count);
|
||||
log_step=0; // Restart, we have a problem...
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if(data==END_BYTE){
|
||||
packet_count++;
|
||||
}else{
|
||||
Serial.print("Error Reading END_BYTE ");
|
||||
Serial.println(data,DEC);
|
||||
}
|
||||
log_step=0; // Restart sequence: new packet...
|
||||
if(flash_led > 0) {
|
||||
digitalWrite(A_LED_PIN,HIGH);
|
||||
flash_led++;
|
||||
if(flash_led>100) flash_led=-1;
|
||||
} else {
|
||||
digitalWrite(A_LED_PIN,LOW);
|
||||
flash_led--;
|
||||
if(flash_led<-100) flash_led=1;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
Serial.print("Number of packets read: ");
|
||||
Serial.println(packet_count);
|
||||
digitalWrite(A_LED_PIN,HIGH);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Mixer.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Motors.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Navigation.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Radio.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : Sensors.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
|
@ -0,0 +1,203 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : System.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
// General Initialization for all APM electronics
|
||||
void APM_Init() {
|
||||
|
||||
int i, j; // Temporary variables used to count things
|
||||
float aux_float[3];
|
||||
|
||||
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
|
||||
pinMode(LED_Red,OUTPUT); //Red LED B (PC2)
|
||||
pinMode(LED_Green,OUTPUT); //Green LED C (PC0)
|
||||
|
||||
pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0)
|
||||
|
||||
pinMode(RELE_pin,OUTPUT); // Rele output
|
||||
|
||||
digitalWrite(RELE_pin,LOW);
|
||||
|
||||
// delay(1000); // Wait until frame is not moving after initial power cord has connected
|
||||
FullBlink(50,20);
|
||||
|
||||
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
APM_ADC.Init(); // APM ADC library initialization
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
|
||||
#ifdef IsGPS
|
||||
GPS.Init(); // GPS Initialization
|
||||
#ifdef IsNEWMTEK
|
||||
delay(250);
|
||||
// DIY Drones MTEK GPS needs binary sentences activated if you upgraded to latest firmware.
|
||||
// If your GPS shows solid blue but LED C (Red) does not go on, your GPS is on NMEA mode
|
||||
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
|
||||
#endif
|
||||
#endif
|
||||
|
||||
readUserConfig(); // Load user configurable items from EEPROM
|
||||
|
||||
// Safety measure for Channel mids
|
||||
if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500;
|
||||
if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
|
||||
if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
|
||||
|
||||
// RC channels Initialization (Quad motors)
|
||||
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
|
||||
APM_RC.OutputCh(1,MIN_THROTTLE);
|
||||
APM_RC.OutputCh(2,MIN_THROTTLE);
|
||||
APM_RC.OutputCh(3,MIN_THROTTLE);
|
||||
|
||||
if (MAGNETOMETER == 1)
|
||||
APM_Compass.Init(); // I2C initialization
|
||||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
|
||||
Serial.begin(115200);
|
||||
//Serial.println("ArduCopter Quadcopter v1.0");
|
||||
|
||||
// Check if we enable the DataFlash log Read Mode (switch)
|
||||
// If we press switch 1 at startup we read the Dataflash eeprom
|
||||
while (digitalRead(SW1_pin)==0)
|
||||
{
|
||||
Serial.println("Entering Log Read Mode...");
|
||||
Log_Read(1,1000);
|
||||
delay(30000);
|
||||
}
|
||||
|
||||
Read_adc_raw();
|
||||
delay(20);
|
||||
|
||||
// Offset values for accels and gyros...
|
||||
AN_OFFSET[3] = acc_offset_x;
|
||||
AN_OFFSET[4] = acc_offset_y;
|
||||
AN_OFFSET[5] = acc_offset_z;
|
||||
aux_float[0] = gyro_offset_roll;
|
||||
aux_float[1] = gyro_offset_pitch;
|
||||
aux_float[2] = gyro_offset_yaw;
|
||||
|
||||
j = 0;
|
||||
// Take the gyro offset values
|
||||
for(i=0;i<300;i++)
|
||||
{
|
||||
Read_adc_raw();
|
||||
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
|
||||
{
|
||||
aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
|
||||
//Serial.print(AN[y]);
|
||||
//Serial.print(",");
|
||||
}
|
||||
//Serial.println();
|
||||
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
|
||||
delay(10);
|
||||
|
||||
RunningLights(j);
|
||||
// Runnings lights effect to let user know that we are taking mesurements
|
||||
if((i % 5) == 0) j++;
|
||||
if(j >= 3) j = 0;
|
||||
}
|
||||
|
||||
// Switch off all ABC lights
|
||||
LightsOff();
|
||||
|
||||
for(int y=0; y<=2; y++)
|
||||
AN_OFFSET[y]=aux_float[y];
|
||||
|
||||
// Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
|
||||
#ifndef CONFIGURATOR
|
||||
for(i=0;i<6;i++)
|
||||
{
|
||||
Serial.print("AN[]:");
|
||||
Serial.println(AN_OFFSET[i]);
|
||||
}
|
||||
Serial.print("Yaw neutral value:");
|
||||
// Serial.println(Neutro_yaw);
|
||||
Serial.print(yaw_mid);
|
||||
#endif
|
||||
|
||||
#ifdef RADIO_TEST_MODE // RADIO TEST MODE TO TEST RADIO CHANNELS
|
||||
while(1)
|
||||
{
|
||||
if (APM_RC.GetState()==1)
|
||||
{
|
||||
Serial.print("AIL:");
|
||||
Serial.print(APM_RC.InputCh(0));
|
||||
Serial.print("ELE:");
|
||||
Serial.print(APM_RC.InputCh(1));
|
||||
Serial.print("THR:");
|
||||
Serial.print(APM_RC.InputCh(2));
|
||||
Serial.print("YAW:");
|
||||
Serial.print(APM_RC.InputCh(3));
|
||||
Serial.print("AUX(mode):");
|
||||
Serial.print(APM_RC.InputCh(4));
|
||||
Serial.print("AUX2:");
|
||||
Serial.print(APM_RC.InputCh(5));
|
||||
Serial.println();
|
||||
delay(200);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
delay(1000);
|
||||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
timer = millis();
|
||||
tlmTimer = millis();
|
||||
Read_adc_raw(); // Initialize ADC readings...
|
||||
delay(20);
|
||||
|
||||
#ifdef IsAM
|
||||
// Switch Left & Right lights on
|
||||
digitalWrite(RI_LED, HIGH);
|
||||
digitalWrite(LE_LED, HIGH);
|
||||
#endif
|
||||
|
||||
motorArmed = 0;
|
||||
digitalWrite(LED_Green,HIGH); // Ready to go...
|
||||
}
|
||||
|
||||
|
||||
void resetPerfData(void) {
|
||||
mainLoop_count = 0;
|
||||
G_Dt_max = 0;
|
||||
gyro_sat_count = 0;
|
||||
adc_constraints = 0;
|
||||
renorm_sqrt_count = 0;
|
||||
renorm_blowup_count = 0;
|
||||
gps_fix_count = 0;
|
||||
perf_mon_timer = millis();
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue