mirror of https://github.com/ArduPilot/ardupilot
Some bug corrections on startup
git-svn-id: https://arducopter.googlecode.com/svn/trunk@660 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234
parent
b7ecadf982
commit
778c68d5ef
|
|
@ -37,8 +37,8 @@ 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
|
||||
#define ARM_DELAY 50 // how long you need to keep rudder to max right for arming motors (units*0.02, 50=1second)
|
||||
#define DISARM_DELAY 25 // how long you need to keep rudder to max left for disarming motors
|
||||
|
||||
/*************************************************************/
|
||||
// AM Mode & Flight information
|
||||
|
|
@ -83,6 +83,8 @@ TODO:
|
|||
//Modes
|
||||
#define STABLE_MODE 0
|
||||
#define ACRO_MODE 1
|
||||
#define AP_NORMAL_MODE 0 // AP disabled => manual flight
|
||||
#define AP_AUTOMATIC_MODE 1 // AP Enabled => Automatic mode (GPS position hold)
|
||||
|
||||
//Axis
|
||||
#define ROLL 0
|
||||
|
|
|
|||
|
|
@ -192,7 +192,7 @@ float command_rx_yaw_diff;
|
|||
int control_roll; // PID control results
|
||||
int control_pitch;
|
||||
int control_yaw;
|
||||
float K_aux;
|
||||
//float K_aux;
|
||||
|
||||
// Attitude PID controls
|
||||
float roll_I=0;
|
||||
|
|
@ -206,11 +206,9 @@ 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;
|
||||
|
|
@ -232,15 +230,8 @@ 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
|
||||
long target_baro_altitude;
|
||||
long press_alt = 0;
|
||||
|
||||
|
||||
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
|
||||
|
|
|
|||
|
|
@ -80,14 +80,14 @@
|
|||
#include "Arducopter.h"
|
||||
#include "ArduUser.h"
|
||||
|
||||
// GPS
|
||||
// GPS library (Include only one library)
|
||||
#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 0.1 // Current software version (only numeric values)
|
||||
#define VER 1.5 // Current software version (only numeric values)
|
||||
|
||||
/* ************************************************************ */
|
||||
/* ************* MAIN PROGRAM - DECLARATIONS ****************** */
|
||||
|
|
@ -95,12 +95,10 @@
|
|||
|
||||
byte flightMode;
|
||||
|
||||
unsigned long currentTime, previousTime, deltaTime;
|
||||
unsigned long currentTime, previousTime;
|
||||
unsigned long mainLoop = 0;
|
||||
unsigned long sensorLoop = 0;
|
||||
unsigned long controlLoop = 0;
|
||||
unsigned long radioLoop = 0;
|
||||
unsigned long motorLoop = 0;
|
||||
unsigned long mediumLoop = 0;
|
||||
unsigned long slowLoop = 0;
|
||||
|
||||
/* ************************************************************ */
|
||||
/* **************** MAIN PROGRAM - SETUP ********************** */
|
||||
|
|
@ -109,63 +107,54 @@ void setup() {
|
|||
|
||||
APM_Init(); // APM Hardware initialization (in System.pde)
|
||||
|
||||
previousTime = millis();
|
||||
mainLoop = millis(); // Initialize timers
|
||||
mediumLoop = mainLoop;
|
||||
GPS_timer = mainLoop;
|
||||
motorArmed = 0;
|
||||
Read_adc_raw(); // Initialize ADC readings...
|
||||
delay(20);
|
||||
delay(10);
|
||||
digitalWrite(LED_Green,HIGH); // Ready to go...
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************ */
|
||||
/* ************** MAIN PROGRAM - MAIN LOOP ******************** */
|
||||
/* ************************************************************ */
|
||||
|
||||
// fast rate
|
||||
// Sensor reading loop is inside APM_ADC and runs at 400Hz (based on Timer2 interrupt)
|
||||
|
||||
// * fast rate loop => Main loop => 200Hz
|
||||
// read sensors
|
||||
// IMU : update attitude
|
||||
// motor control
|
||||
|
||||
// medium rate
|
||||
// read transmitter
|
||||
// Asyncronous task : read transmitter
|
||||
// * medium rate loop (60Hz)
|
||||
// Asyncronous task : read GPS
|
||||
// * slow rate loop (10Hz)
|
||||
// magnetometer
|
||||
// barometer
|
||||
|
||||
// slow rate
|
||||
// barometer (20Hz)
|
||||
// external command/telemetry
|
||||
// GPS
|
||||
// Battery monitor
|
||||
|
||||
void loop()
|
||||
{
|
||||
int aux;
|
||||
int i;
|
||||
float aux_float;
|
||||
//int aux;
|
||||
//int i;
|
||||
//float aux_float;
|
||||
|
||||
currentTime = millis();
|
||||
//deltaTime = currentTime - previousTime;
|
||||
//G_Dt = deltaTime / 1000.0;
|
||||
//previousTime = currentTime;
|
||||
|
||||
// Sensor reading loop is inside APM_ADC and runs at 400Hz
|
||||
|
||||
// Main loop at 200Hz (IMU + control)
|
||||
if (currentTime > (mainLoop + 5)) // 200Hz (every 5ms)
|
||||
if ((currentTime-mainLoop) > 5) // 200Hz (every 5ms)
|
||||
{
|
||||
G_Dt = (currentTime-mainLoop) / 1000.0; // Microseconds!!!
|
||||
G_Dt = (currentTime-mainLoop)*0.001; // Microseconds!!!
|
||||
mainLoop = currentTime;
|
||||
|
||||
//IMU DCM Algorithm
|
||||
Read_adc_raw(); // Read sensors raw data
|
||||
Matrix_update();
|
||||
// Optimization: we don´t need to call this functions all the times
|
||||
//if (IMU_cicle==0)
|
||||
// {
|
||||
Normalize();
|
||||
Drift_correction();
|
||||
// IMU_cicle = 1;
|
||||
// }
|
||||
//else
|
||||
// IMU_cicle = 0;
|
||||
Normalize();
|
||||
Drift_correction();
|
||||
Euler_angles();
|
||||
|
||||
// Read radio values (if new data is available)
|
||||
|
|
@ -175,12 +164,12 @@ void loop()
|
|||
// Attitude control
|
||||
if(flightMode == STABLE_MODE) { // STABLE Mode
|
||||
gled_speed = 1200;
|
||||
if (AP_mode == 0) // Normal mode
|
||||
if (AP_mode == AP_NORMAL_MODE) // Normal mode
|
||||
Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw);
|
||||
else // Automatic mode : GPS position hold mode
|
||||
Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw);
|
||||
}
|
||||
else { // ACRO Mode
|
||||
else { // ACRO Mode
|
||||
gled_speed = 400;
|
||||
Rate_control_v2();
|
||||
// Reset yaw, so if we change to stable mode we continue with the actual yaw direction
|
||||
|
|
@ -189,33 +178,90 @@ void loop()
|
|||
|
||||
// Send output commands to motor ESCs...
|
||||
motor_output();
|
||||
|
||||
// Performance optimization: Magnetometer sensor and pressure sensor are slowly to read (I2C)
|
||||
// so we read them at the end of the loop (all work is done in this loop run...)
|
||||
#ifdef IsMAG
|
||||
if (MAGNETOMETER == 1) {
|
||||
if (MAG_counter > 20) // Read compass data at 10Hz...
|
||||
|
||||
// Autopilot mode functions
|
||||
if (AP_mode == AP_AUTOMATIC_MODE)
|
||||
{
|
||||
MAG_counter=0;
|
||||
APM_Compass.Read(); // Read magnetometer
|
||||
APM_Compass.Calculate(roll,pitch); // Calculate heading
|
||||
if (target_position)
|
||||
{
|
||||
if (GPS.NewData) // New GPS info?
|
||||
{
|
||||
read_GPS_data();
|
||||
Position_control(target_lattitude,target_longitude); // Call GPS position hold routine
|
||||
}
|
||||
}
|
||||
else // First time we enter in GPS position hold we capture the target position as the actual position
|
||||
{
|
||||
if (GPS.Fix){ // We need a GPS Fix to capture the actual position...
|
||||
target_lattitude = GPS.Lattitude;
|
||||
target_longitude = GPS.Longitude;
|
||||
target_position=1;
|
||||
//target_sonar_altitude = sonar_value;
|
||||
target_baro_altitude = press_alt;
|
||||
Initial_Throttle = ch_throttle;
|
||||
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
|
||||
}
|
||||
command_gps_roll=0;
|
||||
command_gps_pitch=0;
|
||||
}
|
||||
}
|
||||
else
|
||||
target_position=0;
|
||||
}
|
||||
|
||||
// Medium loop (about 60Hz)
|
||||
if ((currentTime-mediumLoop)>=17){
|
||||
mediumLoop = currentTime;
|
||||
GPS.Read(); // Read GPS data
|
||||
// Each of the six cases executes at 10Hz
|
||||
switch (medium_loopCounter){
|
||||
case 0: // Magnetometer reading (10Hz)
|
||||
medium_loopCounter++;
|
||||
slowLoop++;
|
||||
#ifdef IsMAG
|
||||
if (MAGNETOMETER == 1) {
|
||||
APM_Compass.Read(); // Read magnetometer
|
||||
APM_Compass.Calculate(roll,pitch); // Calculate heading
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case 1: // Barometer reading (2x10Hz = 20Hz)
|
||||
medium_loopCounter++;
|
||||
#ifdef UseBMP
|
||||
if (APM_BMP085.Read()){
|
||||
read_baro();
|
||||
Baro_new_data = 1;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case 2: // Send serial telemetry (10Hz)
|
||||
medium_loopCounter++;
|
||||
#ifdef CONFIGURATOR
|
||||
sendSerialTelemetry();
|
||||
#endif
|
||||
break;
|
||||
case 3: // Read serial telemetry (10Hz)
|
||||
medium_loopCounter++;
|
||||
#ifdef CONFIGURATOR
|
||||
readSerialCommand();
|
||||
#endif
|
||||
break;
|
||||
case 4: // second Barometer reading (2x10Hz = 20Hz)
|
||||
medium_loopCounter++;
|
||||
#ifdef UseBMP
|
||||
if (APM_BMP085.Read()){
|
||||
read_baro();
|
||||
Baro_new_data = 1;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case 5: // Battery monitor (10Hz)
|
||||
medium_loopCounter=0;
|
||||
#if BATTERY_EVENT == 1
|
||||
read_battery(); // Battery monitor
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef UseBMP
|
||||
#endif
|
||||
|
||||
// Slow loop (10Hz)
|
||||
if((currentTime-tlmTimer)>=100) {
|
||||
//#if BATTERY_EVENT == 1
|
||||
// read_battery(); // Battery monitor
|
||||
//#endif
|
||||
#ifdef CONFIGURATOR
|
||||
readSerialCommand();
|
||||
sendSerialTelemetry();
|
||||
#endif
|
||||
tlmTimer = currentTime;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -167,7 +167,6 @@ void Rate_control_v2()
|
|||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -277,7 +277,6 @@ void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long
|
|||
DataFlash.WriteLong(log_Ground_Speed);
|
||||
DataFlash.WriteLong(log_Ground_Course);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -32,3 +32,67 @@ TODO:
|
|||
- initial functions.
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
void read_GPS_data()
|
||||
{
|
||||
GPS_timer_old=GPS_timer; // Update GPS timer
|
||||
GPS_timer = millis();
|
||||
GPS_Dt = (GPS_timer-GPS_timer_old)*0.001; // GPS_Dt
|
||||
GPS.NewData=0; // We Reset the flag...
|
||||
|
||||
// Write GPS data to DataFlash log
|
||||
Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude,GPS.Altitude,GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
|
||||
|
||||
//if (GPS.Fix >= 2)
|
||||
if (GPS.Fix)
|
||||
digitalWrite(LED_Red,HIGH); // GPS Fix => RED LED
|
||||
else
|
||||
digitalWrite(LED_Red,LOW);
|
||||
}
|
||||
|
||||
/* GPS based Position control */
|
||||
void Position_control(long lat_dest, long lon_dest)
|
||||
{
|
||||
long Lon_diff;
|
||||
long Lat_diff;
|
||||
|
||||
Lon_diff = lon_dest - GPS.Longitude;
|
||||
Lat_diff = lat_dest - GPS.Lattitude;
|
||||
|
||||
// ROLL
|
||||
//Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0]
|
||||
gps_err_roll = (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[0][0] - (float)Lat_diff * DCM_Matrix[1][0];
|
||||
|
||||
gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt;
|
||||
gps_err_roll_old = gps_err_roll;
|
||||
|
||||
gps_roll_I += gps_err_roll * GPS_Dt;
|
||||
gps_roll_I = constrain(gps_roll_I, -800, 800);
|
||||
|
||||
command_gps_roll = KP_GPS_ROLL * gps_err_roll + KD_GPS_ROLL * gps_roll_D + KI_GPS_ROLL * gps_roll_I;
|
||||
command_gps_roll = constrain(command_gps_roll, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
|
||||
|
||||
//Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll*10,KI_GPS_ROLL*gps_roll_I*10,KD_GPS_ROLL*gps_roll_D*10,command_gps_roll*10);
|
||||
|
||||
// PITCH
|
||||
gps_err_pitch = -(float)Lat_diff * DCM_Matrix[0][0] - (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[1][0];
|
||||
|
||||
gps_pitch_D = (gps_err_pitch - gps_err_pitch_old) / GPS_Dt;
|
||||
gps_err_pitch_old = gps_err_pitch;
|
||||
|
||||
gps_pitch_I += gps_err_pitch * GPS_Dt;
|
||||
gps_pitch_I = constrain(gps_pitch_I, -800, 800);
|
||||
|
||||
command_gps_pitch = KP_GPS_PITCH * gps_err_pitch + KD_GPS_PITCH * gps_pitch_D + KI_GPS_PITCH * gps_pitch_I;
|
||||
command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
|
||||
|
||||
//Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10);
|
||||
}
|
||||
|
||||
void Reset_I_terms_navigation()
|
||||
{
|
||||
altitude_I = 0;
|
||||
gps_roll_I = 0;
|
||||
gps_pitch_I = 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ void read_radio()
|
|||
ch_aux2 = APM_RC.InputCh(CH_6) * ch_aux2_slope + ch_aux2_offset;
|
||||
|
||||
// Flight mode
|
||||
if(ch_aux2 > 1800)
|
||||
if(ch_aux2 > 1200)
|
||||
flightMode = ACRO_MODE; // Force to Acro mode from radio
|
||||
else
|
||||
flightMode = STABLE_MODE; // Stable mode (default)
|
||||
|
|
@ -57,12 +57,12 @@ void read_radio()
|
|||
if (flightMode == STABLE_MODE)
|
||||
{
|
||||
if(ch_aux > 1800)
|
||||
AP_mode = 1; // Automatic mode : GPS position hold mode + altitude hold
|
||||
AP_mode = AP_AUTOMATIC_MODE; // Automatic mode : GPS position hold mode + altitude hold
|
||||
else
|
||||
AP_mode = 0; // Normal mode
|
||||
AP_mode = AP_NORMAL_MODE; // Normal mode
|
||||
}
|
||||
|
||||
if (flightMode==0) // IN STABLE MODE we convert stick positions to absoulte angles
|
||||
if (flightMode==STABLE_MODE) // IN STABLE MODE we convert stick positions to absoulte angles
|
||||
{
|
||||
// In Stable mode stick position defines the desired angle in roll, pitch and yaw
|
||||
#ifdef FLIGHT_MODE_X
|
||||
|
|
@ -85,13 +85,50 @@ void read_radio()
|
|||
}
|
||||
|
||||
// Write Radio data to DataFlash log
|
||||
Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
|
||||
Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,ch_aux,ch_aux2);
|
||||
|
||||
// Motor arm logic
|
||||
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
|
||||
if (ch_throttle < (MIN_THROTTLE + 100)) {
|
||||
control_yaw = 0;
|
||||
command_rx_yaw = ToDeg(yaw);
|
||||
if (ch_yaw > 1850) {
|
||||
if (Arming_counter > ARM_DELAY){
|
||||
if(ch_throttle > 800) {
|
||||
motorArmed = 1;
|
||||
minThrottle = MIN_THROTTLE+60; // A minimun value for mantain a bit if throttle
|
||||
}
|
||||
}
|
||||
else
|
||||
Arming_counter++;
|
||||
}
|
||||
else
|
||||
Arming_counter=0;
|
||||
// To Disarm motor output : Throttle down and full yaw left for more than 2 seconds
|
||||
if (ch_yaw < 1150) {
|
||||
if (Disarming_counter > DISARM_DELAY){
|
||||
motorArmed = 0;
|
||||
minThrottle = MIN_THROTTLE;
|
||||
}
|
||||
else
|
||||
Disarming_counter++;
|
||||
}
|
||||
else
|
||||
Disarming_counter=0;
|
||||
}
|
||||
else{
|
||||
Arming_counter=0;
|
||||
Disarming_counter=0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Send output commands to ESC´s
|
||||
void motor_output()
|
||||
{
|
||||
if (ch_throttle < (MIN_THROTTLE + 100)) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely)
|
||||
control_yaw = 0;
|
||||
|
||||
// Quadcopter mix
|
||||
if (motorArmed == 1)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -80,4 +80,21 @@ void calibrateSensors(void) {
|
|||
|
||||
for(gyro=GYROZ; gyro<=GYROY; gyro++)
|
||||
AN_OFFSET[gyro]=aux_float[gyro]; // Update sensor OFFSETs from values read
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef UseBMP
|
||||
void read_baro(void)
|
||||
{
|
||||
float tempPresAlt;
|
||||
|
||||
tempPresAlt = float(APM_BMP085.Press)/101325.0;
|
||||
//tempPresAlt = pow(tempPresAlt, 0.190284);
|
||||
//press_alt = (1.0 - tempPresAlt) * 145366.45;
|
||||
tempPresAlt = pow(tempPresAlt, 0.190295);
|
||||
if (press_alt==0)
|
||||
press_alt = (1.0 - tempPresAlt) * 4433000; // Altitude in cm
|
||||
else
|
||||
press_alt = press_alt*0.9 + ((1.0 - tempPresAlt) * 443300); // Altitude in cm (filtered)
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -42,11 +42,16 @@ void APM_Init() {
|
|||
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
|
||||
// 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);
|
||||
|
||||
FullBlink(50,20);
|
||||
|
||||
APM_ADC.Init(); // APM ADC library initialization
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue