mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
82e51aec82
moved ground start to first arming added ground start flag moved throttle_integrator to 50hz loop CAMERA_STABILIZER deprecated - now always on renamed current logging bit mask to match APM added MA filter to PID - D term Adjusted PIDs based on continued testing and new PID filter added MASK_LOG_SET_DEFAULTS to match APM moved some stuff out of ground start into system start where it belonged Added slower Yaw gains for DCM when the copter is in the air changed camera output to be none scaled PWM fixed bug where ground_temperature was unfiltered shortened Baro startup time fixed issue with Nav_WP integrator not being reset RTL no longer yaws towards home Circle mode for flying a 10m circle around the point where it was engaged. - Not tested at all! Consider Circle mode as alpha. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2966 f9c3cf11-9bcb-44bc-f272-b75c42450872
1229 lines
29 KiB
Plaintext
1229 lines
29 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_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},
|
|
// {"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 RC toggle switch to each position to edit, move aileron stick to select modes."));
|
|
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);
|
|
|
|
// Remember switch position
|
|
_oldSwitchPosition = _switchPosition;
|
|
}
|
|
|
|
// look for stick input
|
|
if (radio_input_switch() == true){
|
|
mode++;
|
|
if(mode >= NUM_MODES)
|
|
mode = 0;
|
|
|
|
// save new mode
|
|
flight_modes[_switchPosition] = mode;
|
|
|
|
// print new mode
|
|
print_switch(_switchPosition, mode);
|
|
}
|
|
|
|
// 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_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
|
|
|
|
|
|
|
|
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) |
|
|
LOGBIT(MOTORS) |
|
|
LOGBIT(OPTFLOW);
|
|
#undef LOGBIT
|
|
|
|
g.log_bitmask.save();
|
|
}
|
|
|
|
/***************************************************************************/
|
|
// 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_gains()
|
|
{
|
|
Serial.printf_P(PSTR("Gains\n"));
|
|
print_divider();
|
|
|
|
// Rate
|
|
Serial.printf_P(PSTR("Rate:\nroll:\n"));
|
|
print_PID(&g.pid_rate_roll);
|
|
Serial.printf_P(PSTR("pitch:\n"));
|
|
print_PID(&g.pid_rate_pitch);
|
|
Serial.printf_P(PSTR("yaw:\n"));
|
|
print_PID(&g.pid_rate_yaw);
|
|
|
|
// Stabilize
|
|
Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
|
|
print_PID(&g.pid_stabilize_roll);
|
|
Serial.printf_P(PSTR("pitch:\n"));
|
|
print_PID(&g.pid_stabilize_pitch);
|
|
Serial.printf_P(PSTR("yaw:\n"));
|
|
print_PID(&g.pid_stabilize_yaw);
|
|
|
|
//Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener);
|
|
//Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);
|
|
|
|
// Nav
|
|
Serial.printf_P(PSTR("Nav:\nlat:\n"));
|
|
print_PID(&g.pid_nav_lat);
|
|
Serial.printf_P(PSTR("long:\n"));
|
|
print_PID(&g.pid_nav_lon);
|
|
Serial.printf_P(PSTR("throttle:\n"));
|
|
print_PID(&g.pid_throttle);
|
|
print_blanks(2);
|
|
}
|
|
*/
|
|
|
|
/*static void report_xtrack()
|
|
{
|
|
Serial.printf_P(PSTR("XTrack\n"));
|
|
print_divider();
|
|
// radio
|
|
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
|
|
"XTRACK angle: %d\n"
|
|
"PITCH_MAX: %ld"),
|
|
(float)g.crosstrack_gain,
|
|
(int)g.crosstrack_entry_angle,
|
|
(long)g.pitch_max);
|
|
print_blanks(2);
|
|
}
|
|
*/
|
|
|
|
/*static void report_throttle()
|
|
{
|
|
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"),
|
|
(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);
|
|
}*/
|
|
|
|
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]);
|
|
}
|
|
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(PID * pid)
|
|
{
|
|
Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"),
|
|
pid->kP(),
|
|
pid->kI(),
|
|
pid->kD(),
|
|
(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)
|
|
{
|
|
Serial.printf_P(PSTR("Pos %d: "),p);
|
|
Serial.println(flight_mode_strings[m]);
|
|
}
|
|
|
|
static void
|
|
print_done()
|
|
{
|
|
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
|
|
}
|
|
|
|
// read at 50Hz
|
|
static bool
|
|
radio_input_switch(void)
|
|
{
|
|
static int8_t bouncer = 0;
|
|
|
|
if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) > 100) {
|
|
bouncer = 10;
|
|
}
|
|
if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) < -100) {
|
|
bouncer = -10;
|
|
}
|
|
if (bouncer >0) {
|
|
bouncer --;
|
|
}
|
|
if (bouncer <0) {
|
|
bouncer ++;
|
|
}
|
|
|
|
if (bouncer == 1 || bouncer == -1) {
|
|
return bouncer;
|
|
}else{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|