diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 1df8b0134c..f549c38468 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -2,16 +2,16 @@ #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 -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! - - -This firmware is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. -*/ + * 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 + * Please contribute your ideas! + * + * + * This firmware is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + */ //////////////////////////////////////////////////////////////////////////////// // Header includes @@ -29,11 +29,11 @@ version 2.1 of the License, or (at your option) any later version. #include #include // ArduPilot Mega RC Library #include // ArduPilot GPS library -#include // Wayne Truchsess I2C lib -#include // Arduino SPI lib +#include // Wayne Truchsess I2C lib +#include // Arduino SPI lib #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library -#include // ArduPilot Mega polymorphic analog getter +#include // ArduPilot Mega polymorphic analog getter #include // ArduPilot Mega TimerProcess #include // ArduPilot barometer library #include // ArduPilot Mega Magnetometer Library @@ -43,12 +43,12 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library -#include // Range finder library -#include // Filter library -#include // Mode Filter from Filter library -#include // LowPassFilter class (inherits from Filter class) +#include // Range finder library +#include // Filter library +#include // Mode Filter from Filter library +#include // LowPassFilter class (inherits from Filter class) #include // APM relay -#include // Photo or video camera +#include // Photo or video camera #include #include @@ -57,7 +57,7 @@ version 2.1 of the License, or (at your option) any later version. #include // MAVLink GCS definitions -#include // Camera/Antenna mount +#include // Camera/Antenna mount // Local modules #include "defines.h" @@ -77,10 +77,10 @@ version 2.1 of the License, or (at your option) any later version. FastSerialPort0(Serial); // FTDI/console FastSerialPort1(Serial1); // GPS port #if TELEMETRY_UART2 == ENABLED - // solder bridge set to enable UART2 instead of USB MUX - FastSerialPort2(Serial3); +// solder bridge set to enable UART2 instead of USB MUX +FastSerialPort2(Serial3); #else - FastSerialPort3(Serial3); // Telemetry port for APM1 +FastSerialPort3(Serial3); // Telemetry port for APM1 #endif // 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 #if OBC_FAILSAFE == ENABLED -#include + #include APM_OBC obc; #endif @@ -106,18 +106,18 @@ Arduino_Mega_ISR_Registry isr_registry; // APM_RC_Class Instance //////////////////////////////////////////////////////////////////////////////// #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 - APM_RC_APM2 APM_RC; +APM_RC_APM2 APM_RC; #else - APM_RC_APM1 APM_RC; +APM_RC_APM1 APM_RC; #endif //////////////////////////////////////////////////////////////////////////////// // Dataflash //////////////////////////////////////////////////////////////////////////////// #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 - DataFlash_APM2 DataFlash; +DataFlash_APM2 DataFlash; #else - DataFlash_APM1 DataFlash; +DataFlash_APM1 DataFlash; #endif @@ -127,7 +127,7 @@ Arduino_Mega_ISR_Registry isr_registry; // // Global parameters are all contained within the 'g' class. // -static Parameters g; +static Parameters g; //////////////////////////////////////////////////////////////////////////////// @@ -152,92 +152,92 @@ static void update_events(void); static GPS *g_gps; // flight modes convenience array -static AP_Int8 *flight_modes = &g.flight_mode1; +static AP_Int8 *flight_modes = &g.flight_mode1; #if HIL_MODE == HIL_MODE_DISABLED // real sensors -#if CONFIG_ADC == ENABLED -static AP_ADC_ADS7844 adc; -#endif + #if CONFIG_ADC == ENABLED +static AP_ADC_ADS7844 adc; + #endif -#ifdef DESKTOP_BUILD -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; -#include -SITL sitl; -#else + #ifdef DESKTOP_BUILD +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; + #include +SITL sitl; + #else -#if CONFIG_BARO == AP_BARO_BMP085 -# if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #if CONFIG_BARO == AP_BARO_BMP085 + # if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 static AP_Baro_BMP085 barometer(true); -# else + # else static AP_Baro_BMP085 barometer(false); -# endif -#elif CONFIG_BARO == AP_BARO_MS5611 -static AP_Baro_MS5611 barometer; -#endif + # endif + #elif CONFIG_BARO == AP_BARO_MS5611 +static AP_Baro_MS5611 barometer; + #endif -static AP_Compass_HMC5843 compass; -#endif +static AP_Compass_HMC5843 compass; + #endif // real GPS selection -#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO + #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO 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); -#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF + #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF 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); -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK + #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK 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); -#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE + #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE AP_GPS_None g_gps_driver(NULL); -#else - #error Unrecognised GPS_PROTOCOL setting. -#endif // GPS PROTOCOL + #else + #error Unrecognised GPS_PROTOCOL setting. + #endif // GPS PROTOCOL -# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 - AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); -# else - AP_InertialSensor_Oilpan ins( &adc ); -#endif // CONFIG_IMU_TYPE + # if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 +AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); + # else +AP_InertialSensor_Oilpan ins( &adc ); + #endif // CONFIG_IMU_TYPE AP_IMU_INS imu( &ins ); AP_AHRS_DCM ahrs(&imu, g_gps); #elif HIL_MODE == HIL_MODE_SENSORS // sensor emulators -AP_ADC_HIL adc; -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; +AP_ADC_HIL adc; +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); AP_InertialSensor_Oilpan ins( &adc ); AP_IMU_Shim imu; AP_AHRS_DCM ahrs(&imu, g_gps); #elif HIL_MODE == HIL_MODE_ATTITUDE -AP_ADC_HIL adc; -AP_IMU_Shim imu; // never used +AP_ADC_HIL adc; +AP_IMU_Shim imu; // never used AP_AHRS_HIL ahrs(&imu, g_gps); AP_GPS_HIL g_gps_driver(NULL); -AP_Compass_HIL compass; // never used -AP_Baro_BMP085_HIL barometer; -#ifdef DESKTOP_BUILD -#include -SITL sitl; +AP_Compass_HIL compass; // never used +AP_Baro_BMP085_HIL barometer; + #ifdef DESKTOP_BUILD + #include +SITL sitl; AP_InertialSensor_Oilpan ins( &adc ); -#endif + #endif #else #error Unrecognised HIL_MODE setting. @@ -251,8 +251,8 @@ AP_TimerProcess timer_scheduler; // GCS selection //////////////////////////////////////////////////////////////////////////////// // -GCS_MAVLINK gcs0; -GCS_MAVLINK gcs3; +GCS_MAVLINK gcs0; +GCS_MAVLINK gcs3; //////////////////////////////////////////////////////////////////////////////// // PITOT selection @@ -262,17 +262,17 @@ ModeFilterInt16_Size5 sonar_mode_filter(2); #if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC AP_AnalogSource_ADC pitot_analog_source( &adc, - CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0); + CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0); #elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0); #endif #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 - // XXX honestly I think these output the same values - // If someone knows, can they confirm it? - AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); +// XXX honestly I think these output the same values +// If someone knows, can they confirm it? +AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); #endif AP_Relay relay; @@ -291,55 +291,56 @@ static bool usb_connected; static const char *comma = ","; static const char* flight_mode_strings[] = { - "Manual", - "Circle", - "Stabilize", - "", - "", - "FBW_A", - "FBW_B", - "", - "", - "", - "Auto", - "RTL", - "Loiter", - "Takeoff", - "Land"}; + "Manual", + "Circle", + "Stabilize", + "", + "", + "FBW_A", + "FBW_B", + "", + "", + "", + "Auto", + "RTL", + "Loiter", + "Takeoff", + "Land" +}; /* Radio values - Channel assignments - 1 Ailerons (rudder if no ailerons) - 2 Elevator - 3 Throttle - 4 Rudder (if we have ailerons) - 5 Aux5 - 6 Aux6 - 7 Aux7 - 8 Aux8/Mode - 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 -*/ + * Channel assignments + * 1 Ailerons (rudder if no ailerons) + * 2 Elevator + * 3 Throttle + * 4 Rudder (if we have ailerons) + * 5 Aux5 + * 6 Aux6 + * 7 Aux7 + * 8 Aux8/Mode + * 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 + */ //////////////////////////////////////////////////////////////////////////////// // Radio //////////////////////////////////////////////////////////////////////////////// // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO -byte control_mode = INITIALISING; +byte control_mode = INITIALISING; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch -byte oldSwitchPosition; +byte oldSwitchPosition; // This is used to enable the inverted flight feature -bool inverted_flight = false; +bool inverted_flight = false; // These are trim values used for elevon control // For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon static uint16_t elevon1_trim = 1500; static uint16_t elevon2_trim = 1500; // These are used in the calculation of elevon1_trim and elevon2_trim static uint16_t ch1_temp = 1500; -static uint16_t ch2_temp = 1500; +static uint16_t ch2_temp = 1500; // These are values received from the GCS if the user is using GCS joystick // control and are substituted for the values coming from the RC radio static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; @@ -351,13 +352,13 @@ static bool rc_override_active = false; //////////////////////////////////////////////////////////////////////////////// // A tracking variable for type of failsafe active // Used for failsafe based on loss of RC signal or GCS signal -static int16_t failsafe; +static int16_t failsafe; // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold // RC receiver should be set up to output a low throttle value when signal is lost -static bool ch3_failsafe; +static bool ch3_failsafe; // A timer used to help recovery from unusual attitudes. If we enter an unusual attitude // while in autonomous flight this variable is used to hold roll at 0 for a recovery period -static byte crash_timer; +static byte crash_timer; // A timer used to track how long since we have received the last GCS heartbeat or rc override message static uint32_t rc_override_fs_timer = 0; @@ -379,18 +380,18 @@ static bool GPS_light; // This is used to scale GPS values for EEPROM storage // 10^7 times Decimal GPS means 1 == 1cm // This approximation makes calculations integer and it's easy to read -static const float t7 = 10000000.0; +static const float t7 = 10000000.0; // We use atan2 and other trig techniques to calaculate angles // A counter used to count down valid gps fixes to allow the gps estimate to settle // before recording our home position (and executing a ground start if we booted with an air start) -static byte ground_start_count = 5; +static byte ground_start_count = 5; // Used to compute a speed estimate from the first valid gps fixes to decide if we are // on the ground or in the air. Used to decide if a ground start is appropriate if we // booted with an air start. -static int16_t ground_start_avg; +static int16_t ground_start_avg; // Tracks if GPS is enabled based on statup routine // If we do not detect GPS at startup, we stop trying and assume GPS is not connected -static bool GPS_enabled = false; +static bool GPS_enabled = false; // true if we have a position estimate from AHRS static bool have_position; @@ -399,89 +400,89 @@ static bool have_position; // Location & Navigation //////////////////////////////////////////////////////////////////////////////// // Constants -const float radius_of_earth = 6378100; // meters -const float gravity = 9.81; // meters/ sec^2 +const float radius_of_earth = 6378100; // meters +const float gravity = 9.81; // meters/ sec^2 // This is the currently calculated direction to fly. // deg * 100 : 0 to 360 -static int32_t nav_bearing_cd; +static int32_t nav_bearing_cd; // This is the direction to the next waypoint or loiter center // deg * 100 : 0 to 360 -static int32_t target_bearing_cd; +static int32_t target_bearing_cd; //This is the direction from the last waypoint to the next waypoint // deg * 100 : 0 to 360 -static int32_t crosstrack_bearing_cd; +static int32_t crosstrack_bearing_cd; // Direction held during phases of takeoff and landing // deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use -static int32_t hold_course = -1; // deg * 100 dir of plane +static int32_t hold_course = -1; // deg * 100 dir of plane // There may be two active commands in Auto mode. // This indicates the active navigation command by index number -static byte nav_command_index; +static byte nav_command_index; // This indicates the active non-navigation command by index number -static byte non_nav_command_index; +static byte non_nav_command_index; // This is the command type (eg navigate to waypoint) of the active navigation command -static byte nav_command_ID = NO_COMMAND; -static byte non_nav_command_ID = NO_COMMAND; +static byte nav_command_ID = NO_COMMAND; +static byte non_nav_command_ID = NO_COMMAND; //////////////////////////////////////////////////////////////////////////////// // Airspeed //////////////////////////////////////////////////////////////////////////////// // The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met. // Also used for flap deployment criteria. Centimeters per second. -static int32_t target_airspeed_cm; +static int32_t target_airspeed_cm; // The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second. -static float airspeed_error_cm; +static float airspeed_error_cm; // The calculated total energy error (kinetic (altitude) plus potential (airspeed)). // Used by the throttle controller -static int32_t energy_error; +static int32_t energy_error; // kinetic portion of energy error (m^2/s^2) -static int32_t airspeed_energy_error; +static int32_t airspeed_energy_error; // An amount that the airspeed should be increased in auto modes based on the user positioning the // throttle stick in the top half of the range. Centimeters per second. -static int16_t airspeed_nudge_cm; +static int16_t airspeed_nudge_cm; // Similar to airspeed_nudge, but used when no airspeed sensor. // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel -static int16_t throttle_nudge = 0; +static int16_t throttle_nudge = 0; //////////////////////////////////////////////////////////////////////////////// // Ground speed //////////////////////////////////////////////////////////////////////////////// // The amount current ground speed is below min ground speed. Centimeters per second -static int32_t groundspeed_undershoot = 0; +static int32_t groundspeed_undershoot = 0; //////////////////////////////////////////////////////////////////////////////// // Location Errors //////////////////////////////////////////////////////////////////////////////// // Difference between current bearing and desired bearing. Hundredths of a degree -static int32_t bearing_error_cd; +static int32_t bearing_error_cd; // Difference between current altitude and desired altitude. Centimeters -static int32_t altitude_error_cm; +static int32_t altitude_error_cm; // Distance perpandicular to the course line that we are off trackline. Meters -static float crosstrack_error; +static float crosstrack_error; //////////////////////////////////////////////////////////////////////////////// // Battery Sensors //////////////////////////////////////////////////////////////////////////////// // Battery pack 1 voltage. Initialized above the low voltage threshold to pre-load the filter and prevent low voltage events at startup. -static float battery_voltage1 = LOW_VOLTAGE * 1.05; +static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery pack 1 instantaneous currrent draw. Amperes -static float current_amps1; +static float current_amps1; // Totalized current (Amp-hours) from battery 1 -static float current_total1; +static float current_total1; // To Do - Add support for second battery pack -//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter +//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter //static float current_amps2; // Current (Amperes) draw from battery 2 //static float current_total2; // Totalized current (Amp-hours) from battery 2 @@ -494,7 +495,7 @@ AP_Airspeed airspeed(&pitot_analog_source); // Altitude Sensor variables //////////////////////////////////////////////////////////////////////////////// // Altitude from the sonar sensor. Meters. Not yet implemented. -static int16_t sonar_alt; +static int16_t sonar_alt; //////////////////////////////////////////////////////////////////////////////// // flight mode specific @@ -503,166 +504,166 @@ static int16_t sonar_alt; static bool takeoff_complete = true; // Flag to indicate if we have landed. //Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown -static bool land_complete; +static bool land_complete; // Altitude threshold to complete a takeoff command in autonomous modes. Centimeters -static int32_t takeoff_altitude; +static int32_t takeoff_altitude; // Minimum pitch to hold during takeoff command execution. Hundredths of a degree -static int16_t takeoff_pitch_cd; +static int16_t takeoff_pitch_cd; //////////////////////////////////////////////////////////////////////////////// // Loiter management //////////////////////////////////////////////////////////////////////////////// // Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree -static int32_t old_target_bearing_cd; +static int32_t old_target_bearing_cd; // Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees -static int32_t loiter_total; +static int32_t loiter_total; // The amount in degrees we have turned since recording old_target_bearing -static int16_t loiter_delta; +static int16_t loiter_delta; // Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees -static int32_t loiter_sum; +static int32_t loiter_sum; // The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds. -static uint32_t loiter_time_ms; +static uint32_t loiter_time_ms; // The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds. -static uint32_t loiter_time_max_ms; +static uint32_t loiter_time_max_ms; //////////////////////////////////////////////////////////////////////////////// // Navigation control variables //////////////////////////////////////////////////////////////////////////////// // The instantaneous desired bank angle. Hundredths of a degree -static int32_t nav_roll_cd; +static int32_t nav_roll_cd; // The instantaneous desired pitch angle. Hundredths of a degree -static int32_t nav_pitch_cd; +static int32_t nav_pitch_cd; //////////////////////////////////////////////////////////////////////////////// // Waypoint distances //////////////////////////////////////////////////////////////////////////////// // Distance between plane and next waypoint. Meters // is not static because AP_Camera uses it -int32_t wp_distance; +int32_t wp_distance; // Distance between previous and next waypoint. Meters -static int32_t wp_totalDistance; +static int32_t wp_totalDistance; //////////////////////////////////////////////////////////////////////////////// // repeating event control //////////////////////////////////////////////////////////////////////////////// // Flag indicating current event type -static byte event_id; +static byte event_id; // when the event was started in ms -static int32_t event_timer_ms; +static int32_t event_timer_ms; // how long to delay the next firing of event in millis -static uint16_t event_delay_ms; +static uint16_t event_delay_ms; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles -static int16_t event_repeat = 0; +static int16_t event_repeat = 0; // per command value, such as PWM for servos -static int16_t event_value; +static int16_t event_value; // the value used to cycle events (alternate value to event_value) -static int16_t event_undo_value; +static int16_t event_undo_value; //////////////////////////////////////////////////////////////////////////////// // Conditional command //////////////////////////////////////////////////////////////////////////////// // A value used in condition commands (eg delay, change alt, etc.) // For example in a change altitude command, it is the altitude to change to. -static int32_t condition_value; +static int32_t condition_value; // A starting value used to check the status of a conditional command. // For example in a delay command the condition_start records that start time for the delay static uint32_t condition_start; // A value used in condition commands. For example the rate at which to change altitude. -static int16_t condition_rate; +static int16_t condition_rate; //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors // Location structure defined in AP_Common //////////////////////////////////////////////////////////////////////////////// // The home location used for RTL. The location is set when we first get stable GPS lock -static struct Location home; +static struct Location home; // Flag for if we have g_gps lock and have set the home location -static bool home_is_set; +static bool home_is_set; // The location of the previous waypoint. Used for track following and altitude ramp calculations -static struct Location prev_WP; +static struct Location prev_WP; // The plane's current location -static struct Location current_loc; +static struct Location current_loc; // The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. -static struct Location next_WP; +static struct Location next_WP; // The location of the active waypoint in Guided mode. -static struct Location guided_WP; +static struct Location guided_WP; // The location structure information from the Nav command being processed -static struct Location next_nav_command; +static struct Location next_nav_command; // The location structure information from the Non-Nav command being processed -static struct Location next_nonnav_command; +static struct Location next_nonnav_command; //////////////////////////////////////////////////////////////////////////////// // Altitude / Climb rate control //////////////////////////////////////////////////////////////////////////////// // The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters -static int32_t target_altitude_cm; +static int32_t target_altitude_cm; // Altitude difference between previous and current waypoint. Centimeters -static int32_t offset_altitude_cm; +static int32_t offset_altitude_cm; //////////////////////////////////////////////////////////////////////////////// // IMU variables //////////////////////////////////////////////////////////////////////////////// // The main loop execution time. Seconds //This is the time between calls to the DCM algorithm and is the Integration time for the gyros. -static float G_Dt = 0.02; +static float G_Dt = 0.02; //////////////////////////////////////////////////////////////////////////////// // Performance monitoring //////////////////////////////////////////////////////////////////////////////// // Timer used to accrue data and trigger recording of the performanc monitoring log message -static int32_t perf_mon_timer; +static int32_t perf_mon_timer; // The maximum main loop execution time recorded in the current performance monitoring interval -static int16_t G_Dt_max = 0; +static int16_t G_Dt_max = 0; // The number of gps fixes recorded in the current performance monitoring interval -static int16_t gps_fix_count = 0; +static int16_t gps_fix_count = 0; // A variable used by developers to track performanc metrics. // Currently used to record the number of GCS heartbeat messages received -static int16_t pmTest1 = 0; +static int16_t pmTest1 = 0; //////////////////////////////////////////////////////////////////////////////// // System Timers //////////////////////////////////////////////////////////////////////////////// // Time in miliseconds of start of main control loop. Milliseconds -static uint32_t fast_loopTimer_ms; +static uint32_t fast_loopTimer_ms; // Time Stamp when fast loop was complete. Milliseconds -static uint32_t fast_loopTimeStamp_ms; +static uint32_t fast_loopTimeStamp_ms; // Number of milliseconds used in last main loop cycle -static uint8_t delta_ms_fast_loop; +static uint8_t delta_ms_fast_loop; // Counter of main loop executions. Used for performance monitoring and failsafe processing -static uint16_t mainLoop_count; +static uint16_t mainLoop_count; // Time in miliseconds of start of medium control loop. Milliseconds -static uint32_t medium_loopTimer_ms; +static uint32_t medium_loopTimer_ms; // Counters for branching from main control loop to slower loops -static byte medium_loopCounter; +static byte medium_loopCounter; // Number of milliseconds used in last medium loop cycle -static uint8_t delta_ms_medium_loop; +static uint8_t delta_ms_medium_loop; // Counters for branching from medium control loop to slower loops -static byte slow_loopCounter; +static byte slow_loopCounter; // Counter to trigger execution of very low rate processes -static byte superslow_loopCounter; +static byte superslow_loopCounter; // Counter to trigger execution of 1 Hz processes -static byte counter_one_herz; +static byte counter_one_herz; // % MCU cycles used -static float load; +static float load; // Camera/Antenna mount tracking and stabilisation stuff @@ -688,108 +689,108 @@ AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs, 1); //////////////////////////////////////////////////////////////////////////////// void setup() { - memcheck_init(); - init_ardupilot(); + memcheck_init(); + init_ardupilot(); } void loop() { - // We want this to execute at 50Hz if possible - // ------------------------------------------- - if (millis() - fast_loopTimer_ms > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer_ms; - load = (float)(fast_loopTimeStamp_ms - fast_loopTimer_ms)/delta_ms_fast_loop; - G_Dt = (float)delta_ms_fast_loop / 1000.f; - fast_loopTimer_ms = millis(); + // We want this to execute at 50Hz if possible + // ------------------------------------------- + if (millis() - fast_loopTimer_ms > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer_ms; + load = (float)(fast_loopTimeStamp_ms - fast_loopTimer_ms)/delta_ms_fast_loop; + G_Dt = (float)delta_ms_fast_loop / 1000.f; + fast_loopTimer_ms = millis(); - mainLoop_count++; + mainLoop_count++; - // Execute the fast loop - // --------------------- - fast_loop(); + // Execute the fast loop + // --------------------- + fast_loop(); - // Execute the medium loop - // ----------------------- - medium_loop(); + // Execute the medium loop + // ----------------------- + medium_loop(); - counter_one_herz++; - if(counter_one_herz == 50){ - one_second_loop(); - counter_one_herz = 0; - } + counter_one_herz++; + if(counter_one_herz == 50) { + one_second_loop(); + counter_one_herz = 0; + } - if (millis() - perf_mon_timer > 20000) { - if (mainLoop_count != 0) { - if (g.log_bitmask & MASK_LOG_PM) - #if HIL_MODE != HIL_MODE_ATTITUDE - Log_Write_Performance(); - #endif + if (millis() - perf_mon_timer > 20000) { + if (mainLoop_count != 0) { + if (g.log_bitmask & MASK_LOG_PM) +#if HIL_MODE != HIL_MODE_ATTITUDE + Log_Write_Performance(); +#endif - resetPerfData(); - } - } + resetPerfData(); + } + } - fast_loopTimeStamp_ms = millis(); - } + fast_loopTimeStamp_ms = millis(); + } } // Main loop 50Hz static void fast_loop() { - // This is the fast loop - we want it to execute at 50Hz if possible - // ----------------------------------------------------------------- - if (delta_ms_fast_loop > G_Dt_max) - G_Dt_max = delta_ms_fast_loop; + // This is the fast loop - we want it to execute at 50Hz if possible + // ----------------------------------------------------------------- + if (delta_ms_fast_loop > G_Dt_max) + G_Dt_max = delta_ms_fast_loop; - // Read radio - // ---------- - read_radio(); + // Read radio + // ---------- + read_radio(); // try to send any deferred messages if the serial port now has // some space available gcs_send_message(MSG_RETRY_DEFERRED); - // check for loss of control signal failsafe condition - // ------------------------------------ - check_short_failsafe(); + // check for loss of control signal failsafe condition + // ------------------------------------ + check_short_failsafe(); - #if HIL_MODE == HIL_MODE_SENSORS - // update hil before AHRS update - gcs_update(); - #endif +#if HIL_MODE == HIL_MODE_SENSORS + // update hil before AHRS update + gcs_update(); +#endif ahrs.update(); - // uses the yaw from the DCM to give more accurate turns - calc_bearing_error(); + // uses the yaw from the DCM to give more accurate turns + calc_bearing_error(); - # if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) - Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor); +# if HIL_MODE == HIL_MODE_DISABLED + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) + Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor); - if (g.log_bitmask & MASK_LOG_RAW) - Log_Write_Raw(); - #endif + if (g.log_bitmask & MASK_LOG_RAW) + Log_Write_Raw(); +#endif - // inertial navigation - // ------------------ - #if INERTIAL_NAVIGATION == ENABLED - // TODO: implement inertial nav function - inertialNavigation(); - #endif + // inertial navigation + // ------------------ +#if INERTIAL_NAVIGATION == ENABLED + // TODO: implement inertial nav function + inertialNavigation(); +#endif - // custom code/exceptions for flight modes - // --------------------------------------- - update_current_flight_mode(); + // custom code/exceptions for flight modes + // --------------------------------------- + update_current_flight_mode(); - // apply desired roll, pitch and yaw to the plane - // ---------------------------------------------- - if (control_mode > MANUAL) - stabilize(); + // apply desired roll, pitch and yaw to the plane + // ---------------------------------------------- + if (control_mode > MANUAL) + stabilize(); - // write out the servo PWM values - // ------------------------------ - set_servos(); + // write out the servo PWM values + // ------------------------------ + set_servos(); gcs_update(); gcs_data_stream_send(); @@ -798,429 +799,429 @@ static void fast_loop() static void medium_loop() { #if MOUNT == ENABLED - camera_mount.update_mount_position(); + camera_mount.update_mount_position(); #endif #if MOUNT2 == ENABLED - camera_mount2.update_mount_position(); + camera_mount2.update_mount_position(); #endif #if CAMERA == ENABLED - g.camera.trigger_pic_cleanup(); + g.camera.trigger_pic_cleanup(); #endif - // This is the start of the medium (10 Hz) loop pieces - // ----------------------------------------- - switch(medium_loopCounter) { + // This is the start of the medium (10 Hz) loop pieces + // ----------------------------------------- + switch(medium_loopCounter) { - // This case deals with the GPS - //------------------------------- - case 0: - medium_loopCounter++; - if(GPS_enabled){ - update_GPS(); - calc_gndspeed_undershoot(); - } + // This case deals with the GPS + //------------------------------- + case 0: + medium_loopCounter++; + if(GPS_enabled) { + update_GPS(); + calc_gndspeed_undershoot(); + } - #if HIL_MODE != HIL_MODE_ATTITUDE - if (g.compass_enabled && compass.read()) { - ahrs.set_compass(&compass); - compass.null_offsets(); - } else { - ahrs.set_compass(NULL); - } - #endif -/*{ -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.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); -Vector3f tempaccel = imu.get_accel(); -Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); -Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); -Serial.println(tempaccel.z, DEC); -}*/ - - break; - - // This case performs some navigation computations - //------------------------------------------------ - case 1: - medium_loopCounter++; - - // Read 6-position switch on radio - // ------------------------------- - read_control_switch(); - - // calculate the plane's desired bearing - // ------------------------------------- - navigate(); - - break; - - // command processing - //------------------------------ - case 2: - medium_loopCounter++; - - // Read Airspeed - // ------------- #if HIL_MODE != HIL_MODE_ATTITUDE - if (airspeed.enabled()) { - read_airspeed(); - } + if (g.compass_enabled && compass.read()) { + ahrs.set_compass(&compass); + compass.null_offsets(); + } else { + ahrs.set_compass(NULL); + } +#endif +/*{ + * 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.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); + * Vector3f tempaccel = imu.get_accel(); + * Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); + * Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); + * Serial.println(tempaccel.z, DEC); + * }*/ + + break; + + // This case performs some navigation computations + //------------------------------------------------ + case 1: + medium_loopCounter++; + + // Read 6-position switch on radio + // ------------------------------- + read_control_switch(); + + // calculate the plane's desired bearing + // ------------------------------------- + navigate(); + + break; + + // command processing + //------------------------------ + case 2: + medium_loopCounter++; + + // Read Airspeed + // ------------- +#if HIL_MODE != HIL_MODE_ATTITUDE + if (airspeed.enabled()) { + read_airspeed(); + } #endif - // Read altitude from sensors - // ------------------ - update_alt(); - if(g.sonar_enabled) sonar_alt = sonar.read(); + // Read altitude from sensors + // ------------------ + update_alt(); + if(g.sonar_enabled) sonar_alt = sonar.read(); - // altitude smoothing - // ------------------ - if (control_mode != FLY_BY_WIRE_B) - calc_altitude_error(); + // altitude smoothing + // ------------------ + if (control_mode != FLY_BY_WIRE_B) + calc_altitude_error(); - // perform next command - // -------------------- - update_commands(); - break; + // perform next command + // -------------------- + update_commands(); + break; - // This case deals with sending high rate telemetry - //------------------------------------------------- - case 3: - medium_loopCounter++; + // This case deals with sending high rate telemetry + //------------------------------------------------- + case 3: + medium_loopCounter++; - #if HIL_MODE != HIL_MODE_ATTITUDE - 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); +#if HIL_MODE != HIL_MODE_ATTITUDE + 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); - if (g.log_bitmask & MASK_LOG_CTUN) - Log_Write_Control_Tuning(); - #endif + if (g.log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); +#endif - if (g.log_bitmask & MASK_LOG_NTUN) - Log_Write_Nav_Tuning(); + if (g.log_bitmask & MASK_LOG_NTUN) + Log_Write_Nav_Tuning(); - if (g.log_bitmask & MASK_LOG_GPS) - Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); - break; + if (g.log_bitmask & MASK_LOG_GPS) + Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); + break; - // This case controls the slow loop - //--------------------------------- - case 4: - medium_loopCounter = 0; - delta_ms_medium_loop = millis() - medium_loopTimer_ms; - medium_loopTimer_ms = millis(); + // This case controls the slow loop + //--------------------------------- + case 4: + medium_loopCounter = 0; + delta_ms_medium_loop = millis() - medium_loopTimer_ms; + medium_loopTimer_ms = millis(); - if (g.battery_monitoring != 0){ - read_battery(); - } + if (g.battery_monitoring != 0) { + read_battery(); + } - slow_loop(); + slow_loop(); #if OBC_FAILSAFE == ENABLED - // perform OBC failsafe checks - obc.check(OBC_MODE(control_mode), - last_heartbeat_ms, - g_gps?g_gps->last_fix_time:0); + // perform OBC failsafe checks + obc.check(OBC_MODE(control_mode), + last_heartbeat_ms, + g_gps ? g_gps->last_fix_time : 0); #endif - break; - } + break; + } } static void slow_loop() { - // This is the slow (3 1/3 Hz) loop pieces - //---------------------------------------- - switch (slow_loopCounter){ - case 0: - slow_loopCounter++; - check_long_failsafe(); - superslow_loopCounter++; - if(superslow_loopCounter >=200) { // 200 = Execute every minute - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.compass_enabled) { - compass.save_offsets(); - } - #endif + // This is the slow (3 1/3 Hz) loop pieces + //---------------------------------------- + switch (slow_loopCounter) { + case 0: + slow_loopCounter++; + check_long_failsafe(); + superslow_loopCounter++; + if(superslow_loopCounter >=200) { // 200 = Execute every minute +#if HIL_MODE != HIL_MODE_ATTITUDE + if(g.compass_enabled) { + compass.save_offsets(); + } +#endif - superslow_loopCounter = 0; - } - break; + superslow_loopCounter = 0; + } + break; - case 1: - slow_loopCounter++; + case 1: + slow_loopCounter++; #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 - update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); #else - update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11); #endif - enable_aux_servos(); + enable_aux_servos(); #if MOUNT == ENABLED - camera_mount.update_mount_type(); + camera_mount.update_mount_type(); #endif #if MOUNT2 == ENABLED - camera_mount2.update_mount_type(); + camera_mount2.update_mount_type(); #endif - break; + break; - case 2: - slow_loopCounter = 0; - update_events(); + case 2: + slow_loopCounter = 0; + update_events(); - mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter + mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter #if USB_MUX_PIN > 0 - check_usb_mux(); + check_usb_mux(); #endif - break; - } + break; + } } static void one_second_loop() { - if (g.log_bitmask & MASK_LOG_CUR) - Log_Write_Current(); + if (g.log_bitmask & MASK_LOG_CUR) + Log_Write_Current(); - // send a heartbeat - gcs_send_message(MSG_HEARTBEAT); + // send a heartbeat + gcs_send_message(MSG_HEARTBEAT); } static void update_GPS(void) { - g_gps->update(); - update_GPS_light(); + g_gps->update(); + update_GPS_light(); // get position from AHRS have_position = ahrs.get_position(¤t_loc); - if (g_gps->new_data && g_gps->fix) { + if (g_gps->new_data && g_gps->fix) { g_gps->new_data = false; - // for performance - // --------------- - gps_fix_count++; + // for performance + // --------------- + gps_fix_count++; - if(ground_start_count > 1){ - ground_start_count--; - ground_start_avg += g_gps->ground_speed; + if(ground_start_count > 1) { + ground_start_count--; + ground_start_avg += g_gps->ground_speed; - } else if (ground_start_count == 1) { - // We countdown N number of good GPS fixes - // so that the altitude is more accurate - // ------------------------------------- - if (current_loc.lat == 0) { - ground_start_count = 5; + } else if (ground_start_count == 1) { + // We countdown N number of good GPS fixes + // so that the altitude is more accurate + // ------------------------------------- + if (current_loc.lat == 0) { + ground_start_count = 5; - } else { - if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){ - startup_ground(); + } else { + if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT) { + startup_ground(); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); - init_home(); - } else if (ENABLE_AIR_START == 0) { - init_home(); - } + init_home(); + } else if (ENABLE_AIR_START == 0) { + init_home(); + } - if (g.compass_enabled) { - // Set compass declination automatically - compass.set_initial_location(g_gps->latitude, g_gps->longitude); - } - ground_start_count = 0; - } - } + if (g.compass_enabled) { + // Set compass declination automatically + compass.set_initial_location(g_gps->latitude, g_gps->longitude); + } + ground_start_count = 0; + } + } // see if we've breached the geo-fence geofence_check(false); - } + } } static void update_current_flight_mode(void) { - if(control_mode == AUTO){ - crash_checker(); + if(control_mode == AUTO) { + crash_checker(); - switch(nav_command_ID){ - case MAV_CMD_NAV_TAKEOFF: - if (hold_course != -1) { - calc_nav_roll(); - } else { - nav_roll_cd = 0; - } - - if(airspeed.use()) { - calc_nav_pitch(); - if (nav_pitch_cd < takeoff_pitch_cd) - nav_pitch_cd = takeoff_pitch_cd; - } else { - nav_pitch_cd = (float)g_gps->ground_speed / (float)g.airspeed_cruise_cm * (float)takeoff_pitch_cd * 0.5; - nav_pitch_cd = constrain(nav_pitch_cd, 500, takeoff_pitch_cd); - } - - g.channel_throttle.servo_out = g.throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle - // What is the case for doing something else? Why wouldn't you want max throttle for TO? - // ****************************** - - break; - - case MAV_CMD_NAV_LAND: - calc_nav_roll(); + switch(nav_command_ID) { + case MAV_CMD_NAV_TAKEOFF: + if (hold_course != -1) { + calc_nav_roll(); + } else { + nav_roll_cd = 0; + } + if(airspeed.use()) { calc_nav_pitch(); - calc_throttle(); - if (!airspeed.use() || land_complete) { - // hold pitch constant in final approach - nav_pitch_cd = g.land_pitch_cd; - } + if (nav_pitch_cd < takeoff_pitch_cd) + nav_pitch_cd = takeoff_pitch_cd; + } else { + nav_pitch_cd = (float)g_gps->ground_speed / (float)g.airspeed_cruise_cm * (float)takeoff_pitch_cd * 0.5; + nav_pitch_cd = constrain(nav_pitch_cd, 500, takeoff_pitch_cd); + } - if (land_complete) { - // we are in the final stage of a landing - force - // zero throttle - g.channel_throttle.servo_out = 0; - } - break; + g.channel_throttle.servo_out = g.throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle + // What is the case for doing something else? Why wouldn't you want max throttle for TO? + // ****************************** - default: - // we are doing normal AUTO flight, the special cases - // are for takeoff and landing - hold_course = -1; - land_complete = false; - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - } - }else{ + break; + + case MAV_CMD_NAV_LAND: + calc_nav_roll(); + + calc_nav_pitch(); + calc_throttle(); + if (!airspeed.use() || land_complete) { + // hold pitch constant in final approach + nav_pitch_cd = g.land_pitch_cd; + } + + if (land_complete) { + // we are in the final stage of a landing - force + // zero throttle + g.channel_throttle.servo_out = 0; + } + break; + + default: + // we are doing normal AUTO flight, the special cases + // are for takeoff and landing + hold_course = -1; + land_complete = false; + calc_nav_roll(); + calc_nav_pitch(); + calc_throttle(); + break; + } + }else{ // hold_course is only used in takeoff and landing hold_course = -1; - switch(control_mode){ - case RTL: - case LOITER: - case GUIDED: - crash_checker(); - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; + switch(control_mode) { + case RTL: + case LOITER: + case GUIDED: + crash_checker(); + calc_nav_roll(); + calc_nav_pitch(); + calc_throttle(); + break; - case FLY_BY_WIRE_A: - // set nav_roll and nav_pitch using sticks - nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd; - nav_pitch_cd = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min_cd; - // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. - nav_pitch_cd = constrain(nav_pitch_cd, -3000, 3000); // trying to give more pitch authority - if (inverted_flight) { - nav_pitch_cd = -nav_pitch_cd; + case FLY_BY_WIRE_A: + // set nav_roll and nav_pitch using sticks + nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd; + nav_pitch_cd = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min_cd; + // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. + nav_pitch_cd = constrain(nav_pitch_cd, -3000, 3000); // trying to give more pitch authority + if (inverted_flight) { + nav_pitch_cd = -nav_pitch_cd; + } + break; + + case FLY_BY_WIRE_B: + // Substitute stick inputs for Navigation control output + // We use g.pitch_limit_min because its magnitude is + // normally greater than g.pitch_limit_max + + // Thanks to Yury MonZon for the altitude limit code! + + nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd; + + float elevator_input; + elevator_input = g.channel_pitch.norm_input(); + + if (g.flybywire_elev_reverse) { + elevator_input = -elevator_input; + } + if ((current_loc.alt >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) { + altitude_error_cm = elevator_input * g.pitch_limit_min_cd; + } else { + altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt; + if (elevator_input < 0) { + altitude_error_cm += elevator_input * g.pitch_limit_min_cd; } - break; + } + calc_throttle(); + calc_nav_pitch(); + break; - case FLY_BY_WIRE_B: - // Substitute stick inputs for Navigation control output - // We use g.pitch_limit_min because its magnitude is - // normally greater than g.pitch_limit_max + case STABILIZE: + nav_roll_cd = 0; + nav_pitch_cd = 0; + // throttle is passthrough + break; - // Thanks to Yury MonZon for the altitude limit code! + case CIRCLE: + // we have no GPS installed and have lost radio contact + // or we just want to fly around in a gentle circle w/o GPS + // ---------------------------------------------------- + nav_roll_cd = g.roll_limit_cd / 3; + nav_pitch_cd = 0; - nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd; - - float elevator_input; - elevator_input = g.channel_pitch.norm_input(); + if (failsafe != FAILSAFE_NONE) { + g.channel_throttle.servo_out = g.throttle_cruise; + } + break; - if (g.flybywire_elev_reverse) { - elevator_input = -elevator_input; - } - if ((current_loc.alt >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) { - altitude_error_cm = elevator_input * g.pitch_limit_min_cd; - } else { - altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt; - if (elevator_input < 0) { - altitude_error_cm += elevator_input * g.pitch_limit_min_cd; - } - } - calc_throttle(); - calc_nav_pitch(); - break; + case MANUAL: + // servo_out is for Sim control only + // --------------------------------- + g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); + g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); + g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); + break; + //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - case STABILIZE: - nav_roll_cd = 0; - nav_pitch_cd = 0; - // throttle is passthrough - break; - - case CIRCLE: - // we have no GPS installed and have lost radio contact - // or we just want to fly around in a gentle circle w/o GPS - // ---------------------------------------------------- - nav_roll_cd = g.roll_limit_cd / 3; - nav_pitch_cd = 0; - - if (failsafe != FAILSAFE_NONE){ - g.channel_throttle.servo_out = g.throttle_cruise; - } - break; - - case MANUAL: - // servo_out is for Sim control only - // --------------------------------- - g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); - g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); - g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); - break; - //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - - } - } + } + } } static void update_navigation() { - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ + // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS + // ------------------------------------------------------------------------ - // distance and bearing calcs only - if(control_mode == AUTO){ - verify_commands(); - }else{ + // distance and bearing calcs only + if(control_mode == AUTO) { + verify_commands(); + }else{ - switch(control_mode){ - case LOITER: - case RTL: - case GUIDED: - update_loiter(); - calc_bearing_error(); - break; + switch(control_mode) { + case LOITER: + case RTL: + case GUIDED: + update_loiter(); + calc_bearing_error(); + break; - } - } + } + } } static void update_alt() { - #if HIL_MODE == HIL_MODE_ATTITUDE - current_loc.alt = g_gps->altitude; - #else - // this function is in place to potentially add a sonar sensor in the future - //altitude_sensor = BARO; +#if HIL_MODE == HIL_MODE_ATTITUDE + current_loc.alt = g_gps->altitude; +#else + // this function is in place to potentially add a sonar sensor in the future + //altitude_sensor = BARO; - if (barometer.healthy) { - current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100) - current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); - } else if (g_gps->fix) { - current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100) - } - #endif + if (barometer.healthy) { + current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100) + current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); + } else if (g_gps->fix) { + current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100) + } +#endif - geofence_check(true); + geofence_check(true); - // Calculate new climb rate - //if(medium_loopCounter == 0 && slow_loopCounter == 0) - // add_altitude_data(millis() / 100, g_gps->altitude / 10); + // Calculate new climb rate + //if(medium_loopCounter == 0 && slow_loopCounter == 0) + // add_altitude_data(millis() / 100, g_gps->altitude / 10); }