Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: remove CLI

Browse Source
This commit is contained in:
Peter Barker 2017-08-11 13:49:03 +10:00 committed by Randy Mackay
parent bff31e8b42
commit fa2b500e93
15 changed files with 9 additions and 860 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.cpp
View File

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

15
APMrover2/GCS_Rover.cpp
View File

@ -1,15 +0,0 @@
#include "GCS_Rover.h"
#include "Rover.h"
bool GCS_Rover::cli_enabled() const
{
#if CLI_ENABLED == ENABLED
return rover.g.cli_enabled;
#else
return false;
#endif
}
AP_HAL::BetterStream* GCS_Rover::cliSerial() {
return rover.cliSerial;
}

3
APMrover2/GCS_Rover.h
View File

@ -21,7 +21,4 @@ private:
GCS_MAVLINK_Rover _chan[MAVLINK_COMM_NUM_BUFFERS];
bool cli_enabled() const override;
AP_HAL::BetterStream* cliSerial() override;
};

167
APMrover2/Log.cpp
View File

@ -3,154 +3,6 @@
#if LOGGING_ENABLED == ENABLED
#if CLI_ENABLED == ENABLED
// 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(&rover, &Rover::print_log_menu, bool));
bool Rover::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(MODE);
PLOG(IMU);
PLOG(CMD);
PLOG(CURRENT);
PLOG(RANGEFINDER);
PLOG(COMPASS);
PLOG(CAMERA);
PLOG(STEERING);
#undef PLOG
}
cliSerial->printf("\n");
DataFlash.ListAvailableLogs(cliSerial);
return(true);
}
int8_t Rover::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) || (static_cast<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(static_cast<uint16_t>(dump_log_num), dump_log_start, dump_log_end);
return 0;
}
int8_t Rover::erase_logs(uint8_t argc, const Menu::arg *argv)
{
DataFlash.EnableWrites(false);
do_erase_logs();
DataFlash.EnableWrites(true);
return 0;
}
int8_t Rover::select_logs(uint8_t argc, const Menu::arg *argv)
{
uint16_t bits = 0;
if (argc != 2) {
cliSerial->printf("missing log type\n");
return(-1);
}
// 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(MODE);
TARG(IMU);
TARG(CMD);
TARG(CURRENT);
TARG(RANGEFINDER);
TARG(COMPASS);
TARG(CAMERA);
TARG(STEERING);
#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 Rover::process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
#endif // CLI_ENABLED == ENABLED
void Rover::do_erase_logs(void)
{
cliSerial->printf("\nErasing log...\n");
DataFlash.EraseAll();
cliSerial->printf("\nLog erased.\n");
}
struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -501,26 +353,8 @@ const LogStructure Rover::log_structure[] = {
void Rover::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
gcs().reset_cli_timeout();
}
#if CLI_ENABLED == ENABLED
// Read the DataFlash log memory : Packet Parser
void Rover::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page)
{
cliSerial->printf("\n" FIRMWARE_STRING
"\nFree RAM: %u\n",
static_cast<uint32_t>(hal.util->available_memory()));
cliSerial->printf("%s\n", HAL_BOARD_NAME);
DataFlash.LogReadProcess(list_entry, start_page, end_page,
FUNCTOR_BIND_MEMBER(&Rover::print_mode, void, AP_HAL::BetterStream *, uint8_t),
cliSerial);
}
#endif // CLI_ENABLED
void Rover::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by DataFlash
@ -537,7 +371,6 @@ void Rover::Log_Write_Startup(uint8_t type) {}
void Rover::Log_Write_Current() {}
void Rover::Log_Write_Nav_Tuning() {}
void Rover::Log_Write_Performance() {}
int8_t Rover::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
void Rover::Log_Write_Control_Tuning() {}
void Rover::Log_Write_Rangefinder() {}
void Rover::Log_Write_Attitude() {}

17
APMrover2/Parameters.cpp
View File

@ -65,15 +65,6 @@ const AP_Param::Info Rover::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: TELEM_DELAY
// @DisplayName: Telemetry startup delay
// @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up
@ -585,19 +576,19 @@ const AP_Param::ConversionInfo conversion_table[] = {
void Rover::load_parameters(void)
{
if (!AP_Param::check_var_info()) {
cliSerial->printf("Bad var table\n");
hal.console->printf("Bad var table\n");
AP_HAL::panic("Bad var table");
}
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
// erase all parameters
cliSerial->printf("Firmware change: erasing EEPROM...\n");
hal.console->printf("Firmware change: erasing EEPROM...\n");
AP_Param::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->printf("done.\n");
hal.console->printf("done.\n");
}
const uint32_t before = micros();
@ -618,7 +609,7 @@ void Rover::load_parameters(void)
Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old };
const uint16_t old_aux_chan_mask = 0x3FFA;
SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap);
cliSerial->printf("load_all took %uus\n", micros() - before);
hal.console->printf("load_all took %uus\n", micros() - before);
// set a more reasonable default NAVL1_PERIOD for rovers
L1_controller.set_default_period(NAVL1_PERIOD);
}

5
APMrover2/Parameters.h
View File

@ -73,7 +73,7 @@ public:
k_param_serial2_baud_old,
k_param_serial2_protocol, // deprecated, can be deleted
k_param_serial_manager, // serial manager library
k_param_cli_enabled,
k_param_cli_enabled_old,
k_param_gcs3,
k_param_gcs_pid_mask,
@ -224,9 +224,6 @@ public:
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay;
#if CLI_ENABLED == ENABLED
AP_Int8 cli_enabled;
#endif
// sensor parameters
AP_Int8 compass_enabled;

37
APMrover2/Rover.h
View File

@ -23,7 +23,6 @@
// Libraries
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Menu/AP_Menu.h>
#include <AP_Param/AP_Param.h>
#include <StorageManager/StorageManager.h>
#include <AP_GPS/AP_GPS.h> // ArduPilot GPS library
@ -122,7 +121,6 @@ public:
void loop(void) override;
private:
AP_HAL::BetterStream* cliSerial;
// must be the first AP_Param variable declared to ensure its
// constructor runs before the constructors of the other AP_Param
@ -434,7 +432,6 @@ private:
bool set_mode(Mode &mode, mode_reason_t reason);
bool mavlink_set_mode(uint8_t mode);
void do_erase_logs(void);
void Log_Write_Performance();
void Log_Write_Steering();
void Log_Write_Startup(uint8_t type);
@ -451,7 +448,6 @@ private:
void Log_Write_Vehicle_Startup_Messages();
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_WheelEncoder();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void log_init(void);
void Log_Arm_Disarm();
@ -496,8 +492,6 @@ private:
void read_battery(void);
void read_receiver_rssi(void);
void read_rangefinders(void);
void zero_eeprom(void);
void print_enabled(bool b);
void init_ardupilot();
void startup_ground(void);
void set_reverse(bool reverse);
@ -509,8 +503,6 @@ private:
void check_usb_mux(void);
uint8_t check_digital_pin(uint8_t pin);
bool should_log(uint32_t mask);
void print_hit_enter();
void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
void notify_mode(enum mode new_mode);
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
@ -545,35 +537,8 @@ private:
#endif
public:
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_erase(uint8_t argc, const Menu::arg *argv);
int8_t setup_mode(uint8_t argc, const Menu::arg *argv);
int8_t reboot_board(uint8_t, const Menu::arg*);
int8_t main_menu_help(uint8_t argc, const Menu::arg *argv);
int8_t test_mode(uint8_t argc, const Menu::arg *argv);
void run_cli(AP_HAL::UARTDriver *port);
void mavlink_delay_cb();
void failsafe_check();
int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
int8_t test_passthru(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_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_rangefinder(uint8_t argc, const Menu::arg *argv);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
#endif
void dataflash_periodic(void);
void update_soft_armed();
@ -584,8 +549,6 @@ public:
void motor_test_stop();
};
#define MENU_FUNC(func) FUNCTOR_BIND(&rover, &Rover::func, int8_t, uint8_t, const Menu::arg *)
extern const AP_HAL::HAL& hal;
extern Rover rover;

5
APMrover2/config.h
View File

@ -209,11 +209,6 @@
#define HIL_SERVOS DISABLED
#endif
// use this to completely disable the CLI
#ifndef CLI_ENABLED
#define CLI_ENABLED ENABLED
#endif
// if RESET_SWITCH_CH is not zero, then this is the PWM value on
// that channel where we reset the control mode to the current switch
// position (to for example return to switched mode after failsafe or

1
APMrover2/make.inc
View File

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

2
APMrover2/sensors.cpp
View File

@ -8,7 +8,7 @@ void Rover::init_compass()
}
if (!compass.init()|| !compass.read()) {
cliSerial->printf("Compass initialisation failed!\n");
hal.console->printf("Compass initialisation failed!\n");
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);

55
APMrover2/setup.cpp
View File

@ -1,55 +0,0 @@
#include "Rover.h"
#if CLI_ENABLED == ENABLED
// Command/function table for the setup menu
static const struct Menu::command setup_menu_commands[] = {
// command function called
// ======= ===============
{"erase", MENU_FUNC(setup_erase)}
};
// 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 Rover::setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
cliSerial->printf("Setup Mode\n"
"\n"
"IMPORTANT: if you have not previously set this system up, use the\n"
"'reset' command to initialize the EEPROM to sensible default values\n"
"and then the 'radio' command to configure for your radio.\n"
"\n");
// Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run();
return 0;
}
int8_t Rover::setup_erase(uint8_t argc, const Menu::arg *argv)
{
int c;
cliSerial->printf("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ");
do {
c = cliSerial->read();
} while (-1 == c);
if (('y' != c) && ('Y' != c)) {
return (-1);
}
zero_eeprom();
return 0;
}
void Rover::zero_eeprom(void)
{
cliSerial->printf("\nErasing EEPROM\n");
StorageManager::erase();
cliSerial->printf("done\n");
}
#endif // CLI_ENABLED

101
APMrover2/system.cpp
View File

@ -8,62 +8,6 @@ The init_ardupilot function processes everything we need for an in - air restart
#include "Rover.h"
#include "version.h"
#if CLI_ENABLED == ENABLED
// This is the help function
int8_t Rover::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"
"\n"
"Move the slide switch and reset to FLY.\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 Rover::reboot_board(uint8_t argc, const Menu::arg *argv)
{
hal.scheduler->reboot(false);
return 0;
}
// the user wants the CLI. It never exits
void Rover::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()
{
rover.mavlink_delay_cb();
@ -79,9 +23,9 @@ void Rover::init_ardupilot()
// initialise console serial port
serial_manager.init_console();
cliSerial->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
hal.util->available_memory());
hal.console->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
hal.util->available_memory());
//
// Check the EEPROM format version before loading any parameters from EEPROM.
@ -106,13 +50,6 @@ void Rover::init_ardupilot()
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
// specify callback function for CLI menu system
#if CLI_ENABLED == ENABLED
if (gcs().cli_enabled()) {
gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Rover::run_cli, void, AP_HAL::UARTDriver *));
}
#endif
BoardConfig.init();
#if HAL_WITH_UAVCAN
BoardConfig_CAN.init();
@ -201,10 +138,6 @@ void Rover::init_ardupilot()
ahrs.set_beacon(&g2.beacon);
#if CLI_ENABLED == ENABLED
gcs().handle_interactive_setup();
#endif
init_capabilities();
startup_ground();
@ -367,34 +300,6 @@ void Rover::check_usb_mux(void)
usb_connected = usb_check;
}
void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case MANUAL:
port->printf("Manual");
break;
case HOLD:
port->printf("HOLD");
break;
case LEARNING:
port->printf("Learning");
break;
case STEERING:
port->printf("Steering");
break;
case AUTO:
port->printf("AUTO");
break;
case RTL:
port->printf("RTL");
break;
default:
port->printf("Mode(%u)", static_cast<uint32_t>(mode));
break;
}
}
// update notify with mode change
void Rover::notify_mode(enum mode new_mode)
{

443
APMrover2/test.cpp
View File

@ -1,443 +0,0 @@
#include "Rover.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_Common for implementation details
static const struct Menu::command test_menu_commands[] = {
{"passthru", MENU_FUNC(test_passthru)},
{"failsafe", MENU_FUNC(test_failsafe)},
{"relay", MENU_FUNC(test_relay)},
{"waypoints", MENU_FUNC(test_wp)},
{"modeswitch", MENU_FUNC(test_modeswitch)},
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
{"gps", MENU_FUNC(test_gps)},
{"ins", MENU_FUNC(test_ins)},
{"rngfndtest", MENU_FUNC(test_rangefinder)},
{"compass", MENU_FUNC(test_mag)},
{"logging", MENU_FUNC(test_logging)},
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
{"shell", MENU_FUNC(test_shell)},
#endif
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
int8_t Rover::test_mode(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Test Mode\n\n");
test_menu.run();
return 0;
}
void Rover::print_hit_enter()
{
cliSerial->printf("Hit Enter to exit.\n\n");
}
int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while (1) {
delay(20);
// New radio frame? (we could use also if((millis()- timer) > 20)
if (hal.rcin->new_input()) {
cliSerial->printf("CH:");
for (int i = 0; i < 8; i++) {
cliSerial->printf("%u", hal.rcin->read(i)); // Print channel values
cliSerial->printf(",");
hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos
}
cliSerial->printf("\n");
}
if (cliSerial->available() > 0) {
return (0);
}
}
return 0;
}
int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test = 0;
print_hit_enter();
for (int i = 0; i < 50; i++) {
delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n");
while (channel_throttle->get_control_in() > 0) {
delay(20);
read_radio();
}
while (1) {
delay(20);
read_radio();
if (channel_throttle->get_control_in() > 0) {
cliSerial->printf("THROTTLE CHANGED %d \n", channel_throttle->get_control_in());
fail_test++;
}
if (oldSwitchPosition != readSwitch()) {
cliSerial->printf("CONTROL MODE CHANGED: ");
print_mode(cliSerial, readSwitch());
cliSerial->printf("\n");
fail_test++;
}
if (throttle_failsafe_active()) {
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->get_radio_in());
print_mode(cliSerial, readSwitch());
cliSerial->printf("\n");
fail_test++;
}
if (fail_test > 0) {
return (0);
}
if (cliSerial->available() > 0) {
cliSerial->printf("LOS caused no change in APM.\n");
return (0);
}
}
}
int8_t Rover::test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while (1) {
cliSerial->printf("Relay on\n");
relay.on(0);
delay(3000);
if (cliSerial->available() > 0) {
return (0);
}
cliSerial->printf("Relay off\n");
relay.off(0);
delay(3000);
if (cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
cliSerial->printf("%u waypoints\n", static_cast<uint32_t>(mission.num_commands()));
cliSerial->printf("Hit radius: %f\n", static_cast<double>(g.waypoint_radius.get()));
for (uint8_t i = 0; i < mission.num_commands(); i++) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(i, temp_cmd)) {
test_wp_print(temp_cmd);
}
}
return (0);
}
void Rover::test_wp_print(const AP_Mission::Mission_Command& cmd)
{
cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%d p3:%d p4:%d \n",
static_cast<int32_t>(cmd.index),
static_cast<int32_t>(cmd.id),
static_cast<int32_t>(cmd.content.location.options),
static_cast<int32_t>(cmd.p1),
(cmd.content.location.alt),
(cmd.content.location.lat),
(cmd.content.location.lng));
}
int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
cliSerial->printf("Control CH ");
cliSerial->printf("%d\n", MODE_CHANNEL);
while (1) {
delay(20);
uint8_t switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition) {
cliSerial->printf("Position %d\n", switchPosition);
oldSwitchPosition = switchPosition;
}
if (cliSerial->available() > 0) {
return (0);
}
}
}
/*
test the dataflash is working
*/
int8_t Rover::test_logging(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Testing dataflash logging\n");
DataFlash.ShowDeviceInfo(cliSerial);
return 0;
}
//-------------------------------------------------------------------------------------------
// tests in this section are for real sensors or sensors that have been simulated
int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
uint32_t last_message_time_ms = 0;
while (1) {
delay(100);
gps.update();
if (gps.last_message_time_ms() != last_message_time_ms) {
last_message_time_ms = gps.last_message_time_ms();
const Location &loc = gps.location();
cliSerial->printf("Lat: %d, Lon %d, Alt: %dm, #sats: %d\n",
(loc.lat),
(loc.lng),
(loc.alt/100),
(gps.num_sats()));
} else {
cliSerial->printf(".");
}
if (cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
{
// cliSerial->printf("Calibrating.");
ahrs.init();
ahrs.set_fly_forward(true);
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
print_hit_enter();
delay(1000);
uint8_t medium_loopCounter = 0;
while (1) {
ins.wait_for_sample();
ahrs.update();
if (g.compass_enabled) {
medium_loopCounter++;
if (medium_loopCounter >= 5) {
compass.read();
medium_loopCounter = 0;
}
}
// We are using the IMU
// ---------------------
Vector3f gyros = ins.get_gyro();
Vector3f accels = ins.get_accel();
cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n",
static_cast<int32_t>(ahrs.roll_sensor / 100),
static_cast<int32_t>(ahrs.pitch_sensor / 100),
static_cast<uint16_t>(ahrs.yaw_sensor / 100),
static_cast<double>(gyros.x), static_cast<double>(gyros.y), static_cast<double>(gyros.z),
static_cast<double>(accels.x), static_cast<double>(accels.y), static_cast<double>(accels.z));
if (cliSerial->available() > 0) {
return (0);
}
}
}
void Rover::print_enabled(bool b)
{
if (b) {
cliSerial->printf("en");
} else {
cliSerial->printf("dis");
}
cliSerial->printf("abled\n");
}
int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
{
if (!g.compass_enabled) {
cliSerial->printf("Compass: ");
print_enabled(false);
return (0);
}
if (!compass.init()) {
cliSerial->printf("Compass initialisation failed!\n");
return 0;
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_compass(&compass);
// we need the AHRS initialised for this test
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
int counter = 0;
float heading = 0;
print_hit_enter();
uint8_t medium_loopCounter = 0;
while (1) {
ins.wait_for_sample();
ahrs.update();
medium_loopCounter++;
if (medium_loopCounter >= 5) {
if (compass.read()) {
// Calculate heading
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: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
static_cast<double>((wrap_360_cd(ToDeg(heading) * 100)) /100),
static_cast<double>(mag.x), static_cast<double>(mag.y), static_cast<double>(mag.z),
static_cast<double>(mag_ofs.x), static_cast<double>(mag_ofs.y), static_cast<double>(mag_ofs.z));
} else {
cliSerial->printf("compass not healthy\n");
}
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->printf("saving offsets\n");
compass.save_offsets();
return (0);
}
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here
int8_t Rover::test_rangefinder(uint8_t argc, const Menu::arg *argv)
{
init_rangefinder();
delay(20);
rangefinder.update();
if (rangefinder.status(0) == RangeFinder::RangeFinder_NotConnected && rangefinder.status(1) == RangeFinder::RangeFinder_NotConnected) {
cliSerial->printf("WARNING: Rangefinder is not enabled\n");
}
print_hit_enter();
float rangefinder_dist_cm_min = 0.0f;
float rangefinder_dist_cm_max = 0.0f;
float voltage_min = 0.0f, voltage_max = 0.0f;
float rangefinder2_dist_cm_min = 0.0f;
float rangefinder2_dist_cm_max = 0.0f;
float voltage2_min = 0.0f, voltage2_max = 0.0f;
uint32_t last_print = 0;
while (true) {
delay(20);
rangefinder.update();
uint32_t now = millis();
float dist_cm = rangefinder.distance_cm(0);
float voltage = rangefinder.voltage_mv(0);
if (is_zero(rangefinder_dist_cm_min)) {
rangefinder_dist_cm_min = dist_cm;
voltage_min = voltage;
}
rangefinder_dist_cm_max = MAX(rangefinder_dist_cm_max, dist_cm);
rangefinder_dist_cm_min = MIN(rangefinder_dist_cm_min, dist_cm);
voltage_min = MIN(voltage_min, voltage);
voltage_max = MAX(voltage_max, voltage);
dist_cm = rangefinder.distance_cm(1);
voltage = rangefinder.voltage_mv(1);
if (is_zero(rangefinder2_dist_cm_min)) {
rangefinder2_dist_cm_min = dist_cm;
voltage2_min = voltage;
}
rangefinder2_dist_cm_max = MAX(rangefinder2_dist_cm_max, dist_cm);
rangefinder2_dist_cm_min = MIN(rangefinder2_dist_cm_min, dist_cm);
voltage2_min = MIN(voltage2_min, voltage);
voltage2_max = MAX(voltage2_max, voltage);
if (now - last_print >= 200) {
cliSerial->printf("rangefinder1 dist=%.1f:%.1fcm volt1=%.2f:%.2f rangefinder2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n",
static_cast<double>(rangefinder_dist_cm_min),
static_cast<double>(rangefinder_dist_cm_max),
static_cast<double>(voltage_min),
static_cast<double>(voltage_max),
static_cast<double>(rangefinder2_dist_cm_min),
static_cast<double>(rangefinder2_dist_cm_max),
static_cast<double>(voltage2_min),
static_cast<double>(voltage2_max));
voltage_min = voltage_max = 0.0f;
voltage2_min = voltage2_max = 0.0f;
rangefinder_dist_cm_min = rangefinder_dist_cm_max = 0.0f;
rangefinder2_dist_cm_min = rangefinder2_dist_cm_max = 0.0f;
last_print = now;
}
if (cliSerial->available() > 0) {
break;
}
}
return (0);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/*
* run a debug shell
*/
int8_t Rover::test_shell(uint8_t argc, const Menu::arg *argv)
{
hal.util->run_debug_shell(cliSerial);
return 0;
}
#endif
#endif // CLI_ENABLED

1
APMrover2/wscript
View File

@ -11,7 +11,6 @@ def build(bld):
'AP_Arming',
'AP_Camera',
'AP_L1_Control',
'AP_Menu',
'AP_Mount',
'AP_Navigation',
'AP_RCMapper',

15
ArduCopter/GCS_Copter.cpp
View File

@ -1,15 +0,0 @@
#include "GCS_Copter.h"
#include "Copter.h"
bool GCS_Copter::cli_enabled() const
{
#if CLI_ENABLED == ENABLED
return copter.g.cli_enabled;
#else
return false;
#endif
}
AP_HAL::BetterStream* GCS_Copter::cliSerial() {
return copter.cliSerial;
}