1ce2308d11
Added Simple option to mode switch positions removed unused OptFlow code added reset lat and lon error removed unused init_auto Fixed log formatting Added simple mode bitmask removed pitchmax added CLI setup for enabling Simple mode to any switch position
1156 lines
28 KiB
Plaintext
1156 lines
28 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
// Functions called from the setup menu
|
|
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_motors (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_accel (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_tune (uint8_t argc, const Menu::arg *argv);
|
|
//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_esc (uint8_t argc, const Menu::arg *argv);
|
|
#ifdef OPTFLOW_ENABLED
|
|
static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv);
|
|
#endif
|
|
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
static int8_t setup_heli (uint8_t argc, const Menu::arg *argv);
|
|
static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv);
|
|
#endif
|
|
|
|
// Command/function table for the setup menu
|
|
const struct Menu::command setup_menu_commands[] PROGMEM = {
|
|
// command function called
|
|
// ======= ===============
|
|
{"erase", setup_erase},
|
|
{"reset", setup_factory},
|
|
{"radio", setup_radio},
|
|
{"frame", setup_frame},
|
|
{"motors", setup_motors},
|
|
{"esc", setup_esc},
|
|
{"level", setup_accel},
|
|
{"modes", setup_flightmodes},
|
|
{"battery", setup_batt_monitor},
|
|
{"sonar", setup_sonar},
|
|
{"compass", setup_compass},
|
|
{"tune", setup_tune},
|
|
// {"offsets", setup_mag_offset},
|
|
{"declination", setup_declination},
|
|
#ifdef OPTFLOW_ENABLED
|
|
{"optflow", setup_optflow},
|
|
#endif
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
{"heli", setup_heli},
|
|
{"gyro", setup_gyro},
|
|
#endif
|
|
{"show", setup_show}
|
|
};
|
|
|
|
// 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)
|
|
{
|
|
// Give the user some guidance
|
|
Serial.printf_P(PSTR("Setup Mode\n\n\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"));
|
|
|
|
if(g.rc_1.radio_min >= 1300){
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("\n!Warning, your radio is not configured!"));
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("\n Type 'radio' to configure now.\n\n"));
|
|
}
|
|
|
|
// Run the setup menu. When the menu exits, we will return to the main menu.
|
|
setup_menu.run();
|
|
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)
|
|
{
|
|
// clear the area
|
|
print_blanks(8);
|
|
|
|
report_version();
|
|
report_radio();
|
|
report_frame();
|
|
report_batt_monitor();
|
|
report_sonar();
|
|
//report_gains();
|
|
//report_xtrack();
|
|
//report_throttle();
|
|
report_flight_modes();
|
|
report_imu();
|
|
report_compass();
|
|
|
|
#ifdef OPTFLOW_ENABLED
|
|
report_optflow();
|
|
#endif
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
report_heli();
|
|
report_gyro();
|
|
#endif
|
|
|
|
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)
|
|
{
|
|
int c;
|
|
|
|
Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n"));
|
|
|
|
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 - reboot APM"));
|
|
|
|
delay(1000);
|
|
//default_gains();
|
|
|
|
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.println("\n\nRadio Setup:");
|
|
uint8_t i;
|
|
|
|
for(i = 0; i < 100;i++){
|
|
delay(20);
|
|
read_radio();
|
|
}
|
|
|
|
if(g.rc_1.radio_in < 500){
|
|
while(1){
|
|
//Serial.printf_P(PSTR("\nNo radio; Check connectors."));
|
|
delay(1000);
|
|
// stop here
|
|
}
|
|
}
|
|
|
|
g.rc_1.radio_min = g.rc_1.radio_in;
|
|
g.rc_2.radio_min = g.rc_2.radio_in;
|
|
g.rc_3.radio_min = g.rc_3.radio_in;
|
|
g.rc_4.radio_min = g.rc_4.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.rc_1.radio_max = g.rc_1.radio_in;
|
|
g.rc_2.radio_max = g.rc_2.radio_in;
|
|
g.rc_3.radio_max = g.rc_3.radio_in;
|
|
g.rc_4.radio_max = g.rc_4.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.rc_1.radio_trim = g.rc_1.radio_in;
|
|
g.rc_2.radio_trim = g.rc_2.radio_in;
|
|
g.rc_4.radio_trim = g.rc_4.radio_in;
|
|
// 3 is not trimed
|
|
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: "));
|
|
while(1){
|
|
|
|
delay(20);
|
|
// Filters radio input - adjust filters in the radio.pde file
|
|
// ----------------------------------------------------------
|
|
read_radio();
|
|
|
|
g.rc_1.update_min_max();
|
|
g.rc_2.update_min_max();
|
|
g.rc_3.update_min_max();
|
|
g.rc_4.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){
|
|
delay(20);
|
|
Serial.flush();
|
|
|
|
g.rc_1.save_eeprom();
|
|
g.rc_2.save_eeprom();
|
|
g.rc_3.save_eeprom();
|
|
g.rc_4.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;
|
|
}
|
|
}
|
|
report_radio();
|
|
return(0);
|
|
}
|
|
|
|
static int8_t
|
|
setup_esc(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
Serial.printf_P(PSTR("\nESC Calibration:\n"
|
|
"-1 Unplug USB and battery\n"
|
|
"-2 Move CLI/FLY switch to FLY mode\n"
|
|
"-3 Move throttle to max, connect battery\n"
|
|
"-4 After two long beeps, throttle to 0, then test\n\n"
|
|
" Press Enter to cancel.\n"));
|
|
|
|
|
|
g.esc_calibrate.set_and_save(1);
|
|
|
|
while(1){
|
|
delay(20);
|
|
|
|
if(Serial.available() > 0){
|
|
g.esc_calibrate.set_and_save(0);
|
|
return(0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
setup_motors(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
while(1){
|
|
delay(20);
|
|
read_radio();
|
|
output_motor_test();
|
|
if(Serial.available() > 0){
|
|
g.esc_calibrate.set_and_save(0);
|
|
return(0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
setup_accel(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
imu.init_accel();
|
|
print_accel_offsets();
|
|
report_imu();
|
|
return(0);
|
|
}
|
|
|
|
static int8_t
|
|
setup_frame(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if (!strcmp_P(argv[1].str, PSTR("x"))) {
|
|
g.frame_orientation.set_and_save(X_FRAME);
|
|
} else if (!strcmp_P(argv[1].str, PSTR("p"))) {
|
|
g.frame_orientation.set_and_save(PLUS_FRAME);
|
|
} else if (!strcmp_P(argv[1].str, PSTR("+"))) {
|
|
g.frame_orientation.set_and_save(PLUS_FRAME);
|
|
} else if (!strcmp_P(argv[1].str, PSTR("v"))) {
|
|
g.frame_orientation.set_and_save(V_FRAME);
|
|
}else{
|
|
Serial.printf_P(PSTR("\nOptions:[x,+,v]\n"));
|
|
report_frame();
|
|
return 0;
|
|
}
|
|
|
|
report_frame();
|
|
return 0;
|
|
}
|
|
|
|
static int8_t
|
|
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
byte _switchPosition = 0;
|
|
byte _oldSwitchPosition = 0;
|
|
byte mode = 0;
|
|
|
|
Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off"));
|
|
print_hit_enter();
|
|
|
|
while(1){
|
|
delay(20);
|
|
read_radio();
|
|
_switchPosition = readSwitch();
|
|
|
|
|
|
// look for control switch change
|
|
if (_oldSwitchPosition != _switchPosition){
|
|
|
|
mode = flight_modes[_switchPosition];
|
|
mode = constrain(mode, 0, NUM_MODES-1);
|
|
|
|
// update the user
|
|
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
|
|
|
// Remember switch position
|
|
_oldSwitchPosition = _switchPosition;
|
|
}
|
|
|
|
// look for stick input
|
|
if (abs(g.rc_1.control_in) > 3000){
|
|
mode++;
|
|
if(mode >= NUM_MODES)
|
|
mode = 0;
|
|
|
|
// save new mode
|
|
flight_modes[_switchPosition] = mode;
|
|
|
|
// print new mode
|
|
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
|
delay(500);
|
|
}
|
|
|
|
// look for stick input
|
|
if (abs(g.rc_4.control_in) > 3000){
|
|
g.simple_modes |= (1<<_switchPosition);
|
|
// print new mode
|
|
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
|
delay(500);
|
|
}
|
|
|
|
// look for stick input
|
|
if (abs(g.rc_4.control_in) < 3000){
|
|
g.simple_modes &= ~(1<<_switchPosition);
|
|
// print new mode
|
|
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
|
delay(500);
|
|
}
|
|
|
|
// escape hatch
|
|
if(Serial.available() > 0){
|
|
for (mode = 0; mode < 6; mode++)
|
|
flight_modes[mode].save();
|
|
|
|
print_done();
|
|
report_flight_modes();
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
setup_declination(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
compass.set_declination(radians(argv[1].f));
|
|
report_compass();
|
|
return 0;
|
|
}
|
|
|
|
static int8_t
|
|
setup_tune(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
g.radio_tuning.set_and_save(argv[1].i);
|
|
report_tuning();
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
static int8_t
|
|
setup_erase(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
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.set_and_save(true);
|
|
init_compass();
|
|
|
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
|
clear_offsets();
|
|
g.compass_enabled.set_and_save(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 (!strcmp_P(argv[1].str, PSTR("off"))) {
|
|
g.battery_monitoring.set_and_save(0);
|
|
|
|
} else if(argv[1].i > 0 && argv[1].i <= 4){
|
|
g.battery_monitoring.set_and_save(argv[1].i);
|
|
|
|
} else {
|
|
Serial.printf_P(PSTR("\nOptions: off, 1-4"));
|
|
}
|
|
|
|
report_batt_monitor();
|
|
return 0;
|
|
}
|
|
|
|
static int8_t
|
|
setup_sonar(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
|
g.sonar_enabled.set_and_save(true);
|
|
|
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
|
g.sonar_enabled.set_and_save(false);
|
|
|
|
}else{
|
|
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
|
|
report_sonar();
|
|
return 0;
|
|
}
|
|
|
|
report_sonar();
|
|
return 0;
|
|
}
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
// Perform heli setup.
|
|
// Called by the setup menu 'radio' command.
|
|
static int8_t
|
|
setup_heli(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
|
|
uint8_t active_servo = 0;
|
|
int value = 0;
|
|
int temp;
|
|
int state = 0; // 0 = set rev+pos, 1 = capture min/max
|
|
int max_roll, max_pitch, min_coll, max_coll, min_tail, max_tail;
|
|
|
|
// initialise swash plate
|
|
heli_init_swash();
|
|
|
|
// source swash plate movements directly from
|
|
heli_manual_override = true;
|
|
|
|
// display initial settings
|
|
report_heli();
|
|
|
|
// display help
|
|
Serial.printf_P(PSTR("Instructions:"));
|
|
print_divider();
|
|
Serial.printf_P(PSTR("\td\t\tdisplay settings\n"));
|
|
Serial.printf_P(PSTR("\t1~4\t\tselect servo\n"));
|
|
Serial.printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
|
|
Serial.printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
|
|
Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
|
|
Serial.printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
|
|
Serial.printf_P(PSTR("\tr\t\treverse servo\n"));
|
|
Serial.printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
|
|
Serial.printf_P(PSTR("\tx\t\texit & save\n"));
|
|
|
|
// start capturing
|
|
while( value != 'x' ) {
|
|
|
|
// read radio although we don't use it yet
|
|
read_radio();
|
|
|
|
// record min/max
|
|
if( state == 1 ) {
|
|
if( abs(g.rc_1.control_in) > max_roll )
|
|
max_roll = abs(g.rc_1.control_in);
|
|
if( abs(g.rc_2.control_in) > max_pitch )
|
|
max_pitch = abs(g.rc_2.control_in);
|
|
if( g.rc_3.radio_in < min_coll )
|
|
min_coll = g.rc_3.radio_in;
|
|
if( g.rc_3.radio_in > max_coll )
|
|
max_coll = g.rc_3.radio_in;
|
|
min_tail = min(g.rc_4.radio_in, min_tail);
|
|
max_tail = max(g.rc_4.radio_in, max_tail);
|
|
//Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out);
|
|
}
|
|
|
|
if( Serial.available() ) {
|
|
value = Serial.read();
|
|
|
|
// process the user's input
|
|
switch( value ) {
|
|
case '1':
|
|
active_servo = CH_1;
|
|
break;
|
|
case '2':
|
|
active_servo = CH_2;
|
|
break;
|
|
case '3':
|
|
active_servo = CH_3;
|
|
break;
|
|
case '4':
|
|
active_servo = CH_4;
|
|
break;
|
|
case 'a':
|
|
case 'A':
|
|
heli_get_servo(active_servo)->radio_trim += 10;
|
|
break;
|
|
case 'c':
|
|
case 'C':
|
|
if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) {
|
|
g.heli_coll_mid = g.rc_3.radio_in;
|
|
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid);
|
|
}
|
|
break;
|
|
case 'd':
|
|
case 'D':
|
|
// display settings
|
|
report_heli();
|
|
break;
|
|
case 'm':
|
|
case 'M':
|
|
if( state == 0 ) {
|
|
state = 1; // switch to capture min/max mode
|
|
Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"),active_servo+1, temp);
|
|
|
|
// reset servo ranges
|
|
g.heli_roll_max = g.heli_pitch_max = 4500;
|
|
g.heli_coll_min = 1000;
|
|
g.heli_coll_max = 2000;
|
|
g.heli_servo_4.radio_min = 1000;
|
|
g.heli_servo_4.radio_max = 2000;
|
|
|
|
// set sensible values in temp variables
|
|
max_roll = abs(g.rc_1.control_in);
|
|
max_pitch = abs(g.rc_2.control_in);
|
|
min_coll = 2000;
|
|
max_coll = 1000;
|
|
min_tail = max_tail = abs(g.rc_4.radio_in);
|
|
}else{
|
|
state = 0; // switch back to normal mode
|
|
// double check values aren't totally terrible
|
|
if( max_roll <= 1000 || max_pitch <= 1000 || (max_coll - min_coll < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
|
|
Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_coll,max_coll,min_tail,max_tail);
|
|
else{
|
|
g.heli_roll_max = max_roll;
|
|
g.heli_pitch_max = max_pitch;
|
|
g.heli_coll_min = min_coll;
|
|
g.heli_coll_max = max_coll;
|
|
g.heli_servo_4.radio_min = min_tail;
|
|
g.heli_servo_4.radio_max = max_tail;
|
|
|
|
// reinitialise swash
|
|
heli_init_swash();
|
|
|
|
// display settings
|
|
report_heli();
|
|
}
|
|
}
|
|
break;
|
|
case 'p':
|
|
case 'P':
|
|
temp = read_num_from_serial();
|
|
if( temp >= -360 && temp <= 360 ) {
|
|
if( active_servo == CH_1 )
|
|
g.heli_servo1_pos = temp;
|
|
if( active_servo == CH_2 )
|
|
g.heli_servo2_pos = temp;
|
|
if( active_servo == CH_3 )
|
|
g.heli_servo3_pos = temp;
|
|
heli_init_swash();
|
|
Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
|
|
}
|
|
break;
|
|
case 'r':
|
|
case 'R':
|
|
heli_get_servo(active_servo)->set_reverse(!heli_get_servo(active_servo)->get_reverse());
|
|
break;
|
|
case 't':
|
|
case 'T':
|
|
temp = read_num_from_serial();
|
|
if( temp > 1000 )
|
|
temp -= 1500;
|
|
if( temp > -500 && temp < 500 ) {
|
|
heli_get_servo(active_servo)->radio_trim = 1500 + temp;
|
|
heli_init_swash();
|
|
Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
|
|
}
|
|
break;
|
|
case 'z':
|
|
case 'Z':
|
|
heli_get_servo(active_servo)->radio_trim -= 10;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// allow swash plate to move
|
|
output_motors_armed();
|
|
|
|
delay(20);
|
|
}
|
|
|
|
// display final settings
|
|
report_heli();
|
|
|
|
// save to eeprom
|
|
g.heli_servo_1.save_eeprom();
|
|
g.heli_servo_2.save_eeprom();
|
|
g.heli_servo_3.save_eeprom();
|
|
g.heli_servo_4.save_eeprom();
|
|
g.heli_servo1_pos.save();
|
|
g.heli_servo2_pos.save();
|
|
g.heli_servo3_pos.save();
|
|
g.heli_roll_max.save();
|
|
g.heli_pitch_max.save();
|
|
g.heli_coll_min.save();
|
|
g.heli_coll_max.save();
|
|
g.heli_coll_mid.save();
|
|
|
|
// return swash plate movements to attitude controller
|
|
heli_manual_override = false;
|
|
|
|
return(0);
|
|
}
|
|
|
|
// setup for external tail gyro (for heli only)
|
|
static int8_t
|
|
setup_gyro(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
|
g.heli_ext_gyro_enabled.set_and_save(true);
|
|
|
|
// optionally capture the gain
|
|
if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) {
|
|
g.heli_ext_gyro_gain = argv[2].i;
|
|
g.heli_ext_gyro_gain.save();
|
|
}
|
|
|
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
|
g.heli_ext_gyro_enabled.set_and_save(false);
|
|
|
|
// capture gain if user simply provides a number
|
|
} else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) {
|
|
g.heli_ext_gyro_enabled.set_and_save(true);
|
|
g.heli_ext_gyro_gain = argv[1].i;
|
|
g.heli_ext_gyro_gain.save();
|
|
|
|
}else{
|
|
Serial.printf_P(PSTR("\nOptions:[on, off] gain\n"));
|
|
}
|
|
|
|
report_gyro();
|
|
return 0;
|
|
}
|
|
|
|
#endif // FRAME_CONFIG == HELI
|
|
|
|
static void clear_offsets()
|
|
{
|
|
Vector3f _offsets(0.0,0.0,0.0);
|
|
compass.set_offsets(_offsets);
|
|
compass.save_offsets();
|
|
}
|
|
|
|
/*static int8_t
|
|
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
Vector3f _offsets;
|
|
|
|
if (!strcmp_P(argv[1].str, PSTR("c"))) {
|
|
clear_offsets();
|
|
report_compass();
|
|
return (0);
|
|
}
|
|
|
|
print_hit_enter();
|
|
init_compass();
|
|
|
|
int _min[3] = {0,0,0};
|
|
int _max[3] = {0,0,0};
|
|
|
|
compass.read();
|
|
compass.calculate(0,0); // roll = 0, pitch = 0
|
|
|
|
while(1){
|
|
delay(50);
|
|
|
|
compass.read();
|
|
compass.calculate(0,0); // roll = 0, pitch = 0
|
|
|
|
if(compass.mag_x < _min[0]) _min[0] = compass.mag_x;
|
|
if(compass.mag_y < _min[1]) _min[1] = compass.mag_y;
|
|
if(compass.mag_z < _min[2]) _min[2] = compass.mag_z;
|
|
|
|
// capture max
|
|
if(compass.mag_x > _max[0]) _max[0] = compass.mag_x;
|
|
if(compass.mag_y > _max[1]) _max[1] = compass.mag_y;
|
|
if(compass.mag_z > _max[2]) _max[2] = compass.mag_z;
|
|
|
|
// calculate offsets
|
|
_offsets.x = (float)(_max[0] + _min[0]) / -2;
|
|
_offsets.y = (float)(_max[1] + _min[1]) / -2;
|
|
_offsets.z = (float)(_max[2] + _min[2]) / -2;
|
|
|
|
// display all to user
|
|
Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"),
|
|
|
|
(uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100,
|
|
|
|
compass.mag_x,
|
|
compass.mag_y,
|
|
compass.mag_z,
|
|
|
|
_offsets.x,
|
|
_offsets.y,
|
|
_offsets.z);
|
|
|
|
if(Serial.available() > 1){
|
|
compass.set_offsets(_offsets);
|
|
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
|
|
report_compass();
|
|
return 0;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
*/
|
|
|
|
#ifdef OPTFLOW_ENABLED
|
|
static int8_t
|
|
setup_optflow(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
|
g.optflow_enabled = true;
|
|
init_optflow();
|
|
|
|
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
|
g.optflow_enabled = false;
|
|
|
|
//} else if(argv[1].i > 10){
|
|
// g.optflow_fov.set_and_save(argv[1].i);
|
|
// optflow.set_field_of_view(g.optflow_fov.get());
|
|
|
|
}else{
|
|
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
|
|
report_optflow();
|
|
return 0;
|
|
}
|
|
|
|
g.optflow_enabled.save();
|
|
report_optflow();
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
|
|
/***************************************************************************/
|
|
// CLI reports
|
|
/***************************************************************************/
|
|
|
|
static void report_batt_monitor()
|
|
{
|
|
Serial.printf_P(PSTR("\nBatt Mointor\n"));
|
|
print_divider();
|
|
if(g.battery_monitoring == 0) print_enabled(false);
|
|
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3 cells"));
|
|
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4 cells"));
|
|
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("batt volts"));
|
|
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_wp(byte index = 255)
|
|
{
|
|
if(index == 255){
|
|
for(byte i = 0; i <= g.waypoint_total; i++){
|
|
struct Location temp = get_command_with_index(i);
|
|
print_wp(&temp, i);
|
|
}
|
|
}else{
|
|
struct Location temp = get_command_with_index(index);
|
|
print_wp(&temp, index);
|
|
}
|
|
}
|
|
|
|
static void report_sonar()
|
|
{
|
|
g.sonar_enabled.load();
|
|
Serial.printf_P(PSTR("Sonar\n"));
|
|
print_divider();
|
|
print_enabled(g.sonar_enabled.get());
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_frame()
|
|
{
|
|
Serial.printf_P(PSTR("Frame\n"));
|
|
print_divider();
|
|
|
|
#if FRAME_CONFIG == QUAD_FRAME
|
|
Serial.printf_P(PSTR("Quad frame\n"));
|
|
#elif FRAME_CONFIG == TRI_FRAME
|
|
Serial.printf_P(PSTR("TRI frame\n"));
|
|
#elif FRAME_CONFIG == HEXA_FRAME
|
|
Serial.printf_P(PSTR("Hexa frame\n"));
|
|
#elif FRAME_CONFIG == Y6_FRAME
|
|
Serial.printf_P(PSTR("Y6 frame\n"));
|
|
#elif FRAME_CONFIG == OCTA_FRAME
|
|
Serial.printf_P(PSTR("Octa frame\n"));
|
|
#elif FRAME_CONFIG == HELI_FRAME
|
|
Serial.printf_P(PSTR("Heli frame\n"));
|
|
#endif
|
|
|
|
#if FRAME_CONFIG != HELI_FRAME
|
|
if(g.frame_orientation == X_FRAME)
|
|
Serial.printf_P(PSTR("X mode\n"));
|
|
else if(g.frame_orientation == PLUS_FRAME)
|
|
Serial.printf_P(PSTR("+ mode\n"));
|
|
else if(g.frame_orientation == V_FRAME)
|
|
Serial.printf_P(PSTR("V mode\n"));
|
|
#endif
|
|
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_radio()
|
|
{
|
|
Serial.printf_P(PSTR("Radio\n"));
|
|
print_divider();
|
|
// radio
|
|
print_radio_values();
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_imu()
|
|
{
|
|
Serial.printf_P(PSTR("IMU\n"));
|
|
print_divider();
|
|
|
|
print_gyro_offsets();
|
|
print_accel_offsets();
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_compass()
|
|
{
|
|
Serial.printf_P(PSTR("Compass\n"));
|
|
print_divider();
|
|
|
|
print_enabled(g.compass_enabled);
|
|
|
|
// mag declination
|
|
Serial.printf_P(PSTR("Mag Dec: %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"),
|
|
offsets.x,
|
|
offsets.y,
|
|
offsets.z);
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_flight_modes()
|
|
{
|
|
Serial.printf_P(PSTR("Flight modes\n"));
|
|
print_divider();
|
|
|
|
for(int i = 0; i < 6; i++ ){
|
|
print_switch(i, flight_modes[i], (g.simple_modes & (1<<i)));
|
|
}
|
|
print_blanks(2);
|
|
}
|
|
|
|
#ifdef OPTFLOW_ENABLED
|
|
void report_optflow()
|
|
{
|
|
Serial.printf_P(PSTR("OptFlow\n"));
|
|
print_divider();
|
|
|
|
print_enabled(g.optflow_enabled);
|
|
|
|
// field of view
|
|
//Serial.printf_P(PSTR("FOV: %4.0f\n"),
|
|
// degrees(g.optflow_fov));
|
|
|
|
print_blanks(2);
|
|
}
|
|
#endif
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
static void report_heli()
|
|
{
|
|
Serial.printf_P(PSTR("Heli\n"));
|
|
print_divider();
|
|
|
|
// main servo settings
|
|
Serial.printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
|
|
Serial.printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo1_pos, (int)g.heli_servo_1.radio_min, (int)g.heli_servo_1.radio_max, (int)g.heli_servo_1.get_reverse());
|
|
Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo2_pos, (int)g.heli_servo_2.radio_min, (int)g.heli_servo_2.radio_max, (int)g.heli_servo_2.get_reverse());
|
|
Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo3_pos, (int)g.heli_servo_3.radio_min, (int)g.heli_servo_3.radio_max, (int)g.heli_servo_3.get_reverse());
|
|
Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)g.heli_servo_4.radio_min, (int)g.heli_servo_4.radio_max, (int)g.heli_servo_4.get_reverse());
|
|
|
|
Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max);
|
|
Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max);
|
|
Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max);
|
|
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_gyro()
|
|
{
|
|
|
|
Serial.printf_P(PSTR("External Gyro:\n"));
|
|
print_divider();
|
|
|
|
print_enabled( g.heli_ext_gyro_enabled );
|
|
if( g.heli_ext_gyro_enabled )
|
|
Serial.printf_P(PSTR("gain: %d"),(int)g.heli_ext_gyro_gain);
|
|
|
|
print_blanks(2);
|
|
}
|
|
|
|
#endif // FRAME_CONFIG == HELI_FRAME
|
|
|
|
/***************************************************************************/
|
|
// CLI utilities
|
|
/***************************************************************************/
|
|
|
|
/*static void
|
|
print_PID(PI * pid)
|
|
{
|
|
Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"),
|
|
pid->kP(),
|
|
pid->kI(),
|
|
(long)pid->imax());
|
|
}
|
|
*/
|
|
|
|
static void
|
|
print_radio_values()
|
|
{
|
|
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
|
|
Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
|
|
Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
|
|
Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
|
|
Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
|
|
Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
|
|
Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
|
|
//Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
|
|
}
|
|
|
|
static void
|
|
print_switch(byte p, byte m, bool b)
|
|
{
|
|
Serial.printf_P(PSTR("Pos %d: "),p);
|
|
Serial.print(flight_mode_strings[m]);
|
|
Serial.printf_P(PSTR(", Simple: "));
|
|
if(b)
|
|
Serial.printf_P(PSTR("T\n"));
|
|
else
|
|
Serial.printf_P(PSTR("F\n"));
|
|
}
|
|
|
|
static void
|
|
print_done()
|
|
{
|
|
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
|
|
}
|
|
|
|
|
|
static void zero_eeprom(void)
|
|
{
|
|
byte b = 0;
|
|
|
|
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"));
|
|
}
|
|
|
|
static 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());
|
|
}
|
|
|
|
static 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());
|
|
}
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
static RC_Channel *
|
|
heli_get_servo(int servo_num){
|
|
if( servo_num == CH_1 )
|
|
return &g.heli_servo_1;
|
|
if( servo_num == CH_2 )
|
|
return &g.heli_servo_2;
|
|
if( servo_num == CH_3 )
|
|
return &g.heli_servo_3;
|
|
if( servo_num == CH_4 )
|
|
return &g.heli_servo_4;
|
|
return NULL;
|
|
}
|
|
|
|
// Used to read integer values from the serial port
|
|
static int read_num_from_serial() {
|
|
byte index = 0;
|
|
byte timeout = 0;
|
|
char data[5] = "";
|
|
|
|
do {
|
|
if (Serial.available() == 0) {
|
|
delay(10);
|
|
timeout++;
|
|
}else{
|
|
data[index] = Serial.read();
|
|
timeout = 0;
|
|
index++;
|
|
}
|
|
}while (timeout < 5 && index < 5);
|
|
|
|
return atoi(data);
|
|
}
|
|
#endif
|
|
|
|
#endif // CLI_ENABLED
|
|
|
|
static void
|
|
print_blanks(int num)
|
|
{
|
|
while(num > 0){
|
|
num--;
|
|
Serial.println("");
|
|
}
|
|
}
|
|
|
|
static void
|
|
print_divider(void)
|
|
{
|
|
for (int i = 0; i < 40; i++) {
|
|
Serial.print("-");
|
|
}
|
|
Serial.println("");
|
|
}
|
|
|
|
static void print_enabled(boolean b)
|
|
{
|
|
if(b)
|
|
Serial.printf_P(PSTR("en"));
|
|
else
|
|
Serial.printf_P(PSTR("dis"));
|
|
Serial.printf_P(PSTR("abled\n"));
|
|
}
|
|
|
|
|
|
static void
|
|
init_esc()
|
|
{
|
|
g.esc_calibrate.set_and_save(0);
|
|
while(1){
|
|
read_radio();
|
|
delay(100);
|
|
dancing_light();
|
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_in);
|
|
|
|
#if FRAME_CONFIG == OCTA_FRAME
|
|
APM_RC.OutputCh(CH_10, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_11, g.rc_3.radio_in);
|
|
#endif
|
|
|
|
}
|
|
}
|
|
|
|
static void print_wp(struct Location *cmd, byte index)
|
|
{
|
|
Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
|
|
(int)index,
|
|
(int)cmd->id,
|
|
(int)cmd->options,
|
|
(int)cmd->p1,
|
|
cmd->alt,
|
|
cmd->lat,
|
|
cmd->lng);
|
|
}
|
|
|
|
static void report_gps()
|
|
{
|
|
Serial.printf_P(PSTR("\nGPS\n"));
|
|
print_divider();
|
|
print_enabled(GPS_enabled);
|
|
print_blanks(2);
|
|
}
|
|
|
|
static void report_version()
|
|
{
|
|
Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get());
|
|
print_divider();
|
|
print_blanks(2);
|
|
}
|
|
|
|
|
|
static void report_tuning()
|
|
{
|
|
Serial.printf_P(PSTR("\nTUNE:\n"));
|
|
print_divider();
|
|
if (g.radio_tuning == 0){
|
|
print_enabled(g.radio_tuning.get());
|
|
}else{
|
|
Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get());
|
|
}
|
|
print_blanks(2);
|
|
}
|