From 1bb79eb2b531a92eb748128b8c46e9c7c0df8c00 Mon Sep 17 00:00:00 2001
From: Jean-Louis Naudin <jlnaudin@gmail.com>
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 <jlnaudin@gmail.com>
---
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 3a73abca0..be6ea6f9d 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 b32fd97cb..b05e974cd 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 58c4e9f46..ae3bec3ae 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 5a31fd23c..058b3e145 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 866dc07a8..a30056d6f 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 b8e7f9280..c563c599f 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 85514f325..9277dbb9f 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 6cece61d6..bae3fe718 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 23c972de7..5442ebe7d 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 2df661650..1eeafe2e3 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 a61f6a9bb..beb85c23e 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)
{
--
GitLab