mirror of https://github.com/ArduPilot/ardupilot
622 lines
14 KiB
Plaintext
622 lines
14 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
// Functions called from the setup menu
|
|
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_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);
|
|
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
|
|
|
|
// Command/function table for the setup menu
|
|
const struct Menu::command setup_menu_commands[] PROGMEM = {
|
|
// command function called
|
|
// ======= ===============
|
|
{"reset", setup_factory},
|
|
{"radio", setup_radio},
|
|
{"modes", setup_flightmodes},
|
|
{"compass", setup_compass},
|
|
{"declination", setup_declination},
|
|
{"battery", setup_batt_monitor},
|
|
{"show", setup_show},
|
|
{"erase", 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
|
|
setup_mode(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
// Give the user some guidance
|
|
Serial.printf_P(PSTR("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();
|
|
}
|
|
|
|
// Print the current configuration.
|
|
// Called by the setup menu 'show' command.
|
|
static int8_t
|
|
setup_show(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
uint8_t i;
|
|
// clear the area
|
|
print_blanks(8);
|
|
|
|
report_radio();
|
|
report_batt_monitor();
|
|
report_gains();
|
|
report_xtrack();
|
|
report_throttle();
|
|
report_flight_modes();
|
|
report_imu();
|
|
report_compass();
|
|
|
|
Serial.printf_P(PSTR("Raw Values\n"));
|
|
print_divider();
|
|
AP_Var_menu_show(argc, argv);
|
|
|
|
return(0);
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
|
|
uint8_t i;
|
|
int c;
|
|
|
|
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
|
|
|
|
do {
|
|
c = Serial.read();
|
|
} while (-1 == c);
|
|
|
|
if (('y' != c) && ('Y' != c))
|
|
return(-1);
|
|
AP_Var::erase_all();
|
|
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
|
|
|
|
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
|
|
|
|
for (;;) {
|
|
}
|
|
// note, cannot actually return here
|
|
return(0);
|
|
}
|
|
|
|
|
|
// Perform radio setup.
|
|
// Called by the setup menu 'radio' command.
|
|
static int8_t
|
|
setup_radio(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
Serial.printf_P(PSTR("\n\nRadio Setup:\n"));
|
|
uint8_t i;
|
|
|
|
for(i = 0; i < 100;i++){
|
|
delay(20);
|
|
read_radio();
|
|
}
|
|
|
|
|
|
if(g.channel_roll.radio_in < 500){
|
|
while(1){
|
|
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
|
|
delay(1000);
|
|
// stop here
|
|
}
|
|
}
|
|
|
|
g.channel_roll.radio_min = g.channel_roll.radio_in;
|
|
g.channel_pitch.radio_min = g.channel_pitch.radio_in;
|
|
g.channel_throttle.radio_min = g.channel_throttle.radio_in;
|
|
g.channel_rudder.radio_min = g.channel_rudder.radio_in;
|
|
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;
|
|
|
|
g.channel_roll.radio_max = g.channel_roll.radio_in;
|
|
g.channel_pitch.radio_max = g.channel_pitch.radio_in;
|
|
g.channel_throttle.radio_max = g.channel_throttle.radio_in;
|
|
g.channel_rudder.radio_max = g.channel_rudder.radio_in;
|
|
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;
|
|
|
|
g.channel_roll.radio_trim = g.channel_roll.radio_in;
|
|
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
|
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
|
|
g.rc_5.radio_trim = 1500;
|
|
g.rc_6.radio_trim = 1500;
|
|
g.rc_7.radio_trim = 1500;
|
|
g.rc_8.radio_trim = 1500;
|
|
|
|
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
|
|
while(1){
|
|
|
|
delay(20);
|
|
// Filters radio input - adjust filters in the radio.pde file
|
|
// ----------------------------------------------------------
|
|
read_radio();
|
|
|
|
g.channel_roll.update_min_max();
|
|
g.channel_pitch.update_min_max();
|
|
g.channel_throttle.update_min_max();
|
|
g.channel_rudder.update_min_max();
|
|
g.rc_5.update_min_max();
|
|
g.rc_6.update_min_max();
|
|
g.rc_7.update_min_max();
|
|
g.rc_8.update_min_max();
|
|
|
|
if(Serial.available() > 0){
|
|
Serial.flush();
|
|
g.channel_roll.save_eeprom();
|
|
g.channel_pitch.save_eeprom();
|
|
g.channel_throttle.save_eeprom();
|
|
g.channel_rudder.save_eeprom();
|
|
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);
|
|
}
|
|
|
|
|
|
static int8_t
|
|
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
byte switchPosition, oldSwitchPosition, mode;
|
|
|
|
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
|
|
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) {
|
|
g.flight_modes[switchPosition] = MANUAL;
|
|
}
|
|
// update our current mode
|
|
mode = g.flight_modes[switchPosition];
|
|
|
|
// update the user
|
|
print_switch(switchPosition, mode);
|
|
|
|
// Remember switch position
|
|
oldSwitchPosition = switchPosition;
|
|
}
|
|
|
|
// look for stick input
|
|
int radioInputSwitch = radio_input_switch();
|
|
|
|
if (radioInputSwitch != 0){
|
|
|
|
mode += radioInputSwitch;
|
|
|
|
while (
|
|
mode != MANUAL &&
|
|
mode != CIRCLE &&
|
|
mode != STABILIZE &&
|
|
mode != FLY_BY_WIRE_A &&
|
|
mode != FLY_BY_WIRE_B &&
|
|
mode != AUTO &&
|
|
mode != RTL &&
|
|
mode != LOITER &&
|
|
mode != TAKEOFF &&
|
|
mode != LAND)
|
|
{
|
|
if (mode < MANUAL)
|
|
mode = LAND;
|
|
else if (mode >LAND)
|
|
mode = MANUAL;
|
|
else
|
|
mode += radioInputSwitch;
|
|
}
|
|
|
|
// Override position 5
|
|
if(switchPosition > 4)
|
|
mode = MANUAL;
|
|
|
|
// save new mode
|
|
g.flight_modes[switchPosition] = mode;
|
|
|
|
// print new mode
|
|
print_switch(switchPosition, mode);
|
|
}
|
|
|
|
// escape hatch
|
|
if(Serial.available() > 0){
|
|
// save changes
|
|
g.flight_modes.save();
|
|
report_flight_modes();
|
|
print_done();
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
setup_declination(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
compass.set_declination(radians(argv[1].f));
|
|
report_compass();
|
|
}
|
|
|
|
|
|
static int8_t
|
|
setup_erase(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
uint8_t i;
|
|
int c;
|
|
|
|
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
|
|
|
|
do {
|
|
c = Serial.read();
|
|
} while (-1 == c);
|
|
|
|
if (('y' != c) && ('Y' != c))
|
|
return(-1);
|
|
zero_eeprom();
|
|
return 0;
|
|
}
|
|
|
|
static int8_t
|
|
setup_compass(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
|
g.compass_enabled = true;
|
|
bool junkbool = compass.init();
|
|
|
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
|
g.compass_enabled = false;
|
|
|
|
} else {
|
|
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
|
|
report_compass();
|
|
return 0;
|
|
}
|
|
|
|
g.compass_enabled.save();
|
|
report_compass();
|
|
return 0;
|
|
}
|
|
|
|
static int8_t
|
|
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if(argv[1].i >= 0 && argv[1].i <= 4){
|
|
g.battery_monitoring.set_and_save(argv[1].i);
|
|
|
|
} else {
|
|
Serial.printf_P(PSTR("\nOptions: 0-4"));
|
|
}
|
|
|
|
report_batt_monitor();
|
|
return 0;
|
|
}
|
|
|
|
/***************************************************************************/
|
|
// CLI defaults
|
|
/***************************************************************************/
|
|
|
|
void
|
|
default_flight_modes()
|
|
{
|
|
g.flight_modes[0] = FLIGHT_MODE_1;
|
|
g.flight_modes[1] = FLIGHT_MODE_2;
|
|
g.flight_modes[2] = FLIGHT_MODE_3;
|
|
g.flight_modes[3] = FLIGHT_MODE_4;
|
|
g.flight_modes[4] = FLIGHT_MODE_5;
|
|
g.flight_modes[5] = FLIGHT_MODE_6;
|
|
g.flight_modes.save();
|
|
}
|
|
|
|
void
|
|
default_log_bitmask()
|
|
{
|
|
|
|
// convenience macro for testing LOG_* and setting LOGBIT_*
|
|
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
|
|
|
|
g.log_bitmask =
|
|
LOGBIT(ATTITUDE_FAST) |
|
|
LOGBIT(ATTITUDE_MED) |
|
|
LOGBIT(GPS) |
|
|
LOGBIT(PM) |
|
|
LOGBIT(CTUN) |
|
|
LOGBIT(NTUN) |
|
|
LOGBIT(MODE) |
|
|
LOGBIT(RAW) |
|
|
LOGBIT(CMD) |
|
|
LOGBIT(CUR);
|
|
#undef LOGBIT
|
|
|
|
g.log_bitmask.save();
|
|
}
|
|
|
|
/***************************************************************************/
|
|
// CLI reports
|
|
/***************************************************************************/
|
|
|
|
void report_batt_monitor()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("Batt Mointor\n"));
|
|
print_divider();
|
|
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
|
|
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell"));
|
|
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell"));
|
|
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
|
|
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
|
|
print_blanks(2);
|
|
}
|
|
void report_radio()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("Radio\n"));
|
|
print_divider();
|
|
// radio
|
|
print_radio_values();
|
|
print_blanks(2);
|
|
}
|
|
|
|
void report_gains()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("Gains\n"));
|
|
print_divider();
|
|
|
|
Serial.printf_P(PSTR("servo roll:\n"));
|
|
print_PID(&g.pidServoRoll);
|
|
|
|
Serial.printf_P(PSTR("servo pitch:\n"));
|
|
print_PID(&g.pidServoPitch);
|
|
|
|
Serial.printf_P(PSTR("servo rudder:\n"));
|
|
print_PID(&g.pidServoRudder);
|
|
|
|
Serial.printf_P(PSTR("nav roll:\n"));
|
|
print_PID(&g.pidNavRoll);
|
|
|
|
Serial.printf_P(PSTR("nav pitch airpseed:\n"));
|
|
print_PID(&g.pidNavPitchAirspeed);
|
|
|
|
Serial.printf_P(PSTR("energry throttle:\n"));
|
|
print_PID(&g.pidTeThrottle);
|
|
|
|
Serial.printf_P(PSTR("nav pitch alt:\n"));
|
|
print_PID(&g.pidNavPitchAltitude);
|
|
|
|
print_blanks(2);
|
|
}
|
|
|
|
void report_xtrack()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("Crosstrack\n"));
|
|
print_divider();
|
|
// radio
|
|
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
|
|
"XTRACK angle: %d\n"),
|
|
(float)g.crosstrack_gain,
|
|
(int)g.crosstrack_entry_angle);
|
|
print_blanks(2);
|
|
}
|
|
|
|
void report_throttle()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("Throttle\n"));
|
|
print_divider();
|
|
|
|
Serial.printf_P(PSTR("min: %d\n"
|
|
"max: %d\n"
|
|
"cruise: %d\n"
|
|
"failsafe_enabled: %d\n"
|
|
"failsafe_value: %d\n"),
|
|
(int)g.throttle_min,
|
|
(int)g.throttle_max,
|
|
(int)g.throttle_cruise,
|
|
(int)g.throttle_fs_enabled,
|
|
(int)g.throttle_fs_value);
|
|
print_blanks(2);
|
|
}
|
|
|
|
void report_imu()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("IMU\n"));
|
|
print_divider();
|
|
|
|
print_gyro_offsets();
|
|
print_accel_offsets();
|
|
print_blanks(2);
|
|
}
|
|
|
|
void report_compass()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("Compass\n"));
|
|
print_divider();
|
|
|
|
print_enabled(g.compass_enabled);
|
|
|
|
// mag declination
|
|
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
|
|
degrees(compass.get_declination()));
|
|
|
|
Vector3f offsets = compass.get_offsets();
|
|
|
|
// mag offsets
|
|
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
|
|
offsets.x,
|
|
offsets.y,
|
|
offsets.z);
|
|
print_blanks(2);
|
|
}
|
|
|
|
void report_flight_modes()
|
|
{
|
|
//print_blanks(2);
|
|
Serial.printf_P(PSTR("Flight modes\n"));
|
|
print_divider();
|
|
|
|
for(int i = 0; i < 6; i++ ){
|
|
print_switch(i, g.flight_modes[i]);
|
|
}
|
|
print_blanks(2);
|
|
}
|
|
|
|
/***************************************************************************/
|
|
// CLI utilities
|
|
/***************************************************************************/
|
|
|
|
void
|
|
print_PID(PID * pid)
|
|
{
|
|
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
|
|
pid->kP(),
|
|
pid->kI(),
|
|
pid->kD(),
|
|
(long)pid->imax());
|
|
}
|
|
|
|
void
|
|
print_radio_values()
|
|
{
|
|
Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
|
|
Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
|
|
Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
|
|
Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
|
|
Serial.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);
|
|
Serial.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);
|
|
Serial.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);
|
|
Serial.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);
|
|
|
|
}
|
|
|
|
void
|
|
print_switch(byte p, byte m)
|
|
{
|
|
Serial.printf_P(PSTR("Pos %d: "),p);
|
|
Serial.println(flight_mode_strings[m]);
|
|
}
|
|
|
|
void
|
|
print_done()
|
|
{
|
|
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
|
|
}
|
|
|
|
void
|
|
print_blanks(int num)
|
|
{
|
|
while(num > 0){
|
|
num--;
|
|
Serial.println("");
|
|
}
|
|
}
|
|
|
|
void
|
|
print_divider(void)
|
|
{
|
|
for (int i = 0; i < 40; i++) {
|
|
Serial.printf_P(PSTR("-"));
|
|
}
|
|
Serial.println("");
|
|
}
|
|
|
|
int8_t
|
|
radio_input_switch(void)
|
|
{
|
|
static int8_t bouncer = 0;
|
|
|
|
|
|
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) {
|
|
bouncer = 10;
|
|
}
|
|
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) {
|
|
bouncer = -10;
|
|
}
|
|
if (bouncer >0) {
|
|
bouncer --;
|
|
}
|
|
if (bouncer <0) {
|
|
bouncer ++;
|
|
}
|
|
|
|
if (bouncer == 1 || bouncer == -1) {
|
|
return bouncer;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
void zero_eeprom(void)
|
|
{
|
|
byte b;
|
|
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
|
|
for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
|
|
eeprom_write_byte((uint8_t *) i, b);
|
|
}
|
|
Serial.printf_P(PSTR("done\n"));
|
|
}
|
|
|
|
void print_enabled(bool b)
|
|
{
|
|
if(b)
|
|
Serial.printf_P(PSTR("en"));
|
|
else
|
|
Serial.printf_P(PSTR("dis"));
|
|
Serial.printf_P(PSTR("abled\n"));
|
|
}
|
|
|
|
void
|
|
print_accel_offsets(void)
|
|
{
|
|
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
|
|
(float)imu.ax(),
|
|
(float)imu.ay(),
|
|
(float)imu.az());
|
|
}
|
|
|
|
void
|
|
print_gyro_offsets(void)
|
|
{
|
|
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
|
|
(float)imu.gx(),
|
|
(float)imu.gy(),
|
|
(float)imu.gz());
|
|
}
|
|
|
|
|