diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index db7fd83216..929f960847 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2,52 +2,52 @@ #define THISFIRMWARE "ArduCopter V2.7.2" /* -ArduCopter Version 2.7 -Lead author: Jason Short -Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni -Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier, Robert Lefebvre, Marco Robustini - -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. - -Special Thanks for Contributors: - -Hein Hollander :Octo Support -Dani Saez :V Ocoto Support -Max Levine :Tri Support, Graphics -Jose Julio :Stabilization Control laws -Randy MacKay :Heli Support -Jani Hiriven :Testing feedback -Andrew Tridgell :Mavlink Support -James Goppert :Mavlink Support -Doug Weibel :Libraries -Mike Smith :Libraries, Coding support -HappyKillmore :Mavlink GCS -Michael Oborne :Mavlink GCS -Jack Dunkle :Alpha testing -Christof Schmid :Alpha testing -Oliver :Piezo support -Guntars :Arming safety suggestion -Igor van Airde :Control Law optimization -Jean-Louis Naudin :Auto Landing -Sandro Benigno :Camera support -Olivier Adler :PPM Encoder -John Arne Birkeland :PPM Encoder -Adam M Rivera :Auto Compass Declination -Marco Robustini :Alpha testing -Angel Fernandez :Alpha testing -Robert Lefebvre :Heli Support & LEDs -Amilcar Lucas :mount and camera configuration -Gregory Fletcher :mount orientation math - -And much more so PLEASE PM me on DIYDRONES to add your contribution to the List - -Requires modified "mrelax" version of Arduino, which can be found here: -http://code.google.com/p/ardupilot-mega/downloads/list - -*/ + * ArduCopter Version 2.7 + * Lead author: Jason Short + * Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni + * Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier, Robert Lefebvre, Marco Robustini + * + * 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. + * + * Special Thanks for Contributors: + * + * Hein Hollander :Octo Support + * Dani Saez :V Ocoto Support + * Max Levine :Tri Support, Graphics + * Jose Julio :Stabilization Control laws + * Randy MacKay :Heli Support + * Jani Hiriven :Testing feedback + * Andrew Tridgell :Mavlink Support + * James Goppert :Mavlink Support + * Doug Weibel :Libraries + * Mike Smith :Libraries, Coding support + * HappyKillmore :Mavlink GCS + * Michael Oborne :Mavlink GCS + * Jack Dunkle :Alpha testing + * Christof Schmid :Alpha testing + * Oliver :Piezo support + * Guntars :Arming safety suggestion + * Igor van Airde :Control Law optimization + * Jean-Louis Naudin :Auto Landing + * Sandro Benigno :Camera support + * Olivier Adler :PPM Encoder + * John Arne Birkeland :PPM Encoder + * Adam M Rivera :Auto Compass Declination + * Marco Robustini :Alpha testing + * Angel Fernandez :Alpha testing + * Robert Lefebvre :Heli Support & LEDs + * Amilcar Lucas :mount and camera configuration + * Gregory Fletcher :mount orientation math + * + * And much more so PLEASE PM me on DIYDRONES to add your contribution to the List + * + * Requires modified "mrelax" version of Arduino, which can be found here: + * http://code.google.com/p/ardupilot-mega/downloads/list + * + */ //////////////////////////////////////////////////////////////////////////////// // Header includes @@ -65,8 +65,8 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include #include // ArduPilot Mega RC Library #include // ArduPilot GPS library -#include // Arduino I2C lib -#include // Arduino SPI lib +#include // Arduino I2C lib +#include // Arduino SPI lib #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library #include @@ -79,27 +79,27 @@ http://code.google.com/p/ardupilot-mega/downloads/list // (only included for makefile libpath to work) #include // TimerProcess is the scheduler for MPU6000 reads. #include -#include // PI library +#include // PI library #include // PID library #include // RC Channel Library -#include // AP Motors library -#include // AP Motors library for Quad -#include // AP Motors library for Tri -#include // AP Motors library for Hexa -#include // AP Motors library for Y6 -#include // AP Motors library for Octa -#include // AP Motors library for OctaQuad -#include // AP Motors library for Heli -#include // AP Motors library for Heli -#include // Range finder library +#include // AP Motors library +#include // AP Motors library for Quad +#include // AP Motors library for Tri +#include // AP Motors library for Hexa +#include // AP Motors library for Y6 +#include // AP Motors library for Octa +#include // AP Motors library for OctaQuad +#include // AP Motors library for Heli +#include // AP Motors library for Heli +#include // Range finder library #include // Optical Flow library -#include // Filter library -#include // Mode Filter from Filter library -#include // Mode Filter from Filter library -#include // GPS Lead filter -#include // APM relay -#include // Photo or video camera -#include // Camera/Antenna mount +#include // Filter library +#include // Mode Filter from Filter library +#include // Mode Filter from Filter library +#include // GPS Lead filter +#include // APM relay +#include // Photo or video camera +#include // Camera/Antenna mount #include // needed for AHRS build #include @@ -149,7 +149,7 @@ Arduino_Mega_ISR_Registry isr_registry; // // Global parameters are all contained within the 'g' class. // -static Parameters g; +static Parameters g; //////////////////////////////////////////////////////////////////////////////// @@ -160,18 +160,18 @@ static void update_events(void); // RC Hardware //////////////////////////////////////////////////////////////////////////////// #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 @@ -197,116 +197,116 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #if HIL_MODE == HIL_MODE_DISABLED // real sensors -#if CONFIG_ADC == ENABLED - AP_ADC_ADS7844 adc; -#endif + #if CONFIG_ADC == ENABLED +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 - AP_Baro_BMP085 barometer(true); -# else - AP_Baro_BMP085 barometer(false); -# endif -#elif CONFIG_BARO == AP_BARO_MS5611 - AP_Baro_MS5611 barometer; -#endif + #if CONFIG_BARO == AP_BARO_BMP085 + # if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +AP_Baro_BMP085 barometer(true); + # else +AP_Baro_BMP085 barometer(false); + # endif + #elif CONFIG_BARO == AP_BARO_MS5611 +AP_Baro_MS5611 barometer; + #endif - AP_Compass_HMC5843 compass; -#endif +AP_Compass_HMC5843 compass; + #endif -#ifdef OPTFLOW_ENABLED - #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 - AP_OpticalFlow_ADNS3080_APM2 optflow(OPTFLOW_CS_PIN); - #else - AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); - #endif -#else - AP_OpticalFlow optflow; -#endif + #ifdef OPTFLOW_ENABLED + #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +AP_OpticalFlow_ADNS3080_APM2 optflow(OPTFLOW_CS_PIN); + #else +AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); + #endif + #else +AP_OpticalFlow optflow; + #endif // real GPS selection -#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO - AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); + #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO +AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); -#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA - AP_GPS_NMEA g_gps_driver(&Serial1); + #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA +AP_GPS_NMEA g_gps_driver(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF - AP_GPS_SIRF g_gps_driver(&Serial1); + #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF +AP_GPS_SIRF g_gps_driver(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX - AP_GPS_UBLOX g_gps_driver(&Serial1); + #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX +AP_GPS_UBLOX g_gps_driver(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK - AP_GPS_MTK g_gps_driver(&Serial1); + #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK +AP_GPS_MTK g_gps_driver(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 - AP_GPS_MTK16 g_gps_driver(&Serial1); + #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 +AP_GPS_MTK16 g_gps_driver(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE - AP_GPS_None g_gps_driver(NULL); + #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 + #if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); -#else + #else AP_InertialSensor_Oilpan ins(&adc); -#endif + #endif AP_IMU_INS imu(&ins); // we don't want to use gps for yaw correction on ArduCopter, so pass // a NULL GPS object pointer static GPS *g_gps_null; -#if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 - AP_AHRS_MPU6000 ahrs(&imu, g_gps, &ins); // only works with APM2 -#else - AP_AHRS_DCM ahrs(&imu, g_gps); -#endif + #if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +AP_AHRS_MPU6000 ahrs(&imu, g_gps, &ins); // only works with APM2 + #else +AP_AHRS_DCM ahrs(&imu, g_gps); + #endif AP_TimerProcess timer_scheduler; #elif HIL_MODE == HIL_MODE_SENSORS - // sensor emulators - AP_ADC_HIL adc; - AP_Baro_BMP085_HIL barometer; - AP_Compass_HIL compass; - AP_GPS_HIL g_gps_driver(NULL); - AP_IMU_Shim imu; - AP_AHRS_DCM ahrs(&imu, g_gps); - AP_PeriodicProcessStub timer_scheduler; - AP_InertialSensor_Stub ins; +// sensor emulators +AP_ADC_HIL adc; +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; +AP_GPS_HIL g_gps_driver(NULL); +AP_IMU_Shim imu; +AP_AHRS_DCM ahrs(&imu, g_gps); +AP_PeriodicProcessStub timer_scheduler; +AP_InertialSensor_Stub ins; - static int32_t gps_base_alt; +static int32_t gps_base_alt; #elif HIL_MODE == HIL_MODE_ATTITUDE - 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; - AP_InertialSensor_Stub ins; - AP_PeriodicProcessStub timer_scheduler; - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); - #endif -#ifdef DESKTOP_BUILD -#include - SITL sitl; -#endif - static int32_t gps_base_alt; +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; +AP_InertialSensor_Stub ins; +AP_PeriodicProcessStub timer_scheduler; + #ifdef OPTFLOW_ENABLED +AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); + #endif + #ifdef DESKTOP_BUILD + #include +SITL sitl; + #endif +static int32_t gps_base_alt; #else - #error Unrecognised HIL_MODE setting. + #error Unrecognised HIL_MODE setting. #endif // HIL MODE @@ -314,8 +314,8 @@ AP_TimerProcess timer_scheduler; //////////////////////////////////////////////////////////////////////////////// // GCS selection //////////////////////////////////////////////////////////////////////////////// -GCS_MAVLINK gcs0; -GCS_MAVLINK gcs3; +GCS_MAVLINK gcs0; +GCS_MAVLINK gcs3; //////////////////////////////////////////////////////////////////////////////// // SONAR selection @@ -323,12 +323,12 @@ GCS_MAVLINK gcs3; // ModeFilterInt16_Size5 sonar_mode_filter(2); #if CONFIG_SONAR == ENABLED - #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC - AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); - #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN - AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN); - #endif - AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter); + #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC +AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); + #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN +AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN); + #endif +AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter); #endif // agmatthews USERHOOKS @@ -336,7 +336,7 @@ ModeFilterInt16_Size5 sonar_mode_filter(2); // User variables //////////////////////////////////////////////////////////////////////////////// #ifdef USERHOOK_VARIABLES -#include USERHOOK_VARIABLES + #include USERHOOK_VARIABLES #endif //////////////////////////////////////////////////////////////////////////////// @@ -344,33 +344,34 @@ ModeFilterInt16_Size5 sonar_mode_filter(2); //////////////////////////////////////////////////////////////////////////////// static const char* flight_mode_strings[] = { - "STABILIZE", // 0 - "ACRO", // 1 - "ALT_HOLD", // 2 - "AUTO", // 3 - "GUIDED", // 4 - "LOITER", // 5 - "RTL", // 6 - "CIRCLE", // 7 - "POSITION", // 8 - "LAND", // 9 - "OF_LOITER", // 10 - "TOY_M", // 11 - "TOY_A"}; // 12 THOR Added for additional Fligt mode + "STABILIZE", // 0 + "ACRO", // 1 + "ALT_HOLD", // 2 + "AUTO", // 3 + "GUIDED", // 4 + "LOITER", // 5 + "RTL", // 6 + "CIRCLE", // 7 + "POSITION", // 8 + "LAND", // 9 + "OF_LOITER", // 10 + "TOY_M", // 11 + "TOY_A" +}; // 12 THOR Added for additional Fligt mode /* Radio values - Channel assignments - 1 Ailerons (rudder if no ailerons) - 2 Elevator - 3 Throttle - 4 Rudder (if we have ailerons) - 5 Mode - 3 position switch - 6 User assignable - 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second) - 8 TBD - 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 Mode - 3 position switch + * 6 User assignable + * 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second) + * 8 TBD + * 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 + */ //Documentation of GLobals: @@ -391,13 +392,13 @@ static int16_t y_rate_error; //////////////////////////////////////////////////////////////////////////////// // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, -static int8_t control_mode = STABILIZE; +static int8_t control_mode = STABILIZE; // This is the state of simple mode. // Set in the control_mode.pde file when the control switch is read -static bool do_simple = false; +static bool do_simple = false; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch -static byte oldSwitchPosition; +static byte oldSwitchPosition; //////////////////////////////////////////////////////////////////////////////// @@ -409,46 +410,46 @@ static byte oldSwitchPosition; // This was added to try and deal with biger motors //static int16_t motor_filtered[11]; -#if FRAME_CONFIG == QUAD_FRAME - #define MOTOR_CLASS AP_MotorsQuad +#if FRAME_CONFIG == QUAD_FRAME + #define MOTOR_CLASS AP_MotorsQuad #endif -#if FRAME_CONFIG == TRI_FRAME - #define MOTOR_CLASS AP_MotorsTri +#if FRAME_CONFIG == TRI_FRAME + #define MOTOR_CLASS AP_MotorsTri #endif -#if FRAME_CONFIG == HEXA_FRAME - #define MOTOR_CLASS AP_MotorsHexa +#if FRAME_CONFIG == HEXA_FRAME + #define MOTOR_CLASS AP_MotorsHexa #endif -#if FRAME_CONFIG == Y6_FRAME - #define MOTOR_CLASS AP_MotorsY6 +#if FRAME_CONFIG == Y6_FRAME + #define MOTOR_CLASS AP_MotorsY6 #endif -#if FRAME_CONFIG == OCTA_FRAME - #define MOTOR_CLASS AP_MotorsOcta +#if FRAME_CONFIG == OCTA_FRAME + #define MOTOR_CLASS AP_MotorsOcta #endif -#if FRAME_CONFIG == OCTA_QUAD_FRAME - #define MOTOR_CLASS AP_MotorsOctaQuad +#if FRAME_CONFIG == OCTA_QUAD_FRAME + #define MOTOR_CLASS AP_MotorsOctaQuad #endif -#if FRAME_CONFIG == HELI_FRAME - #define MOTOR_CLASS AP_MotorsHeli +#if FRAME_CONFIG == HELI_FRAME + #define MOTOR_CLASS AP_MotorsHeli #endif -#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments - #if INSTANT_PWM == 1 - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 - #else - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); - #endif +#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments + #if INSTANT_PWM == 1 +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 + #else +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); + #endif #elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing - #if INSTANT_PWM == 1 - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 - #else - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); - #endif + #if INSTANT_PWM == 1 +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 + #else +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); + #endif #else - #if INSTANT_PWM == 1 - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 - #else - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); - #endif + #if INSTANT_PWM == 1 +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 + #else +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); + #endif #endif //////////////////////////////////////////////////////////////////////////////// @@ -467,7 +468,7 @@ static uint32_t rc_override_fs_timer; //////////////////////////////////////////////////////////////////////////////// // A status flag for the failsafe state // did our throttle dip below the failsafe value? -static boolean failsafe; +static boolean failsafe; //////////////////////////////////////////////////////////////////////////////// // PIDs @@ -492,7 +493,7 @@ static boolean motor_light; static boolean GPS_light; // This is current status for the LED lights state machine // setting this value changes the output of the LEDs -static byte led_mode = NORMAL_LEDS; +static byte led_mode = NORMAL_LEDS; // Blinking indicates GPS status static byte copter_leds_GPS_blink = 0; // Blinking indicates battery status @@ -506,27 +507,27 @@ static int8_t copter_leds_nav_blink = 0; // 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 // We need to scale the longitude up to make these calcs work // to account for decreasing distance between lines of longitude away from the equator -static float scaleLongUp = 1; +static float scaleLongUp = 1; // Sometimes we need to remove the scaling for distance calcs -static float scaleLongDown = 1; +static float scaleLongDown = 1; //////////////////////////////////////////////////////////////////////////////// // Mavlink specific //////////////////////////////////////////////////////////////////////////////// // Used by Mavlink for unknow reasons -static const float radius_of_earth = 6378100; // meters +static const float radius_of_earth = 6378100; // meters // Used by Mavlink for unknow reasons -static const float gravity = 9.81; // meters/ sec^2 +static const float gravity = 9.81; // meters/ sec^2 // Unions for getting byte values -union float_int{ - int32_t int_value; - float float_value; +union float_int { + int32_t int_value; + float float_value; } float_int; @@ -535,32 +536,32 @@ union float_int{ //////////////////////////////////////////////////////////////////////////////// // Status flag indicating we have data that can be used to navigate // Set by a GPS read with 3D fix, or an optical flow read -static bool nav_ok; +static bool nav_ok; // This is the angle from the copter to the "next_WP" location in degrees * 100 -static int32_t target_bearing; +static int32_t target_bearing; // Status of the Waypoint tracking mode. Options include: // NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE -static byte wp_control; +static byte wp_control; // Register containing the index of the current navigation command in the mission script -static int16_t command_nav_index; +static int16_t command_nav_index; // Register containing the index of the previous navigation command in the mission script // Used to manage the execution of conditional commands -static uint8_t prev_nav_index; +static uint8_t prev_nav_index; // Register containing the index of the current conditional command in the mission script -static uint8_t command_cond_index; +static uint8_t command_cond_index; // Used to track the required WP navigation information // options include // NAV_ALTITUDE - have we reached the desired altitude? // NAV_LOCATION - have we reached the desired location? -// NAV_DELAY - have we waited at the waypoint the desired time? -static uint8_t wp_verify_byte; // used for tracking state of navigating waypoints +// NAV_DELAY - have we waited at the waypoint the desired time? +static uint8_t wp_verify_byte; // used for tracking state of navigating waypoints // used to limit the speed ramp up of WP navigation // Acceleration is limited to .5m/s/s static int16_t waypoint_speed_gov; // Used to track how many cm we are from the "next_WP" location -static int32_t long_error, lat_error; +static int32_t long_error, lat_error; // Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s -static boolean loiter_override; +static boolean loiter_override; static int16_t waypoint_radius; static int16_t control_roll; @@ -573,9 +574,9 @@ static int16_t control_pitch; // by the DCM through a few simple equations. They are used throughout the code where cos and sin // would normally be used. // The cos values are defaulted to 1 to get a decent initial value for a level state -static float cos_roll_x = 1; -static float cos_pitch_x = 1; -static float cos_yaw_x = 1; +static float cos_roll_x = 1; +static float cos_pitch_x = 1; +static float cos_yaw_x = 1; static float sin_yaw_y; //////////////////////////////////////////////////////////////////////////////// @@ -593,14 +594,14 @@ int32_t roll_axis; int32_t pitch_axis; // Filters -AP_LeadFilter xLeadFilter; // Long GPS lag filter -AP_LeadFilter yLeadFilter; // Lat GPS lag filter +AP_LeadFilter xLeadFilter; // Long GPS lag filter +AP_LeadFilter yLeadFilter; // Lat GPS lag filter -AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration -AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration +AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration +AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration // Barometer filter -AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration +AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration //////////////////////////////////////////////////////////////////////////////// // Circle Mode / Loiter control @@ -612,17 +613,17 @@ static float circle_angle; // units are in radians, default is 5° per second static const float circle_rate = 0.0872664625; // used to track the delat in Circle Mode -static int32_t old_target_bearing; +static int32_t old_target_bearing; // deg : how many times to circle * 360 for Loiter/Circle Mission command -static int16_t loiter_total; +static int16_t loiter_total; // deg : how far we have turned around a waypoint -static int16_t loiter_sum; +static int16_t loiter_sum; // How long we should stay in Loiter Mode for mission scripting static uint16_t loiter_time_max; // How long have we been loitering - The start time in millis static uint32_t loiter_time; // The synthetic location created to make the copter do circles around a WP -static struct Location circle_WP; +static struct Location circle_WP; //////////////////////////////////////////////////////////////////////////////// @@ -644,39 +645,39 @@ static int8_t CH7_wp_index; // Battery Sensors //////////////////////////////////////////////////////////////////////////////// // Battery Voltage of battery, initialized above threshold for filter -static float battery_voltage1 = LOW_VOLTAGE * 1.05; +static float battery_voltage1 = LOW_VOLTAGE * 1.05; // refers to the instant amp draw – based on an Attopilot Current sensor -static float current_amps1; +static float current_amps1; // refers to the total amps drawn – based on an Attopilot Current sensor -static float current_total1; +static float current_total1; // Used to track if the battery is low - LED output flashes when the batt is low -static bool low_batt = false; +static bool low_batt = false; //////////////////////////////////////////////////////////////////////////////// // Altitude //////////////////////////////////////////////////////////////////////////////// // The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP -static int32_t altitude_error; +static int32_t altitude_error; // The cm/s we are moving up or down based on sensor data - Positive = UP -static int16_t climb_rate_actual; +static int16_t climb_rate_actual; // Used to dither our climb_rate over 50hz -static int16_t climb_rate_error; +static int16_t climb_rate_error; // The cm/s we are moving up or down based on filtered data - Positive = UP -static int16_t climb_rate; +static int16_t climb_rate; // The altitude as reported by Sonar in cm – Values are 20 to 700 generally. -static int16_t sonar_alt; +static int16_t sonar_alt; // The climb_rate as reported by sonar in cm/s -static int16_t sonar_rate; +static int16_t sonar_rate; // The altitude as reported by Baro in cm – Values can be quite high -static int32_t baro_alt; +static int32_t baro_alt; // The climb_rate as reported by Baro in cm/s -static int16_t baro_rate; +static int16_t baro_rate; // used to switch out of Manual Boost -static uint8_t reset_throttle_counter; +static uint8_t reset_throttle_counter; // used to track when to read sensors vs estimate alt -static boolean alt_sensor_flag; -static int16_t saved_toy_throttle; +static boolean alt_sensor_flag; +static int16_t saved_toy_throttle; //////////////////////////////////////////////////////////////////////////////// @@ -686,11 +687,11 @@ static int16_t saved_toy_throttle; // Each Flight mode is a unique combination of these modes // // The current desired control scheme for Yaw -static byte yaw_mode; +static byte yaw_mode; // The current desired control scheme for roll and pitch / navigation -static byte roll_pitch_mode; +static byte roll_pitch_mode; // The current desired control scheme for altitude hold -static byte throttle_mode; +static byte throttle_mode; //////////////////////////////////////////////////////////////////////////////// @@ -699,51 +700,51 @@ static byte throttle_mode; // Flag for monitoring the status of flight // We must be in the air with throttle for 5 seconds before this flag is true // This flag is reset when we are in a manual throttle mode with 0 throttle or disarmed -static boolean takeoff_complete; +static boolean takeoff_complete; // Used to see if we have landed and if we should shut our engines - not fully implemented -static boolean land_complete = true; +static boolean land_complete = true; // An additional throttle added to keep the copter at the same altitude when banking -static int16_t angle_boost; +static int16_t angle_boost; // Push copter down for clean landing -static int16_t landing_boost; +static int16_t landing_boost; // for controlling the landing throttle curve //verifies landings static int16_t ground_detector; // have we reached our desired altitude brefore heading home? -static bool rtl_reached_alt; +static bool rtl_reached_alt; //////////////////////////////////////////////////////////////////////////////// // Navigation general //////////////////////////////////////////////////////////////////////////////// // The location of the copter in relation to home, updated every GPS read -static int32_t home_to_copter_bearing; +static int32_t home_to_copter_bearing; // distance between plane and home in cm -static int32_t home_distance; +static int32_t home_distance; // distance between plane and next_WP in cm // is not static because AP_Camera uses it -int32_t wp_distance; +int32_t wp_distance; //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors //////////////////////////////////////////////////////////////////////////////// // home location is stored when we have a good GPS lock and arm the copter // Can be reset each the copter is re-armed -static struct Location home; +static struct Location home; // Flag for if we have g_gps lock and have set the home location -static boolean home_is_set; +static boolean home_is_set; // Current location of the copter -static struct Location current_loc; +static struct Location current_loc; // lead filtered loc -static struct Location filtered_loc; +static struct Location filtered_loc; // Next WP is the desired location of the copter - the next waypoint or loiter location -static struct Location next_WP; +static struct Location next_WP; // Prev WP is used to get the optimum path from one WP to the next -static struct Location prev_WP; +static struct Location prev_WP; // Holds the current loaded command from the EEPROM for navigation -static struct Location command_nav_queue; +static struct Location command_nav_queue; // Holds the current loaded command from the EEPROM for conditional scripts -static struct Location command_cond_queue; +static struct Location command_cond_queue; // Holds the current loaded command from the EEPROM for guided mode static struct Location guided_WP; @@ -753,11 +754,11 @@ static struct Location guided_WP; //////////////////////////////////////////////////////////////////////////////// // deg * 100, The original angle to the next_WP when the next_WP was set // Also used to check when we pass a WP -static int32_t original_target_bearing; +static int32_t original_target_bearing; // The amount of angle correction applied to target_bearing to bring the copter back on its optimum path -static int16_t crosstrack_error; +static int16_t crosstrack_error; // should we take the waypoint quickly or slow down? -static bool fast_corner; +static bool fast_corner; //////////////////////////////////////////////////////////////////////////////// @@ -765,29 +766,29 @@ static bool fast_corner; //////////////////////////////////////////////////////////////////////////////// // all angles are deg * 100 : target yaw angle // The Commanded ROll from the autopilot. -static int32_t nav_roll; +static int32_t nav_roll; // The Commanded pitch from the autopilot. negative Pitch means go forward. -static int32_t nav_pitch; +static int32_t nav_pitch; // The desired bank towards North (Positive) or South (Negative) -static int32_t auto_roll; -static int32_t auto_pitch; +static int32_t auto_roll; +static int32_t auto_pitch; // Don't be fooled by the fact that Pitch is reversed from Roll in its sign! -static int16_t nav_lat; +static int16_t nav_lat; // The desired bank towards East (Positive) or West (Negative) -static int16_t nav_lon; +static int16_t nav_lon; // The Commanded ROll from the autopilot based on optical flow sensor. -static int32_t of_roll; +static int32_t of_roll; // The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. -static int32_t of_pitch; -static bool slow_wp = false; +static int32_t of_pitch; +static bool slow_wp = false; //////////////////////////////////////////////////////////////////////////////// // Navigation Throttle control //////////////////////////////////////////////////////////////////////////////// // The Commanded Throttle from the autopilot. -static int16_t nav_throttle; // 0-1000 for throttle control +static int16_t nav_throttle; // 0-1000 for throttle control // This is a simple counter to track the amount of throttle used during flight // This could be useful later in determining and debuging current usage and predicting battery life static uint32_t throttle_integrator; @@ -797,37 +798,37 @@ static uint32_t throttle_integrator; //////////////////////////////////////////////////////////////////////////////// // Time when we intiated command in millis - used for controlling decent rate // Used to track the altitude offset for climbrate control -static int8_t alt_change_flag; +static int8_t alt_change_flag; //////////////////////////////////////////////////////////////////////////////// // Navigation Yaw control //////////////////////////////////////////////////////////////////////////////// // The Commanded Yaw from the autopilot. -static int32_t nav_yaw; +static int32_t nav_yaw; // A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot -static int32_t auto_yaw; +static int32_t auto_yaw; // Used to manage the Yaw hold capabilities - static bool yaw_stopped; static uint8_t yaw_timer; // Options include: no tracking, point at next wp, or at a target -static byte yaw_tracking = MAV_ROI_WPNEXT; +static byte yaw_tracking = MAV_ROI_WPNEXT; // In AP Mission scripting we have a fine level of control for Yaw // This is our initial angle for relative Yaw movements -static int32_t command_yaw_start; +static int32_t command_yaw_start; // Timer values used to control the speed of Yaw movements static uint32_t command_yaw_start_time; -static uint16_t command_yaw_time; // how long we are turning -static int32_t command_yaw_end; // what angle are we trying to be +static uint16_t command_yaw_time; // how long we are turning +static int32_t command_yaw_end; // what angle are we trying to be // how many degrees will we turn -static int32_t command_yaw_delta; +static int32_t command_yaw_delta; // Deg/s we should turn -static int16_t command_yaw_speed; +static int16_t command_yaw_speed; // Direction we will turn – 1 = CW, 0 or -1 = CCW -static byte command_yaw_dir; +static byte command_yaw_dir; // Direction we will turn – 1 = relative, 0 = Absolute -static byte command_yaw_relative; +static byte command_yaw_relative; // Yaw will point at this location if yaw_tracking is set to MAV_ROI_LOCATION -static struct Location target_WP; +static struct Location target_WP; @@ -835,22 +836,22 @@ static struct Location target_WP; // Repeat Mission Scripting Command //////////////////////////////////////////////////////////////////////////////// // The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc -static byte event_id; +static byte event_id; // Used to manage the timimng of repeating events static uint32_t event_timer; // How long to delay the next firing of event in millis static uint16_t event_delay; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice -static int16_t event_repeat; +static int16_t event_repeat; // per command value, such as PWM for servos -static int16_t event_value; +static int16_t event_value; // the stored value used to undo commands - such as original PWM command -static int16_t event_undo_value; +static int16_t event_undo_value; //////////////////////////////////////////////////////////////////////////////// // Delay Mission Scripting Command //////////////////////////////////////////////////////////////////////////////// -static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) +static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) static uint32_t condition_start; @@ -859,7 +860,7 @@ static uint32_t condition_start; //////////////////////////////////////////////////////////////////////////////// // Integration time for the gyros (DCM algorithm) // Updated with the fast loop -static float G_Dt = 0.02; +static float G_Dt = 0.02; //////////////////////////////////////////////////////////////////////////////// // Inertial Navigation @@ -886,52 +887,52 @@ static Vector3f speed_error; // Performance monitoring //////////////////////////////////////////////////////////////////////////////// // Used to manage the rate of performance logging messages -static int16_t perf_mon_counter; +static int16_t perf_mon_counter; // The number of GPS fixes we have had -static int16_t gps_fix_count; +static int16_t gps_fix_count; // gps_watchdog checks for bad reads and if we miss 12 in a row, we stop navigating // by lowering nav_lat and navlon to 0 gradually -static byte gps_watchdog; +static byte gps_watchdog; // System Timers // -------------- // Time in microseconds of main control loop -static uint32_t fast_loopTimer; +static uint32_t fast_loopTimer; // Time in microseconds of 50hz control loop -static uint32_t fiftyhz_loopTimer; +static uint32_t fiftyhz_loopTimer; // Counters for branching from 10 hz control loop -static byte medium_loopCounter; +static byte medium_loopCounter; // Counters for branching from 3 1/3hz control loop -static byte slow_loopCounter; +static byte slow_loopCounter; // Counters for branching at 1 hz -static byte counter_one_herz; +static byte counter_one_herz; // used to track the elapsed time between GPS reads -static uint32_t nav_loopTimer; +static uint32_t nav_loopTimer; // Delta Time in milliseconds for navigation computations, updated with every good GPS read -static float dTnav; +static float dTnav; // Counters for branching from 4 minute control loop used to save Compass offsets -static int16_t superslow_loopCounter; +static int16_t superslow_loopCounter; // Loiter timer - Records how long we have been in loiter -static uint32_t loiter_timer; +static uint32_t loiter_timer; // disarms the copter while in Acro or Stabilize mode after 30 seconds of no flight -static uint8_t auto_disarming_counter; +static uint8_t auto_disarming_counter; // prevents duplicate GPS messages from entering system -static uint32_t last_gps_time; +static uint32_t last_gps_time; // 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; // Set true if we have new PWM data to act on from the Radio -static bool new_radio_frame; +static bool new_radio_frame; // Used to auto exit the in-flight leveler -static int16_t auto_level_counter; +static int16_t auto_level_counter; // Reference to the AP relay object - APM1 only AP_Relay relay; // APM2 only #if USB_MUX_PIN > 0 - static bool usb_connected; +static bool usb_connected; #endif @@ -958,13 +959,13 @@ AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs, 1); //////////////////////////////////////////////////////////////////////////////// #ifdef AP_LIMITS - AP_Limits limits; +AP_Limits limits; - AP_Limit_GPSLock gpslock_limit(g_gps); +AP_Limit_GPSLock gpslock_limit(g_gps); - AP_Limit_Geofence geofence_limit(FENCE_START_BYTE, FENCE_WP_SIZE, MAX_FENCEPOINTS, g_gps, &home, ¤t_loc); +AP_Limit_Geofence geofence_limit(FENCE_START_BYTE, FENCE_WP_SIZE, MAX_FENCEPOINTS, g_gps, &home, ¤t_loc); - AP_Limit_Altitude altitude_limit(¤t_loc); +AP_Limit_Altitude altitude_limit(¤t_loc); #endif @@ -973,79 +974,79 @@ AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs, 1); //////////////////////////////////////////////////////////////////////////////// void setup() { - memcheck_init(); - init_ardupilot(); + memcheck_init(); + init_ardupilot(); } void loop() { - uint32_t timer = micros(); + uint32_t timer = micros(); - // We want this to execute fast - // ---------------------------- - if ((timer - fast_loopTimer) >= 10000 && imu.new_data_available()) { - //Log_Write_Data(13, (int32_t)(timer - fast_loopTimer)); + // We want this to execute fast + // ---------------------------- + if ((timer - fast_loopTimer) >= 10000 && imu.new_data_available()) { + //Log_Write_Data(13, (int32_t)(timer - fast_loopTimer)); - //PORTK |= B00010000; - G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops - fast_loopTimer = timer; + //PORTK |= B00010000; + G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops + fast_loopTimer = timer; - // Execute the fast loop - // --------------------- - fast_loop(); - } else { - #ifdef DESKTOP_BUILD - usleep(1000); - #endif - } + // Execute the fast loop + // --------------------- + fast_loop(); + } else { +#ifdef DESKTOP_BUILD + usleep(1000); +#endif + } - // port manipulation for external timing of main loops - //PORTK &= B11101111; + // port manipulation for external timing of main loops + //PORTK &= B11101111; - if ((timer - fiftyhz_loopTimer) >= 20000) { - // store the micros for the 50 hz timer - fiftyhz_loopTimer = timer; + if ((timer - fiftyhz_loopTimer) >= 20000) { + // store the micros for the 50 hz timer + fiftyhz_loopTimer = timer; - // port manipulation for external timing of main loops - //PORTK |= B01000000; + // port manipulation for external timing of main loops + //PORTK |= B01000000; - // reads all of the necessary trig functions for cameras, throttle, etc. - // -------------------------------------------------------------------- - update_trig(); + // reads all of the necessary trig functions for cameras, throttle, etc. + // -------------------------------------------------------------------- + update_trig(); - // Rotate the Nav_lon and nav_lat vectors based on Yaw - // --------------------------------------------------- - calc_loiter_pitch_roll(); + // Rotate the Nav_lon and nav_lat vectors based on Yaw + // --------------------------------------------------- + calc_loiter_pitch_roll(); - // check for new GPS messages - // -------------------------- - update_GPS(); + // check for new GPS messages + // -------------------------- + update_GPS(); - // perform 10hz tasks - // ------------------ - medium_loop(); + // perform 10hz tasks + // ------------------ + medium_loop(); - // Stuff to run at full 50hz, but after the med loops - // -------------------------------------------------- - fifty_hz_loop(); + // Stuff to run at full 50hz, but after the med loops + // -------------------------------------------------- + fifty_hz_loop(); - counter_one_herz++; + counter_one_herz++; - // trgger our 1 hz loop - if(counter_one_herz >= 50){ - super_slow_loop(); - counter_one_herz = 0; - } - perf_mon_counter++; - if (perf_mon_counter > 600 ) { - if (g.log_bitmask & MASK_LOG_PM) - Log_Write_Performance(); - - gps_fix_count = 0; - perf_mon_counter = 0; + // trgger our 1 hz loop + if(counter_one_herz >= 50) { + super_slow_loop(); + counter_one_herz = 0; } - //PORTK &= B10111111; - } + perf_mon_counter++; + if (perf_mon_counter > 600 ) { + if (g.log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); + + gps_fix_count = 0; + perf_mon_counter = 0; + } + //PORTK &= B10111111; + } } // PORTK |= B01000000; // PORTK &= B10111111; @@ -1057,235 +1058,235 @@ static void fast_loop() // some space available gcs_send_message(MSG_RETRY_DEFERRED); - // Read radio - // ---------- - read_radio(); + // Read radio + // ---------- + read_radio(); - // IMU DCM Algorithm - // -------------------- - read_AHRS(); + // IMU DCM Algorithm + // -------------------- + read_AHRS(); - // Inertial Nav - // -------------------- - #if INERTIAL_NAV == ENABLED - calc_inertia(); - #endif + // Inertial Nav + // -------------------- +#if INERTIAL_NAV == ENABLED + calc_inertia(); +#endif - // custom code/exceptions for flight modes - // --------------------------------------- - update_yaw_mode(); - update_roll_pitch_mode(); + // custom code/exceptions for flight modes + // --------------------------------------- + update_yaw_mode(); + update_roll_pitch_mode(); - // write out the servo PWM values - // ------------------------------ - set_servos_4(); + // write out the servo PWM values + // ------------------------------ + set_servos_4(); - // agmatthews - USERHOOKS - #ifdef USERHOOK_FASTLOOP - USERHOOK_FASTLOOP - #endif + // agmatthews - USERHOOKS +#ifdef USERHOOK_FASTLOOP + USERHOOK_FASTLOOP +#endif } static void medium_loop() { - // 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 and Compass - //----------------------------------------- - case 0: - medium_loopCounter++; + // This case deals with the GPS and Compass + //----------------------------------------- + case 0: + medium_loopCounter++; - #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode - if(g.compass_enabled){ - if (compass.read()) { - compass.null_offsets(); - } - } - #endif - - // auto_trim, uses an auto_level algorithm - auto_trim(); - - // record throttle output - // ------------------------------ - throttle_integrator += g.rc_3.servo_out; - break; - - // This case performs some navigation computations - //------------------------------------------------ - case 1: - medium_loopCounter++; - - // calculate the copter's desired bearing and WP distance - // ------------------------------------------------------ - if(nav_ok){ - // clear nav flag - nav_ok = false; - - // calculate distance, angles to target - navigate(); - - // update flight control system - update_navigation(); - - // update log - if (g.log_bitmask & MASK_LOG_NTUN && motors.armed()){ - Log_Write_Nav_Tuning(); - } - } - break; - - // command processing - //------------------- - case 2: - medium_loopCounter++; - - if(control_mode == TOY_A){ - update_toy_throttle(); - - if(throttle_mode == THROTTLE_AUTO){ - update_toy_altitude(); - } - } - - alt_sensor_flag = true; - break; - - // This case deals with sending high rate telemetry - //------------------------------------------------- - case 3: - medium_loopCounter++; - - // perform next command - // -------------------- - if(control_mode == AUTO){ - if(home_is_set == true && g.command_total > 1){ - update_commands(); - } - } - - if(motors.armed()){ - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) - Log_Write_Attitude(); - - if (g.log_bitmask & MASK_LOG_MOTORS) - Log_Write_Motors(); +#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode + if(g.compass_enabled) { + if (compass.read()) { + compass.null_offsets(); } - break; + } +#endif - // This case controls the slow loop - //--------------------------------- - case 4: - medium_loopCounter = 0; + // auto_trim, uses an auto_level algorithm + auto_trim(); - if (g.battery_monitoring != 0){ - read_battery(); - } + // record throttle output + // ------------------------------ + throttle_integrator += g.rc_3.servo_out; + break; - // Accel trims = hold > 2 seconds - // Throttle cruise = switch less than 1 second - // -------------------------------------------- - read_trim_switch(); + // This case performs some navigation computations + //------------------------------------------------ + case 1: + medium_loopCounter++; - // Check for engine arming - // ----------------------- - arm_motors(); + // calculate the copter's desired bearing and WP distance + // ------------------------------------------------------ + if(nav_ok) { + // clear nav flag + nav_ok = false; - // Do an extra baro read for Temp sensing - // --------------------------------------- - #if HIL_MODE != HIL_MODE_ATTITUDE - barometer.read(); - #endif + // calculate distance, angles to target + navigate(); - // agmatthews - USERHOOKS - #ifdef USERHOOK_MEDIUMLOOP - USERHOOK_MEDIUMLOOP - #endif + // update flight control system + update_navigation(); - #if COPTER_LEDS == ENABLED - update_copter_leds(); - #endif + // update log + if (g.log_bitmask & MASK_LOG_NTUN && motors.armed()) { + Log_Write_Nav_Tuning(); + } + } + break; - slow_loop(); - break; + // command processing + //------------------- + case 2: + medium_loopCounter++; - default: - // this is just a catch all - // ------------------------ - medium_loopCounter = 0; - break; - } + if(control_mode == TOY_A) { + update_toy_throttle(); + + if(throttle_mode == THROTTLE_AUTO) { + update_toy_altitude(); + } + } + + alt_sensor_flag = true; + break; + + // This case deals with sending high rate telemetry + //------------------------------------------------- + case 3: + medium_loopCounter++; + + // perform next command + // -------------------- + if(control_mode == AUTO) { + if(home_is_set == true && g.command_total > 1) { + update_commands(); + } + } + + if(motors.armed()) { + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) + Log_Write_Attitude(); + + if (g.log_bitmask & MASK_LOG_MOTORS) + Log_Write_Motors(); + } + break; + + // This case controls the slow loop + //--------------------------------- + case 4: + medium_loopCounter = 0; + + if (g.battery_monitoring != 0) { + read_battery(); + } + + // Accel trims = hold > 2 seconds + // Throttle cruise = switch less than 1 second + // -------------------------------------------- + read_trim_switch(); + + // Check for engine arming + // ----------------------- + arm_motors(); + + // Do an extra baro read for Temp sensing + // --------------------------------------- +#if HIL_MODE != HIL_MODE_ATTITUDE + barometer.read(); +#endif + + // agmatthews - USERHOOKS +#ifdef USERHOOK_MEDIUMLOOP + USERHOOK_MEDIUMLOOP +#endif + +#if COPTER_LEDS == ENABLED + update_copter_leds(); +#endif + + slow_loop(); + break; + + default: + // this is just a catch all + // ------------------------ + medium_loopCounter = 0; + break; + } } // stuff that happens at 50 hz // --------------------------- static void fifty_hz_loop() { - // read altitude sensors or estimate altitude - // ------------------------------------------ - update_altitude_est(); + // read altitude sensors or estimate altitude + // ------------------------------------------ + update_altitude_est(); - // Update the throttle ouput - // ------------------------- - update_throttle_mode(); + // Update the throttle ouput + // ------------------------- + update_throttle_mode(); - // Read Sonar - // ---------- - # if CONFIG_SONAR == ENABLED - if(g.sonar_enabled){ - sonar_alt = sonar.read(); - } - #endif + // Read Sonar + // ---------- +# if CONFIG_SONAR == ENABLED + if(g.sonar_enabled) { + sonar_alt = sonar.read(); + } +#endif - #if TOY_EDF == ENABLED - edf_toy(); - #endif +#if TOY_EDF == ENABLED + edf_toy(); +#endif - // syncronise optical flow reads with altitude reads - #ifdef OPTFLOW_ENABLED - if(g.optflow_enabled){ - update_optical_flow(); - } - #endif + // syncronise optical flow reads with altitude reads +#ifdef OPTFLOW_ENABLED + if(g.optflow_enabled) { + update_optical_flow(); + } +#endif - #ifdef USERHOOK_50HZLOOP - USERHOOK_50HZLOOP - #endif +#ifdef USERHOOK_50HZLOOP + USERHOOK_50HZLOOP +#endif - #if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME - // HIL for a copter needs very fast update of the servo values - gcs_send_message(MSG_RADIO_OUT); - #endif +#if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME + // HIL for a copter needs very fast update of the servo values + gcs_send_message(MSG_RADIO_OUT); +#endif #if MOUNT == ENABLED - // update camera mount's position - camera_mount.update_mount_position(); + // update camera mount's position + camera_mount.update_mount_position(); #endif #if MOUNT2 == ENABLED - // update camera mount's position - camera_mount2.update_mount_position(); + // update camera mount's position + camera_mount2.update_mount_position(); #endif #if CAMERA == ENABLED - g.camera.trigger_pic_cleanup(); + g.camera.trigger_pic_cleanup(); #endif - # if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) - Log_Write_Attitude(); +# if HIL_MODE == HIL_MODE_DISABLED + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) + Log_Write_Attitude(); - if (g.log_bitmask & MASK_LOG_RAW && motors.armed()) - Log_Write_Raw(); - #endif + if (g.log_bitmask & MASK_LOG_RAW && motors.armed()) + Log_Write_Raw(); +#endif - // kick the GCS to process uplink data - gcs_update(); + // kick the GCS to process uplink data + gcs_update(); gcs_data_stream_send(); } @@ -1295,129 +1296,129 @@ static void slow_loop() #if AP_LIMITS == ENABLED - // Run the AP_Limits main loop - limits_loop(); + // Run the AP_Limits main loop + limits_loop(); #endif // AP_LIMITS_ENABLED - // This is the slow (3 1/3 Hz) loop pieces - //---------------------------------------- - switch (slow_loopCounter){ - case 0: - slow_loopCounter++; - superslow_loopCounter++; + // This is the slow (3 1/3 Hz) loop pieces + //---------------------------------------- + switch (slow_loopCounter) { + case 0: + slow_loopCounter++; + superslow_loopCounter++; - if(superslow_loopCounter > 1200){ - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled){ - compass.save_offsets(); - superslow_loopCounter = 0; - } - #endif + if(superslow_loopCounter > 1200) { +#if HIL_MODE != HIL_MODE_ATTITUDE + if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled) { + compass.save_offsets(); + superslow_loopCounter = 0; } +#endif + } - // check the user hasn't updated the frame orientation - if( !motors.armed() ) { - motors.set_frame_orientation(g.frame_orientation); - } + // check the user hasn't updated the frame orientation + if( !motors.armed() ) { + motors.set_frame_orientation(g.frame_orientation); + } - break; + break; - case 1: - slow_loopCounter++; + case 1: + slow_loopCounter++; - // Read 3-position switch on radio - // ------------------------------- - read_control_switch(); - - #if MOUNT == ENABLED - update_aux_servo_function(&g.rc_9, &g.rc_10, &g.rc_11); - #endif - enable_aux_servos(); + // Read 3-position switch on radio + // ------------------------------- + read_control_switch(); #if MOUNT == ENABLED - camera_mount.update_mount_type(); + update_aux_servo_function(&g.rc_9, &g.rc_10, &g.rc_11); +#endif + enable_aux_servos(); + +#if MOUNT == ENABLED + camera_mount.update_mount_type(); #endif #if MOUNT2 == ENABLED - camera_mount2.update_mount_type(); + camera_mount2.update_mount_type(); #endif - // agmatthews - USERHOOKS - #ifdef USERHOOK_SLOWLOOP - USERHOOK_SLOWLOOP - #endif + // agmatthews - USERHOOKS +#ifdef USERHOOK_SLOWLOOP + USERHOOK_SLOWLOOP +#endif - break; + break; - case 2: - slow_loopCounter = 0; - update_events(); + case 2: + slow_loopCounter = 0; + update_events(); - // blink if we are armed - update_lights(); + // blink if we are armed + update_lights(); - if(g.radio_tuning > 0) - tuning(); + if(g.radio_tuning > 0) + tuning(); - #if USB_MUX_PIN > 0 - check_usb_mux(); - #endif - break; +#if USB_MUX_PIN > 0 + check_usb_mux(); +#endif + break; - default: - slow_loopCounter = 0; - break; - } + default: + slow_loopCounter = 0; + break; + } } #define AUTO_DISARMING_DELAY 25 // 1Hz loop static void super_slow_loop() { - if (g.log_bitmask & MASK_LOG_CUR && motors.armed()) - Log_Write_Current(); + if (g.log_bitmask & MASK_LOG_CUR && motors.armed()) + Log_Write_Current(); - #if 0 //CENTER_THROTTLE == 1 - // recalibrate the throttle_cruise to center on the sticks - g.rc_3.set_range((g.throttle_cruise - (MAXIMUM_THROTTLE - g.throttle_cruise)), MAXIMUM_THROTTLE); - g.rc_3.set_range_out(0,1000); - #endif +#if 0 //CENTER_THROTTLE == 1 + // recalibrate the throttle_cruise to center on the sticks + g.rc_3.set_range((g.throttle_cruise - (MAXIMUM_THROTTLE - g.throttle_cruise)), MAXIMUM_THROTTLE); + g.rc_3.set_range_out(0,1000); +#endif - // this function disarms the copter if it has been sitting on the ground for any moment of time greater than 25 seconds - // but only of the control mode is manual - if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){ - auto_disarming_counter++; + // this function disarms the copter if it has been sitting on the ground for any moment of time greater than 25 seconds + // but only of the control mode is manual + if((control_mode <= ACRO) && (g.rc_3.control_in == 0)) { + auto_disarming_counter++; - if(auto_disarming_counter == AUTO_DISARMING_DELAY){ - init_disarm_motors(); - }else if (auto_disarming_counter > AUTO_DISARMING_DELAY){ - auto_disarming_counter = AUTO_DISARMING_DELAY + 1; - } - }else{ - auto_disarming_counter = 0; - } + if(auto_disarming_counter == AUTO_DISARMING_DELAY) { + init_disarm_motors(); + }else if (auto_disarming_counter > AUTO_DISARMING_DELAY) { + auto_disarming_counter = AUTO_DISARMING_DELAY + 1; + } + }else{ + auto_disarming_counter = 0; + } gcs_send_message(MSG_HEARTBEAT); - // agmatthews - USERHOOKS - #ifdef USERHOOK_SUPERSLOWLOOP - USERHOOK_SUPERSLOWLOOP - #endif + // agmatthews - USERHOOKS +#ifdef USERHOOK_SUPERSLOWLOOP + USERHOOK_SUPERSLOWLOOP +#endif - /* - //Serial.printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n", - current_loc.alt, - next_WP.alt, - altitude_error, - g.throttle_cruise.get(), - g.pi_alt_hold.get_integrator(), - wp_distance, - target_bearing, - home_distance, - home_to_copter_bearing); - */ + /* + * //Serial.printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n", + * current_loc.alt, + * next_WP.alt, + * altitude_error, + * g.throttle_cruise.get(), + * g.pi_alt_hold.get_integrator(), + * wp_distance, + * target_bearing, + * home_distance, + * home_to_copter_bearing); + */ } // updated at 10 Hz @@ -1426,337 +1427,337 @@ static void update_optical_flow(void) { static int log_counter = 0; - optflow.update(); - optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + optflow.update(); + optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow - // write to log - log_counter++; - if( log_counter >= 5 ) { - log_counter = 0; - if (g.log_bitmask & MASK_LOG_OPTFLOW){ - Log_Write_Optflow(); - } - } + // write to log + log_counter++; + if( log_counter >= 5 ) { + log_counter = 0; + if (g.log_bitmask & MASK_LOG_OPTFLOW) { + Log_Write_Optflow(); + } + } - /*if(g.optflow_enabled && current_loc.alt < 500){ - if(GPS_enabled){ - // if we have a GPS, we add some detail to the GPS - // XXX this may not ne right - current_loc.lng += optflow.vlon; - current_loc.lat += optflow.vlat; - - // some sort of error correction routine - //current_loc.lng -= ERR_GAIN * (float)(current_loc.lng - x_GPS_speed); // error correction - //current_loc.lng -= ERR_GAIN * (float)(current_loc.lng - x_GPS_speed); // error correction - }else{ - // if we do not have a GPS, use relative from 0 for lat and lon - current_loc.lng = optflow.vlon; - current_loc.lat = optflow.vlat; - } - // OK to run the nav routines - nav_ok = true; - }*/ + /*if(g.optflow_enabled && current_loc.alt < 500){ + * if(GPS_enabled){ + * // if we have a GPS, we add some detail to the GPS + * // XXX this may not ne right + * current_loc.lng += optflow.vlon; + * current_loc.lat += optflow.vlat; + * + * // some sort of error correction routine + * //current_loc.lng -= ERR_GAIN * (float)(current_loc.lng - x_GPS_speed); // error correction + * //current_loc.lng -= ERR_GAIN * (float)(current_loc.lng - x_GPS_speed); // error correction + * }else{ + * // if we do not have a GPS, use relative from 0 for lat and lon + * current_loc.lng = optflow.vlon; + * current_loc.lat = optflow.vlat; + * } + * // OK to run the nav routines + * nav_ok = true; + * }*/ } #endif // called at 50hz static void update_GPS(void) { - // A counter that is used to grab at least 10 reads before commiting the Home location - static byte ground_start_count = 10; + // A counter that is used to grab at least 10 reads before commiting the Home location + static byte ground_start_count = 10; - // return immediately if GPS is not enabled - if( !GPS_enabled ) { - return; - } + // return immediately if GPS is not enabled + if( !GPS_enabled ) { + return; + } - g_gps->update(); - update_GPS_light(); + g_gps->update(); + update_GPS_light(); - if(gps_watchdog < 30){ - gps_watchdog++; - }else{ - // after 12 reads we guess we may have lost GPS signal, stop navigating - // we have lost GPS signal for a moment. Reduce our error to avoid flyaways - auto_roll >>= 1; - auto_pitch >>= 1; - } + if(gps_watchdog < 30) { + gps_watchdog++; + }else{ + // after 12 reads we guess we may have lost GPS signal, stop navigating + // we have lost GPS signal for a moment. Reduce our error to avoid flyaways + auto_roll >>= 1; + auto_pitch >>= 1; + } if (g_gps->new_data && g_gps->fix) { - // clear new data flag - g_gps->new_data = false; + // clear new data flag + g_gps->new_data = false; - // check for duiplicate GPS messages - if(last_gps_time != g_gps->time){ + // check for duiplicate GPS messages + if(last_gps_time != g_gps->time) { - // look for broken GPS - // --------------- - gps_watchdog = 0; + // look for broken GPS + // --------------- + gps_watchdog = 0; - // OK to run the nav routines - // --------------- - nav_ok = true; + // OK to run the nav routines + // --------------- + nav_ok = true; - // for performance monitoring - // -------------------------- - gps_fix_count++; + // for performance monitoring + // -------------------------- + gps_fix_count++; - // used to calculate speed in X and Y, iterms - // ------------------------------------------ - dTnav = (float)(millis() - nav_loopTimer)/ 1000.0; - nav_loopTimer = millis(); + // used to calculate speed in X and Y, iterms + // ------------------------------------------ + dTnav = (float)(millis() - nav_loopTimer)/ 1000.0; + nav_loopTimer = millis(); - // prevent runup from bad GPS - // -------------------------- - dTnav = min(dTnav, 1.0); + // prevent runup from bad GPS + // -------------------------- + dTnav = min(dTnav, 1.0); - if(ground_start_count > 1){ - ground_start_count--; + if(ground_start_count > 1) { + ground_start_count--; - } else if (ground_start_count == 1) { + } 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; + // 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 (g.compass_enabled) { - // Set compass declination automatically - compass.set_initial_location(g_gps->latitude, g_gps->longitude); - } - // save home to eeprom (we must have a good fix to have reached this point) - init_home(); - ground_start_count = 0; - } - } + }else{ + if (g.compass_enabled) { + // Set compass declination automatically + compass.set_initial_location(g_gps->latitude, g_gps->longitude); + } + // save home to eeprom (we must have a good fix to have reached this point) + init_home(); + ground_start_count = 0; + } + } - current_loc.lng = g_gps->longitude; // Lon * 10^7 - current_loc.lat = g_gps->latitude; // Lat * 10^7 + current_loc.lng = g_gps->longitude; // Lon * 10^7 + current_loc.lat = g_gps->latitude; // Lat * 10^7 - calc_XY_velocity(); + calc_XY_velocity(); - if (g.log_bitmask & MASK_LOG_GPS && motors.armed()){ - Log_Write_GPS(); - } + if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) { + Log_Write_GPS(); + } - #if HIL_MODE == HIL_MODE_ATTITUDE // only execute in HIL mode - //update_altitude(); - alt_sensor_flag = true; - #endif - } +#if HIL_MODE == HIL_MODE_ATTITUDE // only execute in HIL mode + //update_altitude(); + alt_sensor_flag = true; +#endif + } - // save GPS time so we don't get duplicate reads - last_gps_time = g_gps->time; - } + // save GPS time so we don't get duplicate reads + last_gps_time = g_gps->time; + } } void update_yaw_mode(void) { - switch(yaw_mode){ - case YAW_ACRO: - g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); - return; - break; + switch(yaw_mode) { + case YAW_ACRO: + g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); + return; + break; - // update to allow external roll/yaw mixing - #if TOY_LOOKUP == TOY_EXTERNAL_MIXER - case YAW_TOY: - #endif + // update to allow external roll/yaw mixing +#if TOY_LOOKUP == TOY_EXTERNAL_MIXER + case YAW_TOY: +#endif - case YAW_HOLD: - if(g.rc_4.control_in != 0){ - g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); - yaw_stopped = false; - yaw_timer = 150; + case YAW_HOLD: + if(g.rc_4.control_in != 0) { + g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); + yaw_stopped = false; + yaw_timer = 150; - }else if (!yaw_stopped){ - g.rc_4.servo_out = get_acro_yaw(0); - yaw_timer--; + }else if (!yaw_stopped) { + g.rc_4.servo_out = get_acro_yaw(0); + yaw_timer--; - if((yaw_timer == 0) || (fabs(omega.z) < .17)){ - yaw_stopped = true; - nav_yaw = ahrs.yaw_sensor; - } - }else{ - // reset target yaw to current yaw if the motors are disarmed or throttle is zero - // Note: we do not want to reset yaw if failsafe has been triggered even though throttle maybe zero (in fact, normally throttle is zero in failsafe) - if(motors.armed() == false || ((g.rc_3.control_in == 0) && (control_mode <= ACRO) && !failsafe)) - nav_yaw = ahrs.yaw_sensor; + if((yaw_timer == 0) || (fabs(omega.z) < .17)) { + yaw_stopped = true; + nav_yaw = ahrs.yaw_sensor; + } + }else{ + // reset target yaw to current yaw if the motors are disarmed or throttle is zero + // Note: we do not want to reset yaw if failsafe has been triggered even though throttle maybe zero (in fact, normally throttle is zero in failsafe) + if(motors.armed() == false || ((g.rc_3.control_in == 0) && (control_mode <= ACRO) && !failsafe)) + nav_yaw = ahrs.yaw_sensor; - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); - } - return; - break; + g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + } + return; + break; - case YAW_LOOK_AT_HOME: - //nav_yaw updated in update_navigation() - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); - break; + case YAW_LOOK_AT_HOME: + //nav_yaw updated in update_navigation() + g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + break; - case YAW_AUTO: - nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second - //Serial.printf("nav_yaw %d ", nav_yaw); - nav_yaw = wrap_360(nav_yaw); - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); - break; - } + case YAW_AUTO: + nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second + //Serial.printf("nav_yaw %d ", nav_yaw); + nav_yaw = wrap_360(nav_yaw); + g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + break; + } } void update_roll_pitch_mode(void) { - if (do_flip){ - if(abs(g.rc_1.control_in) < 4000){ - roll_flip(); - return; - }else{ - // force an exit from the loop if we are not hands off sticks. - do_flip = false; - } - } + if (do_flip) { + if(abs(g.rc_1.control_in) < 4000) { + roll_flip(); + return; + }else{ + // force an exit from the loop if we are not hands off sticks. + do_flip = false; + } + } - switch(roll_pitch_mode){ - case ROLL_PITCH_ACRO: - if(g.axis_enabled){ - roll_axis += (float)g.rc_1.control_in * g.axis_lock_p; - pitch_axis += (float)g.rc_2.control_in * g.axis_lock_p; + switch(roll_pitch_mode) { + case ROLL_PITCH_ACRO: + if(g.axis_enabled) { + roll_axis += (float)g.rc_1.control_in * g.axis_lock_p; + pitch_axis += (float)g.rc_2.control_in * g.axis_lock_p; - roll_axis = wrap_360(roll_axis); - pitch_axis = wrap_360(pitch_axis); + roll_axis = wrap_360(roll_axis); + pitch_axis = wrap_360(pitch_axis); - // in this mode, nav_roll and nav_pitch = the iterm - g.rc_1.servo_out = get_stabilize_roll(roll_axis); - g.rc_2.servo_out = get_stabilize_pitch(pitch_axis); + // in this mode, nav_roll and nav_pitch = the iterm + g.rc_1.servo_out = get_stabilize_roll(roll_axis); + g.rc_2.servo_out = get_stabilize_pitch(pitch_axis); - if (g.rc_3.control_in == 0){ - roll_axis = 0; - pitch_axis = 0; - } + if (g.rc_3.control_in == 0) { + roll_axis = 0; + pitch_axis = 0; + } - }else{ - // ACRO does not get SIMPLE mode ability - #if FRAME_CONFIG == HELI_FRAME - if (motors.flybar_mode == 1){ - g.rc_1.servo_out = g.rc_1.control_in; - g.rc_2.servo_out = g.rc_2.control_in; - } else { - g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); - } - #else - g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); - #endif - } - break; + }else{ + // ACRO does not get SIMPLE mode ability +#if FRAME_CONFIG == HELI_FRAME + if (motors.flybar_mode == 1) { + g.rc_1.servo_out = g.rc_1.control_in; + g.rc_2.servo_out = g.rc_2.control_in; + } else { + g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); + } +#else + g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); +#endif + } + break; - case ROLL_PITCH_STABLE: - // apply SIMPLE mode transform - if(do_simple && new_radio_frame){ - update_simple_mode(); - } + case ROLL_PITCH_STABLE: + // apply SIMPLE mode transform + if(do_simple && new_radio_frame) { + update_simple_mode(); + } - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; + control_roll = g.rc_1.control_in; + control_pitch = g.rc_2.control_in; - // in this mode, nav_roll and nav_pitch = the iterm - g.rc_1.servo_out = get_stabilize_roll(control_roll); - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); - break; + // in this mode, nav_roll and nav_pitch = the iterm + g.rc_1.servo_out = get_stabilize_roll(control_roll); + g.rc_2.servo_out = get_stabilize_pitch(control_pitch); + break; - case ROLL_PITCH_AUTO: - // apply SIMPLE mode transform - if(do_simple && new_radio_frame){ - update_simple_mode(); - } - // mix in user control with Nav control - nav_roll += constrain(wrap_180(auto_roll - nav_roll), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second - nav_pitch += constrain(wrap_180(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second + case ROLL_PITCH_AUTO: + // apply SIMPLE mode transform + if(do_simple && new_radio_frame) { + update_simple_mode(); + } + // mix in user control with Nav control + nav_roll += constrain(wrap_180(auto_roll - nav_roll), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second + nav_pitch += constrain(wrap_180(auto_pitch - nav_pitch), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second - control_roll = g.rc_1.control_mix(nav_roll); - control_pitch = g.rc_2.control_mix(nav_pitch); + control_roll = g.rc_1.control_mix(nav_roll); + control_pitch = g.rc_2.control_mix(nav_pitch); - g.rc_1.servo_out = get_stabilize_roll(control_roll); - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); - break; + g.rc_1.servo_out = get_stabilize_roll(control_roll); + g.rc_2.servo_out = get_stabilize_pitch(control_pitch); + break; - case ROLL_PITCH_STABLE_OF: - // apply SIMPLE mode transform - if(do_simple && new_radio_frame){ - update_simple_mode(); - } + case ROLL_PITCH_STABLE_OF: + // apply SIMPLE mode transform + if(do_simple && new_radio_frame) { + update_simple_mode(); + } - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; + control_roll = g.rc_1.control_in; + control_pitch = g.rc_2.control_in; - // mix in user control with optical flow - g.rc_1.servo_out = get_stabilize_roll(get_of_roll(control_roll)); - g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(control_pitch)); - break; + // mix in user control with optical flow + g.rc_1.servo_out = get_stabilize_roll(get_of_roll(control_roll)); + g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(control_pitch)); + break; - // THOR - // a call out to the main toy logic - case ROLL_PITCH_TOY: - roll_pitch_toy(); - break; - } + // THOR + // a call out to the main toy logic + case ROLL_PITCH_TOY: + roll_pitch_toy(); + break; + } - if(g.rc_3.control_in == 0 && control_mode <= ACRO){ - reset_rate_I(); - reset_stability_I(); - } + if(g.rc_3.control_in == 0 && control_mode <= ACRO) { + reset_rate_I(); + reset_stability_I(); + } - //if(takeoff_complete == false){ - // reset these I terms to prevent awkward tipping on takeoff - //reset_rate_I(); - //reset_stability_I(); - //} + //if(takeoff_complete == false){ + // reset these I terms to prevent awkward tipping on takeoff + //reset_rate_I(); + //reset_stability_I(); + //} - if(new_radio_frame){ - // clear new radio frame info - new_radio_frame = false; + if(new_radio_frame) { + // clear new radio frame info + new_radio_frame = false; - // These values can be used to scale the PID gains - // This allows for a simple gain scheduling implementation - roll_scale_d = g.stabilize_d_schedule * (float)abs(g.rc_1.control_in); - roll_scale_d = (1 - (roll_scale_d / 4500.0)); - roll_scale_d = constrain(roll_scale_d, 0, 1) * g.stabilize_d; + // These values can be used to scale the PID gains + // This allows for a simple gain scheduling implementation + roll_scale_d = g.stabilize_d_schedule * (float)abs(g.rc_1.control_in); + roll_scale_d = (1 - (roll_scale_d / 4500.0)); + roll_scale_d = constrain(roll_scale_d, 0, 1) * g.stabilize_d; - pitch_scale_d = g.stabilize_d_schedule * (float)abs(g.rc_2.control_in); - pitch_scale_d = (1 - (pitch_scale_d / 4500.0)); - pitch_scale_d = constrain(pitch_scale_d, 0, 1) * g.stabilize_d; - } + pitch_scale_d = g.stabilize_d_schedule * (float)abs(g.rc_2.control_in); + pitch_scale_d = (1 - (pitch_scale_d / 4500.0)); + pitch_scale_d = constrain(pitch_scale_d, 0, 1) * g.stabilize_d; + } } // new radio frame is used to make sure we only call this at 50hz void update_simple_mode(void) { - static byte simple_counter = 0; // State machine counter for Simple Mode - static float simple_sin_y=0, simple_cos_x=0; + static byte simple_counter = 0; // State machine counter for Simple Mode + static float simple_sin_y=0, simple_cos_x=0; - // used to manage state machine - // which improves speed of function - simple_counter++; + // used to manage state machine + // which improves speed of function + simple_counter++; - int16_t delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100; + int16_t delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100; - if (simple_counter == 1){ - // roll - simple_cos_x = sin(radians(90 - delta)); + if (simple_counter == 1) { + // roll + simple_cos_x = sin(radians(90 - delta)); - }else if (simple_counter > 2){ - // pitch - simple_sin_y = cos(radians(90 - delta)); - simple_counter = 0; - } + }else if (simple_counter > 2) { + // pitch + simple_sin_y = cos(radians(90 - delta)); + simple_counter = 0; + } - // Rotate input by the initial bearing - int16_t _roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; - int16_t _pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); + // Rotate input by the initial bearing + int16_t _roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; + int16_t _pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); - g.rc_1.control_in = _roll; - g.rc_2.control_in = _pitch; + g.rc_1.control_in = _roll; + g.rc_2.control_in = _pitch; } #define THROTTLE_FILTER_SIZE 2 @@ -1765,735 +1766,735 @@ void update_simple_mode(void) // controls all throttle behavior void update_throttle_mode(void) { - if(do_flip) // this is pretty bad but needed to flip in AP modes. - return; + if(do_flip) // this is pretty bad but needed to flip in AP modes. + return; - int16_t throttle_out; + int16_t throttle_out; - #if AUTO_THROTTLE_HOLD != 0 - static float throttle_avg = 0; // this is initialised to g.throttle_cruise later - #endif +#if AUTO_THROTTLE_HOLD != 0 + static float throttle_avg = 0; // this is initialised to g.throttle_cruise later +#endif - #if FRAME_CONFIG != HELI_FRAME - // calculate angle boost - if(throttle_mode == THROTTLE_MANUAL){ - angle_boost = get_angle_boost(g.rc_3.control_in); - }else{ - angle_boost = get_angle_boost(g.throttle_cruise); - } - #endif +#if FRAME_CONFIG != HELI_FRAME + // calculate angle boost + if(throttle_mode == THROTTLE_MANUAL) { + angle_boost = get_angle_boost(g.rc_3.control_in); + }else{ + angle_boost = get_angle_boost(g.throttle_cruise); + } +#endif - switch(throttle_mode){ - case THROTTLE_MANUAL: - if (g.rc_3.control_in > 0){ - #if FRAME_CONFIG == HELI_FRAME - g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in); - #else - if (control_mode == ACRO){ - g.rc_3.servo_out = g.rc_3.control_in; - }else{ - g.rc_3.servo_out = g.rc_3.control_in + angle_boost; - } - #endif + switch(throttle_mode) { + case THROTTLE_MANUAL: + if (g.rc_3.control_in > 0) { +#if FRAME_CONFIG == HELI_FRAME + g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in); +#else + if (control_mode == ACRO) { + g.rc_3.servo_out = g.rc_3.control_in; + }else{ + g.rc_3.servo_out = g.rc_3.control_in + angle_boost; + } +#endif - #if AUTO_THROTTLE_HOLD != 0 - // ensure throttle_avg has been initialised - if( throttle_avg == 0 ) { - throttle_avg = g.throttle_cruise; - } - // calc average throttle - if ((g.rc_3.control_in > g.throttle_min) && abs(climb_rate) < 60){ - throttle_avg = throttle_avg * .99 + (float)g.rc_3.control_in * .01; - g.throttle_cruise = throttle_avg; - } - #endif +#if AUTO_THROTTLE_HOLD != 0 + // ensure throttle_avg has been initialised + if( throttle_avg == 0 ) { + throttle_avg = g.throttle_cruise; + } + // calc average throttle + if ((g.rc_3.control_in > g.throttle_min) && abs(climb_rate) < 60) { + throttle_avg = throttle_avg * .99 + (float)g.rc_3.control_in * .01; + g.throttle_cruise = throttle_avg; + } +#endif - if (takeoff_complete == false && motors.armed()){ - if (g.rc_3.control_in > g.throttle_cruise){ - // we must be in the air by now - takeoff_complete = true; - } - } + if (takeoff_complete == false && motors.armed()) { + if (g.rc_3.control_in > g.throttle_cruise) { + // we must be in the air by now + takeoff_complete = true; + } + } - }else{ + }else{ - // make sure we also request 0 throttle out - // so the props stop ... properly - // ---------------------------------------- - g.rc_3.servo_out = 0; - } - break; + // make sure we also request 0 throttle out + // so the props stop ... properly + // ---------------------------------------- + g.rc_3.servo_out = 0; + } + break; - case THROTTLE_HOLD: - // allow interactive changing of atitude - if(g.rc_3.control_in < 200){ - reset_throttle_counter = 150; - nav_throttle = get_throttle_rate(-120); - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; - break; - }else if (g.rc_3.control_in > 800){ - reset_throttle_counter = 50; - nav_throttle = get_throttle_rate(180); - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; - break; - } + case THROTTLE_HOLD: + // allow interactive changing of atitude + if(g.rc_3.control_in < 200) { + reset_throttle_counter = 150; + nav_throttle = get_throttle_rate(-120); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; + break; + }else if (g.rc_3.control_in > 800) { + reset_throttle_counter = 50; + nav_throttle = get_throttle_rate(180); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; + break; + } - // allow 1 second of slow down after pilot moves throttle back into deadzone - if(reset_throttle_counter > 0){ - reset_throttle_counter--; - // if 1 second has passed set the target altitude to the current altitude - if(reset_throttle_counter == 0){ - force_new_altitude(max(current_loc.alt, 100)); - }else{ - nav_throttle = get_throttle_rate(0); - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; - break; - } - } + // allow 1 second of slow down after pilot moves throttle back into deadzone + if(reset_throttle_counter > 0) { + reset_throttle_counter--; + // if 1 second has passed set the target altitude to the current altitude + if(reset_throttle_counter == 0) { + force_new_altitude(max(current_loc.alt, 100)); + }else{ + nav_throttle = get_throttle_rate(0); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; + break; + } + } - // else fall through + // else fall through - case THROTTLE_AUTO: + case THROTTLE_AUTO: - if(motors.auto_armed() == true){ + if(motors.auto_armed() == true) { - // how far off are we - altitude_error = get_altitude_error(); + // how far off are we + altitude_error = get_altitude_error(); - int16_t desired_speed; - if(alt_change_flag == REACHED_ALT){ // we are at or above the target alt - desired_speed = g.pi_alt_hold.get_p(altitude_error); // calculate desired speed from lon error - update_throttle_cruise(g.pi_alt_hold.get_i(altitude_error, .02)); - desired_speed = constrain(desired_speed, -250, 250); - nav_throttle = get_throttle_rate(desired_speed); - }else{ - desired_speed = get_desired_climb_rate(); - nav_throttle = get_throttle_rate(desired_speed); - } - } + int16_t desired_speed; + if(alt_change_flag == REACHED_ALT) { // we are at or above the target alt + desired_speed = g.pi_alt_hold.get_p(altitude_error); // calculate desired speed from lon error + update_throttle_cruise(g.pi_alt_hold.get_i(altitude_error, .02)); + desired_speed = constrain(desired_speed, -250, 250); + nav_throttle = get_throttle_rate(desired_speed); + }else{ + desired_speed = get_desired_climb_rate(); + nav_throttle = get_throttle_rate(desired_speed); + } + } - // hack to remove the influence of the ground effect - if(g.sonar_enabled && current_loc.alt < 100 && landing_boost != 0) { - nav_throttle = min(nav_throttle, 0); - } + // hack to remove the influence of the ground effect + if(g.sonar_enabled && current_loc.alt < 100 && landing_boost != 0) { + nav_throttle = min(nav_throttle, 0); + } - #if FRAME_CONFIG == HELI_FRAME - throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle - landing_boost); - #else - throttle_out = g.throttle_cruise + nav_throttle + angle_boost - landing_boost; - #endif +#if FRAME_CONFIG == HELI_FRAME + throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle - landing_boost); +#else + throttle_out = g.throttle_cruise + nav_throttle + angle_boost - landing_boost; +#endif - g.rc_3.servo_out = throttle_out; - break; - } + g.rc_3.servo_out = throttle_out; + break; + } } // called after a GPS read static void update_navigation() { - // wp_distance is in CM - // -------------------- - switch(control_mode){ - case AUTO: - // note: wp_control is handled by commands_logic - verify_commands(); + // wp_distance is in CM + // -------------------- + switch(control_mode) { + case AUTO: + // note: wp_control is handled by commands_logic + verify_commands(); - // calculates desired Yaw - update_auto_yaw(); + // calculates desired Yaw + update_auto_yaw(); - // calculates the desired Roll and Pitch - update_nav_wp(); - break; + // calculates the desired Roll and Pitch + update_nav_wp(); + break; - case GUIDED: - wp_control = WP_MODE; - // check if we are close to point > loiter - wp_verify_byte = 0; - verify_nav_wp(); + case GUIDED: + wp_control = WP_MODE; + // check if we are close to point > loiter + wp_verify_byte = 0; + verify_nav_wp(); - if (wp_control == WP_MODE) { - update_auto_yaw(); - } else { - set_mode(LOITER); - } - update_nav_wp(); - break; + if (wp_control == WP_MODE) { + update_auto_yaw(); + } else { + set_mode(LOITER); + } + update_nav_wp(); + break; - case RTL: - // have we reached the desired Altitude? - if(alt_change_flag <= REACHED_ALT){ // we are at or above the target alt - if(rtl_reached_alt == false){ - rtl_reached_alt = true; - do_RTL(); - } - wp_control = WP_MODE; - // checks if we have made it to home - update_nav_RTL(); - } else{ - // we need to loiter until we are ready to come home - wp_control = LOITER_MODE; - } + case RTL: + // have we reached the desired Altitude? + if(alt_change_flag <= REACHED_ALT) { // we are at or above the target alt + if(rtl_reached_alt == false) { + rtl_reached_alt = true; + do_RTL(); + } + wp_control = WP_MODE; + // checks if we have made it to home + update_nav_RTL(); + } else{ + // we need to loiter until we are ready to come home + wp_control = LOITER_MODE; + } - // calculates desired Yaw - #if FRAME_CONFIG == HELI_FRAME - update_auto_yaw(); - #endif + // calculates desired Yaw +#if FRAME_CONFIG == HELI_FRAME + update_auto_yaw(); +#endif - // calculates the desired Roll and Pitch - update_nav_wp(); - break; + // calculates the desired Roll and Pitch + update_nav_wp(); + break; - // switch passthrough to LOITER - case LOITER: - case POSITION: - // This feature allows us to reposition the quad when the user lets - // go of the sticks + // switch passthrough to LOITER + case LOITER: + case POSITION: + // This feature allows us to reposition the quad when the user lets + // go of the sticks - if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ - if(wp_distance > 500) - loiter_override = true; - } + if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) { + if(wp_distance > 500) + loiter_override = true; + } - // Allow the user to take control temporarily, - if(loiter_override){ - // this sets the copter to not try and nav while we control it - wp_control = NO_NAV_MODE; + // Allow the user to take control temporarily, + if(loiter_override) { + // this sets the copter to not try and nav while we control it + wp_control = NO_NAV_MODE; - // reset LOITER to current position - next_WP.lat = current_loc.lat; - next_WP.lng = current_loc.lng; + // reset LOITER to current position + next_WP.lat = current_loc.lat; + next_WP.lng = current_loc.lng; - if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0){ - loiter_override = false; - wp_control = LOITER_MODE; - } - }else{ - wp_control = LOITER_MODE; - } + if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) { + loiter_override = false; + wp_control = LOITER_MODE; + } + }else{ + wp_control = LOITER_MODE; + } - if(loiter_timer != 0){ - // If we have a safe approach alt set and we have been loitering for 20 seconds(default), begin approach - if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()){ - // just to make sure we clear the timer - loiter_timer = 0; - if(g.rtl_approach_alt == 0){ - set_mode(LAND); - if(home_distance < 300){ - next_WP.lat = home.lat; - next_WP.lng = home.lng; - } - }else{ - if(g.rtl_approach_alt < current_loc.alt){ - set_new_altitude(g.rtl_approach_alt); - } - } - } - } + if(loiter_timer != 0) { + // If we have a safe approach alt set and we have been loitering for 20 seconds(default), begin approach + if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()) { + // just to make sure we clear the timer + loiter_timer = 0; + if(g.rtl_approach_alt == 0) { + set_mode(LAND); + if(home_distance < 300) { + next_WP.lat = home.lat; + next_WP.lng = home.lng; + } + }else{ + if(g.rtl_approach_alt < current_loc.alt) { + set_new_altitude(g.rtl_approach_alt); + } + } + } + } - // calculates the desired Roll and Pitch - update_nav_wp(); - break; + // calculates the desired Roll and Pitch + update_nav_wp(); + break; - case LAND: - if(g.sonar_enabled) - verify_land_sonar(); - else - verify_land_baro(); + case LAND: + if(g.sonar_enabled) + verify_land_sonar(); + else + verify_land_baro(); - // calculates the desired Roll and Pitch - update_nav_wp(); - break; + // calculates the desired Roll and Pitch + update_nav_wp(); + break; - case CIRCLE: - wp_control = CIRCLE_MODE; + case CIRCLE: + wp_control = CIRCLE_MODE; - // calculates desired Yaw - update_auto_yaw(); - update_nav_wp(); - break; + // calculates desired Yaw + update_auto_yaw(); + update_nav_wp(); + break; - case STABILIZE: - case TOY_A: - case TOY_M: - wp_control = NO_NAV_MODE; - update_nav_wp(); - break; - } + case STABILIZE: + case TOY_A: + case TOY_M: + wp_control = NO_NAV_MODE; + update_nav_wp(); + break; + } - // are we in SIMPLE mode? - if(do_simple && g.super_simple){ - // get distance to home - if(home_distance > SUPER_SIMPLE_RADIUS){ // 10m from home - // we reset the angular offset to be a vector from home to the quad - initial_simple_bearing = home_to_copter_bearing; - //Serial.printf("ISB: %d\n", initial_simple_bearing); - } - } + // are we in SIMPLE mode? + if(do_simple && g.super_simple) { + // get distance to home + if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home + // we reset the angular offset to be a vector from home to the quad + initial_simple_bearing = home_to_copter_bearing; + //Serial.printf("ISB: %d\n", initial_simple_bearing); + } + } - if(yaw_mode == YAW_LOOK_AT_HOME){ - if(home_is_set){ - //nav_yaw = point_at_home_yaw(); - nav_yaw = get_bearing_cd(¤t_loc, &home); - } else { - nav_yaw = 0; - } - } + if(yaw_mode == YAW_LOOK_AT_HOME) { + if(home_is_set) { + //nav_yaw = point_at_home_yaw(); + nav_yaw = get_bearing_cd(¤t_loc, &home); + } else { + nav_yaw = 0; + } + } } static void update_nav_RTL() { - // Have we have reached Home? - if(wp_distance <= 200 || check_missed_wp()){ - // if loiter_timer value > 0, we are set to trigger auto_land or approach - set_mode(LOITER); + // Have we have reached Home? + if(wp_distance <= 200 || check_missed_wp()) { + // if loiter_timer value > 0, we are set to trigger auto_land or approach + set_mode(LOITER); - // just un case we arrive and we aren't at the lower RTL alt yet. - set_new_altitude(get_RTL_alt()); + // just un case we arrive and we aren't at the lower RTL alt yet. + set_new_altitude(get_RTL_alt()); - // force loitering above home - next_WP.lat = home.lat; - next_WP.lng = home.lng; + // force loitering above home + next_WP.lat = home.lat; + next_WP.lng = home.lng; - // If failsafe OR auto approach altitude is set - // we will go into automatic land, (g.rtl_approach_alt) is the lowest point - // -1 means disable feature - if(failsafe || g.rtl_approach_alt >= 0) - loiter_timer = millis(); - else - loiter_timer = 0; - } + // If failsafe OR auto approach altitude is set + // we will go into automatic land, (g.rtl_approach_alt) is the lowest point + // -1 means disable feature + if(failsafe || g.rtl_approach_alt >= 0) + loiter_timer = millis(); + else + loiter_timer = 0; + } - slow_wp = true; + slow_wp = true; } static void read_AHRS(void) { - // Perform IMU calculations and get attitude info - //----------------------------------------------- - #if HIL_MODE != HIL_MODE_DISABLED - // update hil before ahrs update - gcs_update(); - #endif + // Perform IMU calculations and get attitude info + //----------------------------------------------- +#if HIL_MODE != HIL_MODE_DISABLED + // update hil before ahrs update + gcs_update(); +#endif - ahrs.update(); - omega = imu.get_gyro(); + ahrs.update(); + omega = imu.get_gyro(); } static void update_trig(void){ - Vector2f yawvector; - Matrix3f temp = ahrs.get_dcm_matrix(); + Vector2f yawvector; + Matrix3f temp = ahrs.get_dcm_matrix(); - yawvector.x = temp.a.x; // sin - yawvector.y = temp.b.x; // cos - yawvector.normalize(); + yawvector.x = temp.a.x; // sin + yawvector.y = temp.b.x; // cos + yawvector.normalize(); - cos_pitch_x = safe_sqrt(1 - (temp.c.x * temp.c.x)); // level = 1 - cos_roll_x = temp.c.z / cos_pitch_x; // level = 1 + cos_pitch_x = safe_sqrt(1 - (temp.c.x * temp.c.x)); // level = 1 + cos_roll_x = temp.c.z / cos_pitch_x; // level = 1 cos_pitch_x = constrain(cos_pitch_x, 0, 1.0); // this relies on constrain() of infinity doing the right thing, // which it does do in avr-libc cos_roll_x = constrain(cos_roll_x, -1.0, 1.0); - sin_yaw_y = yawvector.x; // 1y = north - cos_yaw_x = yawvector.y; // 0x = north + sin_yaw_y = yawvector.x; // 1y = north + cos_yaw_x = yawvector.y; // 0x = north - //flat: - // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, - // 90° = cos_yaw: 1.00, sin_yaw: 0.00, - // 180 = cos_yaw: 0.00, sin_yaw: -1.00, - // 270 = cos_yaw: -1.00, sin_yaw: 0.00, + //flat: + // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, + // 90° = cos_yaw: 1.00, sin_yaw: 0.00, + // 180 = cos_yaw: 0.00, sin_yaw: -1.00, + // 270 = cos_yaw: -1.00, sin_yaw: 0.00, } // updated at 10hz static void update_altitude() { - static int16_t old_sonar_alt = 0; + static int16_t old_sonar_alt = 0; - #if HIL_MODE == HIL_MODE_ATTITUDE - static int32_t old_baro_alt = 0; - // we are in the SIM, fake out the baro and Sonar - int16_t fake_relative_alt = g_gps->altitude - gps_base_alt; - baro_alt = fake_relative_alt; - sonar_alt = fake_relative_alt; +#if HIL_MODE == HIL_MODE_ATTITUDE + static int32_t old_baro_alt = 0; + // we are in the SIM, fake out the baro and Sonar + int16_t fake_relative_alt = g_gps->altitude - gps_base_alt; + baro_alt = fake_relative_alt; + sonar_alt = fake_relative_alt; - baro_rate = (baro_alt - old_baro_alt) * 5; // 5hz - old_baro_alt = baro_alt; + baro_rate = (baro_alt - old_baro_alt) * 5; // 5hz + old_baro_alt = baro_alt; - #else - // This is real life +#else + // This is real life - #if INERTIAL_NAV == ENABLED - baro_rate = accels_velocity.z; + #if INERTIAL_NAV == ENABLED + baro_rate = accels_velocity.z; - #else - // read in Actual Baro Altitude - baro_alt = read_barometer(); + #else + // read in Actual Baro Altitude + baro_alt = read_barometer(); - // calc the vertical accel rate - /* - // 2.6 way - int16_t temp = (baro_alt - old_baro_alt) * 10; - baro_rate = (temp + baro_rate) >> 1; - baro_rate = constrain(baro_rate, -300, 300); - old_baro_alt = baro_alt; - */ + // calc the vertical accel rate + /* + * // 2.6 way + * int16_t temp = (baro_alt - old_baro_alt) * 10; + * baro_rate = (temp + baro_rate) >> 1; + * baro_rate = constrain(baro_rate, -300, 300); + * old_baro_alt = baro_alt; + */ - // Using Tridge's new clamb rate calc - int16_t temp = barometer.get_climb_rate() * 100; - baro_rate = (temp + baro_rate) >> 1; - baro_rate = constrain(baro_rate, -300, 300); - #endif + // Using Tridge's new clamb rate calc + int16_t temp = barometer.get_climb_rate() * 100; + baro_rate = (temp + baro_rate) >> 1; + baro_rate = constrain(baro_rate, -300, 300); + #endif - // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter - #endif + // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter +#endif - if(g.sonar_enabled){ - // filter out offset - float scale; + if(g.sonar_enabled) { + // filter out offset + float scale; - // calc rate of change for Sonar - #if HIL_MODE == HIL_MODE_ATTITUDE - // we are in the SIM, fake outthe Sonar rate - sonar_rate = baro_rate; - #else - // This is real life - // calc the vertical accel rate - // positive = going up. - sonar_rate = (sonar_alt - old_sonar_alt) * 10; - sonar_rate = constrain(sonar_rate, -150, 150); - old_sonar_alt = sonar_alt; - #endif + // calc rate of change for Sonar +#if HIL_MODE == HIL_MODE_ATTITUDE + // we are in the SIM, fake outthe Sonar rate + sonar_rate = baro_rate; +#else + // This is real life + // calc the vertical accel rate + // positive = going up. + sonar_rate = (sonar_alt - old_sonar_alt) * 10; + sonar_rate = constrain(sonar_rate, -150, 150); + old_sonar_alt = sonar_alt; +#endif - if(baro_alt < 800){ - #if SONAR_TILT_CORRECTION == 1 - // correct alt for angle of the sonar - float temp = cos_pitch_x * cos_roll_x; - temp = max(temp, 0.707); - sonar_alt = (float)sonar_alt * temp; - #endif + if(baro_alt < 800) { +#if SONAR_TILT_CORRECTION == 1 + // correct alt for angle of the sonar + float temp = cos_pitch_x * cos_roll_x; + temp = max(temp, 0.707); + sonar_alt = (float)sonar_alt * temp; +#endif - scale = (float)(sonar_alt - 400) / 200.0; - scale = constrain(scale, 0.0, 1.0); - // solve for a blended altitude - current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale); + scale = (float)(sonar_alt - 400) / 200.0; + scale = constrain(scale, 0.0, 1.0); + // solve for a blended altitude + current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale); - // solve for a blended climb_rate - climb_rate_actual = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale; + // solve for a blended climb_rate + climb_rate_actual = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale; - }else{ - // we must be higher than sonar (>800), don't get tricked by bad sonar reads - current_loc.alt = baro_alt; - // dont blend, go straight baro + }else{ + // we must be higher than sonar (>800), don't get tricked by bad sonar reads + current_loc.alt = baro_alt; + // dont blend, go straight baro - climb_rate_actual = baro_rate; - } + climb_rate_actual = baro_rate; + } - }else{ - // NO Sonar case - current_loc.alt = baro_alt; - climb_rate_actual = baro_rate; - } + }else{ + // NO Sonar case + current_loc.alt = baro_alt; + climb_rate_actual = baro_rate; + } - // update the target altitude - verify_altitude(); + // update the target altitude + verify_altitude(); - // calc error - climb_rate_error = (climb_rate_actual - climb_rate) / 5; + // calc error + climb_rate_error = (climb_rate_actual - climb_rate) / 5; - #if INERTIAL_NAV == ENABLED - // inertial_nav - z_error_correction(); - #endif +#if INERTIAL_NAV == ENABLED + // inertial_nav + z_error_correction(); +#endif } static void update_altitude_est() { - if(alt_sensor_flag){ - update_altitude(); - alt_sensor_flag = false; + if(alt_sensor_flag) { + update_altitude(); + alt_sensor_flag = false; - if(g.log_bitmask & MASK_LOG_CTUN && motors.armed()){ - Log_Write_Control_Tuning(); - } + if(g.log_bitmask & MASK_LOG_CTUN && motors.armed()) { + Log_Write_Control_Tuning(); + } - }else{ - // simple dithering of climb rate - climb_rate += climb_rate_error; - current_loc.alt += (climb_rate / 50); - } - //Serial.printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt); + }else{ + // simple dithering of climb rate + climb_rate += climb_rate_error; + current_loc.alt += (climb_rate / 50); + } + //Serial.printf(" %d, %d, %d, %d\n", climb_rate_actual, climb_rate_error, climb_rate, current_loc.alt); } /* -#define THROTTLE_ADJUST 225 -static void -adjust_altitude() -{ - if(g.rc_3.control_in <= (g.throttle_min + THROTTLE_ADJUST)){ - // we remove 0 to 100 PWM from hover - manual_boost = (g.rc_3.control_in - g.throttle_min) - THROTTLE_ADJUST; - manual_boost = max(-THROTTLE_ADJUST, manual_boost); - - }else if (g.rc_3.control_in >= (g.throttle_max - THROTTLE_ADJUST)){ - // we add 0 to 100 PWM to hover - manual_boost = g.rc_3.control_in - (g.throttle_max - THROTTLE_ADJUST); - manual_boost = min(THROTTLE_ADJUST, manual_boost); - - }else { - manual_boost = 0; - } -} -*/ + * #define THROTTLE_ADJUST 225 + * static void + * adjust_altitude() + * { + * if(g.rc_3.control_in <= (g.throttle_min + THROTTLE_ADJUST)){ + * // we remove 0 to 100 PWM from hover + * manual_boost = (g.rc_3.control_in - g.throttle_min) - THROTTLE_ADJUST; + * manual_boost = max(-THROTTLE_ADJUST, manual_boost); + * + * }else if (g.rc_3.control_in >= (g.throttle_max - THROTTLE_ADJUST)){ + * // we add 0 to 100 PWM to hover + * manual_boost = g.rc_3.control_in - (g.throttle_max - THROTTLE_ADJUST); + * manual_boost = min(THROTTLE_ADJUST, manual_boost); + * + * }else { + * manual_boost = 0; + * } + * } + */ static void tuning(){ - tuning_value = (float)g.rc_6.control_in / 1000.0; - g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1 + tuning_value = (float)g.rc_6.control_in / 1000.0; + g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1 - switch(g.radio_tuning){ + switch(g.radio_tuning) { - case CH6_RATE_KD: - g.pid_rate_roll.kD(tuning_value); - g.pid_rate_pitch.kD(tuning_value); - break; + case CH6_RATE_KD: + g.pid_rate_roll.kD(tuning_value); + g.pid_rate_pitch.kD(tuning_value); + break; - case CH6_STABILIZE_KP: - g.pi_stabilize_roll.kP(tuning_value); - g.pi_stabilize_pitch.kP(tuning_value); - break; + case CH6_STABILIZE_KP: + g.pi_stabilize_roll.kP(tuning_value); + g.pi_stabilize_pitch.kP(tuning_value); + break; - case CH6_STABILIZE_KI: - g.pi_stabilize_roll.kI(tuning_value); - g.pi_stabilize_pitch.kI(tuning_value); - break; + case CH6_STABILIZE_KI: + g.pi_stabilize_roll.kI(tuning_value); + g.pi_stabilize_pitch.kI(tuning_value); + break; - case CH6_DAMP: - case CH6_STABILIZE_KD: - g.stabilize_d = tuning_value; - break; + case CH6_DAMP: + case CH6_STABILIZE_KD: + g.stabilize_d = tuning_value; + break; - case CH6_ACRO_KP: - g.acro_p = tuning_value; - break; + case CH6_ACRO_KP: + g.acro_p = tuning_value; + break; - case CH6_RATE_KP: - g.pid_rate_roll.kP(tuning_value); - g.pid_rate_pitch.kP(tuning_value); - break; + case CH6_RATE_KP: + g.pid_rate_roll.kP(tuning_value); + g.pid_rate_pitch.kP(tuning_value); + break; - case CH6_RATE_KI: - g.pid_rate_roll.kI(tuning_value); - g.pid_rate_pitch.kI(tuning_value); - break; + case CH6_RATE_KI: + g.pid_rate_roll.kI(tuning_value); + g.pid_rate_pitch.kI(tuning_value); + break; - case CH6_YAW_KP: - g.pi_stabilize_yaw.kP(tuning_value); - break; + case CH6_YAW_KP: + g.pi_stabilize_yaw.kP(tuning_value); + break; - case CH6_YAW_KI: - g.pi_stabilize_yaw.kI(tuning_value); - break; + case CH6_YAW_KI: + g.pi_stabilize_yaw.kI(tuning_value); + break; - case CH6_YAW_RATE_KP: - g.pid_rate_yaw.kP(tuning_value); - break; + case CH6_YAW_RATE_KP: + g.pid_rate_yaw.kP(tuning_value); + break; - case CH6_YAW_RATE_KD: - g.pid_rate_yaw.kD(tuning_value); - break; + case CH6_YAW_RATE_KD: + g.pid_rate_yaw.kD(tuning_value); + break; - case CH6_THROTTLE_KP: - g.pid_throttle.kP(tuning_value); - break; + case CH6_THROTTLE_KP: + g.pid_throttle.kP(tuning_value); + break; - case CH6_TOP_BOTTOM_RATIO: - motors.top_bottom_ratio = tuning_value; - break; + case CH6_TOP_BOTTOM_RATIO: + motors.top_bottom_ratio = tuning_value; + break; - case CH6_RELAY: - if (g.rc_6.control_in > 525) relay.on(); - if (g.rc_6.control_in < 475) relay.off(); - break; + case CH6_RELAY: + if (g.rc_6.control_in > 525) relay.on(); + if (g.rc_6.control_in < 475) relay.off(); + break; - case CH6_TRAVERSE_SPEED: - g.waypoint_speed_max = g.rc_6.control_in; - break; + case CH6_TRAVERSE_SPEED: + g.waypoint_speed_max = g.rc_6.control_in; + break; - case CH6_LOITER_KP: - g.pi_loiter_lat.kP(tuning_value); - g.pi_loiter_lon.kP(tuning_value); - break; + case CH6_LOITER_KP: + g.pi_loiter_lat.kP(tuning_value); + g.pi_loiter_lon.kP(tuning_value); + break; - case CH6_LOITER_KI: - g.pi_loiter_lat.kI(tuning_value); - g.pi_loiter_lon.kI(tuning_value); - break; + case CH6_LOITER_KI: + g.pi_loiter_lat.kI(tuning_value); + g.pi_loiter_lon.kI(tuning_value); + break; - case CH6_NAV_KP: - g.pid_nav_lat.kP(tuning_value); - g.pid_nav_lon.kP(tuning_value); - break; + case CH6_NAV_KP: + g.pid_nav_lat.kP(tuning_value); + g.pid_nav_lon.kP(tuning_value); + break; - case CH6_LOITER_RATE_KP: - g.pid_loiter_rate_lon.kP(tuning_value); - g.pid_loiter_rate_lat.kP(tuning_value); - break; + case CH6_LOITER_RATE_KP: + g.pid_loiter_rate_lon.kP(tuning_value); + g.pid_loiter_rate_lat.kP(tuning_value); + break; - case CH6_LOITER_RATE_KI: - g.pid_loiter_rate_lon.kI(tuning_value); - g.pid_loiter_rate_lat.kI(tuning_value); - break; + case CH6_LOITER_RATE_KI: + g.pid_loiter_rate_lon.kI(tuning_value); + g.pid_loiter_rate_lat.kI(tuning_value); + break; - case CH6_LOITER_RATE_KD: - g.pid_loiter_rate_lon.kD(tuning_value); - g.pid_loiter_rate_lat.kD(tuning_value); - break; + case CH6_LOITER_RATE_KD: + g.pid_loiter_rate_lon.kD(tuning_value); + g.pid_loiter_rate_lat.kD(tuning_value); + break; - case CH6_NAV_I: - g.pid_nav_lat.kI(tuning_value); - g.pid_nav_lon.kI(tuning_value); - break; + case CH6_NAV_I: + g.pid_nav_lat.kI(tuning_value); + g.pid_nav_lon.kI(tuning_value); + break; - #if FRAME_CONFIG == HELI_FRAME - case CH6_HELI_EXTERNAL_GYRO: - motors.ext_gyro_gain = tuning_value; - break; - #endif - - case CH6_THR_HOLD_KP: - g.pi_alt_hold.kP(tuning_value); - break; - - case CH6_OPTFLOW_KP: - g.pid_optflow_roll.kP(tuning_value); - g.pid_optflow_pitch.kP(tuning_value); - break; - - case CH6_OPTFLOW_KI: - g.pid_optflow_roll.kI(tuning_value); - g.pid_optflow_pitch.kI(tuning_value); - break; - - case CH6_OPTFLOW_KD: - g.pid_optflow_roll.kD(tuning_value); - g.pid_optflow_pitch.kD(tuning_value); - break; - -#if HIL_MODE != HIL_MODE_ATTITUDE // do not allow modifying _kp or _kp_yaw gains in HIL mode - case CH6_AHRS_YAW_KP: - ahrs._kp_yaw.set(tuning_value); - break; - - case CH6_AHRS_KP: - ahrs._kp.set(tuning_value); - break; +#if FRAME_CONFIG == HELI_FRAME + case CH6_HELI_EXTERNAL_GYRO: + motors.ext_gyro_gain = tuning_value; + break; #endif - } + case CH6_THR_HOLD_KP: + g.pi_alt_hold.kP(tuning_value); + break; + + case CH6_OPTFLOW_KP: + g.pid_optflow_roll.kP(tuning_value); + g.pid_optflow_pitch.kP(tuning_value); + break; + + case CH6_OPTFLOW_KI: + g.pid_optflow_roll.kI(tuning_value); + g.pid_optflow_pitch.kI(tuning_value); + break; + + case CH6_OPTFLOW_KD: + g.pid_optflow_roll.kD(tuning_value); + g.pid_optflow_pitch.kD(tuning_value); + break; + +#if HIL_MODE != HIL_MODE_ATTITUDE // do not allow modifying _kp or _kp_yaw gains in HIL mode + case CH6_AHRS_YAW_KP: + ahrs._kp_yaw.set(tuning_value); + break; + + case CH6_AHRS_KP: + ahrs._kp.set(tuning_value); + break; +#endif + + } } // Outputs Nav_Pitch and Nav_Roll static void update_nav_wp() { - if(wp_control == LOITER_MODE){ + if(wp_control == LOITER_MODE) { - // calc error to target - calc_location_error(&next_WP); + // calc error to target + calc_location_error(&next_WP); - // use error as the desired rate towards the target - calc_loiter(long_error, lat_error); + // use error as the desired rate towards the target + calc_loiter(long_error, lat_error); - }else if(wp_control == CIRCLE_MODE){ + }else if(wp_control == CIRCLE_MODE) { - // check if we have missed the WP - int16_t loiter_delta = (target_bearing - old_target_bearing)/100; + // check if we have missed the WP + int16_t loiter_delta = (target_bearing - old_target_bearing)/100; - // reset the old value - old_target_bearing = target_bearing; + // reset the old value + old_target_bearing = target_bearing; - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; + // wrap values + if (loiter_delta > 180) loiter_delta -= 360; + if (loiter_delta < -180) loiter_delta += 360; - // sum the angle around the WP - loiter_sum += loiter_delta; + // sum the angle around the WP + loiter_sum += loiter_delta; - // create a virtual waypoint that circles the next_WP - // Count the degrees we have circulated the WP - //int16_t circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100; + // create a virtual waypoint that circles the next_WP + // Count the degrees we have circulated the WP + //int16_t circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100; - circle_angle += (circle_rate * dTnav); - //1° = 0.0174532925 radians + circle_angle += (circle_rate * dTnav); + //1° = 0.0174532925 radians - // wrap - if (circle_angle > 6.28318531) - circle_angle -= 6.28318531; + // wrap + if (circle_angle > 6.28318531) + circle_angle -= 6.28318531; - next_WP.lng = circle_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); - next_WP.lat = circle_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle)); + next_WP.lng = circle_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); + next_WP.lat = circle_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle)); - // use error as the desired rate towards the target - // nav_lon, nav_lat is calculated + // use error as the desired rate towards the target + // nav_lon, nav_lat is calculated - if(wp_distance > 400){ - calc_nav_rate(get_desired_speed(g.waypoint_speed_max, true)); - }else{ - // calc the lat and long error to the target - calc_location_error(&next_WP); + if(wp_distance > 400) { + calc_nav_rate(get_desired_speed(g.waypoint_speed_max, true)); + }else{ + // calc the lat and long error to the target + calc_location_error(&next_WP); - calc_loiter(long_error, lat_error); - } + calc_loiter(long_error, lat_error); + } - //CIRCLE: angle:29, dist:0, lat:400, lon:242 + //CIRCLE: angle:29, dist:0, lat:400, lon:242 - // debug - //int16_t angleTest = degrees(circle_angle); - //int16_t nroll = nav_roll; - //int16_t npitch = nav_pitch; - //Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll); + // debug + //int16_t angleTest = degrees(circle_angle); + //int16_t nroll = nav_roll; + //int16_t npitch = nav_pitch; + //Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll); - }else if(wp_control == WP_MODE){ - // calc error to target - calc_location_error(&next_WP); + }else if(wp_control == WP_MODE) { + // calc error to target + calc_location_error(&next_WP); - int16_t speed = get_desired_speed(g.waypoint_speed_max, slow_wp); - // use error as the desired rate towards the target - calc_nav_rate(speed); + int16_t speed = get_desired_speed(g.waypoint_speed_max, slow_wp); + // use error as the desired rate towards the target + calc_nav_rate(speed); - }else if(wp_control == NO_NAV_MODE){ - // clear out our nav so we can do things like land straight down - // or change Loiter position + }else if(wp_control == NO_NAV_MODE) { + // clear out our nav so we can do things like land straight down + // or change Loiter position - // We bring copy over our Iterms for wind control, but we don't navigate - nav_lon = g.pid_loiter_rate_lon.get_integrator(); - nav_lat = g.pid_loiter_rate_lon.get_integrator(); + // We bring copy over our Iterms for wind control, but we don't navigate + nav_lon = g.pid_loiter_rate_lon.get_integrator(); + nav_lat = g.pid_loiter_rate_lon.get_integrator(); - nav_lon = constrain(nav_lon, -2000, 2000); // 20° - nav_lat = constrain(nav_lat, -2000, 2000); // 20° - } + nav_lon = constrain(nav_lon, -2000, 2000); // 20° + nav_lat = constrain(nav_lat, -2000, 2000); // 20° + } } static void update_auto_yaw() { - if(wp_control == CIRCLE_MODE){ - auto_yaw = get_bearing_cd(¤t_loc, &circle_WP); + if(wp_control == CIRCLE_MODE) { + auto_yaw = get_bearing_cd(¤t_loc, &circle_WP); - }else if(wp_control == LOITER_MODE){ - // just hold nav_yaw + }else if(wp_control == LOITER_MODE) { + // just hold nav_yaw - }else if(yaw_tracking == MAV_ROI_LOCATION){ - auto_yaw = get_bearing_cd(¤t_loc, &target_WP); + }else if(yaw_tracking == MAV_ROI_LOCATION) { + auto_yaw = get_bearing_cd(¤t_loc, &target_WP); - }else if(yaw_tracking == MAV_ROI_WPNEXT){ - // Point towards next WP - auto_yaw = original_target_bearing; - } + }else if(yaw_tracking == MAV_ROI_WPNEXT) { + // Point towards next WP + auto_yaw = original_target_bearing; + } } /* - MAV_ROI_NONE=0, No region of interest. | - MAV_ROI_WPNEXT=1, Point toward next MISSION. | - MAV_ROI_WPINDEX=2, Point toward given MISSION. | - MAV_ROI_LOCATION=3, Point toward fixed location. | - MAV_ROI_TARGET=4, Point toward of given id. - MAV_ROI_ENUM_END=5, -*/ + * MAV_ROI_NONE=0, No region of interest. | + * MAV_ROI_WPNEXT=1, Point toward next MISSION. | + * MAV_ROI_WPINDEX=2, Point toward given MISSION. | + * MAV_ROI_LOCATION=3, Point toward fixed location. | + * MAV_ROI_TARGET=4, Point toward of given id. + * MAV_ROI_ENUM_END=5, + */