Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

refactoring of code beginning

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@533 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
CaranchoEngineering 2010-09-20 09:38:11 +00:00
parent 241fd41289
commit 9d8ea2563f
3 changed files with 140 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

266
ArducopterNG/ArducopterNG.pde
View File

@ -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
//#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
//#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 ******************* */
/* ************************************************************ */
@ -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 <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"
// 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
/* 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,7 +157,16 @@ void setup() {
}
APM_ADC.Init(); // APM ADC library initialization
DataFlash.Init(); // DataFlash log initialization
DataFlash.StartWrite(1); // Start a write session on page 1
// 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) {
SerPriln("Entering Log Read Mode...");
Log_Read(1,2000);
delay(30000);
}
#ifdef IsGPS
GPS.Init(); // GPS Initialization
@ -192,113 +180,14 @@ void setup() {
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.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)
{
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);
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
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
// -----------------------

24
ArducopterNG/DCM.pde
View File

@ -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 ********************* */

49
ArducopterNG/Sensors.pde
View File

@ -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);
}