From 4ae0b9dc8a45ff0a5f7e077ea30f145e0c354d45 Mon Sep 17 00:00:00 2001 From: uncrustify Date: Tue, 21 Aug 2012 19:19:50 -0700 Subject: [PATCH] uncrustify ArduCopter/test.pde --- ArduCopter/test.pde | 2304 +++++++++++++++++++++---------------------- 1 file changed, 1152 insertions(+), 1152 deletions(-) diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 4d19653acc..a204a64ece 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -4,91 +4,91 @@ // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. -static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); -static int8_t test_radio(uint8_t argc, const Menu::arg *argv); -//static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); -//static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); -static int8_t test_gps(uint8_t argc, const Menu::arg *argv); -//static int8_t test_tri(uint8_t argc, const Menu::arg *argv); -//static int8_t test_adc(uint8_t argc, const Menu::arg *argv); -static int8_t test_ins(uint8_t argc, const Menu::arg *argv); -//static int8_t test_imu(uint8_t argc, const Menu::arg *argv); -//static int8_t test_dcm_eulers(uint8_t argc, const Menu::arg *argv); -//static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); -//static int8_t test_omega(uint8_t argc, const Menu::arg *argv); -//static int8_t test_stab_d(uint8_t argc, const Menu::arg *argv); -static int8_t test_battery(uint8_t argc, const Menu::arg *argv); -//static int8_t test_toy(uint8_t argc, const Menu::arg *argv); -static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv); -//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv); -static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); -static int8_t test_relay(uint8_t argc, const Menu::arg *argv); -static int8_t test_wp(uint8_t argc, const Menu::arg *argv); +static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); +static int8_t test_radio(uint8_t argc, const Menu::arg *argv); +//static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); +//static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); +static int8_t test_gps(uint8_t argc, const Menu::arg *argv); +//static int8_t test_tri(uint8_t argc, const Menu::arg *argv); +//static int8_t test_adc(uint8_t argc, const Menu::arg *argv); +static int8_t test_ins(uint8_t argc, const Menu::arg *argv); +//static int8_t test_imu(uint8_t argc, const Menu::arg *argv); +//static int8_t test_dcm_eulers(uint8_t argc, const Menu::arg *argv); +//static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); +//static int8_t test_omega(uint8_t argc, const Menu::arg *argv); +//static int8_t test_stab_d(uint8_t argc, const Menu::arg *argv); +static int8_t test_battery(uint8_t argc, const Menu::arg *argv); +//static int8_t test_toy(uint8_t argc, const Menu::arg *argv); +static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv); +//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv); +static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); +static int8_t test_relay(uint8_t argc, const Menu::arg *argv); +static int8_t test_wp(uint8_t argc, const Menu::arg *argv); #if HIL_MODE != HIL_MODE_ATTITUDE -static int8_t test_baro(uint8_t argc, const Menu::arg *argv); -static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); +static int8_t test_baro(uint8_t argc, const Menu::arg *argv); +static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); #endif -static int8_t test_mag(uint8_t argc, const Menu::arg *argv); -static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); -static int8_t test_logging(uint8_t argc, const Menu::arg *argv); -//static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); -static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); -//static int8_t test_mission(uint8_t argc, const Menu::arg *argv); +static int8_t test_mag(uint8_t argc, const Menu::arg *argv); +static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); +static int8_t test_logging(uint8_t argc, const Menu::arg *argv); +//static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); +static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); +static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); +//static int8_t test_mission(uint8_t argc, const Menu::arg *argv); // this is declared here to remove compiler errors -extern void print_latlon(BetterStream *s, int32_t lat_or_lon); // in Log.pde +extern void print_latlon(BetterStream *s, int32_t lat_or_lon); // in Log.pde // This is the help function // PSTR is an AVR macro to read strings from flash memory // printf_P is a version of printf that reads from flash memory -/*static int8_t help_test(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("\n" - "Commands:\n" - " radio\n" - " servos\n" - " g_gps\n" - " imu\n" - " battery\n" - "\n")); -}*/ +/*static int8_t help_test(uint8_t argc, const Menu::arg *argv) + * { + * Serial.printf_P(PSTR("\n" + * "Commands:\n" + * " radio\n" + * " servos\n" + * " g_gps\n" + * " imu\n" + * " battery\n" + * "\n")); + * }*/ // Creates a constant array of structs representing menu options // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Coommon for implementation details const struct Menu::command test_menu_commands[] PROGMEM = { - {"pwm", test_radio_pwm}, - {"radio", test_radio}, + {"pwm", test_radio_pwm}, + {"radio", test_radio}, // {"failsafe", test_failsafe}, // {"stabilize", test_stabilize}, - {"gps", test_gps}, -// {"adc", test_adc}, - {"ins", test_ins}, + {"gps", test_gps}, +// {"adc", test_adc}, + {"ins", test_ins}, // {"imu", test_imu}, // {"dcm", test_dcm_eulers}, - //{"omega", test_omega}, + //{"omega", test_omega}, // {"stab_d", test_stab_d}, - {"battery", test_battery}, - {"tune", test_tuning}, - //{"tri", test_tri}, - {"relay", test_relay}, - {"wp", test_wp}, + {"battery", test_battery}, + {"tune", test_tuning}, + //{"tri", test_tri}, + {"relay", test_relay}, + {"wp", test_wp}, // {"toy", test_toy}, #if HIL_MODE != HIL_MODE_ATTITUDE - {"altitude", test_baro}, - {"sonar", test_sonar}, + {"altitude", test_baro}, + {"sonar", test_sonar}, #endif - {"compass", test_mag}, - {"optflow", test_optflow}, - //{"xbee", test_xbee}, - {"eedump", test_eedump}, - {"logging", test_logging}, + {"compass", test_mag}, + {"optflow", test_optflow}, + //{"xbee", test_xbee}, + {"eedump", test_eedump}, + {"logging", test_logging}, // {"rawgps", test_rawgps}, // {"mission", test_mission}, - //{"reverse", test_reverse}, - {"nav", test_wp_nav}, + //{"reverse", test_reverse}, + {"nav", test_wp_nav}, }; // A Macro to create the Menu @@ -97,996 +97,996 @@ MENU(test_menu, "test", test_menu_commands); static int8_t test_mode(uint8_t argc, const Menu::arg *argv) { - //Serial.printf_P(PSTR("Test Mode\n\n")); - test_menu.run(); - return 0; + //Serial.printf_P(PSTR("Test Mode\n\n")); + test_menu.run(); + return 0; } static int8_t test_eedump(uint8_t argc, const Menu::arg *argv) { - int i, j; + int i, j; - // hexdump the EEPROM - for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { - Serial.printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); - Serial.println(); - } - return(0); + // hexdump the EEPROM + for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { + Serial.printf_P(PSTR("%04x:"), i); + for (j = 0; j < 16; j++) + Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); + Serial.println(); + } + return(0); } static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - print_test_disabled(); - return (0); - #else - print_hit_enter(); - delay(1000); +#if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 + print_test_disabled(); + return (0); +#else + print_hit_enter(); + delay(1000); - while(1){ - delay(20); + while(1) { + delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); - // servo Yaw - //APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + // servo Yaw + //APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - g.rc_1.radio_in, - g.rc_2.radio_in, - g.rc_3.radio_in, - g.rc_4.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in); + Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + g.rc_1.radio_in, + g.rc_2.radio_in, + g.rc_3.radio_in, + g.rc_4.radio_in, + g.rc_5.radio_in, + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in); - if(Serial.available() > 0){ - return (0); - } - } - #endif + if(Serial.available() > 0) { + return (0); + } + } +#endif } /* -//static int8_t -//test_tri(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - g.rc_4.servo_out = g.rc_4.control_in; - g.rc_4.calc_pwm(); - - Serial.printf_P(PSTR("input: %d\toutput%d\n"), - g.rc_4.control_in, - g.rc_4.radio_out); - - APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out); - - if(Serial.available() > 0){ - return (0); - } - } -}*/ + * //static int8_t + * //test_tri(uint8_t argc, const Menu::arg *argv) + * { + * print_hit_enter(); + * delay(1000); + * + * while(1){ + * delay(20); + * + * // Filters radio input - adjust filters in the radio.pde file + * // ---------------------------------------------------------- + * read_radio(); + * g.rc_4.servo_out = g.rc_4.control_in; + * g.rc_4.calc_pwm(); + * + * Serial.printf_P(PSTR("input: %d\toutput%d\n"), + * g.rc_4.control_in, + * g.rc_4.radio_out); + * + * APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out); + * + * if(Serial.available() > 0){ + * return (0); + * } + * } + * }*/ /*static int8_t -//test_toy(uint8_t argc, const Menu::arg *argv) -{ - int16_t yaw_rate; - int16_t roll_rate; - g.rc_1.control_in = -2500; - g.rc_2.control_in = 2500; - - g.toy_yaw_rate = 3; - yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; - roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40; - Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); - - g.toy_yaw_rate = 2; - yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; - roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40; - Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); - - g.toy_yaw_rate = 1; - yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; - roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40; - Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); -}*/ + * //test_toy(uint8_t argc, const Menu::arg *argv) + * { + * int16_t yaw_rate; + * int16_t roll_rate; + * g.rc_1.control_in = -2500; + * g.rc_2.control_in = 2500; + * + * g.toy_yaw_rate = 3; + * yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; + * roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40; + * Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); + * + * g.toy_yaw_rate = 2; + * yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; + * roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40; + * Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); + * + * g.toy_yaw_rate = 1; + * yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; + * roll_rate = ((int32_t)g.rc_2.control_in * (yaw_rate/100)) /40; + * Serial.printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); + * }*/ static int8_t test_radio(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + print_hit_enter(); + delay(1000); - while(1){ - delay(20); - read_radio(); + while(1) { + delay(20); + read_radio(); - Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), - g.rc_1.control_in, - g.rc_2.control_in, - g.rc_3.control_in, - g.rc_4.control_in, - g.rc_5.control_in, - g.rc_6.control_in, - g.rc_7.control_in); + Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), + g.rc_1.control_in, + g.rc_2.control_in, + g.rc_3.control_in, + g.rc_4.control_in, + g.rc_5.control_in, + g.rc_6.control_in, + g.rc_7.control_in); - //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100)); + //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100)); - /*Serial.printf_P(PSTR( "min: %d" - "\t in: %d" - "\t pwm_in: %d" - "\t sout: %d" - "\t pwm_out %d\n"), - g.rc_3.radio_min, - g.rc_3.control_in, - g.rc_3.radio_in, - g.rc_3.servo_out, - g.rc_3.pwm_out - ); - */ - if(Serial.available() > 0){ - return (0); - } - } + /*Serial.printf_P(PSTR( "min: %d" + * "\t in: %d" + * "\t pwm_in: %d" + * "\t sout: %d" + * "\t pwm_out %d\n"), + * g.rc_3.radio_min, + * g.rc_3.control_in, + * g.rc_3.radio_in, + * g.rc_3.servo_out, + * g.rc_3.pwm_out + * ); + */ + if(Serial.available() > 0) { + return (0); + } + } } /* -//static int8_t -//test_failsafe(uint8_t argc, const Menu::arg *argv) -{ - - #if THROTTLE_FAILSAFE - byte fail_test; - print_hit_enter(); - for(int16_t i = 0; i < 50; i++){ - delay(20); - read_radio(); - } - - oldSwitchPosition = readSwitch(); - - Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); - while(g.rc_3.control_in > 0){ - delay(20); - read_radio(); - } - - while(1){ - delay(20); - read_radio(); - - if(g.rc_3.control_in > 0){ - Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in); - fail_test++; - } - - if(oldSwitchPosition != readSwitch()){ - Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); - Serial.println(flight_mode_strings[readSwitch()]); - fail_test++; - } - - if(g.throttle_fs_enabled && g.rc_3.get_failsafe()){ - Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in); - Serial.println(flight_mode_strings[readSwitch()]); - fail_test++; - } - - if(fail_test > 0){ - return (0); - } - if(Serial.available() > 0){ - Serial.printf_P(PSTR("LOS caused no change in ACM.\n")); - return (0); - } - } - #else - return (0); - #endif -} -*/ + * //static int8_t + * //test_failsafe(uint8_t argc, const Menu::arg *argv) + * { + * + * #if THROTTLE_FAILSAFE + * byte fail_test; + * print_hit_enter(); + * for(int16_t i = 0; i < 50; i++){ + * delay(20); + * read_radio(); + * } + * + * oldSwitchPosition = readSwitch(); + * + * Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); + * while(g.rc_3.control_in > 0){ + * delay(20); + * read_radio(); + * } + * + * while(1){ + * delay(20); + * read_radio(); + * + * if(g.rc_3.control_in > 0){ + * Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in); + * fail_test++; + * } + * + * if(oldSwitchPosition != readSwitch()){ + * Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); + * Serial.println(flight_mode_strings[readSwitch()]); + * fail_test++; + * } + * + * if(g.throttle_fs_enabled && g.rc_3.get_failsafe()){ + * Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in); + * Serial.println(flight_mode_strings[readSwitch()]); + * fail_test++; + * } + * + * if(fail_test > 0){ + * return (0); + * } + * if(Serial.available() > 0){ + * Serial.printf_P(PSTR("LOS caused no change in ACM.\n")); + * return (0); + * } + * } + * #else + * return (0); + * #endif + * } + */ /* -//static int8_t -//test_stabilize(uint8_t argc, const Menu::arg *argv) -{ - static byte ts_num; - - - print_hit_enter(); - delay(1000); - - // setup the radio - // --------------- - init_rc_in(); - - control_mode = STABILIZE; - Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP()); - Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); - - motors.auto_armed(false); - motors.armed(true); - - while(1){ - // 50 hz - if (millis() - fast_loopTimer > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer; - fast_loopTimer = millis(); - G_Dt = (float)delta_ms_fast_loop / 1000.f; - - if(g.compass_enabled){ - medium_loopCounter++; - if(medium_loopCounter == 5){ - Matrix3f m = dcm.get_dcm_matrix(); - compass.read(); // Read magnetometer - compass.null_offsets(); - medium_loopCounter = 0; - } - } - - // for trim features - read_trim_switch(); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - // IMU - // --- - read_AHRS(); - - // allow us to zero out sensors with control switches - if(g.rc_5.control_in < 600){ - dcm.roll_sensor = dcm.pitch_sensor = 0; - } - - // custom code/exceptions for flight modes - // --------------------------------------- - update_current_flight_mode(); - - // write out the servo PWM values - // ------------------------------ - set_servos_4(); - - ts_num++; - if (ts_num > 10){ - ts_num = 0; - Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "), - (int)(dcm.roll_sensor/100), - (int)(dcm.pitch_sensor/100), - g.rc_1.pwm_out); - - print_motor_out(); - } - // R: 1417, L: 1453 F: 1453 B: 1417 - - //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); - //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); - - if(Serial.available() > 0){ - if(g.compass_enabled){ - compass.save_offsets(); - report_compass(); - } - return (0); - } - - } - } -} -*/ + * //static int8_t + * //test_stabilize(uint8_t argc, const Menu::arg *argv) + * { + * static byte ts_num; + * + * + * print_hit_enter(); + * delay(1000); + * + * // setup the radio + * // --------------- + * init_rc_in(); + * + * control_mode = STABILIZE; + * Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP()); + * Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); + * + * motors.auto_armed(false); + * motors.armed(true); + * + * while(1){ + * // 50 hz + * if (millis() - fast_loopTimer > 19) { + * delta_ms_fast_loop = millis() - fast_loopTimer; + * fast_loopTimer = millis(); + * G_Dt = (float)delta_ms_fast_loop / 1000.f; + * + * if(g.compass_enabled){ + * medium_loopCounter++; + * if(medium_loopCounter == 5){ + * Matrix3f m = dcm.get_dcm_matrix(); + * compass.read(); // Read magnetometer + * compass.null_offsets(); + * medium_loopCounter = 0; + * } + * } + * + * // for trim features + * read_trim_switch(); + * + * // Filters radio input - adjust filters in the radio.pde file + * // ---------------------------------------------------------- + * read_radio(); + * + * // IMU + * // --- + * read_AHRS(); + * + * // allow us to zero out sensors with control switches + * if(g.rc_5.control_in < 600){ + * dcm.roll_sensor = dcm.pitch_sensor = 0; + * } + * + * // custom code/exceptions for flight modes + * // --------------------------------------- + * update_current_flight_mode(); + * + * // write out the servo PWM values + * // ------------------------------ + * set_servos_4(); + * + * ts_num++; + * if (ts_num > 10){ + * ts_num = 0; + * Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "), + * (int)(dcm.roll_sensor/100), + * (int)(dcm.pitch_sensor/100), + * g.rc_1.pwm_out); + * + * print_motor_out(); + * } + * // R: 1417, L: 1453 F: 1453 B: 1417 + * + * //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); + * //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); + * + * if(Serial.available() > 0){ + * if(g.compass_enabled){ + * compass.save_offsets(); + * report_compass(); + * } + * return (0); + * } + * + * } + * } + * } + */ /* -#if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_ADC == ENABLED -//static int8_t -//test_adc(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - Serial.printf_P(PSTR("ADC\n")); - delay(1000); - - adc.Init(&timer_scheduler); - - delay(50); - - while(1){ - for(int16_t i = 0; i < 9; i++){ - Serial.printf_P(PSTR("%.1f,"),adc.Ch(i)); - } - Serial.println(); - delay(20); - if(Serial.available() > 0){ - return (0); - } - } -} -#endif -*/ + * #if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_ADC == ENABLED + * //static int8_t + * //test_adc(uint8_t argc, const Menu::arg *argv) + * { + * print_hit_enter(); + * Serial.printf_P(PSTR("ADC\n")); + * delay(1000); + * + * adc.Init(&timer_scheduler); + * + * delay(50); + * + * while(1){ + * for(int16_t i = 0; i < 9; i++){ + * Serial.printf_P(PSTR("%.1f,"),adc.Ch(i)); + * } + * Serial.println(); + * delay(20); + * if(Serial.available() > 0){ + * return (0); + * } + * } + * } + * #endif + */ /* -static int8_t -test_adc(uint8_t argc, const Menu::arg *argv) -{ - ins.init(&timer_scheduler); - - int8_t mytimer = 0; - startup_ground(); - Serial.println("OK"); - - while(1){ - // We want this to execute fast - // ---------------------------- - uint32_t timer = micros(); - - if ((timer - fast_loopTimer) >= 10000 && imu.new_data_available()) { - G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops - fast_loopTimer = timer; - - read_AHRS(); - - //calc_inertia(); - accels_rotated = ahrs.get_dcm_matrix() * imu.get_accel(); - //accels_rotated += accels_offset; // skew accels to account for long term error using calibration - - mytimer++; - - if ((timer - fiftyhz_loopTimer) >= 20000) { - fiftyhz_loopTimer = timer; - //sensed_loc.lng = sensed_loc.lat = sensed_loc.alt = 0; - - // position fix - //inertial_error_correction(); - } - - if (mytimer >= 10){ - float test = sqrt(sq(accels_rotated.x) + sq(accels_rotated.y) + sq(accels_rotated.z)) / 9.80665; - - Vector3f _accels = imu.get_accel(); - mytimer = 0; - - - Serial.printf("%1.4f, %1.4f, %1.4f | %1.4f, %1.4f, %1.4f | %d, %1.4f, %d, %1.4f \n", - _accels.x, - _accels.y, - _accels.z, - accels_rotated.x, - accels_rotated.y, - accels_rotated.z, - test); - - - } - - if(Serial.available() > 0){ - return (0); - } - } - } - return (0); -} -*/ + * static int8_t + * test_adc(uint8_t argc, const Menu::arg *argv) + * { + * ins.init(&timer_scheduler); + * + * int8_t mytimer = 0; + * startup_ground(); + * Serial.println("OK"); + * + * while(1){ + * // We want this to execute fast + * // ---------------------------- + * uint32_t timer = micros(); + * + * if ((timer - fast_loopTimer) >= 10000 && imu.new_data_available()) { + * G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops + * fast_loopTimer = timer; + * + * read_AHRS(); + * + * //calc_inertia(); + * accels_rotated = ahrs.get_dcm_matrix() * imu.get_accel(); + * //accels_rotated += accels_offset; // skew accels to account for long term error using calibration + * + * mytimer++; + * + * if ((timer - fiftyhz_loopTimer) >= 20000) { + * fiftyhz_loopTimer = timer; + * //sensed_loc.lng = sensed_loc.lat = sensed_loc.alt = 0; + * + * // position fix + * //inertial_error_correction(); + * } + * + * if (mytimer >= 10){ + * float test = sqrt(sq(accels_rotated.x) + sq(accels_rotated.y) + sq(accels_rotated.z)) / 9.80665; + * + * Vector3f _accels = imu.get_accel(); + * mytimer = 0; + * + * + * Serial.printf("%1.4f, %1.4f, %1.4f | %1.4f, %1.4f, %1.4f | %d, %1.4f, %d, %1.4f \n", + * _accels.x, + * _accels.y, + * _accels.z, + * accels_rotated.x, + * accels_rotated.y, + * accels_rotated.z, + * test); + * + * + * } + * + * if(Serial.available() > 0){ + * return (0); + * } + * } + * } + * return (0); + * } + */ static int8_t test_ins(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - print_test_disabled(); - return (0); - #else - float gyro[3], accel[3], temp; - print_hit_enter(); - Serial.printf_P(PSTR("InertialSensor\n")); - delay(1000); +#if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 + print_test_disabled(); + return (0); +#else + float gyro[3], accel[3], temp; + print_hit_enter(); + Serial.printf_P(PSTR("InertialSensor\n")); + delay(1000); - ins.init(&timer_scheduler); + ins.init(&timer_scheduler); - delay(50); + delay(50); - while(1){ - ins.update(); - ins.get_gyros(gyro); - ins.get_accels(accel); - temp = ins.temperature(); + while(1) { + ins.update(); + ins.get_gyros(gyro); + ins.get_accels(accel); + temp = ins.temperature(); - float test = sqrt(sq(accel[0]) + sq(accel[1]) + sq(accel[2])) / 9.80665; + float test = sqrt(sq(accel[0]) + sq(accel[1]) + sq(accel[2])) / 9.80665; - Serial.printf_P(PSTR("g")); + Serial.printf_P(PSTR("g")); - for (int16_t i = 0; i < 3; i++) { - Serial.printf_P(PSTR(" %7.4f"), gyro[i]); - } + for (int16_t i = 0; i < 3; i++) { + Serial.printf_P(PSTR(" %7.4f"), gyro[i]); + } - Serial.printf_P(PSTR(" a")); + Serial.printf_P(PSTR(" a")); - for (int16_t i = 0; i < 3; i++) { - Serial.printf_P(PSTR(" %7.4f"),accel[i]); - } + for (int16_t i = 0; i < 3; i++) { + Serial.printf_P(PSTR(" %7.4f"),accel[i]); + } - Serial.printf_P(PSTR(" t %7.4f "), temp); - Serial.printf_P(PSTR(" | %7.4f \n"), test); + Serial.printf_P(PSTR(" t %7.4f "), temp); + Serial.printf_P(PSTR(" | %7.4f \n"), test); - delay(40); - if(Serial.available() > 0){ - return (0); - } - } - #endif + delay(40); + if(Serial.available() > 0) { + return (0); + } + } +#endif } /* - test the IMU interface + * test the IMU interface */ /* -static int8_t -test_imu(uint8_t argc, const Menu::arg *argv) -{ - #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - print_test_disabled(); - return (0); - #else - Vector3f gyro; - Vector3f accel; - - imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); - - report_imu(); - imu.init_gyro(delay, flash_leds); - report_imu(); - - print_hit_enter(); - delay(1000); - - while(1){ - delay(40); - - imu.update(); - gyro = imu.get_gyro(); - accel = imu.get_accel(); - - Serial.printf_P(PSTR("g %8.4f %8.4f %8.4f"), gyro.x, gyro.y, gyro.z); - Serial.printf_P(PSTR(" a %8.4f %8.4f %8.4f\n"), accel.x, accel.y, accel.z); - - if(Serial.available() > 0){ - return (0); - } - } - #endif -} -*/ + * static int8_t + * test_imu(uint8_t argc, const Menu::arg *argv) + * { + * #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 + * print_test_disabled(); + * return (0); + * #else + * Vector3f gyro; + * Vector3f accel; + * + * imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); + * + * report_imu(); + * imu.init_gyro(delay, flash_leds); + * report_imu(); + * + * print_hit_enter(); + * delay(1000); + * + * while(1){ + * delay(40); + * + * imu.update(); + * gyro = imu.get_gyro(); + * accel = imu.get_accel(); + * + * Serial.printf_P(PSTR("g %8.4f %8.4f %8.4f"), gyro.x, gyro.y, gyro.z); + * Serial.printf_P(PSTR(" a %8.4f %8.4f %8.4f\n"), accel.x, accel.y, accel.z); + * + * if(Serial.available() > 0){ + * return (0); + * } + * } + * #endif + * } + */ /* -static int8_t -test_imu(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - Serial.printf_P(PSTR("ADC\n")); - adc.Init(&timer_scheduler); - - delay(1000); - Vector3f avg; - avg.x = adc.Ch(4); - avg.y = adc.Ch(5); - avg.z = adc.Ch(6); - - //Serial.printf_P(PSTR("init %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z); - Vector3f low = avg; - Vector3f high = avg; - - while(1){ - delay(100); - avg.x = avg.x * .95 + adc.Ch(4) * .05; - avg.y = avg.y * .95 + adc.Ch(5) * .05; - avg.z = avg.z * .95 + adc.Ch(6) * .05; - - if(avg.x > high.x) - high.x = ceil(high.x *.9 + avg.x * .1); - - if(avg.y > high.y) - high.y = ceil(high.y *.9 + avg.y * .1); - - if(avg.z > high.z) - high.z = ceil(high.z *.9 + avg.z * .1); - - // - if(avg.x < low.x) - low.x = floor(low.x *.9 + avg.x * .1); - - if(avg.y < low.y) - low.y = floor(low.y *.9 + avg.y * .1); - - if(avg.z < low.z) - low.z = floor(low.z *.9 + avg.z * .1); - - Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z, low.x, low.y, low.z, high.x, high.y, high.z); - - //Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %d, %d\n"), avg.x, avg.y, avg.z, _count[0], _sum[0]); - - //Serial.println(); - if(Serial.available() > 0){ - Serial.printf_P(PSTR("Y to save\n")); - int16_t c; - c = Serial.read(); - - do { - c = Serial.read(); - } while (-1 == c); - - if (('y' == c) || ('Y' == c)){ - ins._x_high = high.x; - ins._x_low = low.x; - ins._y_high = high.y; - ins._y_low = low.y; - ins._z_high = high.z; - ins._z_low = low.z; - ins._x_high.save(); - ins._x_low.save(); - ins._y_high.save(); - ins._y_low.save(); - ins._z_high.save(); - ins._z_low.save(); - Serial.printf_P(PSTR("saved\n")); - } - - return (0); - } - } -} -*/ + * static int8_t + * test_imu(uint8_t argc, const Menu::arg *argv) + * { + * print_hit_enter(); + * Serial.printf_P(PSTR("ADC\n")); + * adc.Init(&timer_scheduler); + * + * delay(1000); + * Vector3f avg; + * avg.x = adc.Ch(4); + * avg.y = adc.Ch(5); + * avg.z = adc.Ch(6); + * + * //Serial.printf_P(PSTR("init %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z); + * Vector3f low = avg; + * Vector3f high = avg; + * + * while(1){ + * delay(100); + * avg.x = avg.x * .95 + adc.Ch(4) * .05; + * avg.y = avg.y * .95 + adc.Ch(5) * .05; + * avg.z = avg.z * .95 + adc.Ch(6) * .05; + * + * if(avg.x > high.x) + * high.x = ceil(high.x *.9 + avg.x * .1); + * + * if(avg.y > high.y) + * high.y = ceil(high.y *.9 + avg.y * .1); + * + * if(avg.z > high.z) + * high.z = ceil(high.z *.9 + avg.z * .1); + * + * // + * if(avg.x < low.x) + * low.x = floor(low.x *.9 + avg.x * .1); + * + * if(avg.y < low.y) + * low.y = floor(low.y *.9 + avg.y * .1); + * + * if(avg.z < low.z) + * low.z = floor(low.z *.9 + avg.z * .1); + * + * Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z, low.x, low.y, low.z, high.x, high.y, high.z); + * + * //Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %d, %d\n"), avg.x, avg.y, avg.z, _count[0], _sum[0]); + * + * //Serial.println(); + * if(Serial.available() > 0){ + * Serial.printf_P(PSTR("Y to save\n")); + * int16_t c; + * c = Serial.read(); + * + * do { + * c = Serial.read(); + * } while (-1 == c); + * + * if (('y' == c) || ('Y' == c)){ + * ins._x_high = high.x; + * ins._x_low = low.x; + * ins._y_high = high.y; + * ins._y_low = low.y; + * ins._z_high = high.z; + * ins._z_low = low.z; + * ins._x_high.save(); + * ins._x_low.save(); + * ins._y_high.save(); + * ins._y_low.save(); + * ins._z_high.save(); + * ins._z_low.save(); + * Serial.printf_P(PSTR("saved\n")); + * } + * + * return (0); + * } + * } + * } + */ /* - test the DCM code, printing Euler angles + * test the DCM code, printing Euler angles */ /*static int8_t -//test_dcm_eulers(uint8_t argc, const Menu::arg *argv) -{ - - //Serial.printf_P(PSTR("Calibrating.")); - - //dcm.kp_yaw(0.02); - //dcm.ki_yaw(0); - - imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); - - report_imu(); - imu.init_gyro(delay, flash_leds); - report_imu(); - - print_hit_enter(); - delay(1000); - - //float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw; - fast_loopTimer = millis(); - - while(1){ - //delay(20); - if (millis() - fast_loopTimer >=20) { - - // IMU - // --- - read_AHRS(); - medium_loopCounter++; - - if(medium_loopCounter == 4){ - update_trig(); - } - - if(medium_loopCounter == 1){ - medium_loopCounter = 0; - Serial.printf_P(PSTR("dcm: %6.1f, %6.1f, %6.1f omega: %6.1f, %6.1f, %6.1f\n"), - dcm.roll_sensor/100.0, - dcm.pitch_sensor/100.0, - dcm.yaw_sensor/100.0, - degrees(omega.x), - degrees(omega.y), - degrees(omega.z)); - - if(g.compass_enabled){ - compass.read(); // Read magnetometer - compass.null_offsets(); - } - } - fast_loopTimer = millis(); - } - if(Serial.available() > 0){ - return (0); - } - } - return (0); -}*/ + * //test_dcm_eulers(uint8_t argc, const Menu::arg *argv) + * { + * + * //Serial.printf_P(PSTR("Calibrating.")); + * + * //dcm.kp_yaw(0.02); + * //dcm.ki_yaw(0); + * + * imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); + * + * report_imu(); + * imu.init_gyro(delay, flash_leds); + * report_imu(); + * + * print_hit_enter(); + * delay(1000); + * + * //float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw; + * fast_loopTimer = millis(); + * + * while(1){ + * //delay(20); + * if (millis() - fast_loopTimer >=20) { + * + * // IMU + * // --- + * read_AHRS(); + * medium_loopCounter++; + * + * if(medium_loopCounter == 4){ + * update_trig(); + * } + * + * if(medium_loopCounter == 1){ + * medium_loopCounter = 0; + * Serial.printf_P(PSTR("dcm: %6.1f, %6.1f, %6.1f omega: %6.1f, %6.1f, %6.1f\n"), + * dcm.roll_sensor/100.0, + * dcm.pitch_sensor/100.0, + * dcm.yaw_sensor/100.0, + * degrees(omega.x), + * degrees(omega.y), + * degrees(omega.z)); + * + * if(g.compass_enabled){ + * compass.read(); // Read magnetometer + * compass.null_offsets(); + * } + * } + * fast_loopTimer = millis(); + * } + * if(Serial.available() > 0){ + * return (0); + * } + * } + * return (0); + * }*/ static int8_t test_gps(uint8_t argc, const Menu::arg *argv) { // test disabled to save code size for 1280 - #if defined( __AVR_ATmega1280__ ) || HIL_MODE != HIL_MODE_DISABLED - print_test_disabled(); - return (0); - #else - print_hit_enter(); - delay(1000); +#if defined( __AVR_ATmega1280__ ) || HIL_MODE != HIL_MODE_DISABLED + print_test_disabled(); + return (0); +#else + print_hit_enter(); + delay(1000); - while(1){ - delay(333); + while(1) { + delay(333); - // Blink GPS LED if we don't have a fix - // ------------------------------------ - update_GPS_light(); + // Blink GPS LED if we don't have a fix + // ------------------------------------ + update_GPS_light(); - g_gps->update(); + g_gps->update(); - if (g_gps->new_data){ - Serial.printf_P(PSTR("Lat: ")); - print_latlon(&Serial, g_gps->latitude); - Serial.printf_P(PSTR(", Lon ")); - print_latlon(&Serial, g_gps->longitude); - Serial.printf_P(PSTR(", Alt: %ldm, #sats: %d\n"), - g_gps->altitude/100, - g_gps->num_sats); - g_gps->new_data = false; - }else{ - Serial.print("."); - } - if(Serial.available() > 0){ - return (0); - } - } - return 0; - #endif + if (g_gps->new_data) { + Serial.printf_P(PSTR("Lat: ")); + print_latlon(&Serial, g_gps->latitude); + Serial.printf_P(PSTR(", Lon ")); + print_latlon(&Serial, g_gps->longitude); + Serial.printf_P(PSTR(", Alt: %ldm, #sats: %d\n"), + g_gps->altitude/100, + g_gps->num_sats); + g_gps->new_data = false; + }else{ + Serial.print("."); + } + if(Serial.available() > 0) { + return (0); + } + } + return 0; +#endif } // used to test the gain scheduler for Stab_D /* -static int8_t -test_stab_d(uint8_t argc, const Menu::arg *argv) -{ - int16_t i = 0; - g.stabilize_d = 1; - - g.stabilize_d_schedule = 1 - for (i = -4600; i < 4600; i+=10) { - new_radio_frame = true; - g.rc_1.control_in = i; - g.rc_2.control_in = i; - update_roll_pitch_mode(); - Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); - } - g.stabilize_d_schedule = .5 - for (i = -4600; i < 4600; i+=10) { - new_radio_frame = true; - g.rc_1.control_in = i; - g.rc_2.control_in = i; - update_roll_pitch_mode(); - Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); - } - - g.stabilize_d_schedule = 0 - for (i = -4600; i < 4600; i+=10) { - new_radio_frame = true; - g.rc_1.control_in = i; - g.rc_2.control_in = i; - update_roll_pitch_mode(); - Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); - } - -}*/ + * static int8_t + * test_stab_d(uint8_t argc, const Menu::arg *argv) + * { + * int16_t i = 0; + * g.stabilize_d = 1; + * + * g.stabilize_d_schedule = 1 + * for (i = -4600; i < 4600; i+=10) { + * new_radio_frame = true; + * g.rc_1.control_in = i; + * g.rc_2.control_in = i; + * update_roll_pitch_mode(); + * Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); + * } + * g.stabilize_d_schedule = .5 + * for (i = -4600; i < 4600; i+=10) { + * new_radio_frame = true; + * g.rc_1.control_in = i; + * g.rc_2.control_in = i; + * update_roll_pitch_mode(); + * Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); + * } + * + * g.stabilize_d_schedule = 0 + * for (i = -4600; i < 4600; i+=10) { + * new_radio_frame = true; + * g.rc_1.control_in = i; + * g.rc_2.control_in = i; + * update_roll_pitch_mode(); + * Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d); + * } + * + * }*/ /* -//static int8_t -//test_dcm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - Vector3f _cam_vector; - Vector3f _out_vector; - - G_Dt = .02; - - while(1){ - for(byte i = 0; i <= 50; i++){ - delay(20); - // IMU - // --- - read_AHRS(); - } - - Matrix3f temp = dcm.get_dcm_matrix(); - Matrix3f temp_t = dcm.get_dcm_transposed(); - - Serial.printf_P(PSTR("dcm\n" - "%4.4f \t %4.4f \t %4.4f \n" - "%4.4f \t %4.4f \t %4.4f \n" - "%4.4f \t %4.4f \t %4.4f \n\n"), - temp.a.x, temp.a.y, temp.a.z, - temp.b.x, temp.b.y, temp.b.z, - temp.c.x, temp.c.y, temp.c.z); - - int16_t _pitch = degrees(-asin(temp.c.x)); - int16_t _roll = degrees(atan2(temp.c.y, temp.c.z)); - int16_t _yaw = degrees(atan2(temp.b.x, temp.a.x)); - Serial.printf_P(PSTR( "angles\n" - "%d \t %d \t %d\n\n"), - _pitch, - _roll, - _yaw); - - //_out_vector = _cam_vector * temp; - //Serial.printf_P(PSTR( "cam\n" - // "%d \t %d \t %d\n\n"), - // (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100); - - if(Serial.available() > 0){ - return (0); - } - } -} -*/ + * //static int8_t + * //test_dcm(uint8_t argc, const Menu::arg *argv) + * { + * print_hit_enter(); + * delay(1000); + * Serial.printf_P(PSTR("Gyro | Accel\n")); + * Vector3f _cam_vector; + * Vector3f _out_vector; + * + * G_Dt = .02; + * + * while(1){ + * for(byte i = 0; i <= 50; i++){ + * delay(20); + * // IMU + * // --- + * read_AHRS(); + * } + * + * Matrix3f temp = dcm.get_dcm_matrix(); + * Matrix3f temp_t = dcm.get_dcm_transposed(); + * + * Serial.printf_P(PSTR("dcm\n" + * "%4.4f \t %4.4f \t %4.4f \n" + * "%4.4f \t %4.4f \t %4.4f \n" + * "%4.4f \t %4.4f \t %4.4f \n\n"), + * temp.a.x, temp.a.y, temp.a.z, + * temp.b.x, temp.b.y, temp.b.z, + * temp.c.x, temp.c.y, temp.c.z); + * + * int16_t _pitch = degrees(-asin(temp.c.x)); + * int16_t _roll = degrees(atan2(temp.c.y, temp.c.z)); + * int16_t _yaw = degrees(atan2(temp.b.x, temp.a.x)); + * Serial.printf_P(PSTR( "angles\n" + * "%d \t %d \t %d\n\n"), + * _pitch, + * _roll, + * _yaw); + * + * //_out_vector = _cam_vector * temp; + * //Serial.printf_P(PSTR( "cam\n" + * // "%d \t %d \t %d\n\n"), + * // (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100); + * + * if(Serial.available() > 0){ + * return (0); + * } + * } + * } + */ /* -//static int8_t -//test_dcm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - delay(1000); - - while(1){ - Vector3f accels = dcm.get_accel(); - Serial.print("accels.z:"); - Serial.print(accels.z); - Serial.print("omega.z:"); - Serial.print(omega.z); - delay(100); - - if(Serial.available() > 0){ - return (0); - } - } -} -*/ + * //static int8_t + * //test_dcm(uint8_t argc, const Menu::arg *argv) + * { + * print_hit_enter(); + * delay(1000); + * Serial.printf_P(PSTR("Gyro | Accel\n")); + * delay(1000); + * + * while(1){ + * Vector3f accels = dcm.get_accel(); + * Serial.print("accels.z:"); + * Serial.print(accels.z); + * Serial.print("omega.z:"); + * Serial.print(omega.z); + * delay(100); + * + * if(Serial.available() > 0){ + * return (0); + * } + * } + * } + */ /*static int8_t -//test_omega(uint8_t argc, const Menu::arg *argv) -{ - static byte ts_num; - float old_yaw; - - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Omega")); - delay(1000); - - G_Dt = .02; - - while(1){ - delay(20); - // IMU - // --- - read_AHRS(); - - float my_oz = (dcm.yaw - old_yaw) * 50; - - old_yaw = dcm.yaw; - - ts_num++; - if (ts_num > 2){ - ts_num = 0; - //Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz); - Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz); - } - - if(Serial.available() > 0){ - return (0); - } - } - return (0); -} -//*/ + * //test_omega(uint8_t argc, const Menu::arg *argv) + * { + * static byte ts_num; + * float old_yaw; + * + * print_hit_enter(); + * delay(1000); + * Serial.printf_P(PSTR("Omega")); + * delay(1000); + * + * G_Dt = .02; + * + * while(1){ + * delay(20); + * // IMU + * // --- + * read_AHRS(); + * + * float my_oz = (dcm.yaw - old_yaw) * 50; + * + * old_yaw = dcm.yaw; + * + * ts_num++; + * if (ts_num > 2){ + * ts_num = 0; + * //Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz); + * Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz); + * } + * + * if(Serial.available() > 0){ + * return (0); + * } + * } + * return (0); + * } + * //*/ static int8_t test_tuning(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); + print_hit_enter(); - while(1){ - delay(200); - read_radio(); - tuning(); - Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value); + while(1) { + delay(200); + read_radio(); + tuning(); + Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value); - if(Serial.available() > 0){ - return (0); - } - } + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_battery(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // disable this test if we are using 1280 - print_test_disabled(); - return (0); - #else - Serial.printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n")); - Serial.flush(); - while(!Serial.available()){ - delay(100); - } - Serial.flush(); - print_hit_enter(); +#if defined( __AVR_ATmega1280__ ) // disable this test if we are using 1280 + print_test_disabled(); + return (0); +#else + Serial.printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n")); + Serial.flush(); + while(!Serial.available()) { + delay(100); + } + Serial.flush(); + print_hit_enter(); - // allow motors to spin - motors.enable(); - motors.armed(true); + // allow motors to spin + motors.enable(); + motors.armed(true); - while(1){ - delay(100); - read_radio(); - read_battery(); - if (g.battery_monitoring == 3){ - Serial.printf_P(PSTR("V: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); - } else { - Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); - } - motors.throttle_pass_through(); + while(1) { + delay(100); + read_radio(); + read_battery(); + if (g.battery_monitoring == 3) { + Serial.printf_P(PSTR("V: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } else { + Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } + motors.throttle_pass_through(); - if(Serial.available() > 0){ - motors.armed(false); - return (0); - } - } - motors.armed(false); - return (0); - #endif + if(Serial.available() > 0) { + motors.armed(false); + return (0); + } + } + motors.armed(false); + return (0); +#endif } static int8_t test_relay(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - print_test_disabled(); - return (0); - #else +#if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 + print_test_disabled(); + return (0); +#else - print_hit_enter(); - delay(1000); + print_hit_enter(); + delay(1000); - while(1){ - Serial.printf_P(PSTR("Relay on\n")); - relay.on(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } + while(1) { + Serial.printf_P(PSTR("Relay on\n")); + relay.on(); + delay(3000); + if(Serial.available() > 0) { + return (0); + } - Serial.printf_P(PSTR("Relay off\n")); - relay.off(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } - } - #endif + Serial.printf_P(PSTR("Relay off\n")); + relay.off(); + delay(3000); + if(Serial.available() > 0) { + return (0); + } + } +#endif } static int8_t test_wp(uint8_t argc, const Menu::arg *argv) { - delay(1000); + delay(1000); - // save the alitude above home option - Serial.printf_P(PSTR("Hold alt ")); - if(g.RTL_altitude < 0){ - Serial.printf_P(PSTR("\n")); - }else{ - Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100); - } + // save the alitude above home option + Serial.printf_P(PSTR("Hold alt ")); + if(g.RTL_altitude < 0) { + Serial.printf_P(PSTR("\n")); + }else{ + Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100); + } - Serial.printf_P(PSTR("%d wp\n"), (int)g.command_total); - Serial.printf_P(PSTR("Hit rad: %dm\n"), (int)g.waypoint_radius); - //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); + Serial.printf_P(PSTR("%d wp\n"), (int)g.command_total); + Serial.printf_P(PSTR("Hit rad: %dm\n"), (int)g.waypoint_radius); + //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - report_wp(); + report_wp(); - return (0); + return (0); } //static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) { - /* - print_hit_enter(); - delay(1000); - while(1){ - if (Serial3.available()){ - digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS - Serial1.write(Serial3.read()); - digitalWrite(B_LED_PIN, LED_OFF); - } - if (Serial1.available()){ - digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS - Serial3.write(Serial1.read()); - digitalWrite(C_LED_PIN, LED_OFF); - } - if(Serial.available() > 0){ - return (0); - } - } - */ - //} +/* + * print_hit_enter(); + * delay(1000); + * while(1){ + * if (Serial3.available()){ + * digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS + * Serial1.write(Serial3.read()); + * digitalWrite(B_LED_PIN, LED_OFF); + * } + * if (Serial1.available()){ + * digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS + * Serial3.write(Serial1.read()); + * digitalWrite(C_LED_PIN, LED_OFF); + * } + * if(Serial.available() > 0){ + * return (0); + * } + * } + */ +//} /*static int8_t -//test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); - - while(1){ - if (Serial3.available()) - Serial3.write(Serial3.read()); - - if(Serial.available() > 0){ - return (0); - } - } -} -*/ + * //test_xbee(uint8_t argc, const Menu::arg *argv) + * { + * print_hit_enter(); + * delay(1000); + * Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); + * + * while(1){ + * if (Serial3.available()) + * Serial3.write(Serial3.read()); + * + * if(Serial.available() > 0){ + * return (0); + * } + * } + * } + */ #if HIL_MODE != HIL_MODE_ATTITUDE static int8_t test_baro(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - print_test_disabled(); - return (0); - #else - print_hit_enter(); - init_barometer(); + #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 + print_test_disabled(); + return (0); + #else + print_hit_enter(); + init_barometer(); - while(1){ - delay(100); - int32_t alt = read_barometer(); // calls barometer.read() + while(1) { + delay(100); + int32_t alt = read_barometer(); // calls barometer.read() - int32_t pres = barometer.get_pressure(); - int16_t temp = barometer.get_temperature(); - int32_t raw_pres = barometer.get_raw_pressure(); - int32_t raw_temp = barometer.get_raw_temp(); - Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC," - " raw pres: %ld, raw temp: %ld\n"), - alt, pres ,temp, raw_pres, raw_temp); - if(Serial.available() > 0){ - return (0); - } - } - return 0; - #endif + int32_t pres = barometer.get_pressure(); + int16_t temp = barometer.get_temperature(); + int32_t raw_pres = barometer.get_raw_pressure(); + int32_t raw_temp = barometer.get_raw_temp(); + Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC," + " raw pres: %ld, raw temp: %ld\n"), + alt, pres,temp, raw_pres, raw_temp); + if(Serial.available() > 0) { + return (0); + } + } + return 0; + #endif } #endif @@ -1094,107 +1094,107 @@ test_baro(uint8_t argc, const Menu::arg *argv) static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - print_test_disabled(); - return (0); - #else - if(g.compass_enabled) { - print_hit_enter(); +#if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 + print_test_disabled(); + return (0); +#else + if(g.compass_enabled) { + print_hit_enter(); - while(1){ - delay(100); - if (compass.read()) { - float heading = compass.calculate_heading(ahrs.get_dcm_matrix()); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), - (wrap_360(ToDeg(heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z); - } else { - Serial.println_P(PSTR("not healthy")); - } + while(1) { + delay(100); + if (compass.read()) { + float heading = compass.calculate_heading(ahrs.get_dcm_matrix()); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), + (wrap_360(ToDeg(heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z); + } else { + Serial.println_P(PSTR("not healthy")); + } - if(Serial.available() > 0){ - return (0); - } - } - } else { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); - } - return (0); - #endif + if(Serial.available() > 0) { + return (0); + } + } + } else { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } + return (0); +#endif } /* -//static int8_t -//test_reverse(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - g.rc_4.set_reverse(0); - g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_4.servo_out = g.rc_4.control_in; - g.rc_4.calc_pwm(); - Serial.printf_P(PSTR("PWM:%d input: %d\toutput%d "), - APM_RC.InputCh(CH_4), - g.rc_4.control_in, - g.rc_4.radio_out); - APM_RC.OutputCh(CH_6, g.rc_4.radio_out); - - - g.rc_4.set_reverse(1); - g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_4.servo_out = g.rc_4.control_in; - g.rc_4.calc_pwm(); - Serial.printf_P(PSTR("\trev input: %d\toutput%d\n"), - g.rc_4.control_in, - g.rc_4.radio_out); - - APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - - if(Serial.available() > 0){ - g.rc_4.set_reverse(0); - return (0); - } - } -}*/ + * //static int8_t + * //test_reverse(uint8_t argc, const Menu::arg *argv) + * { + * print_hit_enter(); + * delay(1000); + * + * while(1){ + * delay(20); + * + * // Filters radio input - adjust filters in the radio.pde file + * // ---------------------------------------------------------- + * g.rc_4.set_reverse(0); + * g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); + * g.rc_4.servo_out = g.rc_4.control_in; + * g.rc_4.calc_pwm(); + * Serial.printf_P(PSTR("PWM:%d input: %d\toutput%d "), + * APM_RC.InputCh(CH_4), + * g.rc_4.control_in, + * g.rc_4.radio_out); + * APM_RC.OutputCh(CH_6, g.rc_4.radio_out); + * + * + * g.rc_4.set_reverse(1); + * g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); + * g.rc_4.servo_out = g.rc_4.control_in; + * g.rc_4.calc_pwm(); + * Serial.printf_P(PSTR("\trev input: %d\toutput%d\n"), + * g.rc_4.control_in, + * g.rc_4.radio_out); + * + * APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + * + * if(Serial.available() > 0){ + * g.rc_4.set_reverse(0); + * return (0); + * } + * } + * }*/ #if HIL_MODE != HIL_MODE_ATTITUDE /* - test the sonar + * test the sonar */ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv) { - if(g.sonar_enabled == false){ - Serial.printf_P(PSTR("Sonar disabled\n")); - return (0); - } + if(g.sonar_enabled == false) { + Serial.printf_P(PSTR("Sonar disabled\n")); + return (0); + } - // make sure sonar is initialised - init_sonar(); + // make sure sonar is initialised + init_sonar(); - print_hit_enter(); - while(1) { - delay(100); + print_hit_enter(); + while(1) { + delay(100); - Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read()); - //Serial.printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value); + Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read()); + //Serial.printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value); - if(Serial.available() > 0){ - return (0); - } - } + if(Serial.available() > 0) { + return (0); + } + } - return (0); + return (0); } #endif @@ -1202,186 +1202,186 @@ test_sonar(uint8_t argc, const Menu::arg *argv) static int8_t test_optflow(uint8_t argc, const Menu::arg *argv) { - #ifdef OPTFLOW_ENABLED - if(g.optflow_enabled) { - Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); - print_hit_enter(); +#ifdef OPTFLOW_ENABLED + if(g.optflow_enabled) { + Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); + print_hit_enter(); - while(1){ - delay(200); - optflow.update(); - Log_Write_Optflow(); - Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"), - optflow.x, - optflow.dx, - optflow.y, - optflow.dy, - optflow.surface_quality); + while(1) { + delay(200); + optflow.update(); + Log_Write_Optflow(); + Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"), + optflow.x, + optflow.dx, + optflow.y, + optflow.dy, + optflow.surface_quality); - if(Serial.available() > 0){ - return (0); - } - } - } else { - Serial.printf_P(PSTR("OptFlow: ")); - print_enabled(false); - } - return (0); + if(Serial.available() > 0) { + return (0); + } + } + } else { + Serial.printf_P(PSTR("OptFlow: ")); + print_enabled(false); + } + return (0); - #else - print_test_disabled(); - return (0); - #endif +#else + print_test_disabled(); + return (0); +#endif } static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv) { - current_loc.lat = 389539260; - current_loc.lng = -1199540200; + current_loc.lat = 389539260; + current_loc.lng = -1199540200; - next_WP.lat = 389538528; - next_WP.lng = -1199541248; + next_WP.lat = 389538528; + next_WP.lng = -1199541248; - // got 23506;, should be 22800 - navigate(); - Serial.printf("bear: %ld\n", target_bearing); - return 0; + // got 23506;, should be 22800 + navigate(); + Serial.printf("bear: %ld\n", target_bearing); + return 0; } /* - test the dataflash is working + * test the dataflash is working */ static int8_t test_logging(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - print_test_disabled(); - return (0); - #else - Serial.println_P(PSTR("Testing dataflash logging")); - if (!DataFlash.CardInserted()) { - Serial.println_P(PSTR("ERR: No dataflash inserted")); - return 0; - } - DataFlash.ReadManufacturerID(); - Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), - (unsigned)DataFlash.df_manufacturer, - (unsigned)DataFlash.df_device); - Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), - (unsigned)DataFlash.df_NumPages+1, - (unsigned)DataFlash.df_PageSize); - DataFlash.StartRead(DataFlash.df_NumPages+1); - Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), - (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); - return 0; - #endif +#if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 + print_test_disabled(); + return (0); +#else + Serial.println_P(PSTR("Testing dataflash logging")); + if (!DataFlash.CardInserted()) { + Serial.println_P(PSTR("ERR: No dataflash inserted")); + return 0; + } + DataFlash.ReadManufacturerID(); + Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), + (unsigned)DataFlash.df_manufacturer, + (unsigned)DataFlash.df_device); + Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), + (unsigned)DataFlash.df_NumPages+1, + (unsigned)DataFlash.df_PageSize); + DataFlash.StartRead(DataFlash.df_NumPages+1); + Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), + (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); + return 0; +#endif } /* -static int8_t -//test_mission(uint8_t argc, const Menu::arg *argv) -{ - //write out a basic mission to the EEPROM - -//{ -// uint8_t id; ///< command id -// uint8_t options; ///< options bitmask (1<<0 = relative altitude) -// uint8_t p1; ///< param 1 -// int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100) -// int32_t lat; ///< param 3 - Lattitude * 10**7 -// int32_t lng; ///< param 4 - Longitude * 10**7 -//} - - // clear home - {Location t = {0, 0, 0, 0, 0, 0}; - set_cmd_with_index(t,0);} - - // CMD opt pitch alt/cm - {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; - set_cmd_with_index(t,1);} - - if (!strcmp_P(argv[1].str, PSTR("wp"))) { - - // CMD opt - {Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0}; - set_cmd_with_index(t,2);} - // CMD opt - {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; - set_cmd_with_index(t,3);} - - // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; - set_cmd_with_index(t,4);} - - } else { - //2250 = 25 meteres - // CMD opt p1 //alt //NS //WE - {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 - set_cmd_with_index(t,2);} - - // CMD opt dir angle/deg deg/s relative - {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; - set_cmd_with_index(t,3);} - - // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; - set_cmd_with_index(t,4);} - - } - - g.RTL_altitude.set_and_save(300); - g.command_total.set_and_save(4); - g.waypoint_radius.set_and_save(3); - - test_wp(NULL, NULL); - return (0); -} -*/ + * static int8_t + * //test_mission(uint8_t argc, const Menu::arg *argv) + * { + * //write out a basic mission to the EEPROM + * + * //{ + * // uint8_t id; ///< command id + * // uint8_t options; ///< options bitmask (1<<0 = relative altitude) + * // uint8_t p1; ///< param 1 + * // int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100) + * // int32_t lat; ///< param 3 - Lattitude * 10**7 + * // int32_t lng; ///< param 4 - Longitude * 10**7 + * //} + * + * // clear home + * {Location t = {0, 0, 0, 0, 0, 0}; + * set_cmd_with_index(t,0);} + * + * // CMD opt pitch alt/cm + * {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; + * set_cmd_with_index(t,1);} + * + * if (!strcmp_P(argv[1].str, PSTR("wp"))) { + * + * // CMD opt + * {Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0}; + * set_cmd_with_index(t,2);} + * // CMD opt + * {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; + * set_cmd_with_index(t,3);} + * + * // CMD opt + * {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + * set_cmd_with_index(t,4);} + * + * } else { + * //2250 = 25 meteres + * // CMD opt p1 //alt //NS //WE + * {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 + * set_cmd_with_index(t,2);} + * + * // CMD opt dir angle/deg deg/s relative + * {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; + * set_cmd_with_index(t,3);} + * + * // CMD opt + * {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + * set_cmd_with_index(t,4);} + * + * } + * + * g.RTL_altitude.set_and_save(300); + * g.command_total.set_and_save(4); + * g.waypoint_radius.set_and_save(3); + * + * test_wp(NULL, NULL); + * return (0); + * } + */ static void print_hit_enter() { - Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); + Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); } static void print_test_disabled() { - Serial.printf_P(PSTR("Sorry, not 1280 compat.\n")); + Serial.printf_P(PSTR("Sorry, not 1280 compat.\n")); } /* -//static void fake_out_gps() -{ - static float rads; - g_gps->new_data = true; - g_gps->fix = true; - - //int length = g.rc_6.control_in; - rads += .05; - - if (rads > 6.28){ - rads = 0; - } - - g_gps->latitude = 377696000; // Y - g_gps->longitude = -1224319000; // X - g_gps->altitude = 9000; // meters * 100 - - //next_WP.lng = home.lng - length * sin(rads); // X - //next_WP.lat = home.lat + length * cos(rads); // Y -} - -*/ + * //static void fake_out_gps() + * { + * static float rads; + * g_gps->new_data = true; + * g_gps->fix = true; + * + * //int length = g.rc_6.control_in; + * rads += .05; + * + * if (rads > 6.28){ + * rads = 0; + * } + * + * g_gps->latitude = 377696000; // Y + * g_gps->longitude = -1224319000; // X + * g_gps->altitude = 9000; // meters * 100 + * + * //next_WP.lng = home.lng - length * sin(rads); // X + * //next_WP.lat = home.lat + length * cos(rads); // Y + * } + * + */ /* -//static void print_motor_out(){ - Serial.printf("out: R: %d, L: %d F: %d B: %d\n", - (motor_out[CH_1] - g.rc_3.radio_min), - (motor_out[CH_2] - g.rc_3.radio_min), - (motor_out[CH_3] - g.rc_3.radio_min), - (motor_out[CH_4] - g.rc_3.radio_min)); -} -*/ + * //static void print_motor_out(){ + * Serial.printf("out: R: %d, L: %d F: %d B: %d\n", + * (motor_out[CH_1] - g.rc_3.radio_min), + * (motor_out[CH_2] - g.rc_3.radio_min), + * (motor_out[CH_3] - g.rc_3.radio_min), + * (motor_out[CH_4] - g.rc_3.radio_min)); + * } + */ #endif // CLI_ENABLED