Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Remove deprecated/unused CLI and AP_Menu

This commit is contained in:
Jacob Walser 2017-03-28 18:23:05 -04:00
parent 0e180f88ad
commit 69c9dbc286
13 changed files with 1 additions and 1148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/APM_Config.h
View File

@ -12,7 +12,6 @@
//#define PROXIMITY_ENABLED DISABLED // disable proximity sensors
//#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 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
// features below are disabled by default on all boards

4
ArduSub/GCS_Mavlink.cpp
View File

@ -2033,11 +2033,7 @@ void Sub::gcs_check_input(void)
{
for (uint8_t i=0; i<num_gcs; i++) {
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);
#endif
}
}
}

169
ArduSub/Log.cpp
View File

@ -6,150 +6,6 @@
// Code to Write and Read packets from DataFlash log memory
// 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)
{
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" },
};
#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()
{
// only 200(?) bytes are guaranteed by DataFlash
@ -667,15 +507,6 @@ void Sub::log_init(void)
#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::Log_Write_Current() {}
void Sub::Log_Write_Nav_Tuning() {}

9
ArduSub/Parameters.cpp
View File

@ -64,15 +64,6 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Advanced
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
// @DisplayName: Throttle filter cutoff
// @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable

6
ArduSub/Parameters.h
View File

@ -68,8 +68,7 @@ public:
k_param_DataFlash, // DataFlash Logging
k_param_serial_manager, // Serial ports, AP_SerialManager
k_param_notify, // Notify Library, AP_Notify
k_param_cli_enabled, // Old (deprecated) command line interface
k_param_arming, // Arming checks
k_param_arming = 26, // Arming checks
// Sensor objects
@ -227,9 +226,6 @@ public:
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
#if CLI_ENABLED == ENABLED
AP_Int8 cli_enabled;
#endif
AP_Float throttle_filt;

43
ArduSub/Sub.h
View File

@ -30,7 +30,6 @@
// Common dependencies
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Menu/AP_Menu.h>
#include <AP_Param/AP_Param.h>
#include <StorageManager/StorageManager.h>
@ -705,21 +704,6 @@ private:
void terrain_update();
void terrain_logging();
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 auto_trim();
void init_ardupilot();
@ -731,7 +715,6 @@ private:
void update_auto_armed();
void check_usb_mux(void);
bool should_log(uint32_t mask);
void print_hit_enter();
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
bool start_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);
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 run_cli(AP_HAL::UARTDriver *port);
void init_capabilities(void);
@ -802,33 +784,8 @@ private:
public:
void mavlink_delay_cb();
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 Sub sub;

9
ArduSub/config.h
View File

@ -381,12 +381,3 @@
#ifndef AC_TERRAIN
#define AC_TERRAIN DISABLED // Requires Rally enabled as well
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED
#endif

40
ArduSub/flight_mode.cpp
View File

@ -223,43 +223,3 @@ void Sub::notify_flight_mode(control_mode_t mode)
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
ArduSub/make.inc
View File

@ -1,5 +1,4 @@
LIBRARIES += AP_Common
LIBRARIES += AP_Menu
LIBRARIES += AP_Param
LIBRARIES += StorageManager
LIBRARIES += GCS

510
ArduSub/setup.cpp
View File

@ -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);
}

79
ArduSub/system.cpp
View File

@ -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()
{
sub.mavlink_delay_cb();
}
static void failsafe_check_static()
{
sub.failsafe_check();
@ -97,11 +36,6 @@ void Sub::init_ardupilot()
"\n\nFree RAM: %u\n",
(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();
@ -201,19 +135,6 @@ void Sub::init_ardupilot()
USERHOOK_INIT
#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
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first

277
ArduSub/test.cpp
View File

@ -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

1
ArduSub/wscript
View File

@ -17,7 +17,6 @@ def build(bld):
'AP_InertialNav',
'AP_JSButton',
'AP_LeakDetector',
'AP_Menu',
'AP_Motors',
'AP_RCMapper',
'AP_Relay',