mirror of https://github.com/ArduPilot/ardupilot
cleaning up and preparing for CLI
git-svn-id: https://arducopter.googlecode.com/svn/trunk@687 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
669bde1e45
commit
ede3a46cf1
|
|
@ -36,40 +36,71 @@ 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 // Insert correct PIN values for these, JP/17-10-10
|
||||
//#define VDIV2 AN2
|
||||
//#define VDIV3 AN3
|
||||
//#define VDIV4 AN4
|
||||
#define LED_Yellow 36 // A_LED
|
||||
#define LED_Green 37 // B_LED
|
||||
#define LED_Red 35 // C_LED
|
||||
|
||||
//#define AN5
|
||||
//#define AN6
|
||||
#define A_LED_PIN LED_Green // For legacy issues
|
||||
#define B_LED_PIN LED_Yellow
|
||||
#define C_LED_PIN LED_Red
|
||||
|
||||
// Hardware Parameters
|
||||
#define SLIDE_SWITCH_PIN 40
|
||||
#define PUSHBUTTON_PIN 41
|
||||
// Programmable hardware switches/relays
|
||||
#define RELAY 47 // Onboard relay (PL2)
|
||||
#define SW1 41 // Push button close to I2C port (PG0)
|
||||
#define SW2 40 // Slide switch next to DIP switched (PG1)
|
||||
|
||||
#define A_LED_PIN 37
|
||||
#define B_LED_PIN 36
|
||||
#define C_LED_PIN 35
|
||||
// Due limitations of Arduino libraries, these pins needs to be controlled differently so no real PIN numbers
|
||||
//#define DIP1 (PE7)
|
||||
//#define DIP2 (PE6)
|
||||
//#define DIP3 (PL6)
|
||||
//#define DIP4 (PL/)
|
||||
|
||||
#define DIP1 // Insert correct PIN values for these, JP/17-10-10
|
||||
#define DIP2
|
||||
#define DIP3
|
||||
#define DIP4
|
||||
/* ************************************************************** */
|
||||
/* Expansion PIN's that people can use for various things. */
|
||||
|
||||
// AN0 - 7 are located at edge of IMU PCB "above" pressure sensor and Expansion port
|
||||
// AN0 - 5 are also located next to voltage dividers and sliding SW2 switch
|
||||
// AN0 - 3 has 10kOhm resistor in serial, include 3.9kOhm to make it as voltage divider
|
||||
// AN4 - 5 are direct GPIO pins from atmega1280 and they are the latest pins next to SW2 switch
|
||||
// Look more ArduCopter Wiki for voltage dividers and other ports
|
||||
#define AN0 54 // resistor, vdiv use, divider 1 closest to relay
|
||||
#define AN1 55 // resistor, vdiv use, divider 2
|
||||
#define AN2 56 // resistor, vdiv use, divider 3
|
||||
#define AN3 57 // resistor, vdiv use, divider 4 closest to SW2
|
||||
#define AN4 58 // direct GPIO pin, default as analog input, next to SW2 switch
|
||||
#define AN5 59 // direct GPIO pin, default as analog input, next to SW2 switch
|
||||
#define AN6 60 // direct GPIO pin, default as analog input, close to Pressure sensor, Expansion Ports
|
||||
#define AN7 61 // direct GPIO pin, default as analog input, close to Pressure sensor, Expansion Ports
|
||||
|
||||
// AN8 - 15 are located at edge of IMU PCB "above" pressure sensor and Expansion port
|
||||
// AN8 - 15 PINs are not connected anywhere, they are located as last 8 pins on edge of the board above Expansion Ports
|
||||
// even pins (8,10,12,14) are at edge of board, Odd pins (9,11,13,15) are on inner row
|
||||
#define AN8 62 // NC
|
||||
#define AN9 63 // NC
|
||||
#define AN10 64 // NC
|
||||
#define AN11 65 // NC
|
||||
#define AN12 66 // NC
|
||||
#define AN13 67 // NC
|
||||
#define AN14 68 // NC
|
||||
#define AN15 69 // NC
|
||||
|
||||
// Defines for Voltage Dividers
|
||||
#define VDIV1 AN0 // AN0 as default and primary
|
||||
#define VDIV2 AN1 // AN1 for secondary battery
|
||||
#define VDIV3 AN2
|
||||
#define VDIV4 AN3
|
||||
|
||||
|
||||
/* ************************************************** */
|
||||
#define EE_LAST_LOG_PAGE 0xE00
|
||||
#define EE_LAST_LOG_NUM 0xE02
|
||||
#define EE_LOG_1_START 0xE04
|
||||
|
||||
// Serial ports
|
||||
/* ************************************************** */
|
||||
/* Serial port definitions */
|
||||
#define SERIAL0_BAUD 38400 // this is the main USB out 38400 57600 115200
|
||||
#define SERIAL1_BAUD 115200
|
||||
#define SERIAL2_BAUD 115200
|
||||
|
|
@ -99,6 +130,8 @@ TODO:
|
|||
#define SerPor "Telemetry"
|
||||
#endif
|
||||
|
||||
|
||||
/* *********************************************** */
|
||||
// IMU definitions
|
||||
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
|
||||
uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Hardware
|
||||
|
|
@ -109,7 +142,7 @@ int SENSOR_SIGN[]={
|
|||
//{-1,1,-1,1,-1,1,-1,-1,-1};
|
||||
/* APM Hardware definitions, END */
|
||||
|
||||
|
||||
/* *********************************************** */
|
||||
/* General definitions */
|
||||
#define TRUE 1
|
||||
#define FALSE 0
|
||||
|
|
@ -144,12 +177,12 @@ 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 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;
|
||||
|
|
@ -157,8 +190,8 @@ float Omega[3]= {0, 0, 0};
|
|||
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 COGX = 0; // Course overground X axis
|
||||
float COGY = 1; // Course overground Y axis
|
||||
|
||||
float roll = 0;
|
||||
float pitch = 0;
|
||||
|
|
@ -170,6 +203,7 @@ 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 },
|
||||
|
|
@ -257,7 +291,8 @@ long press_alt = 0;
|
|||
#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 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
|
||||
|
|
@ -265,9 +300,10 @@ float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage, initialized a
|
|||
|
||||
// Sonar variables
|
||||
int Sonar_value=0;
|
||||
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
|
||||
int Sonar_Counter=0;
|
||||
|
||||
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
|
||||
|
||||
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
|
||||
byte AP_mode = 2;
|
||||
|
||||
|
|
@ -311,33 +347,32 @@ 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
|
||||
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
|
||||
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 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;
|
||||
//unsigned int GPS_timer = 0;
|
||||
|
||||
|
||||
|
||||
/****************************************************/
|
||||
/*Logging Stuff - These should be 1 (on) or 0 (off)*/
|
||||
/****************************************************/
|
||||
/* ******************************************************** */
|
||||
/* 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£
|
||||
|
|
@ -370,27 +405,27 @@ unsigned long elapsedTime = 0; // for doing custom events
|
|||
|
||||
// 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
|
||||
// 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
|
||||
// SEVERITY_LOW
|
||||
// SEVERITY_MEDIUM
|
||||
// SEVERITY_HIGH
|
||||
// SEVERITY_CRITICAL
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -125,13 +125,15 @@ unsigned long slowLoop = 0;
|
|||
/* ************************************************************ */
|
||||
void setup() {
|
||||
|
||||
APM_Init(); // APM Hardware initialization (in System.pde)
|
||||
APM_Init(); // APM Hardware initialization (in System.pde)
|
||||
|
||||
mainLoop = millis(); // Initialize timers
|
||||
mainLoop = millis(); // Initialize timers
|
||||
mediumLoop = mainLoop;
|
||||
GPS_timer = mainLoop;
|
||||
motorArmed = 0;
|
||||
Read_adc_raw(); // Initialize ADC readings...
|
||||
|
||||
Read_adc_raw(); // Initialize ADC readings...
|
||||
|
||||
delay(10);
|
||||
digitalWrite(LED_Green,HIGH); // Ready to go...
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
www.ArduCopter.com - www.DIYDrones.com
|
||||
Copyright (c) 2010. All rights reserved.
|
||||
An Open Source Arduino based multicopter.
|
||||
|
||||
File : CLI.pde
|
||||
Version : v1.0, Oct 18, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Jani Hirvinen, Jose Julio, Jordi Muñoz,
|
||||
Ted Carancho (aeroquad), Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson, Randy McEvans
|
||||
|
||||
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:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
// CLI Functions
|
||||
// This can be moved later to CLI.pde
|
||||
void RunCLI () {
|
||||
|
||||
// We need to initialize Serial again due it was not initialized during startup.
|
||||
SerBeg(SerBau);
|
||||
SerPri("Welcome to ArduCopter CLI");
|
||||
SerPri("Firmware: ");
|
||||
SerPri(VER);
|
||||
|
||||
// Our main loop that never ends. Only way to get away from here is to reboot APM
|
||||
for (;;) {
|
||||
|
||||
|
||||
|
||||
} // Mainloop ends
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -132,3 +132,4 @@ boolean APMPinRead(volatile unsigned char &Port, byte Pin)
|
|||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -36,41 +36,60 @@ TODO:
|
|||
// General Initialization for all APM electronics
|
||||
void APM_Init() {
|
||||
|
||||
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
|
||||
// Setup proper PIN modes for our switched, LEDs, Relays etc on IMU Board
|
||||
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(RELAY,OUTPUT); // Relay output (PL2)
|
||||
pinMode(SW1,INPUT); // Switch SW1 (PG0)
|
||||
pinMode(SW2,INPUT); // Switch SW2 (PG1)
|
||||
|
||||
APMPinMode(DDRE,7,INPUT); //DIP1
|
||||
APMPinMode(DDRE,6,INPUT); //DIP2
|
||||
APMPinMode(DDRL,6,INPUT); //DIP3
|
||||
APMPinMode(DDRL,7,INPUT); //DIP4
|
||||
// Because DDRE and DDRL Ports are not included to normal Arduino Libraries, we need to
|
||||
// initialize them with a special command
|
||||
APMPinMode(DDRE,7,INPUT); // DIP1, (PE7), Closest to sliding SW2 switch
|
||||
APMPinMode(DDRE,6,INPUT); // DIP2, (PE6)
|
||||
APMPinMode(DDRL,6,INPUT); // DIP3, (PL6)
|
||||
APMPinMode(DDRL,7,INPUT); // DIP4, (PL7)
|
||||
|
||||
// Is CLI mode active or not, if it is fire it up and never return.
|
||||
if(digitalRead(SW2)) {
|
||||
SerPrln("Entering CLI Mode..");
|
||||
RunCLI();
|
||||
}
|
||||
|
||||
/* ********************************************************* */
|
||||
/////////////////////////////////////////////////////////
|
||||
// Normal Initialization sequence starts from here.
|
||||
|
||||
digitalWrite(RELE_pin,LOW);
|
||||
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
|
||||
// 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);
|
||||
|
||||
|
||||
// Make sure that Relay is switched off.
|
||||
digitalWrite(RELAY,LOW);
|
||||
|
||||
// Wiggle LEDs while ESCs are rebooting
|
||||
FullBlink(50,20);
|
||||
|
||||
|
||||
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("$PMTK220,200*2C\r\n"); // 5Hz update rate
|
||||
delay(200);
|
||||
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
|
@ -92,22 +111,23 @@ void APM_Init() {
|
|||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
|
||||
// Serial.begin(115200); // Old mode serial begin, remove soon, by jp/17-10-10
|
||||
|
||||
//Serial.begin(115200); // Old mode serial begin, remove soon, by jp/17-10-10
|
||||
//Serial.println("ArduCopter Quadcopter v1.0");
|
||||
|
||||
|
||||
// Proper Serial port/baud are defined on main .pde and then Arducopter.h with
|
||||
// Choises of Xbee or
|
||||
// Choises of Xbee or normal serial port
|
||||
SerBeg(SerBau);
|
||||
|
||||
// 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)==0)
|
||||
{
|
||||
Serial.println("Entering Log Read Mode...");
|
||||
Serial.println("Entering Log Read Mode..."); // This will be obsole soon due moving to CLI system
|
||||
Log_Read(1,1000);
|
||||
delay(30000);
|
||||
}
|
||||
|
||||
|
||||
calibrateSensors(); // Calibrate neutral values of gyros (in Sensors.pde)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue