zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

refactoring of code beginning 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@533 f9c3cf11-9bcb-44bc-f272-b75c42450872
mission-4.1.18
CaranchoEngineering 15 years ago
parent
241fd41289
commit
9d8ea2563f
3 changed files with 141 additions and 216 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 284
      ArducopterNG/ArducopterNG.pde
  2. 26
      ArducopterNG/DCM.pde
  3. 47
      ArducopterNG/Sensors.pde

284
ArducopterNG/ArducopterNG.pde
Unescape View File

@ -4,7 +4,7 @@ @@ -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 @@ @@ -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: @@ -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: @@ -100,72 +72,79 @@ TODO:
#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 <EEPROM.h> // EEPROM
#include "Arducopter.h"
#include "ArduUser.h"
//#include <GPS_IMU.h> // ArduPilot IMU/SIM GPS Library
// GPS
#include <GPS_MTK.h> // ArduPilot MTK GPS Library
//#include <GPS_IMU.h> // ArduPilot IMU/SIM 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)
#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() { @@ -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() { @@ -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() { @@ -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
// -----------------------

26
ArducopterNG/DCM.pde
Unescape View File

@ -33,30 +33,6 @@ TODO: @@ -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]) @@ -258,4 +234,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
}
}

47
ArducopterNG/Sensors.pde
Unescape View File

@ -23,12 +23,45 @@ @@ -23,12 +23,45 @@
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:
* ************************************************************** */
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);
}

Write Preview
Loading…
Cancel
Save