From bb6e4ac96928c129a8c67f28400dcc58ff4fba59 Mon Sep 17 00:00:00 2001 From: uncrustify Date: Tue, 21 Aug 2012 19:19:50 -0700 Subject: [PATCH] uncrustify ArduCopter/setup.pde --- ArduCopter/setup.pde | 1726 +++++++++++++++++++++--------------------- 1 file changed, 863 insertions(+), 863 deletions(-) diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index a278304836..72fb0b77d6 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -3,54 +3,54 @@ #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_accel_scale (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_range (uint8_t argc, const Menu::arg *argv); +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_accel_scale (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_range (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_optflow (uint8_t argc, const Menu::arg *argv); -static int8_t setup_show (uint8_t argc, const Menu::arg *argv); +static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); +static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv); +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 + #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}, - {"level", setup_accel}, - {"accel", setup_accel_scale}, - {"modes", setup_flightmodes}, - {"battery", setup_batt_monitor}, - {"sonar", setup_sonar}, - {"compass", setup_compass}, - {"tune", setup_tune}, - {"range", setup_range}, + // command function called + // ======= =============== + {"erase", setup_erase}, + {"reset", setup_factory}, + {"radio", setup_radio}, + {"frame", setup_frame}, + {"motors", setup_motors}, + {"level", setup_accel}, + {"accel", setup_accel_scale}, + {"modes", setup_flightmodes}, + {"battery", setup_batt_monitor}, + {"sonar", setup_sonar}, + {"compass", setup_compass}, + {"tune", setup_tune}, + {"range", setup_range}, // {"offsets", setup_mag_offset}, - {"declination", setup_declination}, - {"optflow", setup_optflow}, -#if FRAME_CONFIG == HELI_FRAME - {"heli", setup_heli}, - {"gyro", setup_gyro}, -#endif - {"show", setup_show} + {"declination", setup_declination}, + {"optflow", setup_optflow}, + #if FRAME_CONFIG == HELI_FRAME + {"heli", setup_heli}, + {"gyro", setup_gyro}, + #endif + {"show", setup_show} }; // Create the setup menu object. @@ -60,23 +60,23 @@ MENU(setup_menu, "setup", setup_menu_commands); 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")); + // 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, radio not configured!")); - delay(1000); - Serial.printf_P(PSTR("\n Type 'radio' now.\n\n")); - } + if(g.rc_1.radio_min >= 1300) { + delay(1000); + Serial.printf_P(PSTR("\n!Warning, radio not configured!")); + delay(1000); + Serial.printf_P(PSTR("\n Type 'radio' now.\n\n")); + } - // Run the setup menu. When the menu exits, we will return to the main menu. - setup_menu.run(); + // Run the setup menu. When the menu exits, we will return to the main menu. + setup_menu.run(); return 0; } @@ -85,30 +85,30 @@ setup_mode(uint8_t argc, const Menu::arg *argv) static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { - // clear the area - print_blanks(8); + // 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(); - report_optflow(); + report_version(); + report_radio(); + report_frame(); + report_batt_monitor(); + report_sonar(); + //report_gains(); + //report_xtrack(); + //report_throttle(); + report_flight_modes(); + report_imu(); + report_compass(); + report_optflow(); -#if FRAME_CONFIG == HELI_FRAME - report_heli(); - report_gyro(); -#endif + #if FRAME_CONFIG == HELI_FRAME + report_heli(); + report_gyro(); + #endif AP_Param::show_all(); - return(0); + return(0); } // Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). @@ -116,27 +116,27 @@ setup_show(uint8_t argc, const Menu::arg *argv) static int8_t setup_factory(uint8_t argc, const Menu::arg *argv) { - int16_t c; + int16_t c; - Serial.printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n")); + Serial.printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n")); - do { - c = Serial.read(); - } while (-1 == c); + do { + c = Serial.read(); + } while (-1 == c); - if (('y' != c) && ('Y' != c)) - return(-1); + if (('y' != c) && ('Y' != c)) + return(-1); - AP_Param::erase_all(); - Serial.printf_P(PSTR("\nReboot APM")); + AP_Param::erase_all(); + Serial.printf_P(PSTR("\nReboot APM")); - delay(1000); - //default_gains(); + delay(1000); + //default_gains(); - for (;;) { - } - // note, cannot actually return here - return(0); + for (;; ) { + } + // note, cannot actually return here + return(0); } // Perform radio setup. @@ -144,305 +144,305 @@ setup_factory(uint8_t argc, const Menu::arg *argv) static int8_t setup_radio(uint8_t argc, const Menu::arg *argv) { - Serial.println("\n\nRadio Setup:"); - uint8_t i; + Serial.println("\n\nRadio Setup:"); + uint8_t i; - for(i = 0; i < 100;i++){ - delay(20); - read_radio(); - } + 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 - } - } + 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_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_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; + 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 extremes. Enter to save: ")); - while(1){ + Serial.printf_P(PSTR("\nMove all controls to extremes. Enter to save: ")); + while(1) { - delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); + 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(); + 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(); + 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(); + 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); + print_done(); + break; + } + } + report_radio(); + return(0); } static int8_t setup_motors(uint8_t argc, const Menu::arg *argv) { - Serial.printf_P(PSTR( - "Now connect the main lipo and follow the instruction on the wiki for your frame setup.\n" - "For security remember to disconnect the main lipo after the test, then hit any key to exit.\n" - "Any key to exit.\n")); - while(1){ - delay(20); - read_radio(); - motors.output_test(); - if(Serial.available() > 0){ - g.esc_calibrate.set_and_save(0); - return(0); - } - } + Serial.printf_P(PSTR( + "Now connect the main lipo and follow the instruction on the wiki for your frame setup.\n" + "For security remember to disconnect the main lipo after the test, then hit any key to exit.\n" + "Any key to exit.\n")); + while(1) { + delay(20); + read_radio(); + motors.output_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(IMU::COLD_START, delay, flash_leds, &timer_scheduler); - imu.init_accel(delay, flash_leds); - print_accel_offsets(); - report_imu(); - return(0); + imu.init_accel(delay, flash_leds); + print_accel_offsets(); + report_imu(); + return(0); } static int8_t setup_accel_scale(uint8_t argc, const Menu::arg *argv) { - #if CONFIG_ADC == ENABLED && HIL_MODE == HIL_MODE_DISABLED - int8_t accel_num; - float accel_avg = 0; + #if CONFIG_ADC == ENABLED && HIL_MODE == HIL_MODE_DISABLED + int8_t accel_num; + float accel_avg = 0; - if (!strcmp_P(argv[1].str, PSTR("x"))) { - accel_num = 4; - }else if (!strcmp_P(argv[1].str, PSTR("y"))) { - accel_num = 5; - }else if (!strcmp_P(argv[1].str, PSTR("z"))) { - accel_num = 6; - }else{ - Serial.printf_P(PSTR("x, y, or z\n")); - return 0; - } - print_hit_enter(); - Serial.printf_P(PSTR("ADC\n")); + if (!strcmp_P(argv[1].str, PSTR("x"))) { + accel_num = 4; + }else if (!strcmp_P(argv[1].str, PSTR("y"))) { + accel_num = 5; + }else if (!strcmp_P(argv[1].str, PSTR("z"))) { + accel_num = 6; + }else{ + Serial.printf_P(PSTR("x, y, or z\n")); + return 0; + } + print_hit_enter(); + Serial.printf_P(PSTR("ADC\n")); - adc.Init(&timer_scheduler); // APM ADC library initialization + adc.Init(&timer_scheduler); // APM ADC library initialization - int16_t low, high; + int16_t low, high; - delay(1000); - accel_avg = adc.Ch(accel_num); - low = high = accel_avg; + delay(1000); + accel_avg = adc.Ch(accel_num); + low = high = accel_avg; - while(1){ - delay(50); - accel_avg = accel_avg * .95 + adc.Ch(accel_num) * .05; + while(1) { + delay(50); + accel_avg = accel_avg * .95 + adc.Ch(accel_num) * .05; - if(accel_avg > high) - high = ceil(accel_avg); + if(accel_avg > high) + high = ceil(accel_avg); - if(accel_avg < low) - low = floor(accel_avg); + if(accel_avg < low) + low = floor(accel_avg); - Serial.printf_P(PSTR("%1.2f, %d, %d\n"), accel_avg, low, high); + Serial.printf_P(PSTR("%1.2f, %d, %d\n"), accel_avg, low, high); - if(Serial.available() > 0){ - if(wait_for_yes()){ - if(accel_num == 4){ - ins._x_high = high; - ins._x_low = low; - ins._x_high.save(); - ins._x_low.save(); - }else if(accel_num == 5){ - ins._y_high = high; - ins._y_low = low; - ins._y_high.save(); - ins._y_low.save(); - }else{ - ins._z_high = high; - ins._z_low = low; - ins._z_high.save(); - ins._z_low.save(); - } - print_done(); - } - return (0); - } - } - #else - return 0; - #endif // CONFIG_ADC + if(Serial.available() > 0) { + if(wait_for_yes()) { + if(accel_num == 4) { + ins._x_high = high; + ins._x_low = low; + ins._x_high.save(); + ins._x_low.save(); + }else if(accel_num == 5) { + ins._y_high = high; + ins._y_low = low; + ins._y_high.save(); + ins._y_low.save(); + }else{ + ins._z_high = high; + ins._z_low = low; + ins._z_high.save(); + ins._z_low.save(); + } + print_done(); + } + return (0); + } + } + #else + return 0; + #endif // CONFIG_ADC } 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("\nOp:[x,+,v]\n")); - report_frame(); - return 0; - } + 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("\nOp:[x,+,v]\n")); + report_frame(); + return 0; + } - 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; - int8_t mode = 0; + byte _switchPosition = 0; + byte _oldSwitchPosition = 0; + int8_t mode = 0; - Serial.printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n")); - print_hit_enter(); + Serial.printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n")); + print_hit_enter(); - while(1){ - delay(20); - read_radio(); - _switchPosition = readSwitch(); + while(1) { + delay(20); + read_radio(); + _switchPosition = readSwitch(); - // look for control switch change - if (_oldSwitchPosition != _switchPosition){ + // look for control switch change + if (_oldSwitchPosition != _switchPosition) { - mode = flight_modes[_switchPosition]; - mode = constrain(mode, 0, NUM_MODES-1); + mode = flight_modes[_switchPosition]; + mode = constrain(mode, 0, NUM_MODES-1); - // update the user - print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + // update the user + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); - // Remember switch position - _oldSwitchPosition = _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; + // 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; + // save new mode + flight_modes[_switchPosition] = mode; - // print new mode - print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); - delay(500); - } + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } - // look for stick input - if (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 (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 (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 (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++) + // escape hatch + if(Serial.available() > 0) { + for (mode = 0; mode < 6; mode++) flight_modes[mode].save(); - g.simple_modes.save(); - print_done(); - report_flight_modes(); - return (0); - } - } + g.simple_modes.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; + 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); - //g.radio_tuning_high.set_and_save(1000); - //g.radio_tuning_low.set_and_save(0); - report_tuning(); - return 0; + g.radio_tuning.set_and_save(argv[1].i); + //g.radio_tuning_high.set_and_save(1000); + //g.radio_tuning_low.set_and_save(0); + report_tuning(); + return 0; } static int8_t setup_range(uint8_t argc, const Menu::arg *argv) { - Serial.printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n")); + Serial.printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n")); - g.radio_tuning_low.set_and_save(argv[1].i); - g.radio_tuning_high.set_and_save(argv[2].i); - report_tuning(); - return 0; + g.radio_tuning_low.set_and_save(argv[1].i); + g.radio_tuning_high.set_and_save(argv[2].i); + report_tuning(); + return 0; } @@ -450,73 +450,73 @@ setup_range(uint8_t argc, const Menu::arg *argv) static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) { - zero_eeprom(); - return 0; + 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(); + 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 if (!strcmp_P(argv[1].str, PSTR("off"))) { + clear_offsets(); + g.compass_enabled.set_and_save(false); - }else{ - Serial.printf_P(PSTR("\nOp:[on,off]\n")); - report_compass(); - return 0; - } + }else{ + Serial.printf_P(PSTR("\nOp:[on,off]\n")); + report_compass(); + return 0; + } - g.compass_enabled.save(); - 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); + 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 if(argv[1].i > 0 && argv[1].i <= 4) { + g.battery_monitoring.set_and_save(argv[1].i); - } else { - Serial.printf_P(PSTR("\nOp: off, 3-4")); - } + } else { + Serial.printf_P(PSTR("\nOp: off, 3-4")); + } - report_batt_monitor(); - return 0; + 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); + 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 if (!strcmp_P(argv[1].str, PSTR("off"))) { + g.sonar_enabled.set_and_save(false); - } else if (argc > 1 && (argv[1].i >= 0 && argv[1].i <= 3)) { - g.sonar_enabled.set_and_save(true); // if you set the sonar type, surely you want it on - g.sonar_type.set_and_save(argv[1].i); + } else if (argc > 1 && (argv[1].i >= 0 && argv[1].i <= 3)) { + g.sonar_enabled.set_and_save(true); // if you set the sonar type, surely you want it on + g.sonar_type.set_and_save(argv[1].i); - }else{ - Serial.printf_P(PSTR("\nOp:[on, off, 0-3]\n")); - report_sonar(); - return 0; - } + }else{ + Serial.printf_P(PSTR("\nOp:[on, off, 0-3]\n")); + report_sonar(); + return 0; + } - report_sonar(); - return 0; + report_sonar(); + return 0; } -#if FRAME_CONFIG == HELI_FRAME + #if FRAME_CONFIG == HELI_FRAME // Perform heli setup. // Called by the setup menu 'radio' command. @@ -524,339 +524,339 @@ static int8_t setup_heli(uint8_t argc, const Menu::arg *argv) { - uint8_t active_servo = 0; - int16_t value = 0; - int16_t temp; - int16_t state = 0; // 0 = set rev+pos, 1 = capture min/max - int16_t max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0; + uint8_t active_servo = 0; + int16_t value = 0; + int16_t temp; + int16_t state = 0; // 0 = set rev+pos, 1 = capture min/max + int16_t max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0; - // initialise swash plate - motors.init_swash(); + // initialise swash plate + motors.init_swash(); - // source swash plate movements directly from radio - motors.servo_manual = true; + // source swash plate movements directly from radio + motors.servo_manual = true; - // display initial settings - report_heli(); + // 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\tset pos (i.e. p0 = front, p90 = right)\n")); - Serial.printf_P(PSTR("\tr\t\treverse servo\n")); - Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n")); - Serial.printf_P(PSTR("\tt\tset trim (-500 ~ 500)\n")); - Serial.printf_P(PSTR("\tx\t\texit & save\n")); + // 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\tset pos (i.e. p0 = front, p90 = right)\n")); + Serial.printf_P(PSTR("\tr\t\treverse servo\n")); + Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n")); + Serial.printf_P(PSTR("\tt\tset trim (-500 ~ 500)\n")); + Serial.printf_P(PSTR("\tx\t\texit & save\n")); - // start capturing - while( value != 'x' ) { + // start capturing + while( value != 'x' ) { - // read radio although we don't use it yet - read_radio(); + // read radio although we don't use it yet + read_radio(); - // allow swash plate to move - motors.output_armed(); + // allow swash plate to move + motors.output_armed(); - // 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_out < min_collective ) - min_collective = g.rc_3.radio_out; - if( g.rc_3.radio_out > max_collective ) - max_collective = g.rc_3.radio_out; - min_tail = min(g.rc_4.radio_out, min_tail); - max_tail = max(g.rc_4.radio_out, max_tail); - } + // 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_out < min_collective ) + min_collective = g.rc_3.radio_out; + if( g.rc_3.radio_out > max_collective ) + max_collective = g.rc_3.radio_out; + min_tail = min(g.rc_4.radio_out, min_tail); + max_tail = max(g.rc_4.radio_out, max_tail); + } - if( Serial.available() ) { - value = Serial.read(); + 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_out >= 900 && g.rc_3.radio_out <= 2100 ) { - motors.collective_mid = g.rc_3.radio_out; - Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_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")); + // 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_out >= 900 && g.rc_3.radio_out <= 2100 ) { + motors.collective_mid = g.rc_3.radio_out; + Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_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")); - // reset servo ranges - motors.roll_max = motors.pitch_max = 4500; - motors.collective_min = 1000; - motors.collective_max = 2000; - motors._servo_4->radio_min = 1000; - motors._servo_4->radio_max = 2000; + // reset servo ranges + motors.roll_max = motors.pitch_max = 4500; + motors.collective_min = 1000; + motors.collective_max = 2000; + motors._servo_4->radio_min = 1000; + motors._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_collective = 2000; - max_collective = 1000; - min_tail = max_tail = abs(g.rc_4.radio_out); - }else{ - state = 0; // switch back to normal mode - // double check values aren't totally terrible - if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 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_collective,max_collective,min_tail,max_tail); - else{ - motors.roll_max = max_roll; - motors.pitch_max = max_pitch; - motors.collective_min = min_collective; - motors.collective_max = max_collective; - motors._servo_4->radio_min = min_tail; - motors._servo_4->radio_max = max_tail; + // set sensible values in temp variables + max_roll = abs(g.rc_1.control_in); + max_pitch = abs(g.rc_2.control_in); + min_collective = 2000; + max_collective = 1000; + min_tail = max_tail = abs(g.rc_4.radio_out); + }else{ + state = 0; // switch back to normal mode + // double check values aren't totally terrible + if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 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_collective,max_collective,min_tail,max_tail); + else{ + motors.roll_max = max_roll; + motors.pitch_max = max_pitch; + motors.collective_min = min_collective; + motors.collective_max = max_collective; + motors._servo_4->radio_min = min_tail; + motors._servo_4->radio_max = max_tail; - // reinitialise swash - motors.init_swash(); + // reinitialise swash + motors.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 ) - motors.servo1_pos = temp; - if( active_servo == CH_2 ) - motors.servo2_pos = temp; - if( active_servo == CH_3 ) - motors.servo3_pos = temp; - motors.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; - motors.init_swash(); - Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); - } - break; - case 'u': - case 'U': - temp = 0; - // delay up to 2 seconds for servo type from user - while( !Serial.available() && temp < 20 ) { - temp++; - delay(100); - } - if( Serial.available() ) { - value = Serial.read(); - if( value == 'a' || value == 'A' ) { - g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS); - //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately - Serial.printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed); - } - if( value == 'd' || value == 'D' ) { - g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS); - //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately - Serial.printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed); - } - } - break; - case 'z': - case 'Z': - heli_get_servo(active_servo)->radio_trim -= 10; - break; - } - } + // display settings + report_heli(); + } + } + break; + case 'p': + case 'P': + temp = read_num_from_serial(); + if( temp >= -360 && temp <= 360 ) { + if( active_servo == CH_1 ) + motors.servo1_pos = temp; + if( active_servo == CH_2 ) + motors.servo2_pos = temp; + if( active_servo == CH_3 ) + motors.servo3_pos = temp; + motors.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; + motors.init_swash(); + Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); + } + break; + case 'u': + case 'U': + temp = 0; + // delay up to 2 seconds for servo type from user + while( !Serial.available() && temp < 20 ) { + temp++; + delay(100); + } + if( Serial.available() ) { + value = Serial.read(); + if( value == 'a' || value == 'A' ) { + g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS); + //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately + Serial.printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed); + } + if( value == 'd' || value == 'D' ) { + g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS); + //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately + Serial.printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed); + } + } + break; + case 'z': + case 'Z': + heli_get_servo(active_servo)->radio_trim -= 10; + break; + } + } - delay(20); - } + delay(20); + } - // display final settings - report_heli(); + // display final settings + report_heli(); - // save to eeprom - motors._servo_1->save_eeprom(); - motors._servo_2->save_eeprom(); - motors._servo_3->save_eeprom(); - motors._servo_4->save_eeprom(); - motors.servo1_pos.save(); - motors.servo2_pos.save(); - motors.servo3_pos.save(); - motors.roll_max.save(); - motors.pitch_max.save(); - motors.collective_min.save(); - motors.collective_max.save(); - motors.collective_mid.save(); + // save to eeprom + motors._servo_1->save_eeprom(); + motors._servo_2->save_eeprom(); + motors._servo_3->save_eeprom(); + motors._servo_4->save_eeprom(); + motors.servo1_pos.save(); + motors.servo2_pos.save(); + motors.servo3_pos.save(); + motors.roll_max.save(); + motors.pitch_max.save(); + motors.collective_min.save(); + motors.collective_max.save(); + motors.collective_mid.save(); - // return swash plate movements to attitude controller - motors.servo_manual = false; + // return swash plate movements to attitude controller + motors.servo_manual = false; - return(0); + 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"))) { - motors.ext_gyro_enabled.set_and_save(true); + if (!strcmp_P(argv[1].str, PSTR("on"))) { + motors.ext_gyro_enabled.set_and_save(true); - // optionally capture the gain - if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) { - motors.ext_gyro_gain = argv[2].i; - motors.ext_gyro_gain.save(); - } + // optionally capture the gain + if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) { + motors.ext_gyro_gain = argv[2].i; + motors.ext_gyro_gain.save(); + } - } else if (!strcmp_P(argv[1].str, PSTR("off"))) { - motors.ext_gyro_enabled.set_and_save(false); + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { + motors.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 ) { - motors.ext_gyro_enabled.set_and_save(true); - motors.ext_gyro_gain = argv[1].i; - motors.ext_gyro_gain.save(); + // capture gain if user simply provides a number + } else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) { + motors.ext_gyro_enabled.set_and_save(true); + motors.ext_gyro_gain = argv[1].i; + motors.ext_gyro_gain.save(); - }else{ - Serial.printf_P(PSTR("\nOp:[on, off] gain\n")); - } + }else{ + Serial.printf_P(PSTR("\nOp:[on, off] gain\n")); + } - report_gyro(); - return 0; + report_gyro(); + return 0; } -#endif // FRAME_CONFIG == HELI + #endif // FRAME_CONFIG == HELI static void clear_offsets() { - Vector3f _offsets(0.0,0.0,0.0); - compass.set_offsets(_offsets); - compass.save_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(); - - int16_t _min[3] = {0,0,0}; - int16_t _max[3] = {0,0,0}; - - compass.read(); - - while(1){ - delay(50); - float heading; - - compass.read(); - heading = compass.calculate_heading(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(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; -} -*/ + * 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(); + * + * int16_t _min[3] = {0,0,0}; + * int16_t _max[3] = {0,0,0}; + * + * compass.read(); + * + * while(1){ + * delay(50); + * float heading; + * + * compass.read(); + * heading = compass.calculate_heading(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(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; + * } + */ static int8_t setup_optflow(uint8_t argc, const Menu::arg *argv) { - #ifdef OPTFLOW_ENABLED - if (!strcmp_P(argv[1].str, PSTR("on"))) { - g.optflow_enabled = true; - init_optflow(); + #ifdef OPTFLOW_ENABLED + 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 (!strcmp_P(argv[1].str, PSTR("off"))) { + g.optflow_enabled = false; - }else{ - Serial.printf_P(PSTR("\nOp:[on, off]\n")); - report_optflow(); - return 0; - } + }else{ + Serial.printf_P(PSTR("\nOp:[on, off]\n")); + report_optflow(); + return 0; + } - g.optflow_enabled.save(); - report_optflow(); - #endif - return 0; + g.optflow_enabled.save(); + report_optflow(); + #endif + return 0; } @@ -867,387 +867,387 @@ setup_optflow(uint8_t argc, const Menu::arg *argv) static void report_batt_monitor() { - Serial.printf_P(PSTR("\nBatt Mon:\n")); - print_divider(); - if(g.battery_monitoring == 0) print_enabled(false); - if(g.battery_monitoring == 3) Serial.printf_P(PSTR("volts")); - if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur")); - print_blanks(2); + Serial.printf_P(PSTR("\nBatt Mon:\n")); + print_divider(); + if(g.battery_monitoring == 0) print_enabled(false); + if(g.battery_monitoring == 3) Serial.printf_P(PSTR("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.command_total; i++){ - struct Location temp = get_cmd_with_index(i); - print_wp(&temp, i); - } - }else{ - struct Location temp = get_cmd_with_index(index); - print_wp(&temp, index); - } + if(index == 255) { + for(byte i = 0; i < g.command_total; i++) { + struct Location temp = get_cmd_with_index(i); + print_wp(&temp, i); + } + }else{ + struct Location temp = get_cmd_with_index(index); + print_wp(&temp, index); + } } static void report_sonar() { - Serial.printf_P(PSTR("Sonar\n")); - print_divider(); - print_enabled(g.sonar_enabled.get()); - Serial.printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type); - print_blanks(2); + Serial.printf_P(PSTR("Sonar\n")); + print_divider(); + print_enabled(g.sonar_enabled.get()); + Serial.printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type); + print_blanks(2); } static void report_frame() { - Serial.printf_P(PSTR("Frame\n")); - print_divider(); + 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 == 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 + #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); + print_blanks(2); } static void report_radio() { - Serial.printf_P(PSTR("Radio\n")); - print_divider(); - // radio - print_radio_values(); - print_blanks(2); + 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(); + Serial.printf_P(PSTR("IMU\n")); + print_divider(); - print_gyro_offsets(); - print_accel_offsets(); - print_blanks(2); + print_gyro_offsets(); + print_accel_offsets(); + print_blanks(2); } static void report_compass() { - Serial.printf_P(PSTR("Compass\n")); - print_divider(); + Serial.printf_P(PSTR("Compass\n")); + print_divider(); - print_enabled(g.compass_enabled); + print_enabled(g.compass_enabled); - // mag declination - Serial.printf_P(PSTR("Mag Dec: %4.4f\n"), - degrees(compass.get_declination())); + // mag declination + Serial.printf_P(PSTR("Mag Dec: %4.4f\n"), + degrees(compass.get_declination())); - Vector3f offsets = compass.get_offsets(); + Vector3f offsets = compass.get_offsets(); - // mag offsets - Serial.printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f"), - offsets.x, - offsets.y, - offsets.z); - print_blanks(2); + // mag offsets + Serial.printf_P(PSTR("Mag off: %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(); + Serial.printf_P(PSTR("Flight modes\n")); + print_divider(); - for(int16_t i = 0; i < 6; i++ ){ - print_switch(i, flight_modes[i], (g.simple_modes & (1<radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse()); - Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse()); - Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse()); - Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse()); + // 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)motors.servo1_pos, (int)motors._servo_1->radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse()); + Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse()); + Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse()); + Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse()); - Serial.printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max); - Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max); - Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max); + Serial.printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max); + Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max); + Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max); - // calculate and print servo rate - Serial.printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed); + // calculate and print servo rate + Serial.printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed); - print_blanks(2); + print_blanks(2); } static void report_gyro() { - Serial.printf_P(PSTR("Gyro:\n")); - print_divider(); + Serial.printf_P(PSTR("Gyro:\n")); + print_divider(); - print_enabled( motors.ext_gyro_enabled ); - if( motors.ext_gyro_enabled ) - Serial.printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain); + print_enabled( motors.ext_gyro_enabled ); + if( motors.ext_gyro_enabled ) + Serial.printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain); - print_blanks(2); + print_blanks(2); } -#endif // FRAME_CONFIG == HELI_FRAME + #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()); -} -*/ + * 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); + 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:\t"),p); - Serial.print(flight_mode_strings[m]); - Serial.printf_P(PSTR(",\t\tSimple: ")); - if(b) - Serial.printf_P(PSTR("ON\n")); - else - Serial.printf_P(PSTR("OFF\n")); + Serial.printf_P(PSTR("Pos %d:\t"),p); + Serial.print(flight_mode_strings[m]); + Serial.printf_P(PSTR(",\t\tSimple: ")); + if(b) + Serial.printf_P(PSTR("ON\n")); + else + Serial.printf_P(PSTR("OFF\n")); } static void print_done() { - Serial.printf_P(PSTR("\nSaved\n")); + Serial.printf_P(PSTR("\nSaved\n")); } static void zero_eeprom(void) { - byte b = 0; + byte b = 0; - Serial.printf_P(PSTR("\nErasing EEPROM\n")); + Serial.printf_P(PSTR("\nErasing EEPROM\n")); - for (int16_t i = 0; i < EEPROM_MAX_ADDR; i++) { - eeprom_write_byte((uint8_t *) i, b); - } + for (int16_t i = 0; i < EEPROM_MAX_ADDR; i++) { + eeprom_write_byte((uint8_t *) i, b); + } - Serial.printf_P(PSTR("done\n")); + Serial.printf_P(PSTR("done\n")); } static void print_accel_offsets(void) { - Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\n"), - (float)imu.ax(), // Pitch - (float)imu.ay(), // Roll - (float)imu.az()); // YAW + Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\n"), + (float)imu.ax(), // Pitch + (float)imu.ay(), // Roll + (float)imu.az()); // YAW } static void print_gyro_offsets(void) { - Serial.printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"), - (float)imu.gx(), - (float)imu.gy(), - (float)imu.gz()); + Serial.printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"), + (float)imu.gx(), + (float)imu.gy(), + (float)imu.gz()); } -#if FRAME_CONFIG == HELI_FRAME + #if FRAME_CONFIG == HELI_FRAME static RC_Channel * heli_get_servo(int16_t servo_num){ - if( servo_num == CH_1 ) - return motors._servo_1; - if( servo_num == CH_2 ) - return motors._servo_2; - if( servo_num == CH_3 ) - return motors._servo_3; - if( servo_num == CH_4 ) - return motors._servo_4; - return NULL; + if( servo_num == CH_1 ) + return motors._servo_1; + if( servo_num == CH_2 ) + return motors._servo_2; + if( servo_num == CH_3 ) + return motors._servo_3; + if( servo_num == CH_4 ) + return motors._servo_4; + return NULL; } // Used to read integer values from the serial port static int16_t read_num_from_serial() { - byte index = 0; - byte timeout = 0; - char data[5] = ""; + 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); + do { + if (Serial.available() == 0) { + delay(10); + timeout++; + }else{ + data[index] = Serial.read(); + timeout = 0; + index++; + } + } while (timeout < 5 && index < 5); - return atoi(data); + return atoi(data); } -#endif + #endif #endif // CLI_ENABLED static void print_blanks(int16_t num) { - while(num > 0){ - num--; - Serial.println(""); - } + while(num > 0) { + num--; + Serial.println(""); + } } static bool wait_for_yes() { - int c; - Serial.flush(); - Serial.printf_P(PSTR("Y to save\n")); + int c; + Serial.flush(); + Serial.printf_P(PSTR("Y to save\n")); - do { - c = Serial.read(); - } while (-1 == c); + do { + c = Serial.read(); + } while (-1 == c); - if (('y' == c) || ('Y' == c)) - return true; - else - return false; + if (('y' == c) || ('Y' == c)) + return true; + else + return false; } static void print_divider(void) { - for (int i = 0; i < 40; i++) { - Serial.print("-"); - } - Serial.println(""); + 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")); + if(b) + Serial.printf_P(PSTR("en")); + else + Serial.printf_P(PSTR("dis")); + Serial.printf_P(PSTR("abled\n")); } static void init_esc() { - motors.enable(); - motors.armed(true); - while(1){ - read_radio(); - delay(100); - dancing_light(); - motors.throttle_pass_through(); - } + motors.enable(); + motors.armed(true); + while(1) { + read_radio(); + delay(100); + dancing_light(); + motors.throttle_pass_through(); + } } static void print_wp(struct Location *cmd, byte index) { - float t1 = (float)cmd->lat / t7; - float t2 = (float)cmd->lng / t7; + float t1 = (float)cmd->lat / t7; + float t2 = (float)cmd->lng / t7; - Serial.printf_P(PSTR("cmd#: %d id:%d op:%d p1:%d p2:%ld p3:%4.7f p4:%4.7f \n"), - (int)index, - (int)cmd->id, - (int)cmd->options, - (int)cmd->p1, - (long)cmd->alt, - t1, - t2); + Serial.printf_P(PSTR("cmd#: %d id:%d op:%d p1:%d p2:%ld p3:%4.7f p4:%4.7f \n"), + (int)index, + (int)cmd->id, + (int)cmd->options, + (int)cmd->p1, + (long)cmd->alt, + t1, + t2); } static void report_gps() { - Serial.printf_P(PSTR("\nGPS\n")); - print_divider(); - print_enabled(GPS_enabled); - print_blanks(2); + Serial.printf_P(PSTR("\nGPS\n")); + print_divider(); + print_enabled(GPS_enabled); + print_blanks(2); } static void report_version() { - Serial.printf_P(PSTR("FW Ver: %d\n"),(int)g.format_version.get()); - print_divider(); - print_blanks(2); + Serial.printf_P(PSTR("FW Ver: %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{ - float low = (float)g.radio_tuning_low.get() / 1000; - float high = (float)g.radio_tuning_high.get() / 1000; - Serial.printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high); - } - print_blanks(2); + Serial.printf_P(PSTR("\nTUNE:\n")); + print_divider(); + if (g.radio_tuning == 0) { + print_enabled(g.radio_tuning.get()); + }else{ + float low = (float)g.radio_tuning_low.get() / 1000; + float high = (float)g.radio_tuning_high.get() / 1000; + Serial.printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high); + } + print_blanks(2); }