|
|
|
|
@ -66,7 +66,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
@@ -66,7 +66,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
|
|
|
|
|
{"gps", test_gps}, |
|
|
|
|
// {"adc", test_adc}, |
|
|
|
|
{"ins", test_ins}, |
|
|
|
|
// {"imu", test_imu}, |
|
|
|
|
// {"dcm", test_dcm_eulers}, |
|
|
|
|
//{"omega", test_omega}, |
|
|
|
|
// {"stab_d", test_stab_d}, |
|
|
|
|
@ -440,69 +439,6 @@ test_radio(uint8_t argc, const Menu::arg *argv)
@@ -440,69 +439,6 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
|
|
|
|
* #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_ins(uint8_t argc, const Menu::arg *argv) |
|
|
|
|
{ |
|
|
|
|
@ -510,37 +446,28 @@ test_ins(uint8_t argc, const Menu::arg *argv)
@@ -510,37 +446,28 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|
|
|
|
print_test_disabled(); |
|
|
|
|
return (0); |
|
|
|
|
#else |
|
|
|
|
float gyro[3], accel[3], temp; |
|
|
|
|
Vector3f gyro, accel; |
|
|
|
|
float temp; |
|
|
|
|
print_hit_enter(); |
|
|
|
|
Serial.printf_P(PSTR("InertialSensor\n")); |
|
|
|
|
Serial.printf_P(PSTR("INS\n")); |
|
|
|
|
delay(1000); |
|
|
|
|
|
|
|
|
|
ins.init(&timer_scheduler); |
|
|
|
|
ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler); |
|
|
|
|
|
|
|
|
|
delay(50); |
|
|
|
|
|
|
|
|
|
while(1) { |
|
|
|
|
ins.update(); |
|
|
|
|
ins.get_gyros(gyro); |
|
|
|
|
ins.get_accels(accel); |
|
|
|
|
gyro = ins.get_gyro(); |
|
|
|
|
accel = ins.get_accel(); |
|
|
|
|
temp = ins.temperature(); |
|
|
|
|
|
|
|
|
|
float test = sqrt(sq(accel[0]) + sq(accel[1]) + sq(accel[2])) / 9.80665; |
|
|
|
|
float test = sqrt(sq(accel.x) + sq(accel.y) + sq(accel.z)) / 9.80665; |
|
|
|
|
|
|
|
|
|
Serial.printf_P(PSTR("g")); |
|
|
|
|
|
|
|
|
|
for (int16_t i = 0; i < 3; i++) { |
|
|
|
|
Serial.printf_P(PSTR(" %7.4f"), gyro[i]); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
Serial.printf_P(PSTR(" a")); |
|
|
|
|
|
|
|
|
|
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("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, |
|
|
|
|
temp, test); |
|
|
|
|
|
|
|
|
|
delay(40); |
|
|
|
|
if(Serial.available() > 0) { |
|
|
|
|
@ -550,190 +477,6 @@ test_ins(uint8_t argc, const Menu::arg *argv)
@@ -550,190 +477,6 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
* 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) |
|
|
|
|
* { |
|
|
|
|
* 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 |
|
|
|
|
*/ |
|
|
|
|
/*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); |
|
|
|
|
* }*/ |
|
|
|
|
|
|
|
|
|
static int8_t |
|
|
|
|
test_gps(uint8_t argc, const Menu::arg *argv) |
|
|
|
|
{ |
|
|
|
|
|
rmackay9
12 years ago