2011-09-08 22:29:39 -03:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
|
|
|
|
// Functions called from the setup menu
|
2012-08-21 23:19:51 -03:00
|
|
|
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
|
|
|
|
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
|
|
|
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
|
|
|
|
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
|
|
|
|
static int8_t setup_level (uint8_t argc, const Menu::arg *argv);
|
2013-03-18 07:24:34 -03:00
|
|
|
#if !defined( __AVR_ATmega1280__ )
|
2012-11-05 00:32:13 -04:00
|
|
|
static int8_t setup_accel_scale (uint8_t argc, const Menu::arg *argv);
|
2013-03-18 07:24:34 -03:00
|
|
|
static int8_t setup_set (uint8_t argc, const Menu::arg *argv);
|
|
|
|
#endif
|
2012-08-21 23:19:51 -03:00
|
|
|
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
|
|
|
|
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
|
|
|
|
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2013-03-18 07:24:34 -03:00
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
// Command/function table for the setup menu
|
|
|
|
static const struct Menu::command setup_menu_commands[] PROGMEM = {
|
2012-08-21 23:19:51 -03:00
|
|
|
// command function called
|
|
|
|
// ======= ===============
|
|
|
|
{"reset", setup_factory},
|
|
|
|
{"radio", setup_radio},
|
|
|
|
{"modes", setup_flightmodes},
|
|
|
|
{"level", setup_level},
|
2012-11-20 03:41:04 -04:00
|
|
|
#if !defined( __AVR_ATmega1280__ )
|
2012-11-05 00:32:13 -04:00
|
|
|
{"accel", setup_accel_scale},
|
2012-11-20 03:41:04 -04:00
|
|
|
#endif
|
2012-08-21 23:19:51 -03:00
|
|
|
{"compass", setup_compass},
|
|
|
|
{"declination", setup_declination},
|
|
|
|
{"show", setup_show},
|
2013-03-18 07:24:34 -03:00
|
|
|
#if !defined( __AVR_ATmega1280__ )
|
|
|
|
{"set", setup_set},
|
|
|
|
#endif
|
2012-08-21 23:19:51 -03:00
|
|
|
{"erase", setup_erase},
|
2011-09-08 22:29:39 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
// Create the setup menu object.
|
|
|
|
MENU(setup_menu, "setup", setup_menu_commands);
|
|
|
|
|
|
|
|
// Called from the top-level menu to run the setup menu.
|
|
|
|
static int8_t
|
|
|
|
setup_mode(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
// Give the user some guidance
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Setup Mode\n"
|
2012-08-21 23:19:51 -03:00
|
|
|
"\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();
|
2011-09-08 22:29:39 -03:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Print the current configuration.
|
|
|
|
// Called by the setup menu 'show' command.
|
|
|
|
static int8_t
|
|
|
|
setup_show(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2013-03-18 07:24:34 -03:00
|
|
|
|
|
|
|
#if !defined( __AVR_ATmega1280__ )
|
|
|
|
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_P(PSTR("Parameter not found: '%s'\n"), argv[1]);
|
|
|
|
return 0;
|
|
|
|
}
|
2013-04-19 04:53:52 -03:00
|
|
|
AP_Param::show(param, argv[1].str, type, cliSerial);
|
2013-03-18 07:24:34 -03:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2013-04-19 04:53:52 -03:00
|
|
|
AP_Param::show_all(cliSerial);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
return(0);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2013-03-18 07:24:34 -03:00
|
|
|
|
|
|
|
#if !defined( __AVR_ATmega1280__ )
|
|
|
|
|
|
|
|
//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
|
|
|
|
static int8_t 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_P(PSTR("Invalid command. Usage: set <name> <value>\n"));
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
param = AP_Param::find(argv[1].str, &p_type);
|
|
|
|
if(!param)
|
|
|
|
{
|
|
|
|
cliSerial->printf_P(PSTR("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_P(PSTR("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_P(PSTR("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_P(PSTR("Cannot set parameter of type %d.\n"), p_type);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
|
|
|
|
// Called by the setup menu 'factoryreset' command.
|
|
|
|
static int8_t
|
|
|
|
setup_factory(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
int c;
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
do {
|
2012-11-21 01:19:16 -04:00
|
|
|
c = cliSerial->read();
|
2012-08-21 23:19:51 -03:00
|
|
|
} while (-1 == c);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
if (('y' != c) && ('Y' != c))
|
|
|
|
return(-1);
|
|
|
|
AP_Param::erase_all();
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
for (;; ) {
|
|
|
|
}
|
|
|
|
// note, cannot actually return here
|
|
|
|
return(0);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// Perform radio setup.
|
|
|
|
// Called by the setup menu 'radio' command.
|
|
|
|
static int8_t
|
|
|
|
setup_radio(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\n\nRadio Setup:\n"));
|
2012-08-21 23:19:51 -03:00
|
|
|
uint8_t i;
|
|
|
|
|
|
|
|
for(i = 0; i < 100; i++) {
|
|
|
|
delay(20);
|
|
|
|
read_radio();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-06-03 02:32:08 -03:00
|
|
|
if(channel_roll->radio_in < 500) {
|
2012-08-21 23:19:51 -03:00
|
|
|
while(1) {
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
|
2012-08-21 23:19:51 -03:00
|
|
|
delay(1000);
|
|
|
|
// stop here
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-06-03 02:32:08 -03:00
|
|
|
channel_roll->radio_min = channel_roll->radio_in;
|
|
|
|
channel_pitch->radio_min = channel_pitch->radio_in;
|
|
|
|
channel_throttle->radio_min = channel_throttle->radio_in;
|
|
|
|
channel_rudder->radio_min = channel_rudder->radio_in;
|
2012-08-21 23:19:51 -03:00
|
|
|
g.rc_5.radio_min = g.rc_5.radio_in;
|
|
|
|
g.rc_6.radio_min = g.rc_6.radio_in;
|
|
|
|
g.rc_7.radio_min = g.rc_7.radio_in;
|
|
|
|
g.rc_8.radio_min = g.rc_8.radio_in;
|
|
|
|
|
2013-06-03 02:32:08 -03:00
|
|
|
channel_roll->radio_max = channel_roll->radio_in;
|
|
|
|
channel_pitch->radio_max = channel_pitch->radio_in;
|
|
|
|
channel_throttle->radio_max = channel_throttle->radio_in;
|
|
|
|
channel_rudder->radio_max = channel_rudder->radio_in;
|
2012-08-21 23:19:51 -03:00
|
|
|
g.rc_5.radio_max = g.rc_5.radio_in;
|
|
|
|
g.rc_6.radio_max = g.rc_6.radio_in;
|
|
|
|
g.rc_7.radio_max = g.rc_7.radio_in;
|
|
|
|
g.rc_8.radio_max = g.rc_8.radio_in;
|
|
|
|
|
2013-06-03 02:32:08 -03:00
|
|
|
channel_roll->radio_trim = channel_roll->radio_in;
|
|
|
|
channel_pitch->radio_trim = channel_pitch->radio_in;
|
|
|
|
channel_rudder->radio_trim = channel_rudder->radio_in;
|
2012-08-21 23:19:51 -03:00
|
|
|
g.rc_5.radio_trim = 1500;
|
|
|
|
g.rc_6.radio_trim = 1500;
|
|
|
|
g.rc_7.radio_trim = 1500;
|
|
|
|
g.rc_8.radio_trim = 1500;
|
|
|
|
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
|
2012-08-21 23:19:51 -03:00
|
|
|
while(1) {
|
|
|
|
|
|
|
|
delay(20);
|
|
|
|
// Filters radio input - adjust filters in the radio.pde file
|
|
|
|
// ----------------------------------------------------------
|
|
|
|
read_radio();
|
|
|
|
|
2013-06-03 02:32:08 -03:00
|
|
|
channel_roll->update_min_max();
|
|
|
|
channel_pitch->update_min_max();
|
|
|
|
channel_throttle->update_min_max();
|
|
|
|
channel_rudder->update_min_max();
|
2012-08-21 23:19:51 -03:00
|
|
|
g.rc_5.update_min_max();
|
|
|
|
g.rc_6.update_min_max();
|
|
|
|
g.rc_7.update_min_max();
|
|
|
|
g.rc_8.update_min_max();
|
|
|
|
|
2012-11-21 01:19:16 -04:00
|
|
|
if(cliSerial->available() > 0) {
|
2012-12-04 18:22:21 -04:00
|
|
|
while (cliSerial->available() > 0) {
|
|
|
|
cliSerial->read();
|
|
|
|
}
|
2013-06-03 02:32:08 -03:00
|
|
|
channel_roll->save_eeprom();
|
|
|
|
channel_pitch->save_eeprom();
|
|
|
|
channel_throttle->save_eeprom();
|
|
|
|
channel_rudder->save_eeprom();
|
2012-08-21 23:19:51 -03:00
|
|
|
g.rc_5.save_eeprom();
|
|
|
|
g.rc_6.save_eeprom();
|
|
|
|
g.rc_7.save_eeprom();
|
|
|
|
g.rc_8.save_eeprom();
|
|
|
|
print_done();
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
trim_radio();
|
|
|
|
report_radio();
|
|
|
|
return(0);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static int8_t
|
|
|
|
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2012-12-04 18:22:21 -04:00
|
|
|
uint8_t switchPosition, mode = 0;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
|
2012-08-21 23:19:51 -03:00
|
|
|
print_hit_enter();
|
|
|
|
trim_radio();
|
|
|
|
|
|
|
|
while(1) {
|
|
|
|
delay(20);
|
|
|
|
read_radio();
|
|
|
|
switchPosition = readSwitch();
|
|
|
|
|
|
|
|
|
|
|
|
// look for control switch change
|
|
|
|
if (oldSwitchPosition != switchPosition) {
|
|
|
|
// force position 5 to MANUAL
|
|
|
|
if (switchPosition > 4) {
|
|
|
|
flight_modes[switchPosition] = MANUAL;
|
|
|
|
}
|
|
|
|
// update our current mode
|
|
|
|
mode = flight_modes[switchPosition];
|
|
|
|
|
|
|
|
// update the user
|
|
|
|
print_switch(switchPosition, mode);
|
|
|
|
|
|
|
|
// Remember switch position
|
|
|
|
oldSwitchPosition = switchPosition;
|
|
|
|
}
|
|
|
|
|
|
|
|
// look for stick input
|
|
|
|
int16_t radioInputSwitch = radio_input_switch();
|
|
|
|
|
|
|
|
if (radioInputSwitch != 0) {
|
|
|
|
|
|
|
|
mode += radioInputSwitch;
|
|
|
|
|
|
|
|
while (
|
|
|
|
mode != MANUAL &&
|
|
|
|
mode != CIRCLE &&
|
|
|
|
mode != STABILIZE &&
|
2012-12-04 02:32:37 -04:00
|
|
|
mode != TRAINING &&
|
2013-07-10 10:25:38 -03:00
|
|
|
mode != ACRO &&
|
2012-08-21 23:19:51 -03:00
|
|
|
mode != FLY_BY_WIRE_A &&
|
|
|
|
mode != FLY_BY_WIRE_B &&
|
2013-07-13 07:05:53 -03:00
|
|
|
mode != CRUISE &&
|
2012-08-21 23:19:51 -03:00
|
|
|
mode != AUTO &&
|
|
|
|
mode != RTL &&
|
|
|
|
mode != LOITER)
|
|
|
|
{
|
|
|
|
if (mode < MANUAL)
|
|
|
|
mode = LOITER;
|
|
|
|
else if (mode >LOITER)
|
|
|
|
mode = MANUAL;
|
|
|
|
else
|
|
|
|
mode += radioInputSwitch;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Override position 5
|
|
|
|
if(switchPosition > 4)
|
|
|
|
mode = MANUAL;
|
|
|
|
|
|
|
|
// save new mode
|
|
|
|
flight_modes[switchPosition] = mode;
|
|
|
|
|
|
|
|
// print new mode
|
|
|
|
print_switch(switchPosition, mode);
|
|
|
|
}
|
|
|
|
|
|
|
|
// escape hatch
|
2012-11-21 01:19:16 -04:00
|
|
|
if(cliSerial->available() > 0) {
|
2012-08-21 23:19:51 -03:00
|
|
|
// save changes
|
2011-09-08 22:29:39 -03:00
|
|
|
for (mode=0; mode<6; mode++)
|
|
|
|
flight_modes[mode].save();
|
2012-08-21 23:19:51 -03:00
|
|
|
report_flight_modes();
|
|
|
|
print_done();
|
|
|
|
return (0);
|
|
|
|
}
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static int8_t
|
|
|
|
setup_declination(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
compass.set_declination(radians(argv[1].f));
|
|
|
|
report_compass();
|
2011-09-08 22:29:39 -03:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static int8_t
|
|
|
|
setup_erase(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
int c;
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
do {
|
2012-11-21 01:19:16 -04:00
|
|
|
c = cliSerial->read();
|
2012-08-21 23:19:51 -03:00
|
|
|
} while (-1 == c);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
if (('y' != c) && ('Y' != c))
|
|
|
|
return(-1);
|
|
|
|
zero_eeprom();
|
|
|
|
return 0;
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2012-06-12 03:01:25 -03:00
|
|
|
static int8_t
|
|
|
|
setup_level(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2013-05-02 00:38:28 -03:00
|
|
|
startup_INS_ground(true);
|
2012-08-21 23:19:51 -03:00
|
|
|
return 0;
|
2012-06-12 03:01:25 -03:00
|
|
|
}
|
|
|
|
|
2012-11-20 03:41:04 -04:00
|
|
|
#if !defined( __AVR_ATmega1280__ )
|
2012-11-21 01:19:16 -04:00
|
|
|
/*
|
|
|
|
handle full accelerometer calibration via user dialog
|
|
|
|
*/
|
|
|
|
|
2012-11-05 00:32:13 -04:00
|
|
|
static int8_t
|
|
|
|
setup_accel_scale(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2013-02-19 00:30:51 -04:00
|
|
|
float trim_roll, trim_pitch;
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println_P(PSTR("Initialising gyros"));
|
2013-01-13 01:04:18 -04:00
|
|
|
|
|
|
|
ahrs.init();
|
|
|
|
ahrs.set_fly_forward(true);
|
2013-05-23 22:21:32 -03:00
|
|
|
ahrs.set_wind_estimation(true);
|
2013-01-13 01:04:18 -04:00
|
|
|
|
2013-09-19 05:33:42 -03:00
|
|
|
ins.init(AP_InertialSensor::COLD_START, ins_sample_rate);
|
2012-12-19 18:31:20 -04:00
|
|
|
AP_InertialSensor_UserInteractStream interact(hal.console);
|
2013-09-19 05:33:42 -03:00
|
|
|
bool success = ins.calibrate_accel(&interact, trim_roll, trim_pitch);
|
2012-12-19 18:31:20 -04:00
|
|
|
if (success) {
|
2013-02-19 00:30:51 -04:00
|
|
|
// reset ahrs's trim to suggested values from calibration routine
|
|
|
|
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
2012-11-20 04:27:52 -04:00
|
|
|
}
|
2012-11-05 00:32:13 -04:00
|
|
|
report_ins();
|
|
|
|
return(0);
|
|
|
|
}
|
2012-11-20 03:41:04 -04:00
|
|
|
#endif
|
2012-11-05 00:32:13 -04:00
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
static int8_t
|
|
|
|
setup_compass(uint8_t argc, const Menu::arg *argv)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
|
|
|
if (!compass.init()) {
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
2011-09-08 22:29:39 -03:00
|
|
|
g.compass_enabled = false;
|
|
|
|
} else {
|
|
|
|
g.compass_enabled = true;
|
|
|
|
}
|
2012-08-21 23:19:51 -03:00
|
|
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
|
|
|
g.compass_enabled = false;
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
} else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
|
|
|
|
compass.set_offsets(0,0,0);
|
2012-01-15 19:15:08 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
} else {
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
|
2012-08-21 23:19:51 -03:00
|
|
|
report_compass();
|
|
|
|
return 0;
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
g.compass_enabled.save();
|
|
|
|
report_compass();
|
|
|
|
return 0;
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/***************************************************************************/
|
|
|
|
// CLI reports
|
|
|
|
/***************************************************************************/
|
|
|
|
|
|
|
|
static void report_radio()
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
//print_blanks(2);
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Radio\n"));
|
2012-08-21 23:19:51 -03:00
|
|
|
print_divider();
|
|
|
|
// radio
|
|
|
|
print_radio_values();
|
|
|
|
print_blanks(2);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2012-11-05 00:32:13 -04:00
|
|
|
static void report_ins()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
//print_blanks(2);
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("INS\n"));
|
2012-08-21 23:19:51 -03:00
|
|
|
print_divider();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
print_gyro_offsets();
|
2012-11-05 00:32:13 -04:00
|
|
|
print_accel_offsets_and_scaling();
|
2012-08-21 23:19:51 -03:00
|
|
|
print_blanks(2);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void report_compass()
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
//print_blanks(2);
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Compass: "));
|
2011-09-08 22:29:39 -03:00
|
|
|
|
|
|
|
switch (compass.product_id) {
|
|
|
|
case AP_COMPASS_TYPE_HMC5883L:
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println_P(PSTR("HMC5883L"));
|
2011-09-08 22:29:39 -03:00
|
|
|
break;
|
|
|
|
case AP_COMPASS_TYPE_HMC5843:
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println_P(PSTR("HMC5843"));
|
2011-09-08 22:29:39 -03:00
|
|
|
break;
|
|
|
|
case AP_COMPASS_TYPE_HIL:
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println_P(PSTR("HIL"));
|
2011-09-08 22:29:39 -03:00
|
|
|
break;
|
|
|
|
default:
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println_P(PSTR("??"));
|
2011-09-08 22:29:39 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
print_divider();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
print_enabled(g.compass_enabled);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// mag declination
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Mag Declination: %4.4f\n"),
|
2012-08-21 23:19:51 -03:00
|
|
|
degrees(compass.get_declination()));
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
Vector3f offsets = compass.get_offsets();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// mag offsets
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
|
2012-08-21 23:19:51 -03:00
|
|
|
offsets.x,
|
|
|
|
offsets.y,
|
|
|
|
offsets.z);
|
|
|
|
print_blanks(2);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void report_flight_modes()
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
//print_blanks(2);
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Flight modes\n"));
|
2012-08-21 23:19:51 -03:00
|
|
|
print_divider();
|
|
|
|
|
|
|
|
for(int16_t i = 0; i < 6; i++ ) {
|
|
|
|
print_switch(i, flight_modes[i]);
|
|
|
|
}
|
|
|
|
print_blanks(2);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/***************************************************************************/
|
|
|
|
// CLI utilities
|
|
|
|
/***************************************************************************/
|
|
|
|
|
|
|
|
static void
|
|
|
|
print_radio_values()
|
|
|
|
{
|
2013-06-03 02:32:08 -03:00
|
|
|
cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)channel_roll->radio_min, (int)channel_roll->radio_trim, (int)channel_roll->radio_max);
|
|
|
|
cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)channel_pitch->radio_min, (int)channel_pitch->radio_trim, (int)channel_pitch->radio_max);
|
|
|
|
cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)channel_throttle->radio_min, (int)channel_throttle->radio_trim, (int)channel_throttle->radio_max);
|
|
|
|
cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)channel_rudder->radio_min, (int)channel_rudder->radio_trim, (int)channel_rudder->radio_max);
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
|
|
|
|
cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
|
|
|
|
cliSerial->printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
|
|
|
|
cliSerial->printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
2012-12-04 18:22:21 -04:00
|
|
|
print_switch(uint8_t p, uint8_t m)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Pos %d: "),p);
|
2013-04-20 02:18:10 -03:00
|
|
|
print_flight_mode(cliSerial, m);
|
|
|
|
cliSerial->println();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
print_done()
|
|
|
|
{
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
2012-08-18 06:26:13 -03:00
|
|
|
print_blanks(int16_t num)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
while(num > 0) {
|
|
|
|
num--;
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println();
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
print_divider(void)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
for (int16_t i = 0; i < 40; i++) {
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("-"));
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->println();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static int8_t
|
|
|
|
radio_input_switch(void)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
static int8_t bouncer = 0;
|
|
|
|
|
|
|
|
|
2013-06-03 02:32:08 -03:00
|
|
|
if (int16_t(channel_roll->radio_in - channel_roll->radio_trim) > 100) {
|
2012-08-21 23:19:51 -03:00
|
|
|
bouncer = 10;
|
|
|
|
}
|
2013-06-03 02:32:08 -03:00
|
|
|
if (int16_t(channel_roll->radio_in - channel_roll->radio_trim) < -100) {
|
2012-08-21 23:19:51 -03:00
|
|
|
bouncer = -10;
|
|
|
|
}
|
|
|
|
if (bouncer >0) {
|
|
|
|
bouncer--;
|
|
|
|
}
|
|
|
|
if (bouncer <0) {
|
|
|
|
bouncer++;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (bouncer == 1 || bouncer == -1) {
|
|
|
|
return bouncer;
|
|
|
|
} else {
|
|
|
|
return 0;
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void zero_eeprom(void)
|
|
|
|
{
|
2012-12-04 18:22:21 -04:00
|
|
|
uint8_t b = 0;
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
|
2012-12-12 17:58:28 -04:00
|
|
|
for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i++) {
|
|
|
|
hal.storage->write_byte(i, b);
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("done\n"));
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void print_enabled(bool b)
|
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
if(b)
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("en"));
|
2012-08-21 23:19:51 -03:00
|
|
|
else
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("dis"));
|
|
|
|
cliSerial->printf_P(PSTR("abled\n"));
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
2012-11-05 00:32:13 -04:00
|
|
|
print_accel_offsets_and_scaling(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-11-05 00:32:13 -04:00
|
|
|
Vector3f accel_offsets = ins.get_accel_offsets();
|
|
|
|
Vector3f accel_scale = ins.get_accel_scale();
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
|
2012-11-05 00:32:13 -04:00
|
|
|
(float)accel_offsets.x, // Pitch
|
|
|
|
(float)accel_offsets.y, // Roll
|
|
|
|
(float)accel_offsets.z, // YAW
|
|
|
|
(float)accel_scale.x, // Pitch
|
|
|
|
(float)accel_scale.y, // Roll
|
|
|
|
(float)accel_scale.z); // YAW
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
print_gyro_offsets(void)
|
|
|
|
{
|
2012-11-05 00:32:13 -04:00
|
|
|
Vector3f gyro_offsets = ins.get_gyro_offsets();
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial->printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
|
2012-11-05 00:32:13 -04:00
|
|
|
(float)gyro_offsets.x,
|
|
|
|
(float)gyro_offsets.y,
|
|
|
|
(float)gyro_offsets.z);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#endif // CLI_ENABLED
|