mirror of https://github.com/ArduPilot/ardupilot
Sub: Remove deprecated/unused CLI and AP_Menu
This commit is contained in:
Jacob Walser
parent
0e180f88ad
commit
69c9dbc286
|
|
@ -12,7 +12,6 @@
|
||||||
//#define PROXIMITY_ENABLED DISABLED // disable proximity sensors
|
//#define PROXIMITY_ENABLED DISABLED // disable proximity sensors
|
||||||
//#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
|
//#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
|
||||||
#define GRIPPER_ENABLED DISABLED // disable gripper to save 500bytes of flash
|
#define GRIPPER_ENABLED DISABLED // disable gripper to save 500bytes of flash
|
||||||
//#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space
|
|
||||||
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
|
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
|
||||||
|
|
||||||
// features below are disabled by default on all boards
|
// features below are disabled by default on all boards
|
||||||
|
|
|
||||||
|
|
@ -2033,11 +2033,7 @@ void Sub::gcs_check_input(void)
|
||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs; i++) {
|
for (uint8_t i=0; i<num_gcs; i++) {
|
||||||
if (gcs_chan[i].initialised) {
|
if (gcs_chan[i].initialised) {
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
gcs_chan[i].update(g.cli_enabled==1?FUNCTOR_BIND_MEMBER(&Sub::run_cli, void, AP_HAL::UARTDriver *):NULL);
|
|
||||||
#else
|
|
||||||
gcs_chan[i].update(NULL);
|
gcs_chan[i].update(NULL);
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
169
ArduSub/Log.cpp
169
ArduSub/Log.cpp
|
|
@ -6,150 +6,6 @@
|
||||||
// Code to Write and Read packets from DataFlash log memory
|
// Code to Write and Read packets from DataFlash log memory
|
||||||
// Code to interact with the user to dump or erase logs
|
// Code to interact with the user to dump or erase logs
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
// 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(&sub, &Sub::print_log_menu, bool));
|
|
||||||
|
|
||||||
bool Sub::print_log_menu(void)
|
|
||||||
{
|
|
||||||
cliSerial->printf("logs enabled: ");
|
|
||||||
|
|
||||||
if (0 == g.log_bitmask) {
|
|
||||||
cliSerial->printf("none");
|
|
||||||
} 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(RCIN);
|
|
||||||
PLOG(IMU);
|
|
||||||
PLOG(CMD);
|
|
||||||
PLOG(CURRENT);
|
|
||||||
PLOG(RCOUT);
|
|
||||||
PLOG(OPTFLOW);
|
|
||||||
PLOG(COMPASS);
|
|
||||||
PLOG(CAMERA);
|
|
||||||
PLOG(PID);
|
|
||||||
#undef PLOG
|
|
||||||
}
|
|
||||||
|
|
||||||
cliSerial->println();
|
|
||||||
|
|
||||||
DataFlash.ListAvailableLogs(cliSerial);
|
|
||||||
|
|
||||||
return (true);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
int8_t Sub::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);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
int8_t Sub::erase_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
in_mavlink_delay = true;
|
|
||||||
do_erase_logs();
|
|
||||||
in_mavlink_delay = false;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Sub::select_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
uint16_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 = ~0;
|
|
||||||
} 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(RCIN);
|
|
||||||
TARG(IMU);
|
|
||||||
TARG(CMD);
|
|
||||||
TARG(CURRENT);
|
|
||||||
TARG(RCOUT);
|
|
||||||
TARG(OPTFLOW);
|
|
||||||
TARG(COMPASS);
|
|
||||||
TARG(CAMERA);
|
|
||||||
TARG(PID);
|
|
||||||
#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 Sub::process_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
log_menu.run();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
void Sub::do_erase_logs(void)
|
void Sub::do_erase_logs(void)
|
||||||
{
|
{
|
||||||
gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs");
|
gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs");
|
||||||
|
|
@ -610,22 +466,6 @@ const struct LogStructure Sub::log_structure[] = {
|
||||||
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
|
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
|
||||||
};
|
};
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
// Read the DataFlash log memory
|
|
||||||
void Sub::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page)
|
|
||||||
{
|
|
||||||
cliSerial->printf("\n" FIRMWARE_STRING
|
|
||||||
"\nFree RAM: %u\n",
|
|
||||||
(unsigned) hal.util->available_memory());
|
|
||||||
|
|
||||||
cliSerial->println(HAL_BOARD_NAME);
|
|
||||||
|
|
||||||
DataFlash.LogReadProcess(list_entry, start_page, end_page,
|
|
||||||
FUNCTOR_BIND_MEMBER(&Sub::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t),
|
|
||||||
cliSerial);
|
|
||||||
}
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
void Sub::Log_Write_Vehicle_Startup_Messages()
|
void Sub::Log_Write_Vehicle_Startup_Messages()
|
||||||
{
|
{
|
||||||
// only 200(?) bytes are guaranteed by DataFlash
|
// only 200(?) bytes are guaranteed by DataFlash
|
||||||
|
|
@ -667,15 +507,6 @@ void Sub::log_init(void)
|
||||||
|
|
||||||
#else // LOGGING_ENABLED
|
#else // LOGGING_ENABLED
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
bool Sub::print_log_menu(void) { return true; }
|
|
||||||
int8_t Sub::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
int8_t Sub::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
int8_t Sub::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
int8_t Sub::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
void Sub::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) {}
|
|
||||||
#endif // CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
void Sub::do_erase_logs(void) {}
|
void Sub::do_erase_logs(void) {}
|
||||||
void Sub::Log_Write_Current() {}
|
void Sub::Log_Write_Current() {}
|
||||||
void Sub::Log_Write_Nav_Tuning() {}
|
void Sub::Log_Write_Nav_Tuning() {}
|
||||||
|
|
|
||||||
|
|
@ -64,15 +64,6 @@ const AP_Param::Info Sub::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
|
|
||||||
|
|
||||||
// @Param: PILOT_THR_FILT
|
// @Param: PILOT_THR_FILT
|
||||||
// @DisplayName: Throttle filter cutoff
|
// @DisplayName: Throttle filter cutoff
|
||||||
// @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable
|
// @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable
|
||||||
|
|
|
||||||
|
|
@ -68,8 +68,7 @@ public:
|
||||||
k_param_DataFlash, // DataFlash Logging
|
k_param_DataFlash, // DataFlash Logging
|
||||||
k_param_serial_manager, // Serial ports, AP_SerialManager
|
k_param_serial_manager, // Serial ports, AP_SerialManager
|
||||||
k_param_notify, // Notify Library, AP_Notify
|
k_param_notify, // Notify Library, AP_Notify
|
||||||
k_param_cli_enabled, // Old (deprecated) command line interface
|
k_param_arming = 26, // Arming checks
|
||||||
k_param_arming, // Arming checks
|
|
||||||
|
|
||||||
|
|
||||||
// Sensor objects
|
// Sensor objects
|
||||||
|
|
@ -227,9 +226,6 @@ public:
|
||||||
//
|
//
|
||||||
AP_Int16 sysid_this_mav;
|
AP_Int16 sysid_this_mav;
|
||||||
AP_Int16 sysid_my_gcs;
|
AP_Int16 sysid_my_gcs;
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
AP_Int8 cli_enabled;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
AP_Float throttle_filt;
|
AP_Float throttle_filt;
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -30,7 +30,6 @@
|
||||||
// Common dependencies
|
// Common dependencies
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Common/Location.h>
|
#include <AP_Common/Location.h>
|
||||||
#include <AP_Menu/AP_Menu.h>
|
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
#include <StorageManager/StorageManager.h>
|
#include <StorageManager/StorageManager.h>
|
||||||
|
|
||||||
|
|
@ -705,21 +704,6 @@ private:
|
||||||
void terrain_update();
|
void terrain_update();
|
||||||
void terrain_logging();
|
void terrain_logging();
|
||||||
bool terrain_use();
|
bool terrain_use();
|
||||||
void report_batt_monitor();
|
|
||||||
void report_frame();
|
|
||||||
void report_radio();
|
|
||||||
void report_ins();
|
|
||||||
void report_flight_modes();
|
|
||||||
void report_optflow();
|
|
||||||
void print_radio_values();
|
|
||||||
void print_switch(uint8_t p, uint8_t m, bool b);
|
|
||||||
void print_accel_offsets_and_scaling(void);
|
|
||||||
void print_gyro_offsets(void);
|
|
||||||
void report_compass();
|
|
||||||
void print_blanks(int16_t num);
|
|
||||||
void print_divider(void);
|
|
||||||
void print_enabled(bool b);
|
|
||||||
void report_version();
|
|
||||||
void save_trim();
|
void save_trim();
|
||||||
void auto_trim();
|
void auto_trim();
|
||||||
void init_ardupilot();
|
void init_ardupilot();
|
||||||
|
|
@ -731,7 +715,6 @@ private:
|
||||||
void update_auto_armed();
|
void update_auto_armed();
|
||||||
void check_usb_mux(void);
|
void check_usb_mux(void);
|
||||||
bool should_log(uint32_t mask);
|
bool should_log(uint32_t mask);
|
||||||
void print_hit_enter();
|
|
||||||
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
|
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
|
||||||
bool start_command(const AP_Mission::Mission_Command& cmd);
|
bool start_command(const AP_Mission::Mission_Command& cmd);
|
||||||
bool verify_command(const AP_Mission::Mission_Command& cmd);
|
bool verify_command(const AP_Mission::Mission_Command& cmd);
|
||||||
|
|
@ -775,7 +758,6 @@ private:
|
||||||
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
||||||
|
|
||||||
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
|
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
|
||||||
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
|
|
||||||
void log_init(void);
|
void log_init(void);
|
||||||
void run_cli(AP_HAL::UARTDriver *port);
|
void run_cli(AP_HAL::UARTDriver *port);
|
||||||
void init_capabilities(void);
|
void init_capabilities(void);
|
||||||
|
|
@ -802,33 +784,8 @@ private:
|
||||||
public:
|
public:
|
||||||
void mavlink_delay_cb();
|
void mavlink_delay_cb();
|
||||||
void failsafe_check();
|
void failsafe_check();
|
||||||
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);
|
|
||||||
bool print_log_menu(void);
|
|
||||||
|
|
||||||
int8_t process_logs(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t main_menu_help(uint8_t, const Menu::arg*);
|
|
||||||
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_set(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t setup_show(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t esc_calib(uint8_t argc, const Menu::arg *argv);
|
|
||||||
|
|
||||||
int8_t test_mode(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_baro(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_compass(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_ins(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_rangefinder(uint8_t argc, const Menu::arg *argv);
|
|
||||||
|
|
||||||
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#define MENU_FUNC(func) FUNCTOR_BIND(&sub, &Sub::func, int8_t, uint8_t, const Menu::arg *)
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
extern Sub sub;
|
extern Sub sub;
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -381,12 +381,3 @@
|
||||||
#ifndef AC_TERRAIN
|
#ifndef AC_TERRAIN
|
||||||
#define AC_TERRAIN DISABLED // Requires Rally enabled as well
|
#define AC_TERRAIN DISABLED // Requires Rally enabled as well
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
|
||||||
// Developer Items
|
|
||||||
//
|
|
||||||
|
|
||||||
// use this to completely disable the CLI
|
|
||||||
#ifndef CLI_ENABLED
|
|
||||||
# define CLI_ENABLED ENABLED
|
|
||||||
#endif
|
|
||||||
|
|
|
||||||
|
|
@ -223,43 +223,3 @@ void Sub::notify_flight_mode(control_mode_t mode)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
|
||||||
// print_flight_mode - prints flight mode to serial port.
|
|
||||||
//
|
|
||||||
void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
|
||||||
{
|
|
||||||
switch (mode) {
|
|
||||||
case STABILIZE:
|
|
||||||
port->print("STABILIZE");
|
|
||||||
break;
|
|
||||||
case ACRO:
|
|
||||||
port->print("ACRO");
|
|
||||||
break;
|
|
||||||
case ALT_HOLD:
|
|
||||||
port->print("ALT_HOLD");
|
|
||||||
break;
|
|
||||||
case AUTO:
|
|
||||||
port->print("AUTO");
|
|
||||||
break;
|
|
||||||
case GUIDED:
|
|
||||||
port->print("GUIDED");
|
|
||||||
break;
|
|
||||||
case CIRCLE:
|
|
||||||
port->print("CIRCLE");
|
|
||||||
break;
|
|
||||||
case SURFACE:
|
|
||||||
port->print("SURFACE");
|
|
||||||
break;
|
|
||||||
case POSHOLD:
|
|
||||||
port->print("POSHOLD");
|
|
||||||
break;
|
|
||||||
case MANUAL:
|
|
||||||
port->print("MANUAL");
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
port->printf("Mode(%u)", (unsigned)mode);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,4 @@
|
||||||
LIBRARIES += AP_Common
|
LIBRARIES += AP_Common
|
||||||
LIBRARIES += AP_Menu
|
|
||||||
LIBRARIES += AP_Param
|
LIBRARIES += AP_Param
|
||||||
LIBRARIES += StorageManager
|
LIBRARIES += StorageManager
|
||||||
LIBRARIES += GCS
|
LIBRARIES += GCS
|
||||||
|
|
|
||||||
|
|
@ -1,510 +0,0 @@
|
||||||
#include "Sub.h"
|
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
#define PWM_CALIB_MIN 1000
|
|
||||||
#define PWM_CALIB_MAX 2000
|
|
||||||
#define PWM_HIGHEST_MAX 2200
|
|
||||||
#define PWM_LOWEST_MAX 1200
|
|
||||||
#define PWM_HIGHEST_MIN 1800
|
|
||||||
#define PWM_LOWEST_MIN 800
|
|
||||||
|
|
||||||
// Command/function table for the setup menu
|
|
||||||
static const struct Menu::command setup_menu_commands[] = {
|
|
||||||
{"reset", MENU_FUNC(setup_factory)},
|
|
||||||
{"show", MENU_FUNC(setup_show)},
|
|
||||||
{"set", MENU_FUNC(setup_set)},
|
|
||||||
{"esc_calib", MENU_FUNC(esc_calib)},
|
|
||||||
};
|
|
||||||
|
|
||||||
// Create the setup menu object.
|
|
||||||
MENU(setup_menu, "setup", setup_menu_commands);
|
|
||||||
|
|
||||||
// Called from the top-level menu to run the setup menu.
|
|
||||||
int8_t Sub::setup_mode(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
// Give the user some guidance
|
|
||||||
cliSerial->printf("Setup Mode\n\n\n");
|
|
||||||
|
|
||||||
// Run the setup menu. When the menu exits, we will return to the main menu.
|
|
||||||
setup_menu.run();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
|
|
||||||
// Called by the setup menu 'factoryreset' command.
|
|
||||||
int8_t Sub::setup_factory(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
int16_t c;
|
|
||||||
|
|
||||||
cliSerial->printf("\n'Y' = factory reset, any other key to abort:\n");
|
|
||||||
|
|
||||||
do {
|
|
||||||
c = cliSerial->read();
|
|
||||||
} while (-1 == c);
|
|
||||||
|
|
||||||
if (('y' != c) && ('Y' != c)) {
|
|
||||||
return (-1);
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_Param::erase_all();
|
|
||||||
cliSerial->printf("\nReboot board");
|
|
||||||
|
|
||||||
hal.scheduler->delay(1000);
|
|
||||||
|
|
||||||
for (;;) {
|
|
||||||
}
|
|
||||||
// note, cannot actually return here
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Set a parameter to a specified value. It will cast the value to the current type of the
|
|
||||||
//parameter and make sure it fits in case of INT8 and INT16
|
|
||||||
int8_t Sub::setup_set(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
int8_t value_int8;
|
|
||||||
int16_t value_int16;
|
|
||||||
|
|
||||||
AP_Param *param;
|
|
||||||
enum ap_var_type p_type;
|
|
||||||
|
|
||||||
if (argc!=3) {
|
|
||||||
cliSerial->printf("Invalid command. Usage: set <name> <value>\n");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
param = AP_Param::find(argv[1].str, &p_type);
|
|
||||||
if (!param) {
|
|
||||||
cliSerial->printf("Param not found: %s\n", argv[1].str);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (p_type) {
|
|
||||||
case AP_PARAM_INT8:
|
|
||||||
value_int8 = (int8_t)(argv[2].i);
|
|
||||||
if (argv[2].i!=value_int8) {
|
|
||||||
cliSerial->printf("Value out of range for type INT8\n");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
((AP_Int8*)param)->set_and_save(value_int8);
|
|
||||||
break;
|
|
||||||
case AP_PARAM_INT16:
|
|
||||||
value_int16 = (int16_t)(argv[2].i);
|
|
||||||
if (argv[2].i!=value_int16) {
|
|
||||||
cliSerial->printf("Value out of range for type INT16\n");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
((AP_Int16*)param)->set_and_save(value_int16);
|
|
||||||
break;
|
|
||||||
|
|
||||||
//int32 and float don't need bounds checking, just use the value provoded by Menu::arg
|
|
||||||
case AP_PARAM_INT32:
|
|
||||||
((AP_Int32*)param)->set_and_save(argv[2].i);
|
|
||||||
break;
|
|
||||||
case AP_PARAM_FLOAT:
|
|
||||||
((AP_Float*)param)->set_and_save(argv[2].f);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
cliSerial->printf("Cannot set parameter of type %d.\n", p_type);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Print the current configuration.
|
|
||||||
// Called by the setup menu 'show' command.
|
|
||||||
int8_t Sub::setup_show(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
AP_Param *param;
|
|
||||||
ap_var_type type;
|
|
||||||
|
|
||||||
//If a parameter name is given as an argument to show, print only that parameter
|
|
||||||
if (argc>=2) {
|
|
||||||
|
|
||||||
param=AP_Param::find(argv[1].str, &type);
|
|
||||||
|
|
||||||
if (!param) {
|
|
||||||
cliSerial->printf("Parameter not found: '%s'\n", argv[1].str);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
AP_Param::show(param, argv[1].str, type, cliSerial);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// clear the area
|
|
||||||
print_blanks(8);
|
|
||||||
|
|
||||||
report_version();
|
|
||||||
report_radio();
|
|
||||||
report_frame();
|
|
||||||
report_batt_monitor();
|
|
||||||
report_flight_modes();
|
|
||||||
report_ins();
|
|
||||||
report_compass();
|
|
||||||
report_optflow();
|
|
||||||
|
|
||||||
AP_Param::show_all(cliSerial);
|
|
||||||
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Sub::esc_calib(uint8_t argc,const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
|
|
||||||
|
|
||||||
char c;
|
|
||||||
unsigned max_channels = 0;
|
|
||||||
uint32_t set_mask = 0;
|
|
||||||
|
|
||||||
uint16_t pwm_high = PWM_CALIB_MAX;
|
|
||||||
uint16_t pwm_low = PWM_CALIB_MIN;
|
|
||||||
|
|
||||||
|
|
||||||
if (argc < 2) {
|
|
||||||
cliSerial->printf("Pls provide Channel Mask\n"
|
|
||||||
"\tusage: esc_calib 1010 - enables calibration for 2nd and 4th Motor\n");
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
set_mask = strtol(argv[1].str, NULL, 2);
|
|
||||||
if (set_mask == 0) {
|
|
||||||
cliSerial->printf("no channels chosen");
|
|
||||||
}
|
|
||||||
//cliSerial->printf("\n%d\n",set_mask);
|
|
||||||
set_mask<<=1;
|
|
||||||
/* wait 50 ms */
|
|
||||||
hal.scheduler->delay(50);
|
|
||||||
|
|
||||||
|
|
||||||
cliSerial->printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
|
|
||||||
"\n"
|
|
||||||
"Make sure\n"
|
|
||||||
"\t - that the ESCs are not powered\n"
|
|
||||||
"\t - that safety is off\n"
|
|
||||||
"\t - that the controllers are stopped\n"
|
|
||||||
"\n"
|
|
||||||
"Do you want to start calibration now: y or n?\n");
|
|
||||||
|
|
||||||
/* wait for user input */
|
|
||||||
while (1) {
|
|
||||||
c= cliSerial->read();
|
|
||||||
if (c == 'y' || c == 'Y') {
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
} else if (c == 0x03 || c == 0x63 || c == 'q') {
|
|
||||||
cliSerial->printf("ESC calibration exited\n");
|
|
||||||
return (0);
|
|
||||||
|
|
||||||
} else if (c == 'n' || c == 'N') {
|
|
||||||
cliSerial->printf("ESC calibration aborted\n");
|
|
||||||
return (0);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/* rate limit to ~ 20 Hz */
|
|
||||||
hal.scheduler->delay(50);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/* get number of channels available on the device */
|
|
||||||
max_channels = AP_MOTORS_MAX_NUM_MOTORS;
|
|
||||||
|
|
||||||
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
|
|
||||||
motors.armed(true);
|
|
||||||
|
|
||||||
|
|
||||||
cliSerial->printf("Outputs armed\n");
|
|
||||||
|
|
||||||
|
|
||||||
/* wait for user confirmation */
|
|
||||||
cliSerial->printf("\nHigh PWM set: %d\n"
|
|
||||||
"\n"
|
|
||||||
"Connect battery now and hit c+ENTER after the ESCs confirm the first calibration step\n"
|
|
||||||
"\n", pwm_high);
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
/* set max PWM */
|
|
||||||
for (unsigned i = 0; i < max_channels; i++) {
|
|
||||||
|
|
||||||
if (set_mask & 1<<i) {
|
|
||||||
motors.output_test(i, pwm_high);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
c = cliSerial->read();
|
|
||||||
|
|
||||||
if (c == 'c') {
|
|
||||||
break;
|
|
||||||
|
|
||||||
} else if (c == 0x03 || c == 0x63 || c == 'q') {
|
|
||||||
cliSerial->printf("ESC calibration exited\n");
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* rate limit to ~ 20 Hz */
|
|
||||||
hal.scheduler->delay(50);
|
|
||||||
}
|
|
||||||
|
|
||||||
cliSerial->printf("Low PWM set: %d\n"
|
|
||||||
"\n"
|
|
||||||
"Hit c+Enter when finished\n"
|
|
||||||
"\n", pwm_low);
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
|
|
||||||
/* set disarmed PWM */
|
|
||||||
for (unsigned i = 0; i < max_channels; i++) {
|
|
||||||
if (set_mask & 1<<i) {
|
|
||||||
motors.output_test(i, pwm_low);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
c = cliSerial->read();
|
|
||||||
|
|
||||||
if (c == 'c') {
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
} else if (c == 0x03 || c == 0x63 || c == 'q') {
|
|
||||||
cliSerial->printf("ESC calibration exited\n");
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* rate limit to ~ 20 Hz */
|
|
||||||
hal.scheduler->delay(50);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* disarm */
|
|
||||||
motors.armed(false);
|
|
||||||
|
|
||||||
cliSerial->printf("Outputs disarmed\n");
|
|
||||||
|
|
||||||
cliSerial->printf("ESC calibration finished\n");
|
|
||||||
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/***************************************************************************/
|
|
||||||
// CLI reports
|
|
||||||
/***************************************************************************/
|
|
||||||
|
|
||||||
void Sub::report_batt_monitor()
|
|
||||||
{
|
|
||||||
cliSerial->printf("\nBatt Mon:\n");
|
|
||||||
print_divider();
|
|
||||||
if (battery.num_instances() == 0) {
|
|
||||||
print_enabled(false);
|
|
||||||
} else if (!battery.has_current()) {
|
|
||||||
cliSerial->printf("volts");
|
|
||||||
} else {
|
|
||||||
cliSerial->printf("volts and cur");
|
|
||||||
}
|
|
||||||
print_blanks(2);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::report_frame()
|
|
||||||
{
|
|
||||||
cliSerial->printf("Frame\n");
|
|
||||||
print_divider();
|
|
||||||
|
|
||||||
switch (g.frame_configuration) {
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_BLUEROV1:
|
|
||||||
cliSerial->printf("BlueROV1\n");
|
|
||||||
break;
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_VECTORED:
|
|
||||||
cliSerial->printf("Vectored\n");
|
|
||||||
break;
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_VECTORED_6DOF:
|
|
||||||
cliSerial->printf("Vectored-6DOF\n");
|
|
||||||
break;
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_VECTORED_6DOF_90DEG:
|
|
||||||
cliSerial->printf("Vectored-6DOF-90\n");
|
|
||||||
break;
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_SIMPLEROV_3:
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_SIMPLEROV_4:
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_SIMPLEROV_5:
|
|
||||||
cliSerial->printf("SimpleROV\n");
|
|
||||||
break;
|
|
||||||
case AP_Motors6DOF::SUB_FRAME_CUSTOM:
|
|
||||||
cliSerial->printf("Custom\n");
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
cliSerial->printf("Unknown\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
print_blanks(2);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::report_radio()
|
|
||||||
{
|
|
||||||
cliSerial->printf("Radio\n");
|
|
||||||
print_divider();
|
|
||||||
// radio
|
|
||||||
print_radio_values();
|
|
||||||
print_blanks(2);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::report_ins()
|
|
||||||
{
|
|
||||||
cliSerial->printf("INS\n");
|
|
||||||
print_divider();
|
|
||||||
|
|
||||||
print_gyro_offsets();
|
|
||||||
print_accel_offsets_and_scaling();
|
|
||||||
print_blanks(2);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::report_flight_modes()
|
|
||||||
{
|
|
||||||
cliSerial->printf("Flight modes\n");
|
|
||||||
print_divider();
|
|
||||||
|
|
||||||
for (int16_t i = 0; i < 6; i++) {
|
|
||||||
print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(0, i));
|
|
||||||
}
|
|
||||||
print_blanks(2);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::report_optflow()
|
|
||||||
{
|
|
||||||
#if OPTFLOW == ENABLED
|
|
||||||
cliSerial->printf("OptFlow\n");
|
|
||||||
print_divider();
|
|
||||||
|
|
||||||
print_enabled(optflow.enabled());
|
|
||||||
|
|
||||||
print_blanks(2);
|
|
||||||
#endif // OPTFLOW == ENABLED
|
|
||||||
}
|
|
||||||
|
|
||||||
/***************************************************************************/
|
|
||||||
// CLI utilities
|
|
||||||
/***************************************************************************/
|
|
||||||
|
|
||||||
void Sub::print_radio_values()
|
|
||||||
{
|
|
||||||
for (uint8_t i=0; i<8; i++) {
|
|
||||||
RC_Channel *rc = RC_Channels::rc_channel(i);
|
|
||||||
cliSerial->printf("CH%u: %d | %d\n", (unsigned)i, rc->get_radio_min(), rc->get_radio_max());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::print_switch(uint8_t p, uint8_t m, bool b)
|
|
||||||
{
|
|
||||||
cliSerial->printf("Pos %d:\t",p);
|
|
||||||
print_flight_mode(cliSerial, m);
|
|
||||||
cliSerial->printf(",\t\tSimple: ");
|
|
||||||
if (b) {
|
|
||||||
cliSerial->printf("ON\n");
|
|
||||||
} else {
|
|
||||||
cliSerial->printf("OFF\n");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::print_accel_offsets_and_scaling(void)
|
|
||||||
{
|
|
||||||
const Vector3f &accel_offsets = ins.get_accel_offsets();
|
|
||||||
const Vector3f &accel_scale = ins.get_accel_scale();
|
|
||||||
cliSerial->printf("A_off: %4.2f, %4.2f, %4.2f\nA_scale: %4.2f, %4.2f, %4.2f\n",
|
|
||||||
(double)accel_offsets.x, // Pitch
|
|
||||||
(double)accel_offsets.y, // Roll
|
|
||||||
(double)accel_offsets.z, // YAW
|
|
||||||
(double)accel_scale.x, // Pitch
|
|
||||||
(double)accel_scale.y, // Roll
|
|
||||||
(double)accel_scale.z); // YAW
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::print_gyro_offsets(void)
|
|
||||||
{
|
|
||||||
const Vector3f &gyro_offsets = ins.get_gyro_offsets();
|
|
||||||
cliSerial->printf("G_off: %4.2f, %4.2f, %4.2f\n",
|
|
||||||
(double)gyro_offsets.x,
|
|
||||||
(double)gyro_offsets.y,
|
|
||||||
(double)gyro_offsets.z);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
// report_compass - displays compass information. Also called by compassmot.pde
|
|
||||||
void Sub::report_compass()
|
|
||||||
{
|
|
||||||
cliSerial->printf("Compass\n");
|
|
||||||
print_divider();
|
|
||||||
|
|
||||||
print_enabled(g.compass_enabled);
|
|
||||||
|
|
||||||
// mag declination
|
|
||||||
cliSerial->printf("Mag Dec: %4.4f\n",
|
|
||||||
(double)degrees(compass.get_declination()));
|
|
||||||
|
|
||||||
// mag offsets
|
|
||||||
Vector3f offsets;
|
|
||||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
|
||||||
offsets = compass.get_offsets(i);
|
|
||||||
// mag offsets
|
|
||||||
cliSerial->printf("Mag%d off: %4.4f, %4.4f, %4.4f\n",
|
|
||||||
(int)i,
|
|
||||||
(double)offsets.x,
|
|
||||||
(double)offsets.y,
|
|
||||||
(double)offsets.z);
|
|
||||||
}
|
|
||||||
|
|
||||||
// motor compensation
|
|
||||||
cliSerial->print("Motor Comp: ");
|
|
||||||
if (compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED) {
|
|
||||||
cliSerial->print("Off\n");
|
|
||||||
} else {
|
|
||||||
if (compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE) {
|
|
||||||
cliSerial->print("Throttle");
|
|
||||||
}
|
|
||||||
if (compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT) {
|
|
||||||
cliSerial->print("Current");
|
|
||||||
}
|
|
||||||
Vector3f motor_compensation;
|
|
||||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
|
||||||
motor_compensation = compass.get_motor_compensation(i);
|
|
||||||
cliSerial->printf("\nComMot%d: %4.2f, %4.2f, %4.2f\n",
|
|
||||||
(int)i,
|
|
||||||
(double)motor_compensation.x,
|
|
||||||
(double)motor_compensation.y,
|
|
||||||
(double)motor_compensation.z);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
print_blanks(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::print_blanks(int16_t num)
|
|
||||||
{
|
|
||||||
while (num > 0) {
|
|
||||||
num--;
|
|
||||||
cliSerial->println("");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::print_divider(void)
|
|
||||||
{
|
|
||||||
for (int i = 0; i < 40; i++) {
|
|
||||||
cliSerial->print("-");
|
|
||||||
}
|
|
||||||
cliSerial->println();
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::print_enabled(bool b)
|
|
||||||
{
|
|
||||||
if (b) {
|
|
||||||
cliSerial->print("en");
|
|
||||||
} else {
|
|
||||||
cliSerial->print("dis");
|
|
||||||
}
|
|
||||||
cliSerial->print("abled\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
void Sub::report_version()
|
|
||||||
{
|
|
||||||
cliSerial->printf("FW Ver: %d\n",(int)g.k_format_version);
|
|
||||||
print_divider();
|
|
||||||
print_blanks(2);
|
|
||||||
}
|
|
||||||
|
|
@ -8,72 +8,11 @@
|
||||||
*
|
*
|
||||||
*****************************************************************************/
|
*****************************************************************************/
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
// This is the help function
|
|
||||||
int8_t Sub::main_menu_help(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
cliSerial->printf("Commands:\n"
|
|
||||||
" logs\n"
|
|
||||||
" setup\n"
|
|
||||||
" test\n"
|
|
||||||
" reboot\n"
|
|
||||||
"\n");
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Command/function table for the top-level menu.
|
|
||||||
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 Sub::reboot_board(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
hal.scheduler->reboot(false);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// the user wants the CLI. It never exits
|
|
||||||
void Sub::run_cli(AP_HAL::UARTDriver *port)
|
|
||||||
{
|
|
||||||
cliSerial = port;
|
|
||||||
Menu::set_port(port);
|
|
||||||
port->set_blocking_writes(true);
|
|
||||||
|
|
||||||
// disable the mavlink delay callback
|
|
||||||
hal.scheduler->register_delay_callback(NULL, 5);
|
|
||||||
|
|
||||||
// disable main_loop failsafe
|
|
||||||
failsafe_disable();
|
|
||||||
|
|
||||||
// cut the engines
|
|
||||||
if (motors.armed()) {
|
|
||||||
motors.armed(false);
|
|
||||||
motors.output();
|
|
||||||
}
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
main_menu.run();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
static void mavlink_delay_cb_static()
|
static void mavlink_delay_cb_static()
|
||||||
{
|
{
|
||||||
sub.mavlink_delay_cb();
|
sub.mavlink_delay_cb();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static void failsafe_check_static()
|
static void failsafe_check_static()
|
||||||
{
|
{
|
||||||
sub.failsafe_check();
|
sub.failsafe_check();
|
||||||
|
|
@ -97,11 +36,6 @@ void Sub::init_ardupilot()
|
||||||
"\n\nFree RAM: %u\n",
|
"\n\nFree RAM: %u\n",
|
||||||
(unsigned)hal.util->available_memory());
|
(unsigned)hal.util->available_memory());
|
||||||
|
|
||||||
//
|
|
||||||
// Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)
|
|
||||||
//
|
|
||||||
report_version();
|
|
||||||
|
|
||||||
// load parameters from EEPROM
|
// load parameters from EEPROM
|
||||||
load_parameters();
|
load_parameters();
|
||||||
|
|
||||||
|
|
@ -201,19 +135,6 @@ void Sub::init_ardupilot()
|
||||||
USERHOOK_INIT
|
USERHOOK_INIT
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
if (g.cli_enabled) {
|
|
||||||
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
|
|
||||||
cliSerial->println(msg);
|
|
||||||
if (gcs_chan[1].initialised && (gcs_chan[1].get_uart() != NULL)) {
|
|
||||||
gcs_chan[1].get_uart()->println(msg);
|
|
||||||
}
|
|
||||||
if (num_gcs > 2 && gcs_chan[2].initialised && (gcs_chan[2].get_uart() != NULL)) {
|
|
||||||
gcs_chan[2].get_uart()->println(msg);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED
|
#if HIL_MODE != HIL_MODE_DISABLED
|
||||||
while (barometer.get_last_update() == 0) {
|
while (barometer.get_last_update() == 0) {
|
||||||
// the barometer begins updating when we get the first
|
// the barometer begins updating when we get the first
|
||||||
|
|
|
||||||
277
ArduSub/test.cpp
277
ArduSub/test.cpp
|
|
@ -1,277 +0,0 @@
|
||||||
#include "Sub.h"
|
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
// 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 test_menu_commands[] = {
|
|
||||||
#if HIL_MODE == HIL_MODE_DISABLED
|
|
||||||
{"baro", MENU_FUNC(test_baro)},
|
|
||||||
#endif
|
|
||||||
{"compass", MENU_FUNC(test_compass)},
|
|
||||||
{"ins", MENU_FUNC(test_ins)},
|
|
||||||
{"optflow", MENU_FUNC(test_optflow)},
|
|
||||||
{"relay", MENU_FUNC(test_relay)},
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
||||||
{"shell", MENU_FUNC(test_shell)},
|
|
||||||
#endif
|
|
||||||
#if HIL_MODE == HIL_MODE_DISABLED
|
|
||||||
{"rangefinder", MENU_FUNC(test_rangefinder)},
|
|
||||||
#endif
|
|
||||||
};
|
|
||||||
|
|
||||||
// A Macro to create the Menu
|
|
||||||
MENU(test_menu, "test", test_menu_commands);
|
|
||||||
|
|
||||||
int8_t Sub::test_mode(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
test_menu.run();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if HIL_MODE == HIL_MODE_DISABLED
|
|
||||||
int8_t Sub::test_baro(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
print_hit_enter();
|
|
||||||
init_barometer(true);
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
hal.scheduler->delay(100);
|
|
||||||
read_barometer();
|
|
||||||
|
|
||||||
if (!barometer.healthy()) {
|
|
||||||
cliSerial->println("not healthy");
|
|
||||||
} else {
|
|
||||||
cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n",
|
|
||||||
(double)(baro_alt / 100.0f),
|
|
||||||
(double)barometer.get_pressure(),
|
|
||||||
(double)barometer.get_temperature());
|
|
||||||
}
|
|
||||||
if (cliSerial->available() > 0) {
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
int8_t Sub::test_compass(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
uint8_t delta_ms_fast_loop;
|
|
||||||
uint8_t medium_loopCounter = 0;
|
|
||||||
|
|
||||||
if (!g.compass_enabled) {
|
|
||||||
cliSerial->printf("Compass: ");
|
|
||||||
print_enabled(false);
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!compass.init()) {
|
|
||||||
cliSerial->println("Compass initialisation failed!");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
ahrs.init();
|
|
||||||
ahrs.set_fly_forward(true);
|
|
||||||
ahrs.set_compass(&compass);
|
|
||||||
#if OPTFLOW == ENABLED
|
|
||||||
ahrs.set_optflow(&optflow);
|
|
||||||
#endif
|
|
||||||
report_compass();
|
|
||||||
|
|
||||||
// we need the AHRS initialised for this test
|
|
||||||
ins.init(scheduler.get_loop_rate_hz());
|
|
||||||
ahrs.reset();
|
|
||||||
int16_t counter = 0;
|
|
||||||
float heading = 0;
|
|
||||||
|
|
||||||
print_hit_enter();
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
hal.scheduler->delay(20);
|
|
||||||
if (millis() - fast_loopTimer > 19) {
|
|
||||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
|
||||||
G_Dt = (float)delta_ms_fast_loop / 1000.0f; // used by DCM integrator
|
|
||||||
fast_loopTimer = millis();
|
|
||||||
|
|
||||||
// INS
|
|
||||||
// ---
|
|
||||||
ahrs.update();
|
|
||||||
|
|
||||||
medium_loopCounter++;
|
|
||||||
if (medium_loopCounter == 5) {
|
|
||||||
if (compass.read()) {
|
|
||||||
// Calculate heading
|
|
||||||
const Matrix3f &m = ahrs.get_rotation_body_to_ned();
|
|
||||||
heading = compass.calculate_heading(m);
|
|
||||||
compass.learn_offsets();
|
|
||||||
}
|
|
||||||
medium_loopCounter = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
counter++;
|
|
||||||
if (counter>20) {
|
|
||||||
if (compass.healthy()) {
|
|
||||||
const Vector3f &mag_ofs = compass.get_offsets();
|
|
||||||
const Vector3f &mag = compass.get_field();
|
|
||||||
cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
|
|
||||||
(int)(wrap_360_cd(ToDeg(heading) * 100)) /100,
|
|
||||||
(double)mag.x,
|
|
||||||
(double)mag.y,
|
|
||||||
(double)mag.z,
|
|
||||||
(double)mag_ofs.x,
|
|
||||||
(double)mag_ofs.y,
|
|
||||||
(double)mag_ofs.z);
|
|
||||||
} else {
|
|
||||||
cliSerial->println("compass not healthy");
|
|
||||||
}
|
|
||||||
counter=0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (cliSerial->available() > 0) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// save offsets. This allows you to get sane offset values using
|
|
||||||
// the CLI before you go flying.
|
|
||||||
cliSerial->println("saving offsets");
|
|
||||||
compass.save_offsets();
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Sub::test_ins(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
Vector3f gyro, accel;
|
|
||||||
print_hit_enter();
|
|
||||||
cliSerial->printf("INS\n");
|
|
||||||
hal.scheduler->delay(1000);
|
|
||||||
|
|
||||||
ahrs.init();
|
|
||||||
ins.init(scheduler.get_loop_rate_hz());
|
|
||||||
cliSerial->printf("...done\n");
|
|
||||||
|
|
||||||
hal.scheduler->delay(50);
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
ins.update();
|
|
||||||
gyro = ins.get_gyro();
|
|
||||||
accel = ins.get_accel();
|
|
||||||
|
|
||||||
float test = accel.length() / GRAVITY_MSS;
|
|
||||||
|
|
||||||
cliSerial->printf("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n",
|
|
||||||
(double)accel.x, (double)accel.y, (double)accel.z,
|
|
||||||
(double)gyro.x, (double)gyro.y, (double)gyro.z,
|
|
||||||
(double)test);
|
|
||||||
|
|
||||||
hal.scheduler->delay(40);
|
|
||||||
if (cliSerial->available() > 0) {
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Sub::test_optflow(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
#if OPTFLOW == ENABLED
|
|
||||||
if (optflow.enabled()) {
|
|
||||||
cliSerial->printf("dev id: %d\t",(int)optflow.device_id());
|
|
||||||
print_hit_enter();
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
hal.scheduler->delay(200);
|
|
||||||
optflow.update();
|
|
||||||
const Vector2f& flowRate = optflow.flowRate();
|
|
||||||
cliSerial->printf("flowX : %7.4f\t flowY : %7.4f\t flow qual : %d\n",
|
|
||||||
(double)flowRate.x,
|
|
||||||
(double)flowRate.y,
|
|
||||||
(int)optflow.quality());
|
|
||||||
|
|
||||||
if (cliSerial->available() > 0) {
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
cliSerial->printf("OptFlow: ");
|
|
||||||
print_enabled(false);
|
|
||||||
}
|
|
||||||
return (0);
|
|
||||||
#else
|
|
||||||
return (0);
|
|
||||||
#endif // OPTFLOW == ENABLED
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Sub::test_relay(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
print_hit_enter();
|
|
||||||
hal.scheduler->delay(1000);
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
cliSerial->printf("Relay on\n");
|
|
||||||
relay.on(0);
|
|
||||||
hal.scheduler->delay(3000);
|
|
||||||
if (cliSerial->available() > 0) {
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
|
|
||||||
cliSerial->printf("Relay off\n");
|
|
||||||
relay.off(0);
|
|
||||||
hal.scheduler->delay(3000);
|
|
||||||
if (cliSerial->available() > 0) {
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
||||||
/*
|
|
||||||
* run a debug shell
|
|
||||||
*/
|
|
||||||
int8_t Sub::test_shell(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
hal.util->run_debug_shell(cliSerial);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if HIL_MODE == HIL_MODE_DISABLED
|
|
||||||
/*
|
|
||||||
* test the rangefinders
|
|
||||||
*/
|
|
||||||
int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
#if RANGEFINDER_ENABLED == ENABLED
|
|
||||||
rangefinder.init();
|
|
||||||
|
|
||||||
cliSerial->printf("RangeFinder: %d devices detected\n", rangefinder.num_sensors());
|
|
||||||
|
|
||||||
print_hit_enter();
|
|
||||||
while (1) {
|
|
||||||
hal.scheduler->delay(100);
|
|
||||||
rangefinder.update();
|
|
||||||
|
|
||||||
for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
|
|
||||||
cliSerial->printf("Dev%d: status %d distance_cm %d\n",
|
|
||||||
(int)i,
|
|
||||||
(int)rangefinder.status(i),
|
|
||||||
(int)rangefinder.distance_cm(i));
|
|
||||||
}
|
|
||||||
|
|
||||||
if (cliSerial->available() > 0) {
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
return (0);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void Sub::print_hit_enter()
|
|
||||||
{
|
|
||||||
cliSerial->printf("Hit Enter to exit.\n\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
@ -17,7 +17,6 @@ def build(bld):
|
||||||
'AP_InertialNav',
|
'AP_InertialNav',
|
||||||
'AP_JSButton',
|
'AP_JSButton',
|
||||||
'AP_LeakDetector',
|
'AP_LeakDetector',
|
||||||
'AP_Menu',
|
|
||||||
'AP_Motors',
|
'AP_Motors',
|
||||||
'AP_RCMapper',
|
'AP_RCMapper',
|
||||||
'AP_Relay',
|
'AP_Relay',
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue