From 899e6e66b091905ccb82bed6aa20ea4990f042d6 Mon Sep 17 00:00:00 2001 From: Jean-Louis Naudin Date: Wed, 9 May 2012 07:12:26 +0200 Subject: [PATCH] APMrover v2.0c - tested with APM v2 full kit (Oilpan) - dualversion (IMUless + IMUfull) Signed-off-by: Jean-Louis Naudin --- APMrover2/APM_Config.h | 5 +++ APMrover2/APM_Config_Rover.h | 10 +++--- APMrover2/APMrover2.pde | 67 +++++++++++++++++++++++------------- APMrover2/Attitude.pde | 33 ++---------------- APMrover2/GCS_Mavlink.pde | 26 ++++++-------- APMrover2/Log.pde | 3 +- APMrover2/config.h | 8 +++++ APMrover2/navigation.pde | 23 +------------ APMrover2/radio.pde | 19 ---------- APMrover2/sensors.pde | 3 +- APMrover2/system.pde | 22 +++++++++--- 11 files changed, 97 insertions(+), 122 deletions(-) diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h index 3a73abca02..be6ea6f9d0 100644 --- a/APMrover2/APM_Config.h +++ b/APMrover2/APM_Config.h @@ -10,11 +10,16 @@ #define CONFIG_APM_HARDWARE APM_HARDWARE_APM1 +#define LITE DISABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329 + // if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2 + #define CLI_ENABLED ENABLED #define CLI_SLIDER_ENABLED DISABLED #define CLOSED_LOOP_NAV ENABLED #define AUTO_WP_RADIUS ENABLED +#define TRACE DISABLED + //#include "APM_Config_HILmode.h" // for test in HIL mode with AeroSIM Rc 3.83 #include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder diff --git a/APMrover2/APM_Config_Rover.h b/APMrover2/APM_Config_Rover.h index b32fd97cbc..b05e974cd2 100644 --- a/APMrover2/APM_Config_Rover.h +++ b/APMrover2/APM_Config_Rover.h @@ -11,12 +11,14 @@ //#define GROUND_START_DELAY 1 #define AIRSPEED_SENSOR DISABLED -#define MAGNETOMETER ENABLED -#define LOGGING_ENABLED ENABLED -#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD -#define PARAM_DECLINATION 0.18 // Paris +#if LITE == DISABLED + #define MAGNETOMETER ENABLED + #define LOGGING_ENABLED ENABLED + #define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD + #define PARAM_DECLINATION 0.18 // Paris +#endif ////////////////////////////////////////////////////////////////////////////// // Serial port speeds. // diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 58c4e9f463..ae3bec3aee 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -1,11 +1,11 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "APMrover v2.0b JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer +#define THISFIRMWARE "APMrover v2.0c JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer // This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN) /* -Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler +Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier Please contribute your ideas! @@ -16,7 +16,7 @@ License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. // -// JLN updates: last update 2012-05-01 +// JLN updates: last update 2012-05-04 // // DOLIST: // @@ -24,6 +24,9 @@ version 2.1 of the License, or (at your option) any later version. //------------------------------------------------------------------------------------------------------------------------- // Dev Startup : 2012-04-21 // +// 2012-05-04: Added #define LITE ENABLED for the APM1280 or APM2560 CPU IMUless version +// 2012-05-03: Successful missions tests with a full APM2560 kit (GPS MT3329 + magnetometer HMC5883L) +// 2012-05-03: removing stick mixing in auto mode // 2012-05-01: special update for rover about ground_course if compass is enabled // 2012-04-30: Successfully tested in autonomous nav with a waypoints list recorded in live mode // 2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer @@ -586,7 +589,6 @@ static long nav_pitch; // Calculated radius for the wp turn based on ground speed and max turn angle static long wp_radius; static long toff_yaw; // deg * 100 : yaw angle for takeoff -static long nav_yaw = 1; // deg * 100 : target yaw angle - used only for thermal hunt static long altitude_estimate = 0; // for smoothing GPS output //////////////////////////////////////////////////////////////////////////////// @@ -643,8 +645,7 @@ static struct Location current_loc; static struct Location next_WP; // The location of the active waypoint in Guided mode. static struct Location guided_WP; -static struct Location soarwp1_WP = {0,0,0}; // temp waypoint for Ridge Soaring -static struct Location soarwp2_WP = {0,0,0}; // temp waypoint for Ridge Soaring + // The location structure information from the Nav command being processed static struct Location next_nav_command; // The location structure information from the Non-Nav command being processed @@ -749,11 +750,12 @@ void loop() if (millis() - perf_mon_timer > 20000) { if (mainLoop_count != 0) { + #if LITE == DISABLED if (g.log_bitmask & MASK_LOG_PM) #if HIL_MODE != HIL_MODE_ATTITUDE Log_Write_Performance(); #endif - + #endif resetPerfData(); } } @@ -787,11 +789,14 @@ static void fast_loop() gcs_update(); #endif +#if LITE == DISABLED ahrs.update(); +#endif // uses the yaw from the DCM to give more accurate turns calc_bearing_error(); +#if LITE == DISABLED # if HIL_MODE == HIL_MODE_DISABLED if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); @@ -799,7 +804,7 @@ static void fast_loop() if (g.log_bitmask & MASK_LOG_RAW) Log_Write_Raw(); #endif - +#endif // inertial navigation // ------------------ #if INERTIAL_NAVIGATION == ENABLED @@ -845,7 +850,8 @@ static void medium_loop() update_GPS(); calc_gndspeed_undershoot(); } - + +#if LITE == DISABLED #if HIL_MODE != HIL_MODE_ATTITUDE if (g.compass_enabled && compass.read()) { ahrs.set_compass(&compass); @@ -857,6 +863,7 @@ static void medium_loop() ahrs.set_compass(NULL); } #endif +#endif /*{ Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); @@ -877,7 +884,7 @@ Serial.println(tempaccel.z, DEC); if(g_gps->new_data){ g_gps->new_data = false; - dTnav = millis() - nav_loopTimer; + dTnav = millis() - nav_loopTimer; nav_loopTimer = millis(); // calculate the plane's desired bearing @@ -901,7 +908,7 @@ Serial.println(tempaccel.z, DEC); //------------------------------------------------- case 3: medium_loopCounter++; - +#if LITE == DISABLED #if HIL_MODE != HIL_MODE_ATTITUDE if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); @@ -915,7 +922,7 @@ Serial.println(tempaccel.z, DEC); if (g.log_bitmask & MASK_LOG_GPS) Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); - +#endif // send all requested output streams with rates requested // between 5 and 45 Hz gcs_data_stream_send(5,45); @@ -949,12 +956,13 @@ static void slow_loop() check_long_failsafe(); superslow_loopCounter++; if(superslow_loopCounter >=200) { // 200 = Execute every minute +#if LITE == DISABLED #if HIL_MODE != HIL_MODE_ATTITUDE if(g.compass_enabled) { compass.save_offsets(); } #endif - +#endif superslow_loopCounter = 0; } break; @@ -988,22 +996,31 @@ static void slow_loop() #if USB_MUX_PIN > 0 check_usb_mux(); #endif + +#if TRACE == ENABLED + Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), + (float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100); + // Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"), + // g.command_total, g.command_index, nav_command_index); +#endif break; } } static void one_second_loop() { +#if LITE == DISABLED if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); - +#endif // send a heartbeat gcs_send_message(MSG_HEARTBEAT); gcs_data_stream_send(1,3); } static void update_GPS(void) -{ +{ static uint16_t hdg; + g_gps->update(); update_GPS_light(); @@ -1026,19 +1043,20 @@ static void update_GPS(void) } else { if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){ startup_ground(); - +#if LITE == DISABLED if (g.log_bitmask & MASK_LOG_CMD) Log_Write_Startup(TYPE_GROUNDSTART_MSG); - +#endif init_home(); } else if (ENABLE_AIR_START == 0) { init_home(); } - +#if LITE == DISABLED if (g.compass_enabled) { // Set compass declination automatically compass.set_initial_location(g_gps->latitude, g_gps->longitude); } +#endif ground_start_count = 0; } } @@ -1048,13 +1066,16 @@ static void update_GPS(void) current_loc.lat = g_gps->latitude; // Lat * 10**7 current_loc.alt = max((g_gps->altitude - home.alt),0); ground_speed = g_gps->ground_speed; +#if LITE == DISABLED if (g.compass_enabled) { - ground_course = (wrap_360(ToDeg(compass.heading) * 100)); - } else { - ground_course = g_gps->ground_course; + hdg=(ahrs.yaw_sensor / 100) % 360; + ground_course = hdg * 100; + } else { +#endif + ground_course = abs(g_gps->ground_course); +#if LITE == DISABLED } - - +#endif // see if we've breached the geo-fence geofence_check(false); } diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde index 5a31fd23c2..058b3e145a 100644 --- a/APMrover2/Attitude.pde +++ b/APMrover2/Attitude.pde @@ -6,39 +6,10 @@ static void stabilize() { - float ch1_inf = 1.0; - // Calculate desired servo output for the turn // Wheels Direction // --------------------------------------------- - g.channel_roll.servo_out = nav_roll; - - // Mix Stick input to allow users to override control surfaces - // ----------------------------------------------------------- - if ((control_mode < FLY_BY_WIRE_A) || - (ENABLE_STICK_MIXING == 1 && - geofence_stickmixing() && - control_mode > FLY_BY_WIRE_B && - failsafe == FAILSAFE_NONE)) { - - // TODO: use RC_Channel control_mix function? - ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; - ch1_inf = fabs(ch1_inf); - ch1_inf = min(ch1_inf, 400.0); - ch1_inf = ((400.0 - ch1_inf) /400.0); - - // scale the sensor input based on the stick input - // ----------------------------------------------- - g.channel_roll.servo_out *= ch1_inf; - - // Mix in stick inputs - // ------------------- - g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); - - //Serial.printf_P(PSTR(" servo_out[CH_ROLL] ")); - //Serial.println(servo_out[CH_ROLL],DEC); - - } - + + g.channel_roll.servo_out = nav_roll; g.channel_roll.servo_out = g.channel_roll.servo_out * g.turn_gain; g.channel_rudder.servo_out = g.channel_roll.servo_out; } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 866dc07a81..a30056d6f6 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -114,7 +114,8 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) micros(), ahrs.roll, ahrs.pitch - radians(g.pitch_trim*0.01), - ahrs.yaw, + ToRad(ground_course/100.0), + //ahrs.yaw, omega.x, omega.y, omega.z); @@ -317,9 +318,7 @@ static void NOINLINE send_location(mavlink_channel_t chan) g_gps->ground_speed * rot.a.x, // X speed cm/s g_gps->ground_speed * rot.b.x, // Y speed cm/s g_gps->ground_speed * rot.c.x, - //g_gps->ground_course); // course in 1/100 degree - ground_course); // course in 1/100 degree - + g_gps->ground_course); // course in 1/100 degree } static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) @@ -333,7 +332,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) target_bearing / 100, wp_distance, altitude_error / 1.0e2, - airspeed_error, + groundspeed_error, crosstrack_error); } @@ -357,8 +356,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan) g_gps->hdop, 65535, g_gps->ground_speed, // cm/s - //g_gps->ground_course, // 1/100 degrees, - ground_course, // 1/100 degrees, + g_gps->ground_course, // 1/100 degrees, g_gps->num_sats); #else // MAVLINK10 @@ -372,9 +370,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan) g_gps->hdop, 0.0, g_gps->ground_speed / 100.0, - //ground_course = (wrap_360(ToDeg(compass.heading) * 100)); - //g_gps->ground_course / 100.0); - ground_course / 100.0); + g_gps->ground_course / 100.0); #endif // MAVLINK10 } @@ -484,13 +480,13 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan) static void NOINLINE send_raw_imu2(mavlink_channel_t chan) { - int32_t pressure = barometer.get_pressure(); + int32_t pressure = 0; mavlink_msg_scaled_pressure_send( chan, micros(), pressure/100.0, (pressure - g.ground_pressure)/100.0, - barometer.get_temperature()); + 0); } static void NOINLINE send_raw_imu3(mavlink_channel_t chan) @@ -502,8 +498,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan) mag_offsets.y, mag_offsets.z, compass.get_declination(), - barometer.get_raw_pressure(), - barometer.get_raw_temp(), + 0, + 0, imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az()); } @@ -1048,7 +1044,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param1 == 1 || packet.param2 == 1 || packet.param3 == 1) { - startup_IMU_ground(false); } if (packet.param4 == 1) { trim_radio(); @@ -1153,7 +1148,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: case MAV_ACTION_REBOOT: // this is a rough interpretation - startup_IMU_ground(true); result=1; break; diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index b8e7f92800..c563c599f9 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -1,5 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - +#if LITE == DISABLED #if LOGGING_ENABLED == ENABLED // Code to Write and Read packets from DataFlash log memory @@ -694,3 +694,4 @@ static void Log_Write_Raw() {} #endif // LOGGING_ENABLED +#endif diff --git a/APMrover2/config.h b/APMrover2/config.h index 85514f3258..9277dbb9fa 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -50,6 +50,10 @@ # define CONFIG_APM_HARDWARE APM_HARDWARE_APM1 #endif +#ifndef LITE +# define LITE DISABLED +#endif + #if defined( __AVR_ATmega1280__ ) #define CLI_ENABLED DISABLED #define LOGGING_ENABLED DISABLED @@ -828,6 +832,10 @@ # define HIL_SERVOS DISABLED #endif +#ifndef TRACE +# define TRACE DISABLED +#endif + // use this to completely disable the CLI #ifndef CLI_ENABLED # define CLI_ENABLED ENABLED diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index 6cece61d64..bae3fe7188 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -77,31 +77,10 @@ static void calc_gndspeed_undershoot() static void calc_bearing_error() { - if(takeoff_complete == true || g.compass_enabled == true) { - /* - most of the time we use the yaw sensor for heading, even if - we don't have a compass. The yaw sensor is drift corrected - in the DCM library. We only use the gps ground course - directly if we haven't completed takeoff, as the yaw drift - correction won't have had a chance to kick in. Drift - correction using the GPS typically takes 10 seconds or so - for a 180 degree correction. - */ - bearing_error = nav_bearing - ahrs.yaw_sensor; - } else { - - // TODO: we need to use the Yaw gyro for in between GPS reads, - // maybe as an offset from a saved gryo value. - bearing_error = nav_bearing - ground_course; - } - + bearing_error = nav_bearing - ground_course; bearing_error = wrap_180(bearing_error); } -static void calc_altitude_error() -{ -} - static long wrap_360(long error) { if (error > 36000) error -= 36000; diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index 23c972de73..5442ebe7db 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -86,27 +86,8 @@ static void read_radio() g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4)); - - #if FLAPERON == ENABLED - // JLN update for true flaperons - if (control_mode == MANUAL) { - g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); - g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); - } else { - aileron1 = g.rc_5.radio_trim + (BOOL_TO_SIGN(-g.rc_5.get_reverse()) *g.channel_roll.angle_to_pwm()); - aileron2 = g.rc_6.radio_trim + (BOOL_TO_SIGN(-g.rc_6.get_reverse()) *g.channel_roll.angle_to_pwm()); - - aileron1 = constrain(aileron1,(uint16_t)g.rc_5.radio_min,(uint16_t)g.rc_5.radio_max); - aileron2 = constrain(aileron2,(uint16_t)g.rc_6.radio_min,(uint16_t)g.rc_6.radio_max); - - g.rc_5.set_pwm(aileron1); - g.rc_6.set_pwm(aileron2); - } - #else g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); - #endif - g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 2df661650b..1eeafe2e3c 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -1,5 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - +#if LITE == DISABLED // Sensors are not available in HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE @@ -83,6 +83,7 @@ static void zero_airspeed(void) } #endif // HIL_MODE != HIL_MODE_ATTITUDE +#endif static void read_battery(void) { diff --git a/APMrover2/system.pde b/APMrover2/system.pde index a61f6a9bb8..beb85c23ef 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -9,7 +9,9 @@ The init_ardupilot function processes everything we need for an in - air restart #if CLI_ENABLED == ENABLED // Functions called from the top-level menu +#if LITE == DISABLED static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +#endif static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde @@ -33,7 +35,9 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) static const struct Menu::command main_menu_commands[] PROGMEM = { // command function called // ======= =============== +#if LITE == DISABLED {"logs", process_logs}, +#endif {"setup", setup_mode}, {"test", test_mode}, {"help", main_menu_help}, @@ -155,6 +159,7 @@ static void init_ardupilot() mavlink_system.sysid = g.sysid_this_mav; +#if LITE == DISABLED #if LOGGING_ENABLED == ENABLED DataFlash.Init(); // DataFlash log initialization if (!DataFlash.CardInserted()) { @@ -168,6 +173,7 @@ static void init_ardupilot() DataFlash.start_new_log(); } #endif +#endif #if HIL_MODE != HIL_MODE_ATTITUDE @@ -175,6 +181,7 @@ static void init_ardupilot() adc.Init(&timer_scheduler); // APM ADC library initialization #endif +#if LITE == DISABLED barometer.init(&timer_scheduler); if (g.compass_enabled==true) { @@ -189,7 +196,7 @@ static void init_ardupilot() } } #endif - +#endif // Do GPS init g_gps = &g_gps_driver; g_gps->init(); // GPS Initialization @@ -257,9 +264,10 @@ static void init_ardupilot() startup_ground(); +#if LITE == DISABLED if (g.log_bitmask & MASK_LOG_CMD) Log_Write_Startup(TYPE_GROUNDSTART_MSG); - +#endif set_mode(MANUAL); // set the correct flight mode @@ -286,12 +294,13 @@ static void startup_ground(void) // ----------------------- demo_servos(1); +#if LITE == DISABLED //IMU ground start //------------------------ // startup_IMU_ground(false); - +#endif // read the radio to set trims // --------------------------- trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. @@ -370,8 +379,11 @@ static void set_mode(byte mode) break; } +#if LITE == DISABLED if (g.log_bitmask & MASK_LOG_MODE) Log_Write_Mode(control_mode); +#endif + } static void check_long_failsafe() @@ -413,7 +425,7 @@ static void check_short_failsafe() } } - +#if LITE == DISABLED static void startup_IMU_ground(bool force_accel_level) { #if HIL_MODE != HIL_MODE_ATTITUDE @@ -455,7 +467,7 @@ static void startup_IMU_ground(bool force_accel_level) digitalWrite(A_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF); } - +#endif static void update_GPS_light(void) {