mirror of https://github.com/ArduPilot/ardupilot
Quaternion: added an AP_Quaternion library
this is for experimenting with the Madgwick quaternion system, to see if it is more or less noise sensitive than DCM
This commit is contained in:
Andrew Tridgell
parent
fd2a24f8ed
commit
74eef7018a
|
|
@ -0,0 +1,296 @@
|
|||
/*
|
||||
AP_Quaternion code, based on quaternion code from Jeb Madgwick
|
||||
|
||||
See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
|
||||
|
||||
adapted to APM by Andrew Tridgell
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later
|
||||
version.
|
||||
*/
|
||||
#include <AP_Quaternion.h>
|
||||
|
||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
|
||||
// this is the speed in cm/s above which we first get a yaw lock with
|
||||
// the GPS
|
||||
#define GPS_SPEED_MIN 300
|
||||
|
||||
// this is the speed in cm/s at which we stop using drift correction
|
||||
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
|
||||
#define GPS_SPEED_RESET 100
|
||||
|
||||
void
|
||||
AP_Quaternion::set_compass(Compass *compass)
|
||||
{
|
||||
_compass = compass;
|
||||
}
|
||||
|
||||
// Function to compute one quaternion iteration without magnetometer
|
||||
void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
|
||||
{
|
||||
// Local system variables
|
||||
float norm; // vector norm
|
||||
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion derrivative from gyroscopes elements
|
||||
float f_1, f_2, f_3; // objective function elements
|
||||
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
|
||||
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
|
||||
|
||||
// Axulirary variables to avoid reapeated calcualtions
|
||||
float halfSEq_1 = 0.5f * SEq_1;
|
||||
float halfSEq_2 = 0.5f * SEq_2;
|
||||
float halfSEq_3 = 0.5f * SEq_3;
|
||||
float halfSEq_4 = 0.5f * SEq_4;
|
||||
float twoSEq_1 = 2.0f * SEq_1;
|
||||
float twoSEq_2 = 2.0f * SEq_2;
|
||||
float twoSEq_3 = 2.0f * SEq_3;
|
||||
|
||||
// estimated direction of the gyroscope error (radians)
|
||||
Vector3f w_err;
|
||||
|
||||
// normalise accelerometer vector
|
||||
accel.normalize();
|
||||
|
||||
// Compute the objective function and Jacobian
|
||||
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x;
|
||||
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - accel.y;
|
||||
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - accel.z;
|
||||
J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
|
||||
J_12or23 = 2.0f * SEq_4;
|
||||
J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
|
||||
J_14or21 = twoSEq_2;
|
||||
J_32 = 2.0f * J_14or21; // negated in matrix multiplication
|
||||
J_33 = 2.0f * J_11or24; // negated in matrix multiplication
|
||||
|
||||
// Compute the gradient (matrix multiplication)
|
||||
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1;
|
||||
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
|
||||
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
|
||||
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;
|
||||
|
||||
// Normalise the gradient
|
||||
norm = 1.0/sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
|
||||
if (!isinf(norm)) {
|
||||
SEqHatDot_1 *= norm;
|
||||
SEqHatDot_2 *= norm;
|
||||
SEqHatDot_3 *= norm;
|
||||
SEqHatDot_4 *= norm;
|
||||
}
|
||||
|
||||
// Compute the quaternion derrivative measured by gyroscopes
|
||||
SEqDot_omega_1 = -halfSEq_2 * gyro.x - halfSEq_3 * gyro.y - halfSEq_4 * gyro.z;
|
||||
SEqDot_omega_2 = halfSEq_1 * gyro.x + halfSEq_3 * gyro.z - halfSEq_4 * gyro.y;
|
||||
SEqDot_omega_3 = halfSEq_1 * gyro.y - halfSEq_2 * gyro.z + halfSEq_4 * gyro.x;
|
||||
SEqDot_omega_4 = halfSEq_1 * gyro.z + halfSEq_2 * gyro.y - halfSEq_3 * gyro.x;
|
||||
|
||||
// Compute then integrate the estimated quaternion derrivative
|
||||
SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
|
||||
SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
|
||||
SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
|
||||
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
|
||||
|
||||
// Normalise quaternion
|
||||
norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
|
||||
if (!isinf(norm)) {
|
||||
SEq_1 *= norm;
|
||||
SEq_2 *= norm;
|
||||
SEq_3 *= norm;
|
||||
SEq_4 *= norm;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Function to compute one quaternion iteration including magnetometer
|
||||
void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
|
||||
{
|
||||
// local system variables
|
||||
float norm; // vector norm
|
||||
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements
|
||||
float f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements
|
||||
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements
|
||||
J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //
|
||||
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
|
||||
|
||||
// computed flux in the earth frame
|
||||
Vector3f flux;
|
||||
|
||||
// estimated direction of the gyroscope error (radians)
|
||||
Vector3f w_err;
|
||||
|
||||
// normalise accelerometer vector
|
||||
accel.normalize();
|
||||
|
||||
// normalise the magnetometer measurement
|
||||
mag.normalize();
|
||||
|
||||
// auxiliary variables to avoid repeated calculations
|
||||
float halfSEq_1 = 0.5f * SEq_1;
|
||||
float halfSEq_2 = 0.5f * SEq_2;
|
||||
float halfSEq_3 = 0.5f * SEq_3;
|
||||
float halfSEq_4 = 0.5f * SEq_4;
|
||||
float twoSEq_1 = 2.0f * SEq_1;
|
||||
float twoSEq_2 = 2.0f * SEq_2;
|
||||
float twoSEq_3 = 2.0f * SEq_3;
|
||||
float twoSEq_4 = 2.0f * SEq_4;
|
||||
float twob_x = 2.0f * b_x;
|
||||
float twob_z = 2.0f * b_z;
|
||||
float twob_xSEq_1 = 2.0f * b_x * SEq_1;
|
||||
float twob_xSEq_2 = 2.0f * b_x * SEq_2;
|
||||
float twob_xSEq_3 = 2.0f * b_x * SEq_3;
|
||||
float twob_xSEq_4 = 2.0f * b_x * SEq_4;
|
||||
float twob_zSEq_1 = 2.0f * b_z * SEq_1;
|
||||
float twob_zSEq_2 = 2.0f * b_z * SEq_2;
|
||||
float twob_zSEq_3 = 2.0f * b_z * SEq_3;
|
||||
float twob_zSEq_4 = 2.0f * b_z * SEq_4;
|
||||
float SEq_1SEq_2;
|
||||
float SEq_1SEq_3 = SEq_1 * SEq_3;
|
||||
float SEq_1SEq_4;
|
||||
float SEq_2SEq_3;
|
||||
float SEq_2SEq_4 = SEq_2 * SEq_4;
|
||||
float SEq_3SEq_4;
|
||||
Vector3f twom = mag * 2.0;
|
||||
|
||||
// compute the objective function and Jacobian
|
||||
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x;
|
||||
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - accel.y;
|
||||
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - accel.z;
|
||||
f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x;
|
||||
f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y;
|
||||
f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z;
|
||||
J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
|
||||
J_12or23 = 2.0f * SEq_4;
|
||||
J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
|
||||
J_14or21 = twoSEq_2;
|
||||
J_32 = 2.0f * J_14or21; // negated in matrix multiplication
|
||||
J_33 = 2.0f * J_11or24; // negated in matrix multiplication
|
||||
J_41 = twob_zSEq_3; // negated in matrix multiplication
|
||||
J_42 = twob_zSEq_4;
|
||||
J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication
|
||||
J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
|
||||
J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
|
||||
J_52 = twob_xSEq_3 + twob_zSEq_1;
|
||||
J_53 = twob_xSEq_2 + twob_zSEq_4;
|
||||
J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication
|
||||
J_61 = twob_xSEq_3;
|
||||
J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2;
|
||||
J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3;
|
||||
J_64 = twob_xSEq_2;
|
||||
|
||||
// compute the gradient (matrix multiplication)
|
||||
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
|
||||
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
|
||||
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
|
||||
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
|
||||
|
||||
// normalise the gradient to estimate direction of the gyroscope error
|
||||
norm = 1.0 / sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
|
||||
SEqHatDot_1 *= norm;
|
||||
SEqHatDot_2 *= norm;
|
||||
SEqHatDot_3 *= norm;
|
||||
SEqHatDot_4 *= norm;
|
||||
|
||||
// compute angular estimated direction of the gyroscope error
|
||||
w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;
|
||||
w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
|
||||
w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
|
||||
|
||||
// compute and remove the gyroscope baises
|
||||
gyro_bias += w_err * (deltat * zeta);
|
||||
gyro -= gyro_bias;
|
||||
|
||||
// compute the quaternion rate measured by gyroscopes
|
||||
SEqDot_omega_1 = -halfSEq_2 * gyro.x - halfSEq_3 * gyro.y - halfSEq_4 * gyro.z;
|
||||
SEqDot_omega_2 = halfSEq_1 * gyro.x + halfSEq_3 * gyro.z - halfSEq_4 * gyro.y;
|
||||
SEqDot_omega_3 = halfSEq_1 * gyro.y - halfSEq_2 * gyro.z + halfSEq_4 * gyro.x;
|
||||
SEqDot_omega_4 = halfSEq_1 * gyro.z + halfSEq_2 * gyro.y - halfSEq_3 * gyro.x;
|
||||
|
||||
// compute then integrate the estimated quaternion rate
|
||||
SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
|
||||
SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
|
||||
SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
|
||||
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
|
||||
|
||||
// normalise quaternion
|
||||
norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
|
||||
if (!isinf(norm)) {
|
||||
SEq_1 *= norm;
|
||||
SEq_2 *= norm;
|
||||
SEq_3 *= norm;
|
||||
SEq_4 *= norm;
|
||||
}
|
||||
|
||||
// compute flux in the earth frame
|
||||
// recompute axulirary variables
|
||||
SEq_1SEq_2 = SEq_1 * SEq_2;
|
||||
SEq_1SEq_3 = SEq_1 * SEq_3;
|
||||
SEq_1SEq_4 = SEq_1 * SEq_4;
|
||||
SEq_3SEq_4 = SEq_3 * SEq_4;
|
||||
SEq_2SEq_3 = SEq_2 * SEq_3;
|
||||
SEq_2SEq_4 = SEq_2 * SEq_4;
|
||||
flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3);
|
||||
flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2);
|
||||
flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3);
|
||||
|
||||
// normalise the flux vector to have only components in the x and z
|
||||
b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y));
|
||||
b_z = flux.z;
|
||||
}
|
||||
|
||||
|
||||
// Function to compute one quaternion iteration
|
||||
void AP_Quaternion::update(float deltat)
|
||||
{
|
||||
Vector3f gyro, accel;
|
||||
|
||||
_imu->update();
|
||||
|
||||
// get current IMU state
|
||||
gyro = _imu->get_gyro();
|
||||
gyro.x = -gyro.x;
|
||||
gyro.y = -gyro.y;
|
||||
|
||||
accel = _imu->get_accel();
|
||||
accel.z = -accel.z;
|
||||
|
||||
if (_compass == NULL) {
|
||||
update_IMU(deltat, gyro, accel);
|
||||
} else {
|
||||
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
|
||||
update_MARG(deltat, gyro, accel, mag);
|
||||
}
|
||||
|
||||
// compute the Eulers
|
||||
float test = (SEq_1*SEq_3 - SEq_4*SEq_2);
|
||||
const float singularity = 0.499; // 86.3 degrees?
|
||||
if (test > singularity) {
|
||||
// singularity at south pole
|
||||
// this one is ok..
|
||||
yaw = 2.0 * atan2(SEq_4, SEq_1);
|
||||
pitch = ToRad(-90.0);
|
||||
roll = 0.0;
|
||||
} else if (test < -singularity) {
|
||||
// singularity at north pole
|
||||
// this one is invalid :( .. fix it.
|
||||
yaw = -2.0 * atan2(SEq_4, SEq_1);
|
||||
pitch = ToRad(90.0);
|
||||
roll = 0.0;
|
||||
} else {
|
||||
roll = -(atan2(2.0*(SEq_1*SEq_2 + SEq_3*SEq_4),
|
||||
1 - 2.0*(SEq_2*SEq_2 + SEq_3*SEq_3)));
|
||||
pitch = -safe_asin(2.0*test);
|
||||
yaw = atan2(2.0*(SEq_1*SEq_4 + SEq_2*SEq_3),
|
||||
1 - 2.0*(SEq_3*SEq_3 + SEq_4*SEq_4));
|
||||
}
|
||||
|
||||
// and integer Eulers
|
||||
roll_sensor = 100 * ToDeg(roll);
|
||||
pitch_sensor = 100 * ToDeg(pitch);
|
||||
yaw_sensor = 100 * ToDeg(yaw);
|
||||
if (yaw_sensor < 0) {
|
||||
yaw_sensor += 36000;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
#ifndef AP_Quaternion_h
|
||||
#define AP_Quaternion_h
|
||||
|
||||
#include <AP_Math.h>
|
||||
#include <inttypes.h>
|
||||
#include <AP_Compass.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <AP_IMU.h>
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
class AP_Quaternion
|
||||
{
|
||||
public:
|
||||
// Constructor
|
||||
AP_Quaternion(IMU *imu, GPS *&gps, Compass *compass = NULL) :
|
||||
_imu(imu),
|
||||
_gps(gps),
|
||||
_compass(compass)
|
||||
{
|
||||
// initial quaternion
|
||||
SEq_1 = 1;
|
||||
SEq_2 = 0;
|
||||
SEq_3 = 0;
|
||||
SEq_4 = 0;
|
||||
|
||||
// reference direction of flux in earth frame
|
||||
b_x = 0;
|
||||
b_z = -1;
|
||||
|
||||
// scaled gyro drift limits
|
||||
beta = sqrt(3.0f / 4.0f) * gyroMeasError;
|
||||
zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;
|
||||
}
|
||||
|
||||
// Accessors
|
||||
void set_centripetal(bool b) {_centripetal = b;}
|
||||
bool get_centripetal(void) {return _centripetal;}
|
||||
void set_compass(Compass *compass);
|
||||
|
||||
// Methods
|
||||
void update(float deltat);
|
||||
|
||||
// Euler angles (radians)
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
|
||||
// integer Euler angles (Degrees * 100)
|
||||
int32_t roll_sensor;
|
||||
int32_t pitch_sensor;
|
||||
int32_t yaw_sensor;
|
||||
|
||||
|
||||
private:
|
||||
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
|
||||
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
|
||||
|
||||
bool _have_initial_yaw;
|
||||
|
||||
// Methods
|
||||
void accel_adjust(void);
|
||||
|
||||
// members
|
||||
Compass * _compass;
|
||||
|
||||
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
|
||||
// IMU under us without our noticing.
|
||||
GPS *&_gps;
|
||||
IMU *_imu;
|
||||
|
||||
// true if we are doing centripetal acceleration correction
|
||||
bool _centripetal;
|
||||
|
||||
// maximum gyroscope measurement error in rad/s (set to 5 degrees/second)
|
||||
static const float gyroMeasError = 5.0 * (M_PI/180.0);
|
||||
|
||||
// maximum gyroscope drift rate in radians/s/s (set to 0.2 degrees/s/s)
|
||||
static const float gyroMeasDrift = 0.2 * (PI/180.0);
|
||||
|
||||
float beta;
|
||||
float zeta;
|
||||
|
||||
// quaternion elements
|
||||
float SEq_1, SEq_2, SEq_3, SEq_4;
|
||||
|
||||
float b_x;
|
||||
float b_z;
|
||||
|
||||
// estimate gyroscope biases error
|
||||
Vector3f gyro_bias;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
include ../../../AP_Common/Arduino.mk
|
||||
|
||||
sitl:
|
||||
make -f ../../../../libraries/Desktop/Desktop.mk
|
||||
|
|
@ -0,0 +1,149 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
//
|
||||
// Simple test for the AP_Quaternion library
|
||||
//
|
||||
|
||||
#include <FastSerial.h>
|
||||
#include <SPI.h>
|
||||
#include <I2C.h>
|
||||
#include <Arduino_Mega_ISR_Registry.h>
|
||||
#include <AP_PeriodicProcess.h>
|
||||
#include <AP_InertialSensor.h>
|
||||
#include <AP_ADC.h>
|
||||
#include <AP_IMU.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <AP_Quaternion.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Compass.h>
|
||||
#include <AP_Baro.h>
|
||||
#include <DataFlash.h>
|
||||
#include <APM_RC.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
|
||||
// uncomment this for a APM2 board
|
||||
//#define APM2_HARDWARE
|
||||
|
||||
FastSerialPort(Serial, 0);
|
||||
|
||||
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( 53 );
|
||||
# else
|
||||
AP_ADC_ADS7844 adc;
|
||||
AP_InertialSensor_Oilpan ins( &adc );
|
||||
#endif // CONFIG_IMU_TYPE
|
||||
|
||||
static GPS *g_gps;
|
||||
|
||||
AP_IMU_INS imu(&ins);
|
||||
AP_Quaternion quaternion(&imu, g_gps);
|
||||
|
||||
AP_Baro_BMP085_HIL barometer;
|
||||
|
||||
|
||||
#ifdef APM2_HARDWARE
|
||||
# define A_LED_PIN 27
|
||||
# define C_LED_PIN 25
|
||||
# define LED_ON LOW
|
||||
# define LED_OFF HIGH
|
||||
# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD
|
||||
#else
|
||||
# define A_LED_PIN 37
|
||||
# define C_LED_PIN 35
|
||||
# define LED_ON HIGH
|
||||
# define LED_OFF LOW
|
||||
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||
#endif
|
||||
|
||||
|
||||
static void flash_leds(bool on)
|
||||
{
|
||||
digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON);
|
||||
digitalWrite(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);
|
||||
#endif
|
||||
|
||||
isr_registry.init();
|
||||
scheduler.init(&isr_registry);
|
||||
|
||||
imu.init(IMU::COLD_START, delay, flash_leds, &scheduler);
|
||||
imu.init_accel(delay, flash_leds);
|
||||
|
||||
compass.set_orientation(MAG_ORIENTATION);
|
||||
if (compass.init()) {
|
||||
Serial.printf("Enabling compass\n");
|
||||
compass.null_offsets_enable();
|
||||
quaternion.set_compass(&compass);
|
||||
}
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
static uint16_t counter;
|
||||
static uint32_t last_t, last_print, last_compass;
|
||||
uint32_t now = micros();
|
||||
float deltat;
|
||||
static uint32_t compass_reads;
|
||||
static uint32_t compass_time;
|
||||
|
||||
if (last_t == 0) {
|
||||
last_t = now;
|
||||
return;
|
||||
}
|
||||
deltat = (now - last_t) * 1.0e-6;
|
||||
last_t = now;
|
||||
|
||||
if (now - last_compass > 100*1000UL) {
|
||||
// read compass at 10Hz
|
||||
compass.read();
|
||||
last_compass = now;
|
||||
}
|
||||
|
||||
quaternion.update(deltat);
|
||||
counter++;
|
||||
|
||||
if (now - last_print >= 0.5e6) {
|
||||
Vector3f accel = imu.get_accel();
|
||||
Vector3f gyro = imu.get_gyro();
|
||||
Serial.printf_P(PSTR("r:%4.1f p:%4.1f y:%4.1f g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f) rate=%.1f\n"),
|
||||
ToDeg(quaternion.roll),
|
||||
ToDeg(quaternion.pitch),
|
||||
ToDeg(quaternion.yaw),
|
||||
gyro.x, gyro.y, gyro.z,
|
||||
accel.x, accel.y, accel.z,
|
||||
(1.0e6*counter)/(now-last_print));
|
||||
last_print = now;
|
||||
counter = 0;
|
||||
compass_reads=0;
|
||||
compass_time = 0;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue