Plane: remove CLI

This commit is contained in:
Peter Barker 2017-08-11 14:31:18 +10:00 committed by Randy Mackay
parent c6b9c84d1f
commit 1a665280e9
9 changed files with 12 additions and 416 deletions

View File

@ -96,8 +96,6 @@ void Plane::stats_update(void)
void Plane::setup() void Plane::setup()
{ {
cliSerial = hal.console;
// load the default values of variables listed in var_info[] // load the default values of variables listed in var_info[]
AP_Param::setup_sketch_defaults(); AP_Param::setup_sketch_defaults();

View File

@ -1,19 +1,6 @@
#include "GCS_Plane.h" #include "GCS_Plane.h"
#include "Plane.h" #include "Plane.h"
bool GCS_Plane::cli_enabled() const
{
#if CLI_ENABLED == ENABLED
return plane.g.cli_enabled;
#else
return false;
#endif
}
AP_HAL::BetterStream* GCS_Plane::cliSerial() {
return plane.cliSerial;
}
void GCS_Plane::send_airspeed_calibration(const Vector3f &vg) void GCS_Plane::send_airspeed_calibration(const Vector3f &vg)
{ {
for (uint8_t i=0; i<num_gcs(); i++) { for (uint8_t i=0; i<num_gcs(); i++) {

View File

@ -2,7 +2,6 @@
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include "GCS_Mavlink.h" #include "GCS_Mavlink.h"
#include "config.h" // for CLI_ENABLED
class GCS_Plane : public GCS class GCS_Plane : public GCS
{ {
@ -27,7 +26,4 @@ private:
GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS]; GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS];
bool cli_enabled() const override;
AP_HAL::BetterStream* cliSerial() override;
}; };

View File

@ -3,157 +3,6 @@
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
#if CLI_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash.log memory
// Code to interact with the user to dump or erase logs
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
static const struct Menu::command log_menu_commands[] = {
{"dump", MENU_FUNC(dump_log)},
{"erase", MENU_FUNC(erase_logs)},
{"enable", MENU_FUNC(select_logs)},
{"disable", MENU_FUNC(select_logs)}
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&plane, &Plane::print_log_menu, bool));
bool Plane::print_log_menu(void)
{
cliSerial->printf("logs enabled: \n");
if (0 == g.log_bitmask) {
cliSerial->printf("none\n");
}else{
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", # _s)
PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED);
PLOG(GPS);
PLOG(PM);
PLOG(CTUN);
PLOG(NTUN);
PLOG(MODE);
PLOG(IMU);
PLOG(CMD);
PLOG(CURRENT);
PLOG(COMPASS);
PLOG(TECS);
PLOG(CAMERA);
PLOG(RC);
PLOG(SONAR);
#undef PLOG
}
cliSerial->printf("\n");
DataFlash.ListAvailableLogs(cliSerial);
return(true);
}
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv)
{
int16_t dump_log_num;
uint16_t dump_log_start;
uint16_t dump_log_end;
// check that the requested log number can be read
dump_log_num = argv[1].i;
if (dump_log_num == -2) {
DataFlash.DumpPageInfo(cliSerial);
return(-1);
} else if (dump_log_num <= 0) {
cliSerial->printf("dumping all\n");
Log_Read(0, 1, 0);
return(-1);
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
cliSerial->printf("bad log number\n");
return(-1);
}
DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
return 0;
}
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv)
{
DataFlash.EnableWrites(false);
do_erase_logs();
DataFlash.EnableWrites(true);
return 0;
}
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv)
{
uint32_t bits;
if (argc != 2) {
cliSerial->printf("missing log type\n");
return(-1);
}
bits = 0;
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// that name as the argument to the command, and set the bit in
// bits accordingly.
//
if (!strcasecmp(argv[1].str, "all")) {
bits = 0xFFFFFFFFUL;
} else {
#define TARG(_s) if (!strcasecmp(argv[1].str, # _s)) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST);
TARG(ATTITUDE_MED);
TARG(GPS);
TARG(PM);
TARG(CTUN);
TARG(NTUN);
TARG(MODE);
TARG(IMU);
TARG(CMD);
TARG(CURRENT);
TARG(COMPASS);
TARG(TECS);
TARG(CAMERA);
TARG(RC);
TARG(SONAR);
#undef TARG
}
if (!strcasecmp(argv[0].str, "enable")) {
g.log_bitmask.set_and_save(g.log_bitmask | bits);
}else{
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
}
return(0);
}
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
#endif // CLI_ENABLED == ENABLED
void Plane::do_erase_logs(void)
{
gcs().send_text(MAV_SEVERITY_INFO, "Erasing logs");
DataFlash.EraseAll();
gcs().send_text(MAV_SEVERITY_INFO, "Log erase complete");
}
// Write an attitude packet // Write an attitude packet
void Plane::Log_Write_Attitude(void) void Plane::Log_Write_Attitude(void)
{ {
@ -561,22 +410,6 @@ const struct LogStructure Plane::log_structure[] = {
"AETR", "Qhhhhh", "TimeUS,Ail,Elev,Thr,Rudd,Flap" }, \ "AETR", "Qhhhhh", "TimeUS,Ail,Elev,Thr,Rudd,Flap" }, \
}; };
#if CLI_ENABLED == ENABLED
// Read the DataFlash.log memory : Packet Parser
void Plane::Log_Read(uint16_t list_entry, int16_t start_page, int16_t end_page)
{
cliSerial->printf("\n" FIRMWARE_STRING
"\nFree RAM: %u\n",
(unsigned)hal.util->available_memory());
cliSerial->printf("%s\n", HAL_BOARD_NAME);
DataFlash.LogReadProcess(list_entry, start_page, end_page,
FUNCTOR_BIND_MEMBER(&Plane::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t),
cliSerial);
}
#endif // CLI_ENABLED
void Plane::Log_Write_Vehicle_Startup_Messages() void Plane::Log_Write_Vehicle_Startup_Messages()
{ {
// only 200(?) bytes are guaranteed by DataFlash // only 200(?) bytes are guaranteed by DataFlash
@ -593,21 +426,10 @@ void Plane::Log_Write_Vehicle_Startup_Messages()
void Plane::log_init(void) void Plane::log_init(void)
{ {
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure)); DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
gcs().reset_cli_timeout();
} }
#else // LOGGING_ENABLED #else // LOGGING_ENABLED
#if CLI_ENABLED == ENABLED
bool Plane::print_log_menu(void) { return true; }
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
#endif // CLI_ENABLED == ENABLED
void Plane::do_erase_logs(void) {}
void Plane::Log_Write_Attitude(void) {} void Plane::Log_Write_Attitude(void) {}
void Plane::Log_Write_Fast(void) {} void Plane::Log_Write_Fast(void) {}
void Plane::Log_Write_Performance() {} void Plane::Log_Write_Performance() {}
@ -630,10 +452,6 @@ void Plane::Log_Write_Baro(void) {}
void Plane::Log_Write_Airspeed(void) {} void Plane::Log_Write_Airspeed(void) {}
void Plane::Log_Write_Home_And_Origin() {} void Plane::Log_Write_Home_And_Origin() {}
#if CLI_ENABLED == ENABLED
void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page) {}
#endif // CLI_ENABLED
void Plane::log_init(void) {} void Plane::log_init(void) {}
#endif // LOGGING_ENABLED #endif // LOGGING_ENABLED

View File

@ -39,15 +39,6 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Advanced // @User: Advanced
GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
#if CLI_ENABLED == ENABLED
// @Param: CLI_ENABLED
// @DisplayName: CLI Enable
// @Description: This enables/disables the checking for three carriage returns on telemetry links on startup to enter the diagnostics command line interface
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
GSCALAR(cli_enabled, "CLI_ENABLED", 0),
#endif
// @Group: SERIAL // @Group: SERIAL
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
GOBJECT(serial_manager, "SERIAL", AP_SerialManager), GOBJECT(serial_manager, "SERIAL", AP_SerialManager),
@ -1273,19 +1264,19 @@ const AP_Param::ConversionInfo conversion_table[] = {
void Plane::load_parameters(void) void Plane::load_parameters(void)
{ {
if (!AP_Param::check_var_info()) { if (!AP_Param::check_var_info()) {
cliSerial->printf("Bad parameter table\n"); hal.console->printf("Bad parameter table\n");
AP_HAL::panic("Bad parameter table"); AP_HAL::panic("Bad parameter table");
} }
if (!g.format_version.load() || if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) { g.format_version != Parameters::k_format_version) {
// erase all parameters // erase all parameters
cliSerial->printf("Firmware change: erasing EEPROM...\n"); hal.console->printf("Firmware change: erasing EEPROM...\n");
AP_Param::erase_all(); AP_Param::erase_all();
// save the current format version // save the current format version
g.format_version.set_and_save(Parameters::k_format_version); g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->printf("done.\n"); hal.console->printf("done.\n");
} }
uint32_t before = micros(); uint32_t before = micros();
@ -1319,7 +1310,7 @@ void Plane::load_parameters(void)
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_PLANE); AP_Param::set_frame_type_flags(AP_PARAM_FRAME_PLANE);
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
} }
/* /*
@ -1352,7 +1343,7 @@ void Plane::convert_mixers(void)
chan4->get_function() == SRV_Channel::k_rudder && chan4->get_function() == SRV_Channel::k_rudder &&
!chan2->function_configured() && !chan2->function_configured() &&
!chan4->function_configured()) { !chan4->function_configured()) {
cliSerial->printf("Converting vtail_output %u\n", vtail_output.get()); hal.console->printf("Converting vtail_output %u\n", vtail_output.get());
switch (vtail_output) { switch (vtail_output) {
case MIXING_UPUP: case MIXING_UPUP:
case MIXING_UPUP_SWP: case MIXING_UPUP_SWP:
@ -1388,7 +1379,7 @@ void Plane::convert_mixers(void)
chan2->get_function() == SRV_Channel::k_elevator && chan2->get_function() == SRV_Channel::k_elevator &&
!chan1->function_configured() && !chan1->function_configured() &&
!chan2->function_configured()) { !chan2->function_configured()) {
cliSerial->printf("convert elevon_output %u\n", elevon_output.get()); hal.console->printf("convert elevon_output %u\n", elevon_output.get());
switch (elevon_output) { switch (elevon_output) {
case MIXING_UPUP: case MIXING_UPUP:
case MIXING_UPUP_SWP: case MIXING_UPUP_SWP:

View File

@ -134,7 +134,7 @@ public:
k_param_override_channel, k_param_override_channel,
k_param_stall_prevention, k_param_stall_prevention,
k_param_optflow, k_param_optflow,
k_param_cli_enabled, k_param_cli_enabled_old, // unused - CLI removed
k_param_trim_rc_at_start, k_param_trim_rc_at_start,
k_param_hil_mode, k_param_hil_mode,
k_param_land_disarm_delay, // unused - moved to AP_Landing k_param_land_disarm_delay, // unused - moved to AP_Landing
@ -361,9 +361,6 @@ public:
AP_Int16 sysid_this_mav; AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs; AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay; AP_Int8 telem_delay;
#if CLI_ENABLED == ENABLED
AP_Int8 cli_enabled;
#endif
AP_Float hil_err_limit; AP_Float hil_err_limit;

View File

@ -158,7 +158,6 @@ public:
private: private:
// key aircraft parameters passed to multiple libraries // key aircraft parameters passed to multiple libraries
AP_Vehicle::FixedWing aparm; AP_Vehicle::FixedWing aparm;
AP_HAL::BetterStream* cliSerial;
// Global parameters are all contained within the 'g' and 'g2' classes. // Global parameters are all contained within the 'g' and 'g2' classes.
Parameters g; Parameters g;
@ -824,7 +823,6 @@ private:
void gcs_send_airspeed_calibration(const Vector3f &vg); void gcs_send_airspeed_calibration(const Vector3f &vg);
void gcs_retry_deferred(void); void gcs_retry_deferred(void);
void do_erase_logs(void);
void Log_Write_Fast(void); void Log_Write_Fast(void);
void Log_Write_Attitude(void); void Log_Write_Attitude(void);
void Log_Write_Performance(); void Log_Write_Performance();
@ -845,7 +843,6 @@ private:
void Log_Write_Vehicle_Startup_Messages(); void Log_Write_Vehicle_Startup_Messages();
void Log_Write_AOA_SSA(); void Log_Write_AOA_SSA();
void Log_Write_AETR(); void Log_Write_AETR();
void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page);
void load_parameters(void); void load_parameters(void);
void convert_mixers(void); void convert_mixers(void);
@ -962,17 +959,6 @@ private:
void button_update(void); void button_update(void);
void stats_update(); void stats_update();
void ice_update(void); void ice_update(void);
void report_radio();
void report_ins();
void report_compass();
void print_radio_values();
void print_done();
void print_blanks(int16_t num);
void print_divider(void);
void zero_eeprom(void);
void print_enabled(bool b);
void print_accel_offsets_and_scaling(void);
void print_gyro_offsets(void);
void init_ardupilot(); void init_ardupilot();
void startup_ground(void); void startup_ground(void);
enum FlightMode get_previous_mode(); enum FlightMode get_previous_mode();
@ -984,7 +970,6 @@ private:
void startup_INS_ground(void); void startup_INS_ground(void);
void update_notify(); void update_notify();
void resetPerfData(void); void resetPerfData(void);
void print_comma(void);
bool should_log(uint32_t mask); bool should_log(uint32_t mask);
int8_t throttle_percentage(void); int8_t throttle_percentage(void);
void change_arm_state(void); void change_arm_state(void);
@ -996,7 +981,6 @@ private:
int8_t takeoff_tail_hold(void); int8_t takeoff_tail_hold(void);
int16_t get_takeoff_pitch_min_cd(void); int16_t get_takeoff_pitch_min_cd(void);
void complete_auto_takeoff(void); void complete_auto_takeoff(void);
void print_hit_enter();
void ahrs_update(); void ahrs_update();
void update_speed_height(void); void update_speed_height(void);
void update_GPS_50Hz(void); void update_GPS_50Hz(void);
@ -1088,9 +1072,7 @@ private:
void do_digicam_control(const AP_Mission::Mission_Command& cmd); void do_digicam_control(const AP_Mission::Mission_Command& cmd);
bool start_command_callback(const AP_Mission::Mission_Command &cmd); bool start_command_callback(const AP_Mission::Mission_Command &cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void notify_flight_mode(enum FlightMode mode); void notify_flight_mode(enum FlightMode mode);
void run_cli(AP_HAL::UARTDriver *port);
void log_init(); void log_init();
void init_capabilities(void); void init_capabilities(void);
void dataflash_periodic(void); void dataflash_periodic(void);
@ -1111,36 +1093,8 @@ private:
public: public:
void mavlink_delay_cb(); void mavlink_delay_cb();
void failsafe_check(void); void failsafe_check(void);
bool print_log_menu(void);
int8_t dump_log(uint8_t argc, const Menu::arg *argv);
int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
int8_t select_logs(uint8_t argc, const Menu::arg *argv);
int8_t process_logs(uint8_t argc, const Menu::arg *argv);
int8_t setup_mode(uint8_t argc, const Menu::arg *argv);
int8_t setup_factory(uint8_t argc, const Menu::arg *argv);
int8_t setup_erase(uint8_t argc, const Menu::arg *argv);
int8_t test_mode(uint8_t argc, const Menu::arg *argv);
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
int8_t main_menu_help(uint8_t argc, const Menu::arg *argv);
int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
int8_t test_radio(uint8_t argc, const Menu::arg *argv);
int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
int8_t test_relay(uint8_t argc, const Menu::arg *argv);
int8_t test_wp(uint8_t argc, const Menu::arg *argv);
void test_wp_print(const AP_Mission::Mission_Command& cmd);
int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
int8_t test_logging(uint8_t argc, const Menu::arg *argv);
int8_t test_gps(uint8_t argc, const Menu::arg *argv);
int8_t test_ins(uint8_t argc, const Menu::arg *argv);
int8_t test_mag(uint8_t argc, const Menu::arg *argv);
int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
}; };
#define MENU_FUNC(func) FUNCTOR_BIND(&plane, &Plane::func, int8_t, uint8_t, const Menu::arg *)
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
extern Plane plane; extern Plane plane;

View File

@ -347,12 +347,6 @@
# define SCALING_SPEED 15.0 # define SCALING_SPEED 15.0
#endif #endif
// use this to completely disable the CLI. We now default the CLI to
// off on smaller boards.
#ifndef CLI_ENABLED
#define CLI_ENABLED ENABLED
#endif
// use this to disable geo-fencing // use this to disable geo-fencing
#ifndef GEOFENCE_ENABLED #ifndef GEOFENCE_ENABLED
# define GEOFENCE_ENABLED ENABLED # define GEOFENCE_ENABLED ENABLED

View File

@ -8,61 +8,6 @@
* *
*****************************************************************************/ *****************************************************************************/
#if CLI_ENABLED == ENABLED
// This is the help function
int8_t Plane::main_menu_help(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Commands:\n"
" logs log readback/setup mode\n"
" setup setup mode\n"
" test test mode\n"
" reboot reboot to flight mode\n"
"\n");
return(0);
}
// Command/function table for the top-level menu.
static const struct Menu::command main_menu_commands[] = {
// command function called
// ======= ===============
{"logs", MENU_FUNC(process_logs)},
{"setup", MENU_FUNC(setup_mode)},
{"test", MENU_FUNC(test_mode)},
{"reboot", MENU_FUNC(reboot_board)},
{"help", MENU_FUNC(main_menu_help)},
};
// Create the top-level menu object.
MENU(main_menu, THISFIRMWARE, main_menu_commands);
int8_t Plane::reboot_board(uint8_t argc, const Menu::arg *argv)
{
hal.scheduler->reboot(false);
return 0;
}
// the user wants the CLI. It never exits
void Plane::run_cli(AP_HAL::UARTDriver *port)
{
// disable the failsafe code in the CLI
hal.scheduler->register_timer_failsafe(nullptr,1);
// disable the mavlink delay callback
hal.scheduler->register_delay_callback(nullptr, 5);
cliSerial = port;
Menu::set_port(port);
port->set_blocking_writes(true);
while (1) {
main_menu.run();
}
}
#endif // CLI_ENABLED
static void mavlink_delay_cb_static() static void mavlink_delay_cb_static()
{ {
plane.mavlink_delay_cb(); plane.mavlink_delay_cb();
@ -78,9 +23,9 @@ void Plane::init_ardupilot()
// initialise serial port // initialise serial port
serial_manager.init_console(); serial_manager.init_console();
cliSerial->printf("\n\nInit " FIRMWARE_STRING hal.console->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n", "\n\nFree RAM: %u\n",
(unsigned)hal.util->available_memory()); (unsigned)hal.util->available_memory());
// //
@ -127,13 +72,6 @@ void Plane::init_ardupilot()
gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
// specify callback function for CLI menu system
#if CLI_ENABLED == ENABLED
if (g.cli_enabled) {
gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Plane::run_cli, void, AP_HAL::UARTDriver *));
}
#endif
// Register mavlink_delay_cb, which will run anytime you have // Register mavlink_delay_cb, which will run anytime you have
// 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);
@ -159,7 +97,7 @@ void Plane::init_ardupilot()
// used to detect in-flight resets // used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1); g.num_resets.set_and_save(g.num_resets+1);
// init baro before we start the GCS, so that the CLI baro test works // init baro
barometer.init(); barometer.init();
// initialise rangefinder // initialise rangefinder
@ -196,7 +134,7 @@ void Plane::init_ardupilot()
} }
#endif #endif
if (!compass_ok) { if (!compass_ok) {
cliSerial->printf("Compass initialisation failed!\n"); hal.console->printf("Compass initialisation failed!\n");
g.compass_enabled = false; g.compass_enabled = false;
} else { } else {
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
@ -235,10 +173,6 @@ void Plane::init_ardupilot()
*/ */
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
gcs().handle_interactive_setup();
#endif // CLI_ENABLED
init_capabilities(); init_capabilities();
quadplane.setup(); quadplane.setup();
@ -686,72 +620,6 @@ void Plane::resetPerfData(void)
} }
void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case MANUAL:
port->printf("Manual");
break;
case CIRCLE:
port->printf("Circle");
break;
case STABILIZE:
port->printf("Stabilize");
break;
case TRAINING:
port->printf("Training");
break;
case ACRO:
port->printf("ACRO");
break;
case FLY_BY_WIRE_A:
port->printf("FBW_A");
break;
case AUTOTUNE:
port->printf("AUTOTUNE");
break;
case FLY_BY_WIRE_B:
port->printf("FBW_B");
break;
case CRUISE:
port->printf("CRUISE");
break;
case AUTO:
port->printf("AUTO");
break;
case RTL:
port->printf("RTL");
break;
case LOITER:
port->printf("Loiter");
break;
case AVOID_ADSB:
port->printf("AVOID_ADSB");
break;
case GUIDED:
port->printf("Guided");
break;
case QSTABILIZE:
port->printf("QStabilize");
break;
case QHOVER:
port->printf("QHover");
break;
case QLOITER:
port->printf("QLoiter");
break;
case QLAND:
port->printf("QLand");
break;
case QRTL:
port->printf("QRTL");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;
}
}
// sets notify object flight mode information // sets notify object flight mode information
void Plane::notify_flight_mode(enum FlightMode mode) void Plane::notify_flight_mode(enum FlightMode mode)
{ {
@ -825,13 +693,6 @@ void Plane::notify_flight_mode(enum FlightMode mode)
} }
} }
#if CLI_ENABLED == ENABLED
void Plane::print_comma(void)
{
cliSerial->printf(",");
}
#endif
/* /*
should we log a message type now? should we log a message type now?
*/ */