aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

uncrustify ArduPlane/ArduPlane.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-21 19:19:51 -07:00 committed by Pat Hickey
parent ff4afa767b
commit 0e8a76ccdd
1 changed files with 580 additions and 579 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

207
ArduPlane/ArduPlane.pde
View File

@ -2,16 +2,16 @@
#define THISFIRMWARE "ArduPlane V2.50" #define THISFIRMWARE "ArduPlane V2.50"
/* /*
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher * Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon * Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon
Please contribute your ideas! * Please contribute your ideas!
*
*
This firmware is free software; you can redistribute it and/or * This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*/ */
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Header includes // Header includes
@ -33,7 +33,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <SPI.h> // Arduino SPI lib #include <SPI.h> // Arduino SPI lib
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library #include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library #include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_AnalogSource.h>// ArduPilot Mega polymorphic analog getter #include <AP_AnalogSource.h> // ArduPilot Mega polymorphic analog getter
#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess #include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess
#include <AP_Baro.h> // ArduPilot barometer library #include <AP_Baro.h> // ArduPilot barometer library
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library #include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
@ -77,10 +77,10 @@ version 2.1 of the License, or (at your option) any later version.
FastSerialPort0(Serial); // FTDI/console FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port FastSerialPort1(Serial1); // GPS port
#if TELEMETRY_UART2 == ENABLED #if TELEMETRY_UART2 == ENABLED
// solder bridge set to enable UART2 instead of USB MUX // solder bridge set to enable UART2 instead of USB MUX
FastSerialPort2(Serial3); FastSerialPort2(Serial3);
#else #else
FastSerialPort3(Serial3); // Telemetry port for APM1 FastSerialPort3(Serial3); // Telemetry port for APM1
#endif #endif
// this sets up the parameter table, and sets the default values. This // this sets up the parameter table, and sets the default values. This
@ -91,7 +91,7 @@ AP_Param param_loader(var_info, WP_START_BYTE);
// Outback Challenge failsafe support // Outback Challenge failsafe support
#if OBC_FAILSAFE == ENABLED #if OBC_FAILSAFE == ENABLED
#include <APM_OBC.h> #include <APM_OBC.h>
APM_OBC obc; APM_OBC obc;
#endif #endif
@ -106,18 +106,18 @@ Arduino_Mega_ISR_Registry isr_registry;
// APM_RC_Class Instance // APM_RC_Class Instance
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
APM_RC_APM2 APM_RC; APM_RC_APM2 APM_RC;
#else #else
APM_RC_APM1 APM_RC; APM_RC_APM1 APM_RC;
#endif #endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Dataflash // Dataflash
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
DataFlash_APM2 DataFlash; DataFlash_APM2 DataFlash;
#else #else
DataFlash_APM1 DataFlash; DataFlash_APM1 DataFlash;
#endif #endif
@ -157,61 +157,61 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
#if HIL_MODE == HIL_MODE_DISABLED #if HIL_MODE == HIL_MODE_DISABLED
// real sensors // real sensors
#if CONFIG_ADC == ENABLED #if CONFIG_ADC == ENABLED
static AP_ADC_ADS7844 adc; static AP_ADC_ADS7844 adc;
#endif #endif
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
AP_Baro_BMP085_HIL barometer; AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass; AP_Compass_HIL compass;
#include <SITL.h> #include <SITL.h>
SITL sitl; SITL sitl;
#else #else
#if CONFIG_BARO == AP_BARO_BMP085 #if CONFIG_BARO == AP_BARO_BMP085
# if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 # if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
static AP_Baro_BMP085 barometer(true); static AP_Baro_BMP085 barometer(true);
# else # else
static AP_Baro_BMP085 barometer(false); static AP_Baro_BMP085 barometer(false);
# endif # endif
#elif CONFIG_BARO == AP_BARO_MS5611 #elif CONFIG_BARO == AP_BARO_MS5611
static AP_Baro_MS5611 barometer; static AP_Baro_MS5611 barometer;
#endif #endif
static AP_Compass_HMC5843 compass; static AP_Compass_HMC5843 compass;
#endif #endif
// real GPS selection // real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver(&Serial1); AP_GPS_NMEA g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver(&Serial1); AP_GPS_SIRF g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver(&Serial1); AP_GPS_UBLOX g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver(&Serial1); AP_GPS_MTK g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
AP_GPS_MTK16 g_gps_driver(&Serial1); AP_GPS_MTK16 g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver(NULL); AP_GPS_None g_gps_driver(NULL);
#else #else
#error Unrecognised GPS_PROTOCOL setting. #error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL #endif // GPS PROTOCOL
# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 # if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
# else # else
AP_InertialSensor_Oilpan ins( &adc ); AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE #endif // CONFIG_IMU_TYPE
AP_IMU_INS imu( &ins ); AP_IMU_INS imu( &ins );
AP_AHRS_DCM ahrs(&imu, g_gps); AP_AHRS_DCM ahrs(&imu, g_gps);
@ -233,11 +233,11 @@ AP_AHRS_HIL ahrs(&imu, g_gps);
AP_GPS_HIL g_gps_driver(NULL); AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used AP_Compass_HIL compass; // never used
AP_Baro_BMP085_HIL barometer; AP_Baro_BMP085_HIL barometer;
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
#include <SITL.h> #include <SITL.h>
SITL sitl; SITL sitl;
AP_InertialSensor_Oilpan ins( &adc ); AP_InertialSensor_Oilpan ins( &adc );
#endif #endif
#else #else
#error Unrecognised HIL_MODE setting. #error Unrecognised HIL_MODE setting.
@ -268,11 +268,11 @@ AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0)
#endif #endif
#if SONAR_TYPE == MAX_SONAR_XL #if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
#elif SONAR_TYPE == MAX_SONAR_LV #elif SONAR_TYPE == MAX_SONAR_LV
// XXX honestly I think these output the same values // XXX honestly I think these output the same values
// If someone knows, can they confirm it? // If someone knows, can they confirm it?
AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
#endif #endif
AP_Relay relay; AP_Relay relay;
@ -305,22 +305,23 @@ static const char* flight_mode_strings[] = {
"RTL", "RTL",
"Loiter", "Loiter",
"Takeoff", "Takeoff",
"Land"}; "Land"
};
/* Radio values /* Radio values
Channel assignments * Channel assignments
1 Ailerons (rudder if no ailerons) * 1 Ailerons (rudder if no ailerons)
2 Elevator * 2 Elevator
3 Throttle * 3 Throttle
4 Rudder (if we have ailerons) * 4 Rudder (if we have ailerons)
5 Aux5 * 5 Aux5
6 Aux6 * 6 Aux6
7 Aux7 * 7 Aux7
8 Aux8/Mode * 8 Aux8/Mode
Each Aux channel can be configured to have any of the available auxiliary functions assigned to it. * Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
See libraries/RC_Channel/RC_Channel_aux.h for more information * See libraries/RC_Channel/RC_Channel_aux.h for more information
*/ */
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Radio // Radio
@ -713,7 +714,7 @@ void loop()
medium_loop(); medium_loop();
counter_one_herz++; counter_one_herz++;
if(counter_one_herz == 50){ if(counter_one_herz == 50) {
one_second_loop(); one_second_loop();
counter_one_herz = 0; counter_one_herz = 0;
} }
@ -721,9 +722,9 @@ void loop()
if (millis() - perf_mon_timer > 20000) { if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) { if (mainLoop_count != 0) {
if (g.log_bitmask & MASK_LOG_PM) if (g.log_bitmask & MASK_LOG_PM)
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
Log_Write_Performance(); Log_Write_Performance();
#endif #endif
resetPerfData(); resetPerfData();
} }
@ -753,30 +754,30 @@ static void fast_loop()
// ------------------------------------ // ------------------------------------
check_short_failsafe(); check_short_failsafe();
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS
// update hil before AHRS update // update hil before AHRS update
gcs_update(); gcs_update();
#endif #endif
ahrs.update(); ahrs.update();
// uses the yaw from the DCM to give more accurate turns // uses the yaw from the DCM to give more accurate turns
calc_bearing_error(); calc_bearing_error();
# if HIL_MODE == HIL_MODE_DISABLED # if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor); Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor);
if (g.log_bitmask & MASK_LOG_RAW) if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw(); Log_Write_Raw();
#endif #endif
// inertial navigation // inertial navigation
// ------------------ // ------------------
#if INERTIAL_NAVIGATION == ENABLED #if INERTIAL_NAVIGATION == ENABLED
// TODO: implement inertial nav function // TODO: implement inertial nav function
inertialNavigation(); inertialNavigation();
#endif #endif
// custom code/exceptions for flight modes // custom code/exceptions for flight modes
// --------------------------------------- // ---------------------------------------
@ -817,28 +818,28 @@ static void medium_loop()
//------------------------------- //-------------------------------
case 0: case 0:
medium_loopCounter++; medium_loopCounter++;
if(GPS_enabled){ if(GPS_enabled) {
update_GPS(); update_GPS();
calc_gndspeed_undershoot(); calc_gndspeed_undershoot();
} }
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if (g.compass_enabled && compass.read()) { if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
compass.null_offsets(); compass.null_offsets();
} else { } else {
ahrs.set_compass(NULL); ahrs.set_compass(NULL);
} }
#endif #endif
/*{ /*{
Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); * Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); * Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); * Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
Vector3f tempaccel = imu.get_accel(); * Vector3f tempaccel = imu.get_accel();
Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); * Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); * Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t"));
Serial.println(tempaccel.z, DEC); * Serial.println(tempaccel.z, DEC);
}*/ * }*/
break; break;
@ -890,13 +891,13 @@ Serial.println(tempaccel.z, DEC);
case 3: case 3:
medium_loopCounter++; medium_loopCounter++;
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor); Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor);
if (g.log_bitmask & MASK_LOG_CTUN) if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
#endif #endif
if (g.log_bitmask & MASK_LOG_NTUN) if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
@ -912,7 +913,7 @@ Serial.println(tempaccel.z, DEC);
delta_ms_medium_loop = millis() - medium_loopTimer_ms; delta_ms_medium_loop = millis() - medium_loopTimer_ms;
medium_loopTimer_ms = millis(); medium_loopTimer_ms = millis();
if (g.battery_monitoring != 0){ if (g.battery_monitoring != 0) {
read_battery(); read_battery();
} }
@ -922,7 +923,7 @@ Serial.println(tempaccel.z, DEC);
// perform OBC failsafe checks // perform OBC failsafe checks
obc.check(OBC_MODE(control_mode), obc.check(OBC_MODE(control_mode),
last_heartbeat_ms, last_heartbeat_ms,
g_gps?g_gps->last_fix_time:0); g_gps ? g_gps->last_fix_time : 0);
#endif #endif
break; break;
@ -933,17 +934,17 @@ static void slow_loop()
{ {
// This is the slow (3 1/3 Hz) loop pieces // This is the slow (3 1/3 Hz) loop pieces
//---------------------------------------- //----------------------------------------
switch (slow_loopCounter){ switch (slow_loopCounter) {
case 0: case 0:
slow_loopCounter++; slow_loopCounter++;
check_long_failsafe(); check_long_failsafe();
superslow_loopCounter++; superslow_loopCounter++;
if(superslow_loopCounter >=200) { // 200 = Execute every minute if(superslow_loopCounter >=200) { // 200 = Execute every minute
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled) { if(g.compass_enabled) {
compass.save_offsets(); compass.save_offsets();
} }
#endif #endif
superslow_loopCounter = 0; superslow_loopCounter = 0;
} }
@ -1005,7 +1006,7 @@ static void update_GPS(void)
// --------------- // ---------------
gps_fix_count++; gps_fix_count++;
if(ground_start_count > 1){ if(ground_start_count > 1) {
ground_start_count--; ground_start_count--;
ground_start_avg += g_gps->ground_speed; ground_start_avg += g_gps->ground_speed;
@ -1017,7 +1018,7 @@ static void update_GPS(void)
ground_start_count = 5; ground_start_count = 5;
} else { } else {
if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){ if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT) {
startup_ground(); startup_ground();
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
@ -1043,10 +1044,10 @@ static void update_GPS(void)
static void update_current_flight_mode(void) static void update_current_flight_mode(void)
{ {
if(control_mode == AUTO){ if(control_mode == AUTO) {
crash_checker(); crash_checker();
switch(nav_command_ID){ switch(nav_command_ID) {
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
if (hold_course != -1) { if (hold_course != -1) {
calc_nav_roll(); calc_nav_roll();
@ -1100,7 +1101,7 @@ static void update_current_flight_mode(void)
// hold_course is only used in takeoff and landing // hold_course is only used in takeoff and landing
hold_course = -1; hold_course = -1;
switch(control_mode){ switch(control_mode) {
case RTL: case RTL:
case LOITER: case LOITER:
case GUIDED: case GUIDED:
@ -1161,7 +1162,7 @@ static void update_current_flight_mode(void)
nav_roll_cd = g.roll_limit_cd / 3; nav_roll_cd = g.roll_limit_cd / 3;
nav_pitch_cd = 0; nav_pitch_cd = 0;
if (failsafe != FAILSAFE_NONE){ if (failsafe != FAILSAFE_NONE) {
g.channel_throttle.servo_out = g.throttle_cruise; g.channel_throttle.servo_out = g.throttle_cruise;
} }
break; break;
@ -1185,11 +1186,11 @@ static void update_navigation()
// ------------------------------------------------------------------------ // ------------------------------------------------------------------------
// distance and bearing calcs only // distance and bearing calcs only
if(control_mode == AUTO){ if(control_mode == AUTO) {
verify_commands(); verify_commands();
}else{ }else{
switch(control_mode){ switch(control_mode) {
case LOITER: case LOITER:
case RTL: case RTL:
case GUIDED: case GUIDED:
@ -1204,9 +1205,9 @@ static void update_navigation()
static void update_alt() static void update_alt()
{ {
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude; current_loc.alt = g_gps->altitude;
#else #else
// this function is in place to potentially add a sonar sensor in the future // this function is in place to potentially add a sonar sensor in the future
//altitude_sensor = BARO; //altitude_sensor = BARO;
@ -1216,7 +1217,7 @@ static void update_alt()
} else if (g_gps->fix) { } else if (g_gps->fix) {
current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100) current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100)
} }
#endif #endif
geofence_check(true); geofence_check(true);