mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: remove CLI
This commit is contained in:
parent
c6b9c84d1f
commit
1a665280e9
@ -96,8 +96,6 @@ void Plane::stats_update(void)
|
|||||||
|
|
||||||
void Plane::setup()
|
void Plane::setup()
|
||||||
{
|
{
|
||||||
cliSerial = hal.console;
|
|
||||||
|
|
||||||
// load the default values of variables listed in var_info[]
|
// load the default values of variables listed in var_info[]
|
||||||
AP_Param::setup_sketch_defaults();
|
AP_Param::setup_sketch_defaults();
|
||||||
|
|
||||||
|
@ -1,19 +1,6 @@
|
|||||||
#include "GCS_Plane.h"
|
#include "GCS_Plane.h"
|
||||||
#include "Plane.h"
|
#include "Plane.h"
|
||||||
|
|
||||||
bool GCS_Plane::cli_enabled() const
|
|
||||||
{
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
return plane.g.cli_enabled;
|
|
||||||
#else
|
|
||||||
return false;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_HAL::BetterStream* GCS_Plane::cliSerial() {
|
|
||||||
return plane.cliSerial;
|
|
||||||
}
|
|
||||||
|
|
||||||
void GCS_Plane::send_airspeed_calibration(const Vector3f &vg)
|
void GCS_Plane::send_airspeed_calibration(const Vector3f &vg)
|
||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs(); i++) {
|
for (uint8_t i=0; i<num_gcs(); i++) {
|
||||||
|
@ -2,7 +2,6 @@
|
|||||||
|
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
#include "GCS_Mavlink.h"
|
#include "GCS_Mavlink.h"
|
||||||
#include "config.h" // for CLI_ENABLED
|
|
||||||
|
|
||||||
class GCS_Plane : public GCS
|
class GCS_Plane : public GCS
|
||||||
{
|
{
|
||||||
@ -27,7 +26,4 @@ private:
|
|||||||
|
|
||||||
GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS];
|
GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
|
|
||||||
bool cli_enabled() const override;
|
|
||||||
AP_HAL::BetterStream* cliSerial() override;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
@ -3,157 +3,6 @@
|
|||||||
|
|
||||||
#if LOGGING_ENABLED == ENABLED
|
#if LOGGING_ENABLED == ENABLED
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
// Code to Write and Read packets from DataFlash.log memory
|
|
||||||
// Code to interact with the user to dump or erase logs
|
|
||||||
|
|
||||||
// Creates a constant array of structs representing menu options
|
|
||||||
// and stores them in Flash memory, not RAM.
|
|
||||||
// User enters the string in the console to call the functions on the right.
|
|
||||||
// See class Menu in AP_Coommon for implementation details
|
|
||||||
static const struct Menu::command log_menu_commands[] = {
|
|
||||||
{"dump", MENU_FUNC(dump_log)},
|
|
||||||
{"erase", MENU_FUNC(erase_logs)},
|
|
||||||
{"enable", MENU_FUNC(select_logs)},
|
|
||||||
{"disable", MENU_FUNC(select_logs)}
|
|
||||||
};
|
|
||||||
|
|
||||||
// A Macro to create the Menu
|
|
||||||
MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&plane, &Plane::print_log_menu, bool));
|
|
||||||
|
|
||||||
bool Plane::print_log_menu(void)
|
|
||||||
{
|
|
||||||
cliSerial->printf("logs enabled: \n");
|
|
||||||
|
|
||||||
if (0 == g.log_bitmask) {
|
|
||||||
cliSerial->printf("none\n");
|
|
||||||
}else{
|
|
||||||
// Macro to make the following code a bit easier on the eye.
|
|
||||||
// Pass it the capitalised name of the log option, as defined
|
|
||||||
// in defines.h but without the LOG_ prefix. It will check for
|
|
||||||
// the bit being set and print the name of the log option to suit.
|
|
||||||
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", # _s)
|
|
||||||
PLOG(ATTITUDE_FAST);
|
|
||||||
PLOG(ATTITUDE_MED);
|
|
||||||
PLOG(GPS);
|
|
||||||
PLOG(PM);
|
|
||||||
PLOG(CTUN);
|
|
||||||
PLOG(NTUN);
|
|
||||||
PLOG(MODE);
|
|
||||||
PLOG(IMU);
|
|
||||||
PLOG(CMD);
|
|
||||||
PLOG(CURRENT);
|
|
||||||
PLOG(COMPASS);
|
|
||||||
PLOG(TECS);
|
|
||||||
PLOG(CAMERA);
|
|
||||||
PLOG(RC);
|
|
||||||
PLOG(SONAR);
|
|
||||||
#undef PLOG
|
|
||||||
}
|
|
||||||
|
|
||||||
cliSerial->printf("\n");
|
|
||||||
|
|
||||||
DataFlash.ListAvailableLogs(cliSerial);
|
|
||||||
return(true);
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
int16_t dump_log_num;
|
|
||||||
uint16_t dump_log_start;
|
|
||||||
uint16_t dump_log_end;
|
|
||||||
|
|
||||||
// check that the requested log number can be read
|
|
||||||
dump_log_num = argv[1].i;
|
|
||||||
|
|
||||||
if (dump_log_num == -2) {
|
|
||||||
DataFlash.DumpPageInfo(cliSerial);
|
|
||||||
return(-1);
|
|
||||||
} else if (dump_log_num <= 0) {
|
|
||||||
cliSerial->printf("dumping all\n");
|
|
||||||
Log_Read(0, 1, 0);
|
|
||||||
return(-1);
|
|
||||||
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
|
|
||||||
cliSerial->printf("bad log number\n");
|
|
||||||
return(-1);
|
|
||||||
}
|
|
||||||
|
|
||||||
DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
|
|
||||||
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
DataFlash.EnableWrites(false);
|
|
||||||
do_erase_logs();
|
|
||||||
DataFlash.EnableWrites(true);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
uint32_t bits;
|
|
||||||
|
|
||||||
if (argc != 2) {
|
|
||||||
cliSerial->printf("missing log type\n");
|
|
||||||
return(-1);
|
|
||||||
}
|
|
||||||
|
|
||||||
bits = 0;
|
|
||||||
|
|
||||||
// Macro to make the following code a bit easier on the eye.
|
|
||||||
// Pass it the capitalised name of the log option, as defined
|
|
||||||
// in defines.h but without the LOG_ prefix. It will check for
|
|
||||||
// that name as the argument to the command, and set the bit in
|
|
||||||
// bits accordingly.
|
|
||||||
//
|
|
||||||
if (!strcasecmp(argv[1].str, "all")) {
|
|
||||||
bits = 0xFFFFFFFFUL;
|
|
||||||
} else {
|
|
||||||
#define TARG(_s) if (!strcasecmp(argv[1].str, # _s)) bits |= MASK_LOG_ ## _s
|
|
||||||
TARG(ATTITUDE_FAST);
|
|
||||||
TARG(ATTITUDE_MED);
|
|
||||||
TARG(GPS);
|
|
||||||
TARG(PM);
|
|
||||||
TARG(CTUN);
|
|
||||||
TARG(NTUN);
|
|
||||||
TARG(MODE);
|
|
||||||
TARG(IMU);
|
|
||||||
TARG(CMD);
|
|
||||||
TARG(CURRENT);
|
|
||||||
TARG(COMPASS);
|
|
||||||
TARG(TECS);
|
|
||||||
TARG(CAMERA);
|
|
||||||
TARG(RC);
|
|
||||||
TARG(SONAR);
|
|
||||||
#undef TARG
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!strcasecmp(argv[0].str, "enable")) {
|
|
||||||
g.log_bitmask.set_and_save(g.log_bitmask | bits);
|
|
||||||
}else{
|
|
||||||
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
|
|
||||||
}
|
|
||||||
return(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
log_menu.run();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
void Plane::do_erase_logs(void)
|
|
||||||
{
|
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Erasing logs");
|
|
||||||
DataFlash.EraseAll();
|
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Log erase complete");
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Write an attitude packet
|
// Write an attitude packet
|
||||||
void Plane::Log_Write_Attitude(void)
|
void Plane::Log_Write_Attitude(void)
|
||||||
{
|
{
|
||||||
@ -561,22 +410,6 @@ const struct LogStructure Plane::log_structure[] = {
|
|||||||
"AETR", "Qhhhhh", "TimeUS,Ail,Elev,Thr,Rudd,Flap" }, \
|
"AETR", "Qhhhhh", "TimeUS,Ail,Elev,Thr,Rudd,Flap" }, \
|
||||||
};
|
};
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
// Read the DataFlash.log memory : Packet Parser
|
|
||||||
void Plane::Log_Read(uint16_t list_entry, int16_t start_page, int16_t end_page)
|
|
||||||
{
|
|
||||||
cliSerial->printf("\n" FIRMWARE_STRING
|
|
||||||
"\nFree RAM: %u\n",
|
|
||||||
(unsigned)hal.util->available_memory());
|
|
||||||
|
|
||||||
cliSerial->printf("%s\n", HAL_BOARD_NAME);
|
|
||||||
|
|
||||||
DataFlash.LogReadProcess(list_entry, start_page, end_page,
|
|
||||||
FUNCTOR_BIND_MEMBER(&Plane::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t),
|
|
||||||
cliSerial);
|
|
||||||
}
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
void Plane::Log_Write_Vehicle_Startup_Messages()
|
void Plane::Log_Write_Vehicle_Startup_Messages()
|
||||||
{
|
{
|
||||||
// only 200(?) bytes are guaranteed by DataFlash
|
// only 200(?) bytes are guaranteed by DataFlash
|
||||||
@ -593,21 +426,10 @@ void Plane::Log_Write_Vehicle_Startup_Messages()
|
|||||||
void Plane::log_init(void)
|
void Plane::log_init(void)
|
||||||
{
|
{
|
||||||
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||||
|
|
||||||
gcs().reset_cli_timeout();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#else // LOGGING_ENABLED
|
#else // LOGGING_ENABLED
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
bool Plane::print_log_menu(void) { return true; }
|
|
||||||
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
|
|
||||||
#endif // CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
void Plane::do_erase_logs(void) {}
|
|
||||||
void Plane::Log_Write_Attitude(void) {}
|
void Plane::Log_Write_Attitude(void) {}
|
||||||
void Plane::Log_Write_Fast(void) {}
|
void Plane::Log_Write_Fast(void) {}
|
||||||
void Plane::Log_Write_Performance() {}
|
void Plane::Log_Write_Performance() {}
|
||||||
@ -630,10 +452,6 @@ void Plane::Log_Write_Baro(void) {}
|
|||||||
void Plane::Log_Write_Airspeed(void) {}
|
void Plane::Log_Write_Airspeed(void) {}
|
||||||
void Plane::Log_Write_Home_And_Origin() {}
|
void Plane::Log_Write_Home_And_Origin() {}
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page) {}
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
void Plane::log_init(void) {}
|
void Plane::log_init(void) {}
|
||||||
|
|
||||||
#endif // LOGGING_ENABLED
|
#endif // LOGGING_ENABLED
|
||||||
|
@ -39,15 +39,6 @@ const AP_Param::Info Plane::var_info[] = {
|
|||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
|
GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
// @Param: CLI_ENABLED
|
|
||||||
// @DisplayName: CLI Enable
|
|
||||||
// @Description: This enables/disables the checking for three carriage returns on telemetry links on startup to enter the diagnostics command line interface
|
|
||||||
// @Values: 0:Disabled,1:Enabled
|
|
||||||
// @User: Advanced
|
|
||||||
GSCALAR(cli_enabled, "CLI_ENABLED", 0),
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// @Group: SERIAL
|
// @Group: SERIAL
|
||||||
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
|
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
|
||||||
GOBJECT(serial_manager, "SERIAL", AP_SerialManager),
|
GOBJECT(serial_manager, "SERIAL", AP_SerialManager),
|
||||||
@ -1273,19 +1264,19 @@ const AP_Param::ConversionInfo conversion_table[] = {
|
|||||||
void Plane::load_parameters(void)
|
void Plane::load_parameters(void)
|
||||||
{
|
{
|
||||||
if (!AP_Param::check_var_info()) {
|
if (!AP_Param::check_var_info()) {
|
||||||
cliSerial->printf("Bad parameter table\n");
|
hal.console->printf("Bad parameter table\n");
|
||||||
AP_HAL::panic("Bad parameter table");
|
AP_HAL::panic("Bad parameter table");
|
||||||
}
|
}
|
||||||
if (!g.format_version.load() ||
|
if (!g.format_version.load() ||
|
||||||
g.format_version != Parameters::k_format_version) {
|
g.format_version != Parameters::k_format_version) {
|
||||||
|
|
||||||
// erase all parameters
|
// erase all parameters
|
||||||
cliSerial->printf("Firmware change: erasing EEPROM...\n");
|
hal.console->printf("Firmware change: erasing EEPROM...\n");
|
||||||
AP_Param::erase_all();
|
AP_Param::erase_all();
|
||||||
|
|
||||||
// save the current format version
|
// save the current format version
|
||||||
g.format_version.set_and_save(Parameters::k_format_version);
|
g.format_version.set_and_save(Parameters::k_format_version);
|
||||||
cliSerial->printf("done.\n");
|
hal.console->printf("done.\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t before = micros();
|
uint32_t before = micros();
|
||||||
@ -1319,7 +1310,7 @@ void Plane::load_parameters(void)
|
|||||||
|
|
||||||
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_PLANE);
|
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_PLANE);
|
||||||
|
|
||||||
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
|
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -1352,7 +1343,7 @@ void Plane::convert_mixers(void)
|
|||||||
chan4->get_function() == SRV_Channel::k_rudder &&
|
chan4->get_function() == SRV_Channel::k_rudder &&
|
||||||
!chan2->function_configured() &&
|
!chan2->function_configured() &&
|
||||||
!chan4->function_configured()) {
|
!chan4->function_configured()) {
|
||||||
cliSerial->printf("Converting vtail_output %u\n", vtail_output.get());
|
hal.console->printf("Converting vtail_output %u\n", vtail_output.get());
|
||||||
switch (vtail_output) {
|
switch (vtail_output) {
|
||||||
case MIXING_UPUP:
|
case MIXING_UPUP:
|
||||||
case MIXING_UPUP_SWP:
|
case MIXING_UPUP_SWP:
|
||||||
@ -1388,7 +1379,7 @@ void Plane::convert_mixers(void)
|
|||||||
chan2->get_function() == SRV_Channel::k_elevator &&
|
chan2->get_function() == SRV_Channel::k_elevator &&
|
||||||
!chan1->function_configured() &&
|
!chan1->function_configured() &&
|
||||||
!chan2->function_configured()) {
|
!chan2->function_configured()) {
|
||||||
cliSerial->printf("convert elevon_output %u\n", elevon_output.get());
|
hal.console->printf("convert elevon_output %u\n", elevon_output.get());
|
||||||
switch (elevon_output) {
|
switch (elevon_output) {
|
||||||
case MIXING_UPUP:
|
case MIXING_UPUP:
|
||||||
case MIXING_UPUP_SWP:
|
case MIXING_UPUP_SWP:
|
||||||
|
@ -134,7 +134,7 @@ public:
|
|||||||
k_param_override_channel,
|
k_param_override_channel,
|
||||||
k_param_stall_prevention,
|
k_param_stall_prevention,
|
||||||
k_param_optflow,
|
k_param_optflow,
|
||||||
k_param_cli_enabled,
|
k_param_cli_enabled_old, // unused - CLI removed
|
||||||
k_param_trim_rc_at_start,
|
k_param_trim_rc_at_start,
|
||||||
k_param_hil_mode,
|
k_param_hil_mode,
|
||||||
k_param_land_disarm_delay, // unused - moved to AP_Landing
|
k_param_land_disarm_delay, // unused - moved to AP_Landing
|
||||||
@ -361,9 +361,6 @@ public:
|
|||||||
AP_Int16 sysid_this_mav;
|
AP_Int16 sysid_this_mav;
|
||||||
AP_Int16 sysid_my_gcs;
|
AP_Int16 sysid_my_gcs;
|
||||||
AP_Int8 telem_delay;
|
AP_Int8 telem_delay;
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
AP_Int8 cli_enabled;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
AP_Float hil_err_limit;
|
AP_Float hil_err_limit;
|
||||||
|
|
||||||
|
@ -158,7 +158,6 @@ public:
|
|||||||
private:
|
private:
|
||||||
// key aircraft parameters passed to multiple libraries
|
// key aircraft parameters passed to multiple libraries
|
||||||
AP_Vehicle::FixedWing aparm;
|
AP_Vehicle::FixedWing aparm;
|
||||||
AP_HAL::BetterStream* cliSerial;
|
|
||||||
|
|
||||||
// Global parameters are all contained within the 'g' and 'g2' classes.
|
// Global parameters are all contained within the 'g' and 'g2' classes.
|
||||||
Parameters g;
|
Parameters g;
|
||||||
@ -824,7 +823,6 @@ private:
|
|||||||
void gcs_send_airspeed_calibration(const Vector3f &vg);
|
void gcs_send_airspeed_calibration(const Vector3f &vg);
|
||||||
void gcs_retry_deferred(void);
|
void gcs_retry_deferred(void);
|
||||||
|
|
||||||
void do_erase_logs(void);
|
|
||||||
void Log_Write_Fast(void);
|
void Log_Write_Fast(void);
|
||||||
void Log_Write_Attitude(void);
|
void Log_Write_Attitude(void);
|
||||||
void Log_Write_Performance();
|
void Log_Write_Performance();
|
||||||
@ -845,7 +843,6 @@ private:
|
|||||||
void Log_Write_Vehicle_Startup_Messages();
|
void Log_Write_Vehicle_Startup_Messages();
|
||||||
void Log_Write_AOA_SSA();
|
void Log_Write_AOA_SSA();
|
||||||
void Log_Write_AETR();
|
void Log_Write_AETR();
|
||||||
void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page);
|
|
||||||
|
|
||||||
void load_parameters(void);
|
void load_parameters(void);
|
||||||
void convert_mixers(void);
|
void convert_mixers(void);
|
||||||
@ -962,17 +959,6 @@ private:
|
|||||||
void button_update(void);
|
void button_update(void);
|
||||||
void stats_update();
|
void stats_update();
|
||||||
void ice_update(void);
|
void ice_update(void);
|
||||||
void report_radio();
|
|
||||||
void report_ins();
|
|
||||||
void report_compass();
|
|
||||||
void print_radio_values();
|
|
||||||
void print_done();
|
|
||||||
void print_blanks(int16_t num);
|
|
||||||
void print_divider(void);
|
|
||||||
void zero_eeprom(void);
|
|
||||||
void print_enabled(bool b);
|
|
||||||
void print_accel_offsets_and_scaling(void);
|
|
||||||
void print_gyro_offsets(void);
|
|
||||||
void init_ardupilot();
|
void init_ardupilot();
|
||||||
void startup_ground(void);
|
void startup_ground(void);
|
||||||
enum FlightMode get_previous_mode();
|
enum FlightMode get_previous_mode();
|
||||||
@ -984,7 +970,6 @@ private:
|
|||||||
void startup_INS_ground(void);
|
void startup_INS_ground(void);
|
||||||
void update_notify();
|
void update_notify();
|
||||||
void resetPerfData(void);
|
void resetPerfData(void);
|
||||||
void print_comma(void);
|
|
||||||
bool should_log(uint32_t mask);
|
bool should_log(uint32_t mask);
|
||||||
int8_t throttle_percentage(void);
|
int8_t throttle_percentage(void);
|
||||||
void change_arm_state(void);
|
void change_arm_state(void);
|
||||||
@ -996,7 +981,6 @@ private:
|
|||||||
int8_t takeoff_tail_hold(void);
|
int8_t takeoff_tail_hold(void);
|
||||||
int16_t get_takeoff_pitch_min_cd(void);
|
int16_t get_takeoff_pitch_min_cd(void);
|
||||||
void complete_auto_takeoff(void);
|
void complete_auto_takeoff(void);
|
||||||
void print_hit_enter();
|
|
||||||
void ahrs_update();
|
void ahrs_update();
|
||||||
void update_speed_height(void);
|
void update_speed_height(void);
|
||||||
void update_GPS_50Hz(void);
|
void update_GPS_50Hz(void);
|
||||||
@ -1088,9 +1072,7 @@ private:
|
|||||||
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
||||||
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
|
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
|
||||||
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
|
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
|
||||||
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
|
|
||||||
void notify_flight_mode(enum FlightMode mode);
|
void notify_flight_mode(enum FlightMode mode);
|
||||||
void run_cli(AP_HAL::UARTDriver *port);
|
|
||||||
void log_init();
|
void log_init();
|
||||||
void init_capabilities(void);
|
void init_capabilities(void);
|
||||||
void dataflash_periodic(void);
|
void dataflash_periodic(void);
|
||||||
@ -1111,36 +1093,8 @@ private:
|
|||||||
public:
|
public:
|
||||||
void mavlink_delay_cb();
|
void mavlink_delay_cb();
|
||||||
void failsafe_check(void);
|
void failsafe_check(void);
|
||||||
bool print_log_menu(void);
|
|
||||||
int8_t dump_log(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t select_logs(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t process_logs(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t setup_mode(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t setup_factory(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t setup_erase(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_mode(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t main_menu_help(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_radio(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
|
||||||
void test_wp_print(const AP_Mission::Mission_Command& cmd);
|
|
||||||
int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_logging(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_ins(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
|
|
||||||
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#define MENU_FUNC(func) FUNCTOR_BIND(&plane, &Plane::func, int8_t, uint8_t, const Menu::arg *)
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
extern Plane plane;
|
extern Plane plane;
|
||||||
|
|
||||||
|
@ -347,12 +347,6 @@
|
|||||||
# define SCALING_SPEED 15.0
|
# define SCALING_SPEED 15.0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// use this to completely disable the CLI. We now default the CLI to
|
|
||||||
// off on smaller boards.
|
|
||||||
#ifndef CLI_ENABLED
|
|
||||||
#define CLI_ENABLED ENABLED
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// use this to disable geo-fencing
|
// use this to disable geo-fencing
|
||||||
#ifndef GEOFENCE_ENABLED
|
#ifndef GEOFENCE_ENABLED
|
||||||
# define GEOFENCE_ENABLED ENABLED
|
# define GEOFENCE_ENABLED ENABLED
|
||||||
|
@ -8,61 +8,6 @@
|
|||||||
*
|
*
|
||||||
*****************************************************************************/
|
*****************************************************************************/
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
|
|
||||||
// This is the help function
|
|
||||||
int8_t Plane::main_menu_help(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
cliSerial->printf("Commands:\n"
|
|
||||||
" logs log readback/setup mode\n"
|
|
||||||
" setup setup mode\n"
|
|
||||||
" test test mode\n"
|
|
||||||
" reboot reboot to flight mode\n"
|
|
||||||
"\n");
|
|
||||||
return(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Command/function table for the top-level menu.
|
|
||||||
static const struct Menu::command main_menu_commands[] = {
|
|
||||||
// command function called
|
|
||||||
// ======= ===============
|
|
||||||
{"logs", MENU_FUNC(process_logs)},
|
|
||||||
{"setup", MENU_FUNC(setup_mode)},
|
|
||||||
{"test", MENU_FUNC(test_mode)},
|
|
||||||
{"reboot", MENU_FUNC(reboot_board)},
|
|
||||||
{"help", MENU_FUNC(main_menu_help)},
|
|
||||||
};
|
|
||||||
|
|
||||||
// Create the top-level menu object.
|
|
||||||
MENU(main_menu, THISFIRMWARE, main_menu_commands);
|
|
||||||
|
|
||||||
int8_t Plane::reboot_board(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
|
||||||
hal.scheduler->reboot(false);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// the user wants the CLI. It never exits
|
|
||||||
void Plane::run_cli(AP_HAL::UARTDriver *port)
|
|
||||||
{
|
|
||||||
// disable the failsafe code in the CLI
|
|
||||||
hal.scheduler->register_timer_failsafe(nullptr,1);
|
|
||||||
|
|
||||||
// disable the mavlink delay callback
|
|
||||||
hal.scheduler->register_delay_callback(nullptr, 5);
|
|
||||||
|
|
||||||
cliSerial = port;
|
|
||||||
Menu::set_port(port);
|
|
||||||
port->set_blocking_writes(true);
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
main_menu.run();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
|
|
||||||
static void mavlink_delay_cb_static()
|
static void mavlink_delay_cb_static()
|
||||||
{
|
{
|
||||||
plane.mavlink_delay_cb();
|
plane.mavlink_delay_cb();
|
||||||
@ -78,9 +23,9 @@ void Plane::init_ardupilot()
|
|||||||
// initialise serial port
|
// initialise serial port
|
||||||
serial_manager.init_console();
|
serial_manager.init_console();
|
||||||
|
|
||||||
cliSerial->printf("\n\nInit " FIRMWARE_STRING
|
hal.console->printf("\n\nInit " FIRMWARE_STRING
|
||||||
"\n\nFree RAM: %u\n",
|
"\n\nFree RAM: %u\n",
|
||||||
(unsigned)hal.util->available_memory());
|
(unsigned)hal.util->available_memory());
|
||||||
|
|
||||||
|
|
||||||
//
|
//
|
||||||
@ -127,13 +72,6 @@ void Plane::init_ardupilot()
|
|||||||
gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
|
gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
|
||||||
|
|
||||||
|
|
||||||
// specify callback function for CLI menu system
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
if (g.cli_enabled) {
|
|
||||||
gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Plane::run_cli, void, AP_HAL::UARTDriver *));
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// Register mavlink_delay_cb, which will run anytime you have
|
// Register mavlink_delay_cb, which will run anytime you have
|
||||||
// more than 5ms remaining in your call to hal.scheduler->delay
|
// more than 5ms remaining in your call to hal.scheduler->delay
|
||||||
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
|
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
|
||||||
@ -159,7 +97,7 @@ void Plane::init_ardupilot()
|
|||||||
// used to detect in-flight resets
|
// used to detect in-flight resets
|
||||||
g.num_resets.set_and_save(g.num_resets+1);
|
g.num_resets.set_and_save(g.num_resets+1);
|
||||||
|
|
||||||
// init baro before we start the GCS, so that the CLI baro test works
|
// init baro
|
||||||
barometer.init();
|
barometer.init();
|
||||||
|
|
||||||
// initialise rangefinder
|
// initialise rangefinder
|
||||||
@ -196,7 +134,7 @@ void Plane::init_ardupilot()
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
if (!compass_ok) {
|
if (!compass_ok) {
|
||||||
cliSerial->printf("Compass initialisation failed!\n");
|
hal.console->printf("Compass initialisation failed!\n");
|
||||||
g.compass_enabled = false;
|
g.compass_enabled = false;
|
||||||
} else {
|
} else {
|
||||||
ahrs.set_compass(&compass);
|
ahrs.set_compass(&compass);
|
||||||
@ -235,10 +173,6 @@ void Plane::init_ardupilot()
|
|||||||
*/
|
*/
|
||||||
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
|
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
gcs().handle_interactive_setup();
|
|
||||||
#endif // CLI_ENABLED
|
|
||||||
|
|
||||||
init_capabilities();
|
init_capabilities();
|
||||||
|
|
||||||
quadplane.setup();
|
quadplane.setup();
|
||||||
@ -686,72 +620,6 @@ void Plane::resetPerfData(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
|
||||||
{
|
|
||||||
switch (mode) {
|
|
||||||
case MANUAL:
|
|
||||||
port->printf("Manual");
|
|
||||||
break;
|
|
||||||
case CIRCLE:
|
|
||||||
port->printf("Circle");
|
|
||||||
break;
|
|
||||||
case STABILIZE:
|
|
||||||
port->printf("Stabilize");
|
|
||||||
break;
|
|
||||||
case TRAINING:
|
|
||||||
port->printf("Training");
|
|
||||||
break;
|
|
||||||
case ACRO:
|
|
||||||
port->printf("ACRO");
|
|
||||||
break;
|
|
||||||
case FLY_BY_WIRE_A:
|
|
||||||
port->printf("FBW_A");
|
|
||||||
break;
|
|
||||||
case AUTOTUNE:
|
|
||||||
port->printf("AUTOTUNE");
|
|
||||||
break;
|
|
||||||
case FLY_BY_WIRE_B:
|
|
||||||
port->printf("FBW_B");
|
|
||||||
break;
|
|
||||||
case CRUISE:
|
|
||||||
port->printf("CRUISE");
|
|
||||||
break;
|
|
||||||
case AUTO:
|
|
||||||
port->printf("AUTO");
|
|
||||||
break;
|
|
||||||
case RTL:
|
|
||||||
port->printf("RTL");
|
|
||||||
break;
|
|
||||||
case LOITER:
|
|
||||||
port->printf("Loiter");
|
|
||||||
break;
|
|
||||||
case AVOID_ADSB:
|
|
||||||
port->printf("AVOID_ADSB");
|
|
||||||
break;
|
|
||||||
case GUIDED:
|
|
||||||
port->printf("Guided");
|
|
||||||
break;
|
|
||||||
case QSTABILIZE:
|
|
||||||
port->printf("QStabilize");
|
|
||||||
break;
|
|
||||||
case QHOVER:
|
|
||||||
port->printf("QHover");
|
|
||||||
break;
|
|
||||||
case QLOITER:
|
|
||||||
port->printf("QLoiter");
|
|
||||||
break;
|
|
||||||
case QLAND:
|
|
||||||
port->printf("QLand");
|
|
||||||
break;
|
|
||||||
case QRTL:
|
|
||||||
port->printf("QRTL");
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
port->printf("Mode(%u)", (unsigned)mode);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// sets notify object flight mode information
|
// sets notify object flight mode information
|
||||||
void Plane::notify_flight_mode(enum FlightMode mode)
|
void Plane::notify_flight_mode(enum FlightMode mode)
|
||||||
{
|
{
|
||||||
@ -825,13 +693,6 @@ void Plane::notify_flight_mode(enum FlightMode mode)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
|
||||||
void Plane::print_comma(void)
|
|
||||||
{
|
|
||||||
cliSerial->printf(",");
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
should we log a message type now?
|
should we log a message type now?
|
||||||
*/
|
*/
|
||||||
|
Loading…
Reference in New Issue
Block a user