mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
APMrover2: fix indentation leading to compiler warning
GCC 6 has a new warning about misleading indentation: ../../APMrover2/system.cpp: In member function ‘void Rover::set_mode(mode)’: ../../APMrover2/system.cpp:272:5: warning: this ‘if’ clause does not guard... [-Wmisleading-indentation] if (control_mode == AUTO) ^~ ../../APMrover2/system.cpp:275:2: note: ...this statement, but the^Bn latter is misleadingly indented as if it is guarded by the ‘if’ control_mode = mode; ^~~~~~~~~~~~ The issue here is that we are mixing tabs and spaces. Remove tabs and re-indent the code.
This commit is contained in:
parent
7af888633d
commit
368a72044c
@ -1,8 +1,8 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
/*****************************************************************************
|
/*****************************************************************************
|
||||||
The init_ardupilot function processes everything we need for an in - air restart
|
The init_ardupilot function processes everything we need for an in - air restart
|
||||||
We will determine later if we are actually on the ground and process a
|
We will determine later if we are actually on the ground and process a
|
||||||
ground start in that case.
|
ground start in that case.
|
||||||
|
|
||||||
*****************************************************************************/
|
*****************************************************************************/
|
||||||
|
|
||||||
@ -14,26 +14,26 @@ The init_ardupilot function processes everything we need for an in - air restart
|
|||||||
// This is the help function
|
// This is the help function
|
||||||
int8_t Rover::main_menu_help(uint8_t argc, const Menu::arg *argv)
|
int8_t Rover::main_menu_help(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
cliSerial->printf("Commands:\n"
|
cliSerial->printf("Commands:\n"
|
||||||
" logs log readback/setup mode\n"
|
" logs log readback/setup mode\n"
|
||||||
" setup setup mode\n"
|
" setup setup mode\n"
|
||||||
" test test mode\n"
|
" test test mode\n"
|
||||||
"\n"
|
"\n"
|
||||||
"Move the slide switch and reset to FLY.\n"
|
"Move the slide switch and reset to FLY.\n"
|
||||||
"\n");
|
"\n");
|
||||||
return(0);
|
return(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Command/function table for the top-level menu.
|
// Command/function table for the top-level menu.
|
||||||
|
|
||||||
static const struct Menu::command main_menu_commands[] = {
|
static const struct Menu::command main_menu_commands[] = {
|
||||||
// command function called
|
// command function called
|
||||||
// ======= ===============
|
// ======= ===============
|
||||||
{"logs", MENU_FUNC(process_logs)},
|
{"logs", MENU_FUNC(process_logs)},
|
||||||
{"setup", MENU_FUNC(setup_mode)},
|
{"setup", MENU_FUNC(setup_mode)},
|
||||||
{"test", MENU_FUNC(test_mode)},
|
{"test", MENU_FUNC(test_mode)},
|
||||||
{"reboot", MENU_FUNC(reboot_board)},
|
{"reboot", MENU_FUNC(reboot_board)},
|
||||||
{"help", MENU_FUNC(main_menu_help)}
|
{"help", MENU_FUNC(main_menu_help)}
|
||||||
};
|
};
|
||||||
|
|
||||||
// Create the top-level menu object.
|
// Create the top-level menu object.
|
||||||
@ -80,14 +80,14 @@ void Rover::init_ardupilot()
|
|||||||
// initialise console serial port
|
// initialise console serial port
|
||||||
serial_manager.init_console();
|
serial_manager.init_console();
|
||||||
|
|
||||||
cliSerial->printf("\n\nInit " FIRMWARE_STRING
|
cliSerial->printf("\n\nInit " FIRMWARE_STRING
|
||||||
"\n\nFree RAM: %u\n",
|
"\n\nFree RAM: %u\n",
|
||||||
hal.util->available_memory());
|
hal.util->available_memory());
|
||||||
|
|
||||||
//
|
//
|
||||||
// Check the EEPROM format version before loading any parameters from EEPROM.
|
// Check the EEPROM format version before loading any parameters from EEPROM.
|
||||||
//
|
//
|
||||||
|
|
||||||
load_parameters();
|
load_parameters();
|
||||||
|
|
||||||
BoardConfig.init();
|
BoardConfig.init();
|
||||||
@ -108,11 +108,11 @@ void Rover::init_ardupilot()
|
|||||||
// init baro before we start the GCS, so that the CLI baro test works
|
// init baro before we start the GCS, so that the CLI baro test works
|
||||||
barometer.init();
|
barometer.init();
|
||||||
|
|
||||||
// init the GCS
|
// init the GCS
|
||||||
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
|
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
|
||||||
|
|
||||||
// we start by assuming USB connected, as we initialed the serial
|
// we start by assuming USB connected, as we initialed the serial
|
||||||
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
|
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
|
||||||
usb_connected = true;
|
usb_connected = true;
|
||||||
check_usb_mux();
|
check_usb_mux();
|
||||||
|
|
||||||
@ -130,7 +130,7 @@ void Rover::init_ardupilot()
|
|||||||
frsky_telemetry.init(serial_manager);
|
frsky_telemetry.init(serial_manager);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
mavlink_system.sysid = g.sysid_this_mav;
|
mavlink_system.sysid = g.sysid_this_mav;
|
||||||
|
|
||||||
#if LOGGING_ENABLED == ENABLED
|
#if LOGGING_ENABLED == ENABLED
|
||||||
log_init();
|
log_init();
|
||||||
@ -142,29 +142,29 @@ void Rover::init_ardupilot()
|
|||||||
// more than 5ms remaining in your call to hal.scheduler->delay
|
// more than 5ms remaining in your call to hal.scheduler->delay
|
||||||
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
|
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
|
||||||
|
|
||||||
if (g.compass_enabled==true) {
|
if (g.compass_enabled==true) {
|
||||||
if (!compass.init()|| !compass.read()) {
|
if (!compass.init()|| !compass.read()) {
|
||||||
cliSerial->println("Compass initialisation failed!");
|
cliSerial->println("Compass initialisation failed!");
|
||||||
g.compass_enabled = false;
|
g.compass_enabled = false;
|
||||||
} else {
|
} else {
|
||||||
ahrs.set_compass(&compass);
|
ahrs.set_compass(&compass);
|
||||||
//compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
//compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// initialise sonar
|
// initialise sonar
|
||||||
init_sonar();
|
init_sonar();
|
||||||
|
|
||||||
// and baro for EKF
|
// and baro for EKF
|
||||||
init_barometer();
|
init_barometer();
|
||||||
|
|
||||||
// Do GPS init
|
// Do GPS init
|
||||||
gps.init(&DataFlash, serial_manager);
|
gps.init(&DataFlash, serial_manager);
|
||||||
|
|
||||||
rc_override_active = hal.rcin->set_overrides(rc_override, 8);
|
rc_override_active = hal.rcin->set_overrides(rc_override, 8);
|
||||||
|
|
||||||
init_rc_in(); // sets up rc channels from radio
|
init_rc_in(); // sets up rc channels from radio
|
||||||
init_rc_out(); // sets up the timer libs
|
init_rc_out(); // sets up the timer libs
|
||||||
|
|
||||||
relay.init();
|
relay.init();
|
||||||
|
|
||||||
@ -181,12 +181,12 @@ void Rover::init_ardupilot()
|
|||||||
|
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
#if CLI_ENABLED == ENABLED
|
||||||
// If the switch is in 'menu' mode, run the main menu.
|
// If the switch is in 'menu' mode, run the main menu.
|
||||||
//
|
//
|
||||||
// Since we can't be sure that the setup or test mode won't leave
|
// Since we can't be sure that the setup or test mode won't leave
|
||||||
// the system in an odd state, we don't let the user exit the top
|
// the system in an odd state, we don't let the user exit the top
|
||||||
// menu; they must reset in order to fly.
|
// menu; they must reset in order to fly.
|
||||||
//
|
//
|
||||||
if (g.cli_enabled == 1) {
|
if (g.cli_enabled == 1) {
|
||||||
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
|
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
|
||||||
cliSerial->println(msg);
|
cliSerial->println(msg);
|
||||||
@ -199,15 +199,15 @@ void Rover::init_ardupilot()
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
init_capabilities();
|
init_capabilities();
|
||||||
|
|
||||||
startup_ground();
|
startup_ground();
|
||||||
|
|
||||||
set_mode((enum mode)g.initial_mode.get());
|
set_mode((enum mode)g.initial_mode.get());
|
||||||
|
|
||||||
// set the correct flight mode
|
// set the correct flight mode
|
||||||
// ---------------------------
|
// ---------------------------
|
||||||
reset_control_switch();
|
reset_control_switch();
|
||||||
}
|
}
|
||||||
|
|
||||||
//********************************************************************************
|
//********************************************************************************
|
||||||
@ -216,23 +216,23 @@ void Rover::init_ardupilot()
|
|||||||
void Rover::startup_ground(void)
|
void Rover::startup_ground(void)
|
||||||
{
|
{
|
||||||
set_mode(INITIALISING);
|
set_mode(INITIALISING);
|
||||||
|
|
||||||
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
|
|
||||||
|
|
||||||
#if(GROUND_START_DELAY > 0)
|
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
|
||||||
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With delay");
|
|
||||||
delay(GROUND_START_DELAY * 1000);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//IMU ground start
|
#if(GROUND_START_DELAY > 0)
|
||||||
//------------------------
|
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With delay");
|
||||||
|
delay(GROUND_START_DELAY * 1000);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//IMU ground start
|
||||||
|
//------------------------
|
||||||
//
|
//
|
||||||
|
|
||||||
startup_INS_ground();
|
startup_INS_ground();
|
||||||
|
|
||||||
// read the radio to set trims
|
// read the radio to set trims
|
||||||
// ---------------------------
|
// ---------------------------
|
||||||
trim_radio();
|
trim_radio();
|
||||||
|
|
||||||
// initialise mission library
|
// initialise mission library
|
||||||
mission.init();
|
mission.init();
|
||||||
@ -244,7 +244,7 @@ void Rover::startup_ground(void)
|
|||||||
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
|
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
|
||||||
ins.set_dataflash(&DataFlash);
|
ins.set_dataflash(&DataFlash);
|
||||||
|
|
||||||
gcs_send_text(MAV_SEVERITY_INFO,"Ready to drive");
|
gcs_send_text(MAV_SEVERITY_INFO,"Ready to drive");
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -256,23 +256,24 @@ void Rover::set_reverse(bool reverse)
|
|||||||
if (in_reverse == reverse) {
|
if (in_reverse == reverse) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
g.pidSpeedThrottle.reset_I();
|
g.pidSpeedThrottle.reset_I();
|
||||||
in_reverse = reverse;
|
in_reverse = reverse;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Rover::set_mode(enum mode mode)
|
void Rover::set_mode(enum mode mode)
|
||||||
{
|
{
|
||||||
|
|
||||||
if(control_mode == mode){
|
if (control_mode == mode){
|
||||||
// don't switch modes if we are already in the correct mode.
|
// don't switch modes if we are already in the correct mode.
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// If we are changing out of AUTO mode reset the loiter timer
|
// If we are changing out of AUTO mode reset the loiter timer
|
||||||
if (control_mode == AUTO)
|
if (control_mode == AUTO) {
|
||||||
loiter_time = 0;
|
loiter_time = 0;
|
||||||
|
}
|
||||||
|
|
||||||
control_mode = mode;
|
control_mode = mode;
|
||||||
throttle_last = 0;
|
throttle_last = 0;
|
||||||
throttle = 500;
|
throttle = 500;
|
||||||
set_reverse(false);
|
set_reverse(false);
|
||||||
@ -281,45 +282,44 @@ void Rover::set_mode(enum mode mode)
|
|||||||
if (control_mode != AUTO) {
|
if (control_mode != AUTO) {
|
||||||
auto_triggered = false;
|
auto_triggered = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch(control_mode)
|
|
||||||
{
|
|
||||||
case MANUAL:
|
|
||||||
case HOLD:
|
|
||||||
case LEARNING:
|
|
||||||
case STEERING:
|
|
||||||
auto_throttle_mode = false;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case AUTO:
|
switch(control_mode) {
|
||||||
auto_throttle_mode = true;
|
case MANUAL:
|
||||||
rtl_complete = false;
|
case HOLD:
|
||||||
restart_nav();
|
case LEARNING:
|
||||||
break;
|
case STEERING:
|
||||||
|
auto_throttle_mode = false;
|
||||||
|
break;
|
||||||
|
|
||||||
case RTL:
|
case AUTO:
|
||||||
auto_throttle_mode = true;
|
auto_throttle_mode = true;
|
||||||
do_RTL();
|
rtl_complete = false;
|
||||||
break;
|
restart_nav();
|
||||||
|
break;
|
||||||
|
|
||||||
case GUIDED:
|
case RTL:
|
||||||
auto_throttle_mode = true;
|
auto_throttle_mode = true;
|
||||||
rtl_complete = false;
|
do_RTL();
|
||||||
/*
|
break;
|
||||||
when entering guided mode we set the target as the current
|
|
||||||
location. This matches the behaviour of the copter code.
|
|
||||||
*/
|
|
||||||
guided_WP = current_loc;
|
|
||||||
set_guided_WP();
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
case GUIDED:
|
||||||
auto_throttle_mode = true;
|
auto_throttle_mode = true;
|
||||||
do_RTL();
|
rtl_complete = false;
|
||||||
break;
|
/*
|
||||||
}
|
when entering guided mode we set the target as the current
|
||||||
|
location. This matches the behaviour of the copter code.
|
||||||
|
*/
|
||||||
|
guided_WP = current_loc;
|
||||||
|
set_guided_WP();
|
||||||
|
break;
|
||||||
|
|
||||||
if (should_log(MASK_LOG_MODE)) {
|
default:
|
||||||
|
auto_throttle_mode = true;
|
||||||
|
do_RTL();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (should_log(MASK_LOG_MODE)) {
|
||||||
DataFlash.Log_Write_Mode(control_mode);
|
DataFlash.Log_Write_Mode(control_mode);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -344,7 +344,7 @@ bool Rover::mavlink_set_mode(uint8_t mode)
|
|||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
called to set/unset a failsafe event.
|
called to set/unset a failsafe event.
|
||||||
*/
|
*/
|
||||||
void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
|
void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
|
||||||
{
|
{
|
||||||
@ -365,8 +365,8 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
|
|||||||
|
|
||||||
failsafe.triggered &= failsafe.bits;
|
failsafe.triggered &= failsafe.bits;
|
||||||
|
|
||||||
if (failsafe.triggered == 0 &&
|
if (failsafe.triggered == 0 &&
|
||||||
failsafe.bits != 0 &&
|
failsafe.bits != 0 &&
|
||||||
millis() - failsafe.start_time > g.fs_timeout*1000 &&
|
millis() - failsafe.start_time > g.fs_timeout*1000 &&
|
||||||
control_mode != RTL &&
|
control_mode != RTL &&
|
||||||
control_mode != HOLD) {
|
control_mode != HOLD) {
|
||||||
@ -388,15 +388,15 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
|
|||||||
void Rover::startup_INS_ground(void)
|
void Rover::startup_INS_ground(void)
|
||||||
{
|
{
|
||||||
gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC");
|
gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC");
|
||||||
mavlink_delay(500);
|
mavlink_delay(500);
|
||||||
|
|
||||||
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
|
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
|
||||||
// -----------------------
|
// -----------------------
|
||||||
gcs_send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");
|
gcs_send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");
|
||||||
mavlink_delay(1000);
|
mavlink_delay(1000);
|
||||||
|
|
||||||
ahrs.init();
|
ahrs.init();
|
||||||
ahrs.set_fly_forward(true);
|
ahrs.set_fly_forward(true);
|
||||||
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
|
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
|
||||||
|
|
||||||
ins.init(scheduler.get_loop_rate_hz());
|
ins.init(scheduler.get_loop_rate_hz());
|
||||||
@ -411,9 +411,9 @@ void Rover::update_notify()
|
|||||||
}
|
}
|
||||||
|
|
||||||
void Rover::resetPerfData(void) {
|
void Rover::resetPerfData(void) {
|
||||||
mainLoop_count = 0;
|
mainLoop_count = 0;
|
||||||
G_Dt_max = 0;
|
G_Dt_max = 0;
|
||||||
perf_mon_timer = millis();
|
perf_mon_timer = millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user