diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 3ce1a46590..f38b656f06 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -4,41 +4,30 @@ // 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); #if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS 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_battery(uint8_t argc, const Menu::arg *argv); +static int8_t test_compass(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_gps(uint8_t argc, const Menu::arg *argv); +static int8_t test_ins(uint8_t argc, const Menu::arg *argv); +static int8_t test_logging(uint8_t argc, const Menu::arg *argv); +static int8_t test_motors(uint8_t argc, const Menu::arg *argv); +static int8_t test_nav(uint8_t argc, const Menu::arg *argv); +static int8_t test_optflow(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_relay(uint8_t argc, const Menu::arg *argv); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -static int8_t test_shell(uint8_t argc, const Menu::arg *argv); +static int8_t test_shell(uint8_t argc, const Menu::arg *argv); +#endif +#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS +static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); #endif -static int8_t test_motors(uint8_t argc, const Menu::arg *argv); +//static int8_t test_toy(uint8_t argc, const Menu::arg *argv); +static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); +static int8_t test_wp(uint8_t argc, const Menu::arg *argv); // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -60,29 +49,30 @@ static int8_t test_motors(uint8_t argc, const Menu::arg *argv); // 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}, - {"gps", test_gps}, - {"ins", test_ins}, - {"battery", test_battery}, - {"tune", test_tuning}, - {"relay", test_relay}, - {"wp", test_wp}, -// {"toy", test_toy}, #if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS {"baro", test_baro}, - {"sonar", test_sonar}, #endif - {"compass", test_mag}, - {"optflow", test_optflow}, - //{"xbee", test_xbee}, + {"battery", test_battery}, + {"compass", test_compass}, {"eedump", test_eedump}, + {"gps", test_gps}, + {"ins", test_ins}, {"logging", test_logging}, - {"nav", test_wp_nav}, + {"motors", test_motors}, + {"nav", test_nav}, + {"optflow", test_optflow}, + {"pwm", test_radio_pwm}, + {"radio", test_radio}, + {"relay", test_relay}, #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 {"shell", test_shell}, #endif - {"motors", test_motors}, +#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS + {"sonar", test_sonar}, +#endif +// {"toy", test_toy}, + {"tune", test_tuning}, + {"wp", test_wp}, }; // A Macro to create the Menu @@ -91,181 +81,182 @@ MENU(test_menu, "test", test_menu_commands); static int8_t test_mode(uint8_t argc, const Menu::arg *argv) { - //cliSerial->printf_P(PSTR("Test Mode\n\n")); test_menu.run(); return 0; } +#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS static int8_t -test_eedump(uint8_t argc, const Menu::arg *argv) -{ - - // hexdump the EEPROM - for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i += 16) { - cliSerial->printf_P(PSTR("%04x:"), i); - for (uint16_t j = 0; j < 16; j++) { - int b = hal.storage->read_byte(i+j); - cliSerial->printf_P(PSTR(" %02x"), b); - } - cliSerial->println(); - } - return(0); -} - - -static int8_t -test_radio_pwm(uint8_t argc, const Menu::arg *argv) +test_baro(uint8_t argc, const Menu::arg *argv) { + int32_t alt; print_hit_enter(); - delay(1000); + init_barometer(); while(1) { - delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - // servo Yaw - //APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - - cliSerial->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); + delay(100); + alt = read_barometer(); + if (!barometer.healthy) { + cliSerial->println_P(PSTR("not healthy")); + } else { + cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"), + alt / 100.0, + barometer.get_pressure(), 0.1*barometer.get_temperature()); + } if(cliSerial->available() > 0) { return (0); } } + return 0; } - -/* -//static int8_t -//test_toy(uint8_t argc, const Menu::arg *argv) -{ - - for(altitude_error = 2000; altitude_error > -100; altitude_error--){ - int16_t temp = get_desired_climb_rate(); - cliSerial->printf("%ld, %d\n", altitude_error, temp); - } - return 0; -} -{ wp_distance = 0; - int16_t max_speed = 0; - - for(int16_t i = 0; i < 200; i++){ - int32_t temp = 2 * 100 * (wp_distance - wp_nav.get_waypoint_radius()); - max_speed = sqrtf((float)temp); - max_speed = min(max_speed, wp_nav.get_horizontal_speed()); - cliSerial->printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance); - wp_distance += 100; - } - 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; - * cliSerial->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; - * cliSerial->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; - * cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); - * }*/ +#endif static int8_t -test_radio(uint8_t argc, const Menu::arg *argv) +test_battery(uint8_t argc, const Menu::arg *argv) { + // check if radio is calibration + pre_arm_rc_checks(); + if(!ap.pre_arm_rc_check) { + cliSerial->print_P(PSTR("radio not calibrated, exiting")); + return(0); + } + + cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n")); + while (cliSerial->read() != -1); /* flush */ + while(!cliSerial->available()) { /* wait for input */ + delay(100); + } + while (cliSerial->read() != -1); /* flush */ print_hit_enter(); - delay(1000); + + // allow motors to spin + output_min(); + motors.armed(true); while(1) { - delay(20); + delay(100); read_radio(); + read_battery(); + if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) { + cliSerial->printf_P(PSTR("V: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } else { + cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } + motors.throttle_pass_through(); - - cliSerial->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); - - //cliSerial->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)); - - /*cliSerial->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(cliSerial->available() > 0) { + motors.armed(false); return (0); } } + motors.armed(false); + return (0); } static int8_t -test_ins(uint8_t argc, const Menu::arg *argv) +test_compass(uint8_t argc, const Menu::arg *argv) { - Vector3f gyro, accel; - print_hit_enter(); - cliSerial->printf_P(PSTR("INS\n")); - delay(1000); + uint8_t delta_ms_fast_loop; + + if (!g.compass_enabled) { + cliSerial->printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } + + if (!compass.init()) { + cliSerial->println_P(PSTR("Compass initialisation failed!")); + return 0; + } ahrs.init(); + ahrs.set_fly_forward(true); + ahrs.set_compass(&compass); + report_compass(); + + // we need the AHRS initialised for this test ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, flash_leds); + ahrs.reset(); + int16_t counter = 0; + float heading = 0; - delay(50); + print_hit_enter(); while(1) { - ins.update(); - gyro = ins.get_gyro(); - accel = ins.get_accel(); - - float test = accel.length() / GRAVITY_MSS; + delay(20); + if (millis() - fast_loopTimer > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer; + G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator + fast_loopTimer = millis(); - cliSerial->printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"), - accel.x, accel.y, accel.z, - gyro.x, gyro.y, gyro.z, - test); + // INS + // --- + ahrs.update(); - delay(40); - if(cliSerial->available() > 0) { - return (0); + medium_loopCounter++; + if(medium_loopCounter == 5) { + if (compass.read()) { + // Calculate heading + const Matrix3f &m = ahrs.get_dcm_matrix(); + heading = compass.calculate_heading(m); + compass.null_offsets(); + } + medium_loopCounter = 0; + } + + counter++; + if (counter>20) { + if (compass.healthy) { + Vector3f maggy = compass.get_offsets(); + cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), + (wrap_360_cd(ToDeg(heading) * 100)) /100, + (int)compass.mag_x, + (int)compass.mag_y, + (int)compass.mag_z, + maggy.x, + maggy.y, + maggy.z); + } else { + cliSerial->println_P(PSTR("compass not healthy")); + } + counter=0; + } + } + if (cliSerial->available() > 0) { + break; + } + } + + // save offsets. This allows you to get sane offset values using + // the CLI before you go flying. + cliSerial->println_P(PSTR("saving offsets")); + compass.save_offsets(); + return (0); +} + +static int8_t +test_eedump(uint8_t argc, const Menu::arg *argv) +{ + + // hexdump the EEPROM + for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i += 16) { + cliSerial->printf_P(PSTR("%04x:"), i); + for (uint16_t j = 0; j < 16; j++) { + int b = hal.storage->read_byte(i+j); + cliSerial->printf_P(PSTR(" %02x"), b); } + cliSerial->println(); } + return(0); } static int8_t @@ -303,224 +294,234 @@ test_gps(uint8_t argc, const Menu::arg *argv) } static int8_t -test_tuning(uint8_t argc, const Menu::arg *argv) +test_ins(uint8_t argc, const Menu::arg *argv) { + Vector3f gyro, accel; print_hit_enter(); + cliSerial->printf_P(PSTR("INS\n")); + delay(1000); + + ahrs.init(); + ins.init(AP_InertialSensor::COLD_START, + ins_sample_rate, + flash_leds); + + delay(50); while(1) { - delay(200); - read_radio(); - tuning(); - cliSerial->printf_P(PSTR("tune: %1.3f\n"), tuning_value); + ins.update(); + gyro = ins.get_gyro(); + accel = ins.get_accel(); + + float test = accel.length() / GRAVITY_MSS; + + cliSerial->printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"), + accel.x, accel.y, accel.z, + gyro.x, gyro.y, gyro.z, + test); + delay(40); if(cliSerial->available() > 0) { return (0); } } } +/* + * test the dataflash is working + */ static int8_t -test_battery(uint8_t argc, const Menu::arg *argv) +test_logging(uint8_t argc, const Menu::arg *argv) { - // check if radio is calibration - pre_arm_rc_checks(); - if(!ap.pre_arm_rc_check) { - cliSerial->print_P(PSTR("radio not calibrated, exiting")); - return(0); - } + cliSerial->println_P(PSTR("Testing dataflash logging")); + DataFlash.ShowDeviceInfo(cliSerial); + return 0; +} - cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n")); - while (cliSerial->read() != -1); /* flush */ - while(!cliSerial->available()) { /* wait for input */ - delay(100); - } - while (cliSerial->read() != -1); /* flush */ - print_hit_enter(); +static int8_t +test_motors(uint8_t argc, const Menu::arg *argv) +{ + cliSerial->printf_P(PSTR( + "Connect battery for this test.\n" + "Motors will not spin in channel order (1,2,3,4) but by frame position order.\n" + "Front (& right of centerline) motor first, then in clockwise order around frame.\n" + "http://code.google.com/p/arducopter/wiki/AC2_Props_2 for demo video.\n" + "Remember to disconnect battery after this test.\n" + "Any key to exit.\n")); - // allow motors to spin - output_min(); - motors.armed(true); + // ensure all values have been sent to motors + motors.set_update_rate(g.rc_speed); + motors.set_frame_orientation(g.frame_orientation); + motors.set_min_throttle(g.throttle_min); + motors.set_mid_throttle(g.throttle_mid); + motors.set_max_throttle(g.throttle_max); + + // enable motors + init_rc_out(); while(1) { - delay(100); + delay(20); read_radio(); - read_battery(); - if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) { - cliSerial->printf_P(PSTR("V: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); - } else { - cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); - } - motors.throttle_pass_through(); - + motors.output_test(); if(cliSerial->available() > 0) { - motors.armed(false); - return (0); + g.esc_calibrate.set_and_save(0); + return(0); } } - motors.armed(false); - return (0); } -static int8_t test_relay(uint8_t argc, const Menu::arg *argv) +static int8_t +test_nav(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + current_loc.lat = 389539260; + current_loc.lng = -1199540200; - while(1) { - cliSerial->printf_P(PSTR("Relay on\n")); - relay.on(); - delay(3000); - if(cliSerial->available() > 0) { - return (0); - } + wp_nav.set_destination(pv_latlon_to_vector(389538528,-1199541248,0)); - cliSerial->printf_P(PSTR("Relay off\n")); - relay.off(); - delay(3000); - if(cliSerial->available() > 0) { - return (0); - } - } + // got 23506;, should be 22800 + update_navigation(); + cliSerial->printf_P(PSTR("bear: %ld\n"), wp_bearing); + return 0; } - static int8_t -test_wp(uint8_t argc, const Menu::arg *argv) +test_optflow(uint8_t argc, const Menu::arg *argv) { - delay(1000); - - // save the alitude above home option - cliSerial->printf_P(PSTR("Hold alt ")); - if(g.rtl_altitude < 0) { - cliSerial->printf_P(PSTR("\n")); - }else{ - cliSerial->printf_P(PSTR("of %dm\n"), (int)g.rtl_altitude / 100); - } - - cliSerial->printf_P(PSTR("%d wp\n"), (int)g.command_total); - cliSerial->printf_P(PSTR("Hit rad: %dm\n"), (int)wp_nav.get_waypoint_radius()); +#if OPTFLOW == ENABLED + if(g.optflow_enabled) { + cliSerial->printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); + print_hit_enter(); - report_wp(); + while(1) { + delay(200); + optflow.update(millis()); + Log_Write_Optflow(); + cliSerial->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(cliSerial->available() > 0) { + return (0); + } + } + } else { + cliSerial->printf_P(PSTR("OptFlow: ")); + print_enabled(false); + } return (0); +#else + return (0); +#endif // OPTFLOW == ENABLED } -#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS static int8_t -test_baro(uint8_t argc, const Menu::arg *argv) +test_radio_pwm(uint8_t argc, const Menu::arg *argv) { - int32_t alt; print_hit_enter(); - init_barometer(); + delay(1000); while(1) { - delay(100); - alt = read_barometer(); + delay(20); + + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + // servo Yaw + //APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + + cliSerial->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 (!barometer.healthy) { - cliSerial->println_P(PSTR("not healthy")); - } else { - cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"), - alt / 100.0, - barometer.get_pressure(), 0.1*barometer.get_temperature()); - } if(cliSerial->available() > 0) { return (0); } } - return 0; } -#endif - static int8_t -test_mag(uint8_t argc, const Menu::arg *argv) +test_radio(uint8_t argc, const Menu::arg *argv) { - uint8_t delta_ms_fast_loop; + print_hit_enter(); + delay(1000); - if (!g.compass_enabled) { - cliSerial->printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); - } + while(1) { + delay(20); + read_radio(); - if (!compass.init()) { - cliSerial->println_P(PSTR("Compass initialisation failed!")); - return 0; - } - ahrs.init(); - ahrs.set_fly_forward(true); - ahrs.set_compass(&compass); - report_compass(); + cliSerial->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); - // we need the AHRS initialised for this test - ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); - ahrs.reset(); - int16_t counter = 0; - float heading = 0; + //cliSerial->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)); + + /*cliSerial->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(cliSerial->available() > 0) { + return (0); + } + } +} +static int8_t test_relay(uint8_t argc, const Menu::arg *argv) +{ print_hit_enter(); + delay(1000); while(1) { - delay(20); - if (millis() - fast_loopTimer > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer; - G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator - fast_loopTimer = millis(); - - // INS - // --- - ahrs.update(); - - medium_loopCounter++; - if(medium_loopCounter == 5) { - if (compass.read()) { - // Calculate heading - const Matrix3f &m = ahrs.get_dcm_matrix(); - heading = compass.calculate_heading(m); - compass.null_offsets(); - } - medium_loopCounter = 0; - } - - counter++; - if (counter>20) { - if (compass.healthy) { - Vector3f maggy = compass.get_offsets(); - cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), - (wrap_360_cd(ToDeg(heading) * 100)) /100, - (int)compass.mag_x, - (int)compass.mag_y, - (int)compass.mag_z, - maggy.x, - maggy.y, - maggy.z); - } else { - cliSerial->println_P(PSTR("compass not healthy")); - } - counter=0; - } + cliSerial->printf_P(PSTR("Relay on\n")); + relay.on(); + delay(3000); + if(cliSerial->available() > 0) { + return (0); } - if (cliSerial->available() > 0) { - break; + + cliSerial->printf_P(PSTR("Relay off\n")); + relay.off(); + delay(3000); + if(cliSerial->available() > 0) { + return (0); } } +} - // save offsets. This allows you to get sane offset values using - // the CLI before you go flying. - cliSerial->println_P(PSTR("saving offsets")); - compass.save_offsets(); - return (0); +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +/* + * run a debug shell + */ +static int8_t +test_shell(uint8_t argc, const Menu::arg *argv) +{ + hal.util->run_debug_shell(cliSerial); + return 0; } +#endif #if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS /* @@ -553,109 +554,91 @@ test_sonar(uint8_t argc, const Menu::arg *argv) } #endif - -static int8_t -test_optflow(uint8_t argc, const Menu::arg *argv) +/* +//static int8_t +//test_toy(uint8_t argc, const Menu::arg *argv) { -#if OPTFLOW == ENABLED - if(g.optflow_enabled) { - cliSerial->printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); - print_hit_enter(); - - while(1) { - delay(200); - optflow.update(millis()); - Log_Write_Optflow(); - cliSerial->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(cliSerial->available() > 0) { - return (0); - } - } - } else { - cliSerial->printf_P(PSTR("OptFlow: ")); - print_enabled(false); - } - return (0); -#else - return (0); -#endif // OPTFLOW == ENABLED + for(altitude_error = 2000; altitude_error > -100; altitude_error--){ + int16_t temp = get_desired_climb_rate(); + cliSerial->printf("%ld, %d\n", altitude_error, temp); + } + return 0; } +{ wp_distance = 0; + int16_t max_speed = 0; + + for(int16_t i = 0; i < 200; i++){ + int32_t temp = 2 * 100 * (wp_distance - wp_nav.get_waypoint_radius()); + max_speed = sqrtf((float)temp); + max_speed = min(max_speed, wp_nav.get_horizontal_speed()); + cliSerial->printf("Zspeed: %ld, %d, %ld\n", temp, max_speed, wp_distance); + wp_distance += 100; + } + 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; + * cliSerial->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; + * cliSerial->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; + * cliSerial->printf("yaw_rate, %d, roll_rate, %d\n", yaw_rate, roll_rate); + * }*/ static int8_t -test_wp_nav(uint8_t argc, const Menu::arg *argv) +test_tuning(uint8_t argc, const Menu::arg *argv) { - current_loc.lat = 389539260; - current_loc.lng = -1199540200; - - wp_nav.set_destination(pv_latlon_to_vector(389538528,-1199541248,0)); - - // got 23506;, should be 22800 - update_navigation(); - cliSerial->printf_P(PSTR("bear: %ld\n"), wp_bearing); - return 0; -} + print_hit_enter(); -/* - * test the dataflash is working - */ + while(1) { + delay(200); + read_radio(); + tuning(); + cliSerial->printf_P(PSTR("tune: %1.3f\n"), tuning_value); -static int8_t -test_logging(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->println_P(PSTR("Testing dataflash logging")); - DataFlash.ShowDeviceInfo(cliSerial); - return 0; + if(cliSerial->available() > 0) { + return (0); + } + } } -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -/* - * run a debug shell - */ static int8_t -test_shell(uint8_t argc, const Menu::arg *argv) +test_wp(uint8_t argc, const Menu::arg *argv) { - hal.util->run_debug_shell(cliSerial); - return 0; -} -#endif + delay(1000); -static int8_t -test_motors(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf_P(PSTR( - "Connect battery for this test.\n" - "Motors will not spin in channel order (1,2,3,4) but by frame position order.\n" - "Front (& right of centerline) motor first, then in clockwise order around frame.\n" - "http://code.google.com/p/arducopter/wiki/AC2_Props_2 for demo video.\n" - "Remember to disconnect battery after this test.\n" - "Any key to exit.\n")); + // save the alitude above home option + cliSerial->printf_P(PSTR("Hold alt ")); + if(g.rtl_altitude < 0) { + cliSerial->printf_P(PSTR("\n")); + }else{ + cliSerial->printf_P(PSTR("of %dm\n"), (int)g.rtl_altitude / 100); + } - // ensure all values have been sent to motors - motors.set_update_rate(g.rc_speed); - motors.set_frame_orientation(g.frame_orientation); - motors.set_min_throttle(g.throttle_min); - motors.set_mid_throttle(g.throttle_mid); - motors.set_max_throttle(g.throttle_max); + cliSerial->printf_P(PSTR("%d wp\n"), (int)g.command_total); + cliSerial->printf_P(PSTR("Hit rad: %dm\n"), (int)wp_nav.get_waypoint_radius()); - // enable motors - init_rc_out(); + report_wp(); - while(1) { - delay(20); - read_radio(); - motors.output_test(); - if(cliSerial->available() > 0) { - g.esc_calibrate.set_and_save(0); - return(0); - } - } + return (0); } static void print_hit_enter()