From e73834d6ebba7e98fabc03577ff8c2c99ea8c80f Mon Sep 17 00:00:00 2001 From: uncrustify Date: Tue, 21 Aug 2012 19:19:51 -0700 Subject: [PATCH] uncrustify ArduPlane/test.pde --- ArduPlane/test.pde | 756 ++++++++++++++++++++++----------------------- 1 file changed, 378 insertions(+), 378 deletions(-) diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index bcd30528aa..1a2b66275e 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -4,63 +4,63 @@ // 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_passthru(uint8_t argc, const Menu::arg *argv); -static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); -static int8_t test_gps(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_passthru(uint8_t argc, const Menu::arg *argv); +static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); +static int8_t test_gps(uint8_t argc, const Menu::arg *argv); #if CONFIG_ADC == ENABLED -static int8_t test_adc(uint8_t argc, const Menu::arg *argv); +static int8_t test_adc(uint8_t argc, const Menu::arg *argv); #endif -static int8_t test_imu(uint8_t argc, const Menu::arg *argv); -static int8_t test_battery(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_airspeed(uint8_t argc, const Menu::arg *argv); -static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); -static int8_t test_mag(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_modeswitch(uint8_t argc, const Menu::arg *argv); -static int8_t test_logging(uint8_t argc, const Menu::arg *argv); +static int8_t test_imu(uint8_t argc, const Menu::arg *argv); +static int8_t test_battery(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_airspeed(uint8_t argc, const Menu::arg *argv); +static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); +static int8_t test_mag(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_modeswitch(uint8_t argc, const Menu::arg *argv); +static int8_t test_logging(uint8_t argc, const Menu::arg *argv); // 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_Common for implementation details static const struct Menu::command test_menu_commands[] PROGMEM = { - {"pwm", test_radio_pwm}, - {"radio", test_radio}, - {"passthru", test_passthru}, - {"failsafe", test_failsafe}, - {"battery", test_battery}, - {"relay", test_relay}, - {"waypoints", test_wp}, - {"xbee", test_xbee}, - {"eedump", test_eedump}, - {"modeswitch", test_modeswitch}, + {"pwm", test_radio_pwm}, + {"radio", test_radio}, + {"passthru", test_passthru}, + {"failsafe", test_failsafe}, + {"battery", test_battery}, + {"relay", test_relay}, + {"waypoints", test_wp}, + {"xbee", test_xbee}, + {"eedump", test_eedump}, + {"modeswitch", test_modeswitch}, - // Tests below here are for hardware sensors only present - // when real sensors are attached or they are emulated + // Tests below here are for hardware sensors only present + // when real sensors are attached or they are emulated #if HIL_MODE == HIL_MODE_DISABLED -#if CONFIG_ADC == ENABLED - {"adc", test_adc}, -#endif - {"gps", test_gps}, - {"rawgps", test_rawgps}, - {"imu", test_imu}, - {"airspeed", test_airspeed}, - {"airpressure", test_pressure}, - {"compass", test_mag}, + #if CONFIG_ADC == ENABLED + {"adc", test_adc}, + #endif + {"gps", test_gps}, + {"rawgps", test_rawgps}, + {"imu", test_imu}, + {"airspeed", test_airspeed}, + {"airpressure", test_pressure}, + {"compass", test_mag}, #elif HIL_MODE == HIL_MODE_SENSORS - {"adc", test_adc}, - {"gps", test_gps}, - {"imu", test_imu}, - {"compass", test_mag}, + {"adc", test_adc}, + {"gps", test_gps}, + {"imu", test_imu}, + {"compass", test_mag}, #elif HIL_MODE == HIL_MODE_ATTITUDE #endif - {"logging", test_logging}, + {"logging", test_logging}, }; @@ -70,81 +70,81 @@ 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(); + Serial.printf_P(PSTR("Test Mode\n\n")); + test_menu.run(); 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 int8_t test_eedump(uint8_t argc, const Menu::arg *argv) { - intptr_t i, j; + intptr_t 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) { - print_hit_enter(); - delay(1000); + 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(); - Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - (int)g.channel_roll.radio_in, - (int)g.channel_pitch.radio_in, - (int)g.channel_throttle.radio_in, - (int)g.channel_rudder.radio_in, - (int)g.rc_5.radio_in, - (int)g.rc_6.radio_in, - (int)g.rc_7.radio_in, - (int)g.rc_8.radio_in); + Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + (int)g.channel_roll.radio_in, + (int)g.channel_pitch.radio_in, + (int)g.channel_throttle.radio_in, + (int)g.channel_rudder.radio_in, + (int)g.rc_5.radio_in, + (int)g.rc_6.radio_in, + (int)g.rc_7.radio_in, + (int)g.rc_8.radio_in); - if(Serial.available() > 0){ - return (0); - } - } + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_passthru(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + print_hit_enter(); + delay(1000); - while(1){ - delay(20); + while(1) { + delay(20); // New radio frame? (we could use also if((millis()- timer) > 20) - if (APM_RC.GetState() == 1){ + if (APM_RC.GetState() == 1) { Serial.print("CH:"); - for(int16_t i = 0; i < 8; i++){ - Serial.print(APM_RC.InputCh(i)); // Print channel values + for(int16_t i = 0; i < 8; i++) { + Serial.print(APM_RC.InputCh(i)); // Print channel values Serial.print(","); APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos } Serial.println(); } - if (Serial.available() > 0){ + if (Serial.available() > 0) { return (0); } } @@ -154,242 +154,242 @@ test_passthru(uint8_t argc, const Menu::arg *argv) static int8_t test_radio(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + print_hit_enter(); + delay(1000); - // read the radio to set trims - // --------------------------- - trim_radio(); + // read the radio to set trims + // --------------------------- + trim_radio(); - while(1){ - delay(20); - read_radio(); + while(1) { + delay(20); + read_radio(); - g.channel_roll.calc_pwm(); - g.channel_pitch.calc_pwm(); - g.channel_throttle.calc_pwm(); - g.channel_rudder.calc_pwm(); + g.channel_roll.calc_pwm(); + g.channel_pitch.calc_pwm(); + g.channel_throttle.calc_pwm(); + g.channel_rudder.calc_pwm(); - // write out the servo PWM values - // ------------------------------ - set_servos(); + // write out the servo PWM values + // ------------------------------ + set_servos(); - Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - (int)g.channel_roll.control_in, - (int)g.channel_pitch.control_in, - (int)g.channel_throttle.control_in, - (int)g.channel_rudder.control_in, - (int)g.rc_5.control_in, - (int)g.rc_6.control_in, - (int)g.rc_7.control_in, - (int)g.rc_8.control_in); + Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + (int)g.channel_roll.control_in, + (int)g.channel_pitch.control_in, + (int)g.channel_throttle.control_in, + (int)g.channel_rudder.control_in, + (int)g.rc_5.control_in, + (int)g.rc_6.control_in, + (int)g.rc_7.control_in, + (int)g.rc_8.control_in); - if(Serial.available() > 0){ - return (0); - } - } + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv) { - byte fail_test; - print_hit_enter(); - for(int16_t i = 0; i < 50; i++){ - delay(20); - read_radio(); - } + byte fail_test; + print_hit_enter(); + for(int16_t i = 0; i < 50; i++) { + delay(20); + read_radio(); + } - // read the radio to set trims - // --------------------------- - trim_radio(); + // read the radio to set trims + // --------------------------- + trim_radio(); - oldSwitchPosition = readSwitch(); + oldSwitchPosition = readSwitch(); - Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); - while(g.channel_throttle.control_in > 0){ - delay(20); - read_radio(); - } + Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); + while(g.channel_throttle.control_in > 0) { + delay(20); + read_radio(); + } - while(1){ - delay(20); - read_radio(); + while(1) { + delay(20); + read_radio(); - if(g.channel_throttle.control_in > 0){ - Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)g.channel_throttle.control_in); - fail_test++; - } + if(g.channel_throttle.control_in > 0) { + Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)g.channel_throttle.control_in); + fail_test++; + } - if(oldSwitchPosition != readSwitch()){ - Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); - Serial.println(flight_mode_strings[readSwitch()]); - 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.channel_throttle.get_failsafe()){ - Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.radio_in); - Serial.println(flight_mode_strings[readSwitch()]); - fail_test++; - } + if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()) { + Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.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 APM.\n")); - return (0); - } - } + if(fail_test > 0) { + return (0); + } + if(Serial.available() > 0) { + Serial.printf_P(PSTR("LOS caused no change in APM.\n")); + return (0); + } + } } static int8_t test_battery(uint8_t argc, const Menu::arg *argv) { -if (g.battery_monitoring == 3 || g.battery_monitoring == 4) { - print_hit_enter(); - delta_ms_medium_loop = 100; + if (g.battery_monitoring == 3 || g.battery_monitoring == 4) { + print_hit_enter(); + delta_ms_medium_loop = 100; - 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, mAh: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); - } + 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, mAh: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } - // write out the servo PWM values - // ------------------------------ - set_servos(); + // write out the servo PWM values + // ------------------------------ + set_servos(); - if(Serial.available() > 0){ - return (0); - } - } -} else { - Serial.printf_P(PSTR("Not enabled\n")); - return (0); -} + if(Serial.available() > 0) { + return (0); + } + } + } else { + Serial.printf_P(PSTR("Not enabled\n")); + return (0); + } } static int8_t test_relay(uint8_t argc, const Menu::arg *argv) { - 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); - } - } + Serial.printf_P(PSTR("Relay off\n")); + relay.off(); + delay(3000); + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_wp(uint8_t argc, const Menu::arg *argv) { - delay(1000); + delay(1000); - // save the alitude above home option - if (g.RTL_altitude_cm < 0){ - Serial.printf_P(PSTR("Hold current altitude\n")); - }else{ - Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100); - } + // save the alitude above home option + if (g.RTL_altitude_cm < 0) { + Serial.printf_P(PSTR("Hold current altitude\n")); + }else{ + Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100); + } - Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); - Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); - Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); + Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); + Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); + Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - for(byte i = 0; i <= g.command_total; i++){ - struct Location temp = get_cmd_with_index(i); - test_wp_print(&temp, i); - } + for(byte i = 0; i <= g.command_total; i++) { + struct Location temp = get_cmd_with_index(i); + test_wp_print(&temp, i); + } - return (0); + return (0); } static void test_wp_print(struct Location *cmd, byte wp_index) { - Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), - (int)wp_index, - (int)cmd->id, - (int)cmd->options, - (int)cmd->p1, - (long)cmd->alt, - (long)cmd->lat, - (long)cmd->lng); + Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), + (int)wp_index, + (int)cmd->id, + (int)cmd->options, + (int)cmd->p1, + (long)cmd->alt, + (long)cmd->lat, + (long)cmd->lng); } 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")); + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); - while(1){ + while(1) { - if (Serial3.available()) - Serial3.write(Serial3.read()); + if (Serial3.available()) + Serial3.write(Serial3.read()); - if(Serial.available() > 0){ - return (0); - } - } + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + print_hit_enter(); + delay(1000); - Serial.printf_P(PSTR("Control CH ")); + Serial.printf_P(PSTR("Control CH ")); - Serial.println(FLIGHT_MODE_CHANNEL, DEC); + Serial.println(FLIGHT_MODE_CHANNEL, DEC); - while(1){ - delay(20); - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - Serial.printf_P(PSTR("Position %d\n"), (int)switchPosition); - oldSwitchPosition = switchPosition; - } - if(Serial.available() > 0){ - return (0); - } - } + while(1) { + delay(20); + byte switchPosition = readSwitch(); + if (oldSwitchPosition != switchPosition) { + Serial.printf_P(PSTR("Position %d\n"), (int)switchPosition); + oldSwitchPosition = switchPosition; + } + if(Serial.available() > 0) { + return (0); + } + } } /* - test the dataflash is working + * test the dataflash is working */ static int8_t test_logging(uint8_t argc, const Menu::arg *argv) { - Serial.println_P(PSTR("Testing dataflash logging")); + Serial.println_P(PSTR("Testing dataflash logging")); if (!DataFlash.CardInserted()) { Serial.println_P(PSTR("ERR: No dataflash inserted")); return 0; @@ -412,111 +412,111 @@ test_logging(uint8_t argc, const Menu::arg *argv) #if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS -#if CONFIG_ADC == ENABLED + #if CONFIG_ADC == ENABLED static int8_t test_adc(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - adc.Init(&timer_scheduler); - delay(1000); - Serial.printf_P(PSTR("ADC\n")); - delay(1000); + print_hit_enter(); + adc.Init(&timer_scheduler); + delay(1000); + Serial.printf_P(PSTR("ADC\n")); + delay(1000); - while(1){ - for (int16_t i=0;i<9;i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i)); - Serial.println(); - delay(100); - if(Serial.available() > 0){ - return (0); - } - } + while(1) { + for (int16_t i=0; i<9; i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i)); + Serial.println(); + delay(100); + if(Serial.available() > 0) { + return (0); + } + } } -#endif // CONFIG_ADC + #endif // CONFIG_ADC static int8_t test_gps(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + 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: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), - (long)g_gps->latitude, - (long)g_gps->longitude, - (long)g_gps->altitude/100, - (int)g_gps->num_sats); - }else{ - Serial.printf_P(PSTR(".")); - } - if(Serial.available() > 0){ - return (0); - } - } + if (g_gps->new_data) { + Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), + (long)g_gps->latitude, + (long)g_gps->longitude, + (long)g_gps->altitude/100, + (int)g_gps->num_sats); + }else{ + Serial.printf_P(PSTR(".")); + } + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_imu(uint8_t argc, const Menu::arg *argv) { - //Serial.printf_P(PSTR("Calibrating.")); - imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); + //Serial.printf_P(PSTR("Calibrating.")); + imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); ahrs.reset(); - print_hit_enter(); - delay(1000); + print_hit_enter(); + delay(1000); - while(1){ - delay(20); - if (millis() - fast_loopTimer_ms > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer_ms; - G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator - fast_loopTimer_ms = millis(); + while(1) { + delay(20); + if (millis() - fast_loopTimer_ms > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer_ms; + G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator + fast_loopTimer_ms = millis(); - // IMU - // --- - ahrs.update(); + // IMU + // --- + ahrs.update(); - if(g.compass_enabled) { - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); + if(g.compass_enabled) { + medium_loopCounter++; + if(medium_loopCounter == 5) { + compass.read(); medium_loopCounter = 0; - } - } + } + } - // We are using the IMU - // --------------------- - Vector3f gyros = imu.get_gyro(); + // We are using the IMU + // --------------------- + Vector3f gyros = imu.get_gyro(); Vector3f accels = imu.get_accel(); - Serial.printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"), + Serial.printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"), (int)ahrs.roll_sensor / 100, (int)ahrs.pitch_sensor / 100, (uint16_t)ahrs.yaw_sensor / 100, gyros.x, gyros.y, gyros.z, accels.x, accels.y, accels.z); - } - if(Serial.available() > 0){ - return (0); - } - } + } + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { - if (!g.compass_enabled) { + if (!g.compass_enabled) { Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); + print_enabled(false); + return (0); } compass.set_orientation(MAG_ORIENTATION); @@ -528,29 +528,29 @@ test_mag(uint8_t argc, const Menu::arg *argv) report_compass(); // we need the AHRS initialised for this test - imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); + imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); ahrs.reset(); - int16_t counter = 0; + int16_t counter = 0; float heading = 0; - //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); + //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); print_hit_enter(); while(1) { - delay(20); - if (millis() - fast_loopTimer_ms > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer_ms; - G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator - fast_loopTimer_ms = millis(); + delay(20); + if (millis() - fast_loopTimer_ms > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer_ms; + G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator + fast_loopTimer_ms = millis(); - // IMU - // --- - ahrs.update(); + // IMU + // --- + ahrs.update(); medium_loopCounter++; - if(medium_loopCounter == 5){ + if(medium_loopCounter == 5) { if (compass.read()) { // Calculate heading Matrix3f m = ahrs.get_dcm_matrix(); @@ -560,8 +560,8 @@ test_mag(uint8_t argc, const Menu::arg *argv) medium_loopCounter = 0; } - counter++; - if (counter>20) { + counter++; + if (counter>20) { if (compass.healthy) { Vector3f maggy = compass.get_offsets(); Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), @@ -577,14 +577,14 @@ test_mag(uint8_t argc, const Menu::arg *argv) } counter=0; } - } + } if (Serial.available() > 0) { break; } } // save offsets. This allows you to get sane offset values using - // the CLI before you go flying. + // the CLI before you go flying. Serial.println_P(PSTR("saving offsets")); compass.save_offsets(); return (0); @@ -601,46 +601,46 @@ static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv) { float airspeed_ch = pitot_analog_source.read(); - // Serial.println(pitot_analog_source.read()); + // Serial.println(pitot_analog_source.read()); Serial.printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch); - if (!airspeed.enabled()) { - Serial.printf_P(PSTR("airspeed: ")); - print_enabled(false); - return (0); + if (!airspeed.enabled()) { + Serial.printf_P(PSTR("airspeed: ")); + print_enabled(false); + return (0); - }else{ - print_hit_enter(); - zero_airspeed(); - Serial.printf_P(PSTR("airspeed: ")); - print_enabled(true); + }else{ + print_hit_enter(); + zero_airspeed(); + Serial.printf_P(PSTR("airspeed: ")); + print_enabled(true); - while(1){ - delay(20); - read_airspeed(); - Serial.printf_P(PSTR("%.1f m/s\n"), airspeed.get_airspeed()); + while(1) { + delay(20); + read_airspeed(); + Serial.printf_P(PSTR("%.1f m/s\n"), airspeed.get_airspeed()); - if(Serial.available() > 0){ - return (0); - } - } - } + if(Serial.available() > 0) { + return (0); + } + } + } } static int8_t test_pressure(uint8_t argc, const Menu::arg *argv) { - Serial.printf_P(PSTR("Uncalibrated relative airpressure\n")); - print_hit_enter(); + Serial.printf_P(PSTR("Uncalibrated relative airpressure\n")); + print_hit_enter(); - home.alt = 0; - wp_distance = 0; - init_barometer(); + home.alt = 0; + wp_distance = 0; + init_barometer(); - while(1){ - delay(100); - current_loc.alt = read_barometer() + home.alt; + while(1) { + delay(100); + current_loc.alt = read_barometer() + home.alt; if (!barometer.healthy) { Serial.println_P(PSTR("not healthy")); @@ -650,33 +650,33 @@ test_pressure(uint8_t argc, const Menu::arg *argv) barometer.get_pressure(), 0.1*barometer.get_temperature()); } - if(Serial.available() > 0){ - return (0); - } - } + if(Serial.available() > 0) { + return (0); + } + } } static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + 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); - } - } + 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); + } + } } #endif // HIL_MODE == HIL_MODE_DISABLED