aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: port to AP_HAL

Browse Source
This commit is contained in:
Pat Hickey 2012-11-14 08:10:15 -08:00 committed by Andrew Tridgell
parent 6cc231ae7d
commit 9bf69d4e0d
12 changed files with 169 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -6,9 +6,9 @@
as published by the Free Software Foundation; either version 2.1
of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
#include <AP_AHRS.h>
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
@ -87,6 +87,7 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
if (_wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
*airspeed_ret = constrain(*airspeed_ret,
_gps->ground_speed*0.01 - _wind_max,
_gps->ground_speed*0.01 + _wind_max);
@ -102,7 +103,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
Vector3f trim = _trim.get();
// debug -- remove me!
Serial.printf_P(PSTR("\nadd_trim before R:%4.2f P:%4.2f\n"),ToDeg(trim.x),ToDeg(trim.y));
hal.console->printf_P(PSTR("\nadd_trim before R:%4.2f P:%4.2f\n"),
ToDeg(trim.x),ToDeg(trim.y));
// add new trim
trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0));
@ -112,5 +114,6 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
_trim.set_and_save(trim);
// debug -- remove me!
Serial.printf_P(PSTR("add_trim after R:%4.2f P:%4.2f\n"),ToDeg(trim.x),ToDeg(trim.y));
hal.console->printf_P(PSTR("add_trim after R:%4.2f P:%4.2f\n"),
ToDeg(trim.x),ToDeg(trim.y));
}

24
libraries/AP_AHRS/AP_AHRS.h
View File

@ -1,5 +1,5 @@
#ifndef AP_AHRS_H
#define AP_AHRS_H
#ifndef __AP_AHRS_H__
#define __AP_AHRS_H__
/*
* AHRS (Attitude Heading Reference System) interface for ArduPilot
*
@ -18,12 +18,6 @@
#include <AP_Baro.h>
#include <AP_Param.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class AP_AHRS
{
public:
@ -42,24 +36,24 @@ public:
}
// empty init
virtual void init( AP_PeriodicProcess * scheduler = NULL ) {
virtual void init() {
};
// Accessors
void set_fly_forward(bool b) {
void set_fly_forward(bool b) {
_fly_forward = b;
}
void set_compass(Compass *compass) {
void set_compass(Compass *compass) {
_compass = compass;
}
void set_barometer(AP_Baro *barometer) {
void set_barometer(AP_Baro *barometer) {
_barometer = barometer;
}
void set_airspeed(AP_Airspeed *airspeed) {
void set_airspeed(AP_Airspeed *airspeed) {
_airspeed = airspeed;
}
AP_InertialSensor* get_ins() {
AP_InertialSensor* get_ins() {
return _ins;
}
@ -202,4 +196,4 @@ protected:
#include <AP_AHRS_MPU6000.h>
#include <AP_AHRS_HIL.h>
#endif // AP_AHRS_H
#endif // __AP_AHRS_H__

10
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -13,8 +13,12 @@
* as published by the Free Software Foundation; either version 2.1
* of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
#include <AP_AHRS.h>
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
@ -428,7 +432,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// add in wind estimate
velocity += _wind;
last_correction_time = millis();
last_correction_time = hal.scheduler->millis();
_have_gps_lock = false;
// update position delta for get_position()
@ -606,7 +610,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
// See http://gentlenav.googlecode.com/files/WindEstimation.pdf
Vector3f fuselageDirection = _dcm_matrix.colx();
Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
uint32_t now = millis();
uint32_t now = hal.scheduler->millis();
// scrap our data and start over if we're taking too long to get a direction change
if (now - _last_wind_time > 10000) {

6
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -1,5 +1,5 @@
#ifndef AP_AHRS_DCM_H
#define AP_AHRS_DCM_H
#ifndef __AP_AHRS_DCM_H__
#define __AP_AHRS_DCM_H__
/*
* DCM based AHRS (Attitude Heading Reference System) interface for
* ArduPilot
@ -136,4 +136,4 @@ private:
Vector3f _wind;
};
#endif // AP_AHRS_DCM_H
#endif // __AP_AHRS_DCM_H__

1
libraries/AP_AHRS/AP_AHRS_HIL.cpp
View File

@ -10,7 +10,6 @@
* version.
*/
#include <FastSerial.h>
#include <AP_AHRS.h>
/**************************************************/

16
libraries/AP_AHRS/AP_AHRS_HIL.h
View File

@ -1,13 +1,11 @@
#ifndef AP_AHRS_HIL_H
#define AP_AHRS_HIL_H
#ifndef __AP_AHRS_HIL_H__
#define __AP_AHRS_HIL_H__
class AP_AHRS_HIL : public AP_AHRS
{
public:
// Constructors
AP_AHRS_HIL(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(ins, gps)
{
}
AP_AHRS_HIL(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(ins, gps) {}
// Accessors
Vector3f get_gyro(void) {
@ -21,12 +19,12 @@ public:
}
// Methods
void update(void) {
void update(void) {
_ins->update();
}
void setHil(float roll, float pitch, float yaw,
float rollRate, float pitchRate, float yawRate);
void setHil(float roll, float pitch, float yaw,
float rollRate, float pitchRate, float yawRate);
// return the current estimate of the gyro drift
Vector3f get_gyro_drift(void) {
@ -48,4 +46,4 @@ private:
Vector3f _omega;
};
#endif
#endif // __AP_AHRS_HIL_H__

81
libraries/AP_AHRS/AP_AHRS_MPU6000.cpp
View File

@ -10,8 +10,10 @@
* as published by the Free Software Foundation; either version 2.1
* of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
#include <AP_AHRS.h>
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
@ -28,24 +30,20 @@
#define SPIN_RATE_LIMIT 20
void
AP_AHRS_MPU6000::init( AP_PeriodicProcess * scheduler )
AP_AHRS_MPU6000::init()
{
bool timer_running = false;
// suspend timer so interrupts on spi bus do not interfere with communication to mpu6000
if( scheduler != NULL ) {
timer_running = scheduler->running();
scheduler->suspend_timer();
}
// suspend timer so interrupts on spi bus do not interfere with
// communication to mpu6000
hal.scheduler->suspend_timer_procs();
_mpu6000->dmp_init();
push_gains_to_dmp();
_mpu6000->push_gyro_offsets_to_dmp();
// restart timer
if( timer_running ) {
scheduler->resume_timer();
}
hal.scheduler->resume_timer_procs();
};
// run a full MPU6000 update round
@ -87,8 +85,9 @@ float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians)
}
}
// Function to correct the gyroX and gyroY bias (roll and pitch) using the gravity vector from accelerometers
// We use the internal chip axis definition to make the bias correction because both sensors outputs (gyros and accels)
// Function to correct the gyroX and gyroY bias (roll and pitch) using the
// gravity vector from accelerometers We use the internal chip axis definition
// to make the bias correction because both sensors outputs (gyros and accels)
// are in chip axis definition
void AP_AHRS_MPU6000::drift_correction( float deltat )
{
@ -97,40 +96,52 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
// Get current values for gyros
_accel_vector = _ins->get_accel();
// We take the accelerometer readings and cumulate to average them and obtain the gravity vector
// We take the accelerometer readings and cumulate to average them and
// obtain the gravity vector
_accel_filtered += _accel_vector;
_accel_filtered_samples++;
_gyro_bias_from_gravity_counter++;
// We make the bias calculation and correction at a lower rate (GYRO_BIAS_FROM_GRAVITY_RATE)
// We make the bias calculation and correction at a lower rate
// (GYRO_BIAS_FROM_GRAVITY_RATE)
if( _gyro_bias_from_gravity_counter == GYRO_BIAS_FROM_GRAVITY_RATE ) {
_gyro_bias_from_gravity_counter = 0;
_accel_filtered /= _accel_filtered_samples; // average
// Adjust ground reference : Accel Cross Gravity to obtain the error between gravity from accels and gravity from attitude solution
// Adjust ground reference : Accel Cross Gravity to obtain the error
// between gravity from accels and gravity from attitude solution
// errorRollPitch are in Accel LSB units
errorRollPitch[0] = _accel_filtered.y * _dcm_matrix.c.z + _accel_filtered.z * _dcm_matrix.c.x;
errorRollPitch[1] = -_accel_filtered.z * _dcm_matrix.c.y - _accel_filtered.x * _dcm_matrix.c.z;
errorRollPitch[0] = _accel_filtered.y * _dcm_matrix.c.z
+ _accel_filtered.z * _dcm_matrix.c.x;
errorRollPitch[1] = -_accel_filtered.z * _dcm_matrix.c.y
- _accel_filtered.x * _dcm_matrix.c.z;
errorRollPitch[0] *= deltat * 1000;
errorRollPitch[1] *= deltat * 1000;
// we limit to maximum gyro drift rate on each axis
float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat / _gyro_bias_from_gravity_gain; //0.65*0.04 / 0.005 = 5.2
errorRollPitch[0] = constrain(errorRollPitch[0], -drift_limit, drift_limit);
errorRollPitch[1] = constrain(errorRollPitch[1], -drift_limit, drift_limit);
// 0.65*0.04 / 0.005 = 5.2
float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat
/ _gyro_bias_from_gravity_gain;
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
errorRollPitch[0] = constrain(errorRollPitch[0],
-drift_limit, drift_limit);
errorRollPitch[1] = constrain(errorRollPitch[1],
-drift_limit, drift_limit);
// We correct gyroX and gyroY bias using the error vector
_gyro_bias[0] += errorRollPitch[0]*_gyro_bias_from_gravity_gain;
_gyro_bias[1] += errorRollPitch[1]*_gyro_bias_from_gravity_gain;
// TO-DO: fix this. Currently it makes the roll and pitch drift more!
// If bias values are greater than 1 LSB we update the hardware offset registers
// If bias values are greater than 1 LSB we update the hardware offset
// registers
if( fabs(_gyro_bias[0])>1.0 ) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0);
//_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0);
//_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that we have already corrected on registers...
//_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that
// we have already corrected on registers...
}
if (fabs(_gyro_bias[1])>1.0) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0);
@ -167,14 +178,16 @@ AP_AHRS_MPU6000::reset(bool recover_eulers)
}
}
// push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
// push offsets down from IMU to INS (required so MPU6000 can perform it's own
// attitude estimation)
void
AP_AHRS_MPU6000::push_offsets_to_ins()
{
// push down gyro offsets (TO-DO: why are x and y offsets are reversed?!)
_mpu6000->push_gyro_offsets_to_dmp();
// push down accelerometer offsets (TO-DO: why are x and y offsets are reversed?!)
// push down accelerometer offsets
// (TO-DO: why are x and y offsets are reversed?!)
_mpu6000->push_accel_offsets_to_dmp();
}
@ -220,18 +233,23 @@ AP_AHRS_MPU6000::yaw_error_compass(void)
//
// drift_correction_yaw - yaw drift correction using the compass
// we have no way to update the dmp with it's actual heading from our compass
// instead we use the yaw_corrected variable to hold what we think is the real heading
// we also record what the dmp said it's last heading was in the yaw_last_uncorrected variable so that on the next iteration we can add the change in yaw to our estimate
// we have no way to update the dmp with it's actual heading from our
// compass instead we use the yaw_corrected variable to hold what we think
// is the real heading we also record what the dmp said it's last heading
// was in the yaw_last_uncorrected variable so that on the next iteration we
// can add the change in yaw to our estimate
//
void
AP_AHRS_MPU6000::drift_correction_yaw(void)
{
static float yaw_corrected = HEADING_UNKNOWN;
static float last_dmp_yaw = HEADING_UNKNOWN;
float dmp_roll, dmp_pitch, dmp_yaw; // roll pitch and yaw values from dmp
float yaw_delta; // change in yaw according to dmp
float yaw_error; // difference between heading and corrected yaw
// roll pitch and yaw values from dmp
float dmp_roll, dmp_pitch, dmp_yaw;
// change in yaw according to dmp
float yaw_delta;
// difference between heading and corrected yaw
float yaw_error;
static float heading;
// get uncorrected yaw values from dmp
@ -282,7 +300,8 @@ void
AP_AHRS_MPU6000::euler_angles(void)
{
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
//quaternion.to_euler(&roll, &pitch, &yaw); // cannot use this because the quaternion is not correct for yaw drift
// cannot use this because the quaternion is not correct for yaw drift
//quaternion.to_euler(&roll, &pitch, &yaw);
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;

94
libraries/AP_AHRS/AP_AHRS_MPU6000.h
View File

@ -1,5 +1,5 @@
#ifndef AP_AHRS_MPU6000_H
#define AP_AHRS_MPU6000_H
#ifndef __AP_AHRS_MPU6000_H__
#define __AP_AHRS_MPU6000_H__
/*
* DCM based AHRS (Attitude Heading Reference System) interface for
* ArduPilot
@ -10,89 +10,101 @@
* version 2.1 of the License, or (at your option) any later version.
*/
#define GYRO_BIAS_FROM_GRAVITY_RATE 4 // Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
#define HEADING_UNKNOWN -9999 // Initial value to detect that compass correction is not initialized
// Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
#define GYRO_BIAS_FROM_GRAVITY_RATE 4
// Initial value to detect that compass correction is not initialized
#define HEADING_UNKNOWN -9999
// max rate of gyro drift in gyro_LSB_units/s (16.4LSB = 1deg/s)
static const float _MPU6000_gyro_drift_rate = 0.5; // This correspond to 0.03 degrees/s/s;
// 0.5 corresponds to 0.03 degrees/s/s;
static const float _MPU6000_gyro_drift_rate = 0.5;
class AP_AHRS_MPU6000 : public AP_AHRS
{
public:
// Constructors
AP_AHRS_MPU6000(AP_InertialSensor_MPU6000 *mpu6000, GPS *&gps) : AP_AHRS(mpu6000, gps), _mpu6000(mpu6000)
AP_AHRS_MPU6000(AP_InertialSensor_MPU6000 *mpu6000, GPS *&gps) :
AP_AHRS(mpu6000, gps),
// ki and ki_yaw are experimentally derived from the simulator
_ki(0.0087),
_ki_yaw(0.01),
_mpu6000(mpu6000),
// dmp related variable initialisation
_compass_bias_time(0),
_gyro_bias_from_gravity_gain(0.008)
{
_dcm_matrix.identity();
// these are experimentally derived from the simulator
// with large drift levels
_ki = 0.0087;
_ki_yaw = 0.01;
// dmp related variable initialisation
_gyro_bias_from_gravity_gain = 0.008;
_compass_bias_time = 0;
}
// initialisation routine to start MPU6000's dmp
void init( AP_PeriodicProcess * scheduler = NULL );
void init();
// return the smoothed gyro vector corrected for drift
Vector3f get_gyro(void) {
Vector3f get_gyro(void) {
return _ins->get_gyro();
}
Matrix3f get_dcm_matrix(void) {
Matrix3f get_dcm_matrix(void) {
return _dcm_matrix;
}
// return the current drift correction integrator value
Vector3f get_gyro_drift(void) {
Vector3f get_gyro_drift(void) {
return _omega_I;
}
// Methods
void update(void);
void reset(bool recover_eulers = false);
void update(void);
void reset(bool recover_eulers = false);
// push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
void push_offsets_to_ins();
void push_gains_to_dmp();
// push offsets down from IMU to INS (required so MPU6000 can perform it's
// own attitude estimation)
void push_offsets_to_ins();
void push_gains_to_dmp();
// status reporting
float get_error_rp(void);
float get_error_yaw(void);
float get_error_rp(void);
float get_error_yaw(void);
// set_as_secondary - avoid running some steps twice (imu updates) if this is a secondary ahrs
void set_as_secondary(bool secondary) { _secondary_ahrs = secondary; }
// set_as_secondary - avoid running some steps twice (imu updates) if
// this is a secondary ahrs
void set_as_secondary(bool secondary) {
_secondary_ahrs = secondary;
}
private:
float _ki;
float _ki_yaw;
AP_InertialSensor_MPU6000 *_mpu6000;
AP_InertialSensor_MPU6000 *_mpu6000;
// Methods
void drift_correction(float deltat);
void drift_correction(float deltat);
// Compass correction variables. TO-DO: move or replace?
float wrap_PI(float angle_in_radians); // TO-DO: move this to standard AP_AHRS methods
// TO-DO: move wrap_PI to standard AP_AHRS methods
float wrap_PI(float angle_in_radians);
long _compass_bias_time;
void drift_correction_yaw(void);
float yaw_error_compass();
void euler_angles(void);
void drift_correction_yaw(void);
float yaw_error_compass();
void euler_angles(void);
Vector3f _accel_filtered;
int16_t _accel_filtered_samples;
float _gyro_bias[3]; // bias_tracking
float _gyro_bias_from_gravity_gain; // bias correction algorithm gain
// bias_tracking
float _gyro_bias[3];
// bias correction algorithm gain
float _gyro_bias_from_gravity_gain;
uint8_t _gyro_bias_from_gravity_counter;
// primary representation of attitude
Matrix3f _dcm_matrix;
Vector3f _accel_vector; // current accel vector
Vector3f _omega_I; // Omega Integrator correction
// current accel vector
Vector3f _accel_vector;
// Omega Integrator correction
Vector3f _omega_I;
Vector3f _omega_I_sum;
float _omega_I_sum_time;
@ -104,4 +116,4 @@ private:
bool _secondary_ahrs;
};
#endif // AP_AHRS_MPU6000_H
#endif // __AP_AHRS_MPU6000_H__

88
libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
View File

@ -4,57 +4,42 @@
// Simple test for the AP_AHRS interface
//
#include <FastSerial.h>
#include <SPI.h>
#include <I2C.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_HAL.h>
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Math.h>
#include <AP_Param.h>
#include <AP_InertialSensor.h>
#include <AP_ADC.h>
#include <AP_GPS.h>
#include <AP_AHRS.h>
#include <AP_Math.h>
#include <AP_AnalogSource.h>
#include <AP_AnalogSource_Arduino.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Compass.h>
#include <AP_Declination.h>
#include <AP_Airspeed.h>
#include <AP_Baro.h>
#include <AP_Semaphore.h>
#include <DataFlash.h>
#include <APM_RC.h>
#include <GCS_MAVLink.h>
#include <Filter.h>
#include <AP_Buffer.h>
// uncomment this for a APM2 board
#include <AP_HAL_AVR.h>
/* Only testing with APM2 for now. */
#define APM2_HARDWARE
#define WITH_GPS 0
FastSerialPort0(Serial);
FastSerialPort1(Serial1);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
#ifdef DESKTOP_BUILD
AP_Compass_HIL compass;
#else
AP_Compass_HMC5843 compass;
#endif
#ifdef APM2_HARDWARE
AP_InertialSensor_MPU6000 ins;
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
# else
AP_ADC_ADS7844 adc;
AP_InertialSensor_Oilpan ins( &adc );
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
#endif
static GPS *g_gps;
AP_Compass_HMC5843 compass;
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
GPS *g_gps;
AP_GPS_Auto g_gps_driver(hal.uart1, &g_gps);
// choose which AHRS system to use
AP_AHRS_DCM ahrs(&ins, g_gps);
@ -63,6 +48,9 @@ AP_AHRS_DCM ahrs(&ins, g_gps);
AP_Baro_BMP085_HIL barometer;
#define HIGH 1
#define LOW 0
#ifdef APM2_HARDWARE
# define A_LED_PIN 27
# define C_LED_PIN 25
@ -80,46 +68,32 @@ AP_Baro_BMP085_HIL barometer;
static void flash_leds(bool on)
{
digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON);
digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
hal.gpio->write(A_LED_PIN, on ? LED_OFF : LED_ON);
hal.gpio->write(C_LED_PIN, on ? LED_ON : LED_OFF);
}
void setup(void)
{
Serial.begin(115200);
Serial.println("Starting up...");
#ifndef DESKTOP_BUILD
I2c.begin();
I2c.timeOut(5);
I2c.setSpeed(true);
#endif
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16);
#ifdef APM2_HARDWARE
// we need to stop the barometer from holding the SPI bus
pinMode(40, OUTPUT);
digitalWrite(40, HIGH);
hal.gpio->pinMode(40, GPIO_OUTPUT);
hal.gpio->write(40, HIGH);
#endif
isr_registry.init();
scheduler.init(&isr_registry);
ins.init(AP_InertialSensor::COLD_START,
AP_InertialSensor::RATE_100HZ,
delay, flash_leds, &scheduler);
ins.init_accel(delay, flash_leds);
flash_leds);
ins.init_accel(flash_leds);
compass.set_orientation(MAG_ORIENTATION);
ahrs.init();
if( compass.init() ) {
Serial.printf("Enabling compass\n");
hal.console->printf("Enabling compass\n");
ahrs.set_compass(&compass);
} else {
Serial.printf("No compass detected\n");
hal.console->printf("No compass detected\n");
}
g_gps = &g_gps_driver;
#if WITH_GPS
@ -131,7 +105,7 @@ void loop(void)
{
static uint16_t counter;
static uint32_t last_t, last_print, last_compass;
uint32_t now = micros();
uint32_t now = hal.scheduler->micros();
float heading = 0;
if (last_t == 0) {
@ -153,9 +127,11 @@ void loop(void)
ahrs.update();
counter++;
if (now - last_print >= 0.5e6) {
if (now - last_print >= 100000 /* 100ms : 10hz */) {
Vector3f drift = ahrs.get_gyro_drift();
Serial.printf_P(PSTR("r:%4.1f p:%4.1f y:%4.1f drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"),
hal.console->printf_P(
PSTR("r:%4.1f p:%4.1f y:%4.1f "
"drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"),
ToDeg(ahrs.roll),
ToDeg(ahrs.pitch),
ToDeg(ahrs.yaw),
@ -168,3 +144,5 @@ void loop(void)
counter = 0;
}
}
AP_HAL_MAIN();

0
libraries/AP_AHRS/examples/AHRS_Test/Arduino.h Normal file
View File

0
libraries/AP_AHRS/examples/AHRS_Test/nocore.inoflag Normal file
View File

0
libraries/AP_AHRS/examples/AHRS_Test/norelax.inoflag Normal file
View File