Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: move to use new INS library instead of IMU library

This commit is contained in:
rmackay9 2012-11-05 13:32:38 +09:00
parent ef727bbb3c
commit a1b4ec6d0e
14 changed files with 97 additions and 491 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduCopter/ArduCopter.pde
View File

@ -78,7 +78,6 @@
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Curve.h> // Curve used to linearlise throttle pwm to thrust
#include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_PeriodicProcess.h> // Parent header of Timer
// (only included for makefile libpath to work)
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
@ -98,6 +97,7 @@
#include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library
#include <Filter.h> // Filter library
#include <AP_Buffer.h> // APM FIFO Buffer
#include <ModeFilter.h> // Mode Filter from Filter library
#include <AverageFilter.h> // Mode Filter from Filter library
#include <AP_LeadFilter.h> // GPS Lead filter
@ -268,21 +268,20 @@ AP_InertialSensor_MPU6000 ins;
#else
AP_InertialSensor_Oilpan ins(&adc);
#endif
AP_IMU_INS imu(&ins);
// we don't want to use gps for yaw correction on ArduCopter, so pass
// a NULL GPS object pointer
static GPS *g_gps_null;
#if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
AP_AHRS_MPU6000 ahrs(&imu, g_gps, &ins); // only works with APM2
AP_AHRS_MPU6000 ahrs(&ins, g_gps); // only works with APM2
#else
AP_AHRS_DCM ahrs(&imu, g_gps);
AP_AHRS_DCM ahrs(&ins, g_gps);
#endif
// ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM
#if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
AP_AHRS_MPU6000 ahrs2(&imu, g_gps, &ins); // only works with APM2
AP_AHRS_MPU6000 ahrs2(&ins, g_gps); // only works with APM2
#endif
#elif HIL_MODE == HIL_MODE_SENSORS
@ -291,20 +290,19 @@ AP_ADC_HIL adc;
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL);
AP_IMU_Shim imu;
AP_AHRS_DCM ahrs(&imu, g_gps);
AP_InertialSensor_Stub ins;
AP_AHRS_DCM ahrs(&ins, g_gps);
static int32_t gps_base_alt;
#elif HIL_MODE == HIL_MODE_ATTITUDE
AP_ADC_HIL adc;
AP_IMU_Shim imu;
AP_AHRS_HIL ahrs(&imu, g_gps);
AP_InertialSensor_Stub ins;
AP_AHRS_HIL ahrs(&ins, g_gps);
AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used
AP_Baro_BMP085_HIL barometer;
AP_InertialSensor_Stub ins;
#ifdef OPTFLOW_ENABLED
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
AP_OpticalFlow_ADNS3080 optflow(&spi3_semaphore,OPTFLOW_CS_PIN);
@ -919,8 +917,8 @@ static uint32_t last_gps_time;
// Set true if we have new PWM data to act on from the Radio
static bool new_radio_frame;
// Used to auto exit the in-flight leveler
static int16_t auto_level_counter;
// Used to auto exit the roll_pitch_trim saving function
static uint8_t save_trim_counter;
// Reference to the AP relay object - APM1 only
AP_Relay relay;
@ -984,7 +982,7 @@ void loop()
// We want this to execute fast
// ----------------------------
num_samples = imu.num_samples_available();
num_samples = ins.num_samples_available();
if (num_samples >= NUM_IMU_SAMPLES_FOR_100HZ) {
#if DEBUG_FAST_LOOP == ENABLED
@ -1148,8 +1146,8 @@ static void medium_loop()
}
#endif
// auto_trim, uses an auto_level algorithm
auto_trim();
// save_trim - stores roll and pitch radio inputs to ahrs
save_trim();
// record throttle output
// ------------------------------
@ -2067,7 +2065,7 @@ static void read_AHRS(void)
#endif
ahrs.update();
omega = imu.get_gyro();
omega = ins.get_gyro();
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.update();

4
ArduCopter/Attitude.pde
View File

@ -647,7 +647,7 @@ static int16_t get_z_damping()
float get_world_Z_accel()
{
accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
accels_rot = ahrs.get_dcm_matrix() * ins.get_accel();
//Serial.printf("z %1.4f\n", accels_rot.z);
return accels_rot.z;
}
@ -695,7 +695,7 @@ static float fullDampP = 0.100;
float get_world_Z_accel()
{
accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
accels_rot = ahrs.get_dcm_matrix() * ins.get_accel();
return accels_rot.z;
}

18
ArduCopter/GCS_Mavlink.pde
View File

@ -395,8 +395,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
Vector3f accel = ins.get_accel();
Vector3f gyro = ins.get_gyro();
mavlink_msg_raw_imu_send(
chan,
micros(),
@ -432,8 +432,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
compass.get_declination(),
barometer.get_raw_pressure(),
barometer.get_raw_temp(),
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
ins.gx(), ins.gy(), ins.gz(),
ins.ax(), ins.ay(), ins.az());
}
static void NOINLINE send_gps_status(mavlink_channel_t chan)
@ -1065,7 +1065,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.param1 == 1 ||
packet.param2 == 1 ||
packet.param3 == 1) {
imu.init_accel(mavlink_delay, flash_leds);
ins.init_accel(mavlink_delay, flash_leds);
}
if (packet.param4 == 1) {
trim_radio();
@ -1748,9 +1748,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
accels.y = (float)packet.yacc / 1000.0;
accels.z = (float)packet.zacc / 1000.0;
imu.set_gyro(gyros);
ins.set_gyro_offsets(gyros);
imu.set_accel(accels);
ins.set_accel_offsets(accels);
// set AHRS hil sensor
@ -1814,9 +1814,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
accels.y = (float)packet.yacc / 1000.0;
accels.z = (float)packet.zacc / 1000.0;
imu.set_gyro(gyros);
ins.set_gyro_offsets(gyros);
imu.set_accel(accels);
ins.set_accel_offsets(accels);
compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
break;

9
ArduCopter/Log.pde
View File

@ -292,7 +292,8 @@ static void Log_Read_GPS()
#if INERTIAL_NAV == ENABLED
static void Log_Write_Raw()
{
Vector3f accel = imu.get_accel();
Vector3f gyro = ins.get_gyro();
Vector3f accel = ins.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -345,8 +346,8 @@ static void Log_Read_Raw()
#else
static void Log_Write_Raw()
{
Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel();
Vector3f gyro = ins.get_gyro();
Vector3f accel = ins.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -724,7 +725,7 @@ static void Log_Write_Performance()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteByte( imu.adc_constraints); //1
DataFlash.WriteByte( 0); //1 - was adc_constraints
DataFlash.WriteByte( ahrs.renorm_range_count); //2
DataFlash.WriteByte( ahrs.renorm_blowup_count); //3
DataFlash.WriteByte( gps_fix_count); //4

4
ArduCopter/Parameters.h
View File

@ -99,8 +99,8 @@ public:
//
// 140: Sensor parameters
//
k_param_imu = 140, // sensor calibration
k_param_battery_monitoring,
k_param_imu = 140, // deprecated - can be deleted
k_param_battery_monitoring = 141,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
k_param_input_voltage,

10
ArduCopter/Parameters.pde
View File

@ -363,18 +363,14 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(compass, "COMPASS_", Compass),
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
#if HIL_MODE == HIL_MODE_DISABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
GOBJECT(ins, "INS_", AP_InertialSensor_Oilpan),
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
#if HIL_MODE == HIL_MODE_DISABLED
GOBJECT(ins, "INS_", AP_InertialSensor),
#endif
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
// @Group: IMU_
// @Path: ../libraries/AP_IMU/IMU.cpp
GOBJECT(imu, "IMU_", IMU),
// @Group: AHRS_
// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
GOBJECT(ahrs, "AHRS_", AP_AHRS),

100
ArduCopter/control_modes.pde
View File

@ -148,102 +148,38 @@ static void read_trim_switch()
}
}
}else if (option == CH7_AUTO_TRIM) {
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
auto_level_counter = 10;
if(g.rc_7.radio_in > CH_7_PWM_TRIGGER && control_mode <= ACRO && g.rc_3.control_in == 0) {
save_trim_counter = 15;
}
}
}
static void auto_trim()
// save_trim - adds roll and pitch trims from the radio to ahrs
static void save_trim()
{
if(auto_level_counter > 0) {
//g.rc_1.dead_zone = 60; // 60 = .6 degrees
//g.rc_2.dead_zone = 60;
float roll_trim, pitch_trim;
auto_level_counter--;
if(save_trim_counter > 0) {
save_trim_counter--;
if( motors.armed() ) {
// set high AHRS gains to force accelerometers to impact attitude estimate
ahrs.set_fast_gains(true);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.set_fast_gains(true);
#endif
}
// first few iterations we simply flash the leds
led_mode = SAVE_TRIM_LEDS;
trim_accel();
led_mode = AUTO_TRIM_LEDS;
do_simple = false;
if(auto_level_counter == 1) {
//g.rc_1.dead_zone = 0; // 60 = .6 degrees
//g.rc_2.dead_zone = 0;
led_mode = NORMAL_LEDS;
clear_leds();
imu.save();
if(save_trim_counter == 1) {
// save roll trim
roll_trim = ToRad((float)g.rc_1.control_in/100.0);
pitch_trim = ToRad((float)g.rc_2.control_in/100.0);
ahrs.add_trim(roll_trim, pitch_trim);
Serial.printf_P(PSTR("\nTrim Roll:%4.2f Pitch:%4.2f\n"),ToDeg(roll_trim), ToDeg(pitch_trim));
reset_control_switch();
// go back to normal AHRS gains
if( motors.armed() ) {
ahrs.set_fast_gains(false);
#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.set_fast_gains(false);
#endif
}
// switch back leds to normal
led_mode = NORMAL_LEDS;
//Serial.println("Done");
auto_level_counter = 0;
save_trim_counter = 0;
}
}
}
/*
* How this works:
* Level Example:
* A_off: -14.00, -20.59, -30.80
*
* Right roll Example:
* A_off: -6.73, 89.05, -46.02
*
* Left Roll Example:
* A_off: -18.11, -160.31, -56.42
*
* Pitch Forward:
* A_off: -127.00, -22.16, -50.09
*
* Pitch Back:
* A_off: 201.95, -24.00, -88.56
*/
static void trim_accel()
{
reset_stability_I();
float trim_roll = (float)g.rc_1.control_in / 30000.0;
float trim_pitch = (float)g.rc_2.control_in / 30000.0;
trim_roll = constrain(trim_roll, -1.5, 1.5);
trim_pitch = constrain(trim_pitch, -1.5, 1.5);
if(g.rc_1.control_in > 200) { // Roll Right
imu.ay(imu.ay() - trim_roll);
}else if (g.rc_1.control_in < -200) {
imu.ay(imu.ay() - trim_roll);
}
if(g.rc_2.control_in > 200) { // Pitch Back
imu.ax(imu.ax() + trim_pitch);
}else if (g.rc_2.control_in < -200) {
imu.ax(imu.ax() + trim_pitch);
}
/*
* Serial.printf_P(PSTR("r:%1.2f %1.2f \t| p:%1.2f %1.2f\n"),
* trim_roll,
* (float)imu.ay(),
* trim_pitch,
* (float)imu.ax());
* //*/
}

2
ArduCopter/defines.h
View File

@ -67,7 +67,7 @@
// LED output
#define NORMAL_LEDS 0
#define AUTO_TRIM_LEDS 1
#define SAVE_TRIM_LEDS 1
#define CH_7_PWM_TRIGGER 1800

2
ArduCopter/inertia.pde
View File

@ -6,7 +6,7 @@ void calc_inertia()
{
// rotate accels based on DCM
// --------------------------
accels_rotated = ahrs.get_dcm_matrix() * imu.get_accel();
accels_rotated = ahrs.get_dcm_matrix() * ins.get_accel();
//accels_rotated += accels_offset; // skew accels to account for long term error using calibration
accels_rotated.z += 9.805; // remove influence of gravity

2
ArduCopter/leds.pde
View File

@ -6,7 +6,7 @@ static void update_lights()
update_GPS_light();
break;
case AUTO_TRIM_LEDS:
case SAVE_TRIM_LEDS:
dancing_light();
break;
}

20
ArduCopter/motors.pde
View File

@ -3,7 +3,6 @@
// 10 = 1 second
#define ARM_DELAY 20
#define DISARM_DELAY 20
#define LEVEL_DELAY 100
// called at 10hz
@ -12,7 +11,7 @@ static void arm_motors()
static int16_t arming_counter;
// don't allow arming/disarming in anything but manual
if ((g.rc_3.control_in > 0) || (arming_counter > LEVEL_DELAY)) {
if (g.rc_3.control_in > 0) {
arming_counter = 0;
return;
}
@ -30,13 +29,7 @@ static void arm_motors()
// full right
if (tmp > 4000) {
if (arming_counter == LEVEL_DELAY) {
//Serial.printf("\nAL\n");
// begin auto leveling
auto_level_counter = 250;
arming_counter = 0;
}else if (arming_counter == ARM_DELAY) {
if (arming_counter == ARM_DELAY) {
if(motors.armed() == false) {
// arm the motors and configure for flight
@ -74,14 +67,7 @@ static void arm_motors()
// full left
}else if (tmp < -4000) {
if (arming_counter == LEVEL_DELAY) {
//Serial.printf("\nLEV\n");
// begin manual leveling
imu.init_accel(mavlink_delay, flash_leds);
arming_counter = 0;
}else if (arming_counter == DISARM_DELAY) {
if (arming_counter == DISARM_DELAY) {
if(motors.armed()) {
// arm the motors and configure for flight
init_disarm_motors();

104
ArduCopter/setup.pde
View File

@ -97,7 +97,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
//report_xtrack();
//report_throttle();
report_flight_modes();
report_imu();
report_ins();
report_compass();
report_optflow();
@ -247,10 +247,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv)
{
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
imu.init_accel(delay, flash_leds);
print_accel_offsets();
report_imu();
ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
ins.init_accel(delay, flash_leds);
report_ins();
return(0);
}
@ -258,69 +257,10 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_accel_scale(uint8_t argc, const Menu::arg *argv)
{
#if CONFIG_ADC == ENABLED && HIL_MODE == HIL_MODE_DISABLED
int8_t accel_num;
float accel_avg = 0;
if (!strcmp_P(argv[1].str, PSTR("x"))) {
accel_num = 4;
}else if (!strcmp_P(argv[1].str, PSTR("y"))) {
accel_num = 5;
}else if (!strcmp_P(argv[1].str, PSTR("z"))) {
accel_num = 6;
}else{
Serial.printf_P(PSTR("x, y, or z\n"));
return 0;
}
print_hit_enter();
Serial.printf_P(PSTR("ADC\n"));
adc.Init(&timer_scheduler); // APM ADC library initialization
int16_t low, high;
delay(1000);
accel_avg = adc.Ch(accel_num);
low = high = accel_avg;
while(1) {
delay(50);
accel_avg = accel_avg * .95 + adc.Ch(accel_num) * .05;
if(accel_avg > high)
high = ceil(accel_avg);
if(accel_avg < low)
low = floor(accel_avg);
Serial.printf_P(PSTR("%1.2f, %d, %d\n"), accel_avg, low, high);
if(Serial.available() > 0) {
if(wait_for_yes()) {
if(accel_num == 4) {
ins._x_high = high;
ins._x_low = low;
ins._x_high.save();
ins._x_low.save();
}else if(accel_num == 5) {
ins._y_high = high;
ins._y_low = low;
ins._y_high.save();
ins._y_low.save();
}else{
ins._z_high = high;
ins._z_low = low;
ins._z_high.save();
ins._z_low.save();
}
print_done();
}
return (0);
}
}
#else
return 0;
#endif // CONFIG_ADC
ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
ins.calibrate_accel(delay, flash_leds);
report_ins();
return(0);
}
static int8_t
@ -873,13 +813,13 @@ static void report_radio()
print_blanks(2);
}
static void report_imu()
static void report_ins()
{
Serial.printf_P(PSTR("IMU\n"));
Serial.printf_P(PSTR("INS\n"));
print_divider();
print_gyro_offsets();
print_accel_offsets();
print_accel_offsets_and_scaling();
print_blanks(2);
}
@ -1029,21 +969,27 @@ static void zero_eeprom(void)
}
static void
print_accel_offsets(void)
print_accel_offsets_and_scaling(void)
{
Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\n"),
(float)imu.ax(), // Pitch
(float)imu.ay(), // Roll
(float)imu.az()); // YAW
Vector3f accel_offsets = ins.get_accel_offsets();
Vector3f accel_scale = ins.get_accel_scale();
Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\tA_scale: %4.2f, %4.2f, %4.2f\n"),
(float)accel_offsets.x, // Pitch
(float)accel_offsets.y, // Roll
(float)accel_offsets.z, // YAW
(float)accel_scale.x, // Pitch
(float)accel_scale.y, // Roll
(float)accel_scale.z); // YAW
}
static void
print_gyro_offsets(void)
{
Vector3f gyro_offsets = ins.get_gyro_offsets();
Serial.printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
(float)imu.gx(),
(float)imu.gy(),
(float)imu.gz());
(float)gyro_offsets.x,
(float)gyro_offsets.y,
(float)gyro_offsets.z);
}
#if FRAME_CONFIG == HELI_FRAME

4
ArduCopter/system.pde
View File

@ -367,9 +367,9 @@ static void startup_ground(void)
// Warm up and read Gyro offsets
// -----------------------------
imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
ins.init(AP_InertialSensor::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
#if CLI_ENABLED == ENABLED
report_imu();
report_ins();
#endif
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).

279
ArduCopter/test.pde
View File

@ -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)
* #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)
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)
#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)
{