From 9d8ea2563fcb4601074833c0ee42fc9471c9dd7d Mon Sep 17 00:00:00 2001 From: CaranchoEngineering Date: Mon, 20 Sep 2010 09:38:11 +0000 Subject: [PATCH] refactoring of code beginning git-svn-id: https://arducopter.googlecode.com/svn/trunk@533 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArducopterNG/ArducopterNG.pde | 284 ++++++++++++---------------------- ArducopterNG/DCM.pde | 26 +--- ArducopterNG/Sensors.pde | 47 +++++- 3 files changed, 141 insertions(+), 216 deletions(-) diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 61b6d800ac..0357c2a212 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -4,7 +4,7 @@ An Open Source Arduino based multicopter. File : Arducopter.pde - Version : v1.0, Aug 27, 2010 + Version : v0.1, October 2010 Author(s): ArduCopter Team Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz, Jani Hirvinen, Ken McEwans, Roberto Navoni, @@ -35,23 +35,11 @@ /* 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 @@ -59,34 +47,18 @@ TODO: //#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!) -/**********************************************/ - +// 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 // 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 ******************* */ /* ************************************************************ */ @@ -100,72 +72,79 @@ TODO: #include // ArduPilot Mega BMP085 Library #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Magnetometer Library -//#include "defines.h" -//#include "console.h" #include // I2C Communication library - -#ifdef UseBMP #include // ArduPilot Mega BMP085 Library -#endif +#include // EEPROM +#include "Arducopter.h" +#include "ArduUser.h" -//#include // ArduPilot IMU/SIM GPS Library +// GPS #include // ArduPilot MTK GPS Library +//#include // ArduPilot IMU/SIM GPS Library //#include // ArduPilot Ublox GPS Library //#include // ArduPilot NMEA GPS library -// EEPROM storage for user configurable values -#include // EEPROM -#include "Arducopter.h" -#include "ArduUser.h" - /* Software version */ -#define VER 1.40 // Current software version (only numeric values) - - - +#define VER 0.1 // Current software version (only numeric values) /* ************************************************************ */ /* ************* MAIN PROGRAM - DECLARATIONS ****************** */ /* ************************************************************ */ - - - - +#define ROLL 0 +#define PITCH 1 +#define YAW 2 +#define XAXIS 0 +#define YAXIS 1 +#define ZAXIS 2 + +#define GYROZ 0 +#define GYROX 1 +#define GYROY 2 +#define ACCELX 3 +#define ACCELY 4 +#define ACCELZ 5 +#define LASTSENSOR 6 +int rawADC[6]; +int zeroADC[6]; +int dataADC[6]; +byte channel; +int sensorSign[6] = {1, 1, 1, 1, 1, 1}; // GYROZ, GYROX, GYROY, ACCELX, ACCELY, ACCELZ + +#define STABLE 0 +#define ACRO 1 +byte flightMode; + +unsigned long currentTime, previousTime, deltaTime; +unsigned long sensorLoop = 0; +unsigned long controlLoop = 0; +unsigned long radioLoop = 0; +unsigned long motorLoop = 0; /* ************************************************************ */ /* **************** MAIN PROGRAM - SETUP ********************** */ /* ************************************************************ */ void setup() { - - //APM_Init_IO(); APM_Init(); - //APM_Init_ADC(); //APM_Init_Radio(); //APM_Init_Serial(); //APM_Init_xx -// Just add this in now and edit later - int i, j; - 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); - APM_RC.Init(); // APM Radio initialization + // APM Radio initialization + APM_RC.Init(); // 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); - // delay(1000); // Wait until frame is not moving after initial power cord has connected for(i = 0; i <= 50; i++) { digitalWrite(LED_Green, HIGH); digitalWrite(LED_Yellow, HIGH); @@ -178,127 +157,37 @@ void setup() { } 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; - - if (MAGNETOMETER == 1) - APM_Compass.Init(); // I2C initialization + DataFlash.Init(); // DataFlash log initialization DataFlash.StartWrite(1); // Start a write session on page 1 - - SerBeg(SerBau); // Initialize SerialXX.port, IsXBEE define declares which port -#ifndef CONFIGURATOR - SerPri("ArduCopter Quadcopter v"); - SerPriln(VER) - SerPri("Serial ready on port: "); // Printout greeting to selecter serial port - SerPriln(SerPor); // Printout serial port name -#endif - // 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) - { + while (digitalRead(SW1_pin)==0) { SerPriln("Entering Log Read Mode..."); Log_Read(1,2000); delay(30000); } - Read_adc_raw(); - delay(10); - - // 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; - //SerPri(AN[y]); - //SerPri(","); - } - //SerPriln(); - Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); - delay(10); - - // Runnings lights effect to let user know that we are taking mesurements - if(j == 0) { - digitalWrite(LED_Green, HIGH); - digitalWrite(LED_Yellow, LOW); - digitalWrite(LED_Red, LOW); - } - else if (j == 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); - } - if((i % 5) == 0) j++; - if(j >= 3) j = 0; - } - digitalWrite(LED_Green, LOW); - digitalWrite(LED_Yellow, LOW); - digitalWrite(LED_Red, LOW); - - 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++) - { - SerPri("AN[]:"); - SerPriln(AN_OFFSET[i]); - } - SerPri("Yaw neutral value:"); - // SerPriln(Neutro_yaw); - SerPri(yaw_mid); -#endif - delay(1000); + #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 - DataFlash.StartWrite(1); // Start a write session on page 1 - timer = millis(); - tlmTimer = millis(); - Read_adc_raw(); // Initialize ADC readings... - delay(20); + readUserConfig(); // Load user configurable items from EEPROM -#ifdef IsAM - // Switch Left & Right lights on - digitalWrite(RI_LED, HIGH); - digitalWrite(LE_LED, HIGH); -#endif + if (MAGNETOMETER == 1) + APM_Compass.Init(); // I2C initialization - motorArmed = 0; - digitalWrite(LED_Green,HIGH); // Ready to go... + calibrateSensors(); + previousTime = millis(); + motorArmed = 0; + digitalWrite(LED_Green,HIGH); // Ready to go... } @@ -306,21 +195,39 @@ void setup() { /* ************************************************************ */ /* ************** MAIN PROGRAM - MAIN LOOP ******************** */ /* ************************************************************ */ + +// fast rate +// read sensors +// update attitude +// motor control + +// medium rate +// read transmitter +// magnetometer +// barometer + +// slow rate +// external command/telemetry +// GPS + + 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.0f; - fast_loopTimer = millis(); - mainLoop_count++; - // Execute the fast loop - // --------------------- - // fast_loop(); - // - PWM Updates - // - Stabilization - // - Altitude correction - if(FL_mode == 0) { // Changed for variable + currentTime = millis(); + deltaTime = currentTime - previousTime; + G_Dt = deltaTime / 1000.0; + previousTime = currentTime; + + // Read Sensors ************************************************** + if (currentTime > sensorLoop + 2) { // 500Hz (every 2ms) + for (channel = GYROZ; channel < LASTSENSOR; channel++) { + dataADC[channel] = readADC(channel); // defined in Sensors.pde + } + sensorLoop = currentTime; + } + + // Update ArduCopter Control ************************************* + if (currentTime > controlLoop + 5) { // 200Hz (every 5ms) + if(flightMode == STABLE) { // Changed for variable gled_speed = 1200; Attitude_control_v3(); } @@ -330,6 +237,15 @@ void loop() { // Reset yaw, so if we change to stable mode we continue with the actual yaw direction command_rx_yaw = ToDeg(yaw); } + controlLoop = currentTime; + } + + // Execute the fast loop + // --------------------- + // fast_loop(); + // - PWM Updates + // - Stabilization + // - Altitude correction // Execute the medium loop // ----------------------- diff --git a/ArducopterNG/DCM.pde b/ArducopterNG/DCM.pde index a8c28a9005..cb585ecc13 100644 --- a/ArducopterNG/DCM.pde +++ b/ArducopterNG/DCM.pde @@ -33,30 +33,6 @@ 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 ********************* */ @@ -258,4 +234,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) } } - + diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde index 5a8bb468bf..f20749de37 100644 --- a/ArducopterNG/Sensors.pde +++ b/ArducopterNG/Sensors.pde @@ -23,12 +23,45 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . -* ************************************************************** * -ChangeLog: - - -* ************************************************************** * -TODO: +* ************************************************************** */ +int readADC(byte channel) { + if (sensorSign[channel] < 0) + return (zeroADC[channel] - APM_ADC.Ch(channel)); + else + return (APM_ADC.Ch(channel) - zeroADC[channel]); +} -* ************************************************************** */ +void calibrateSensors(void) { + int j = 0; + // Take the gyro offset values + for(int i=0;i<300;i++) { + for (channel = GYROZ; channel < LASTSENSOR; channel++) { + rawADC[channel] = APM_ADC.Ch(channel); + zeroADC[channel] = (zeroADC[channel] * 0.8) + (rawADC[channel] * 0.2); + Log_Write_Sensor(rawADC[GYROZ], rawADC[GYROX], rawADC[GYROY], rawADC[ACCELX], rawADC[ACCELY], rawADC[ACCELZ], receiverData[THROTTLE]); + } + delay(5); + // Runnings lights effect to let user know that we are taking mesurements + if(j == 0) { + digitalWrite(LED_Green, HIGH); + digitalWrite(LED_Yellow, LOW); + digitalWrite(LED_Red, LOW); + } + else if (j == 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); + } + if((i % 5) == 0) j++; + if(j >= 3) j = 0; + } + digitalWrite(LED_Green, LOW); + digitalWrite(LED_Yellow, LOW); + digitalWrite(LED_Red, LOW); +}