1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

Merge branch 'amorale/OP_1403_gen_i2c_spi_hmc5x83_driver' into next

This commit is contained in:
Alessio Morale 2014-08-02 22:05:55 +02:00
commit 99f09d588f
21 changed files with 793 additions and 2002 deletions

View File

@ -1,1348 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Attitude Copter Control Attitude Estimation
* @brief Acquires sensor data and computes attitude estimate
* Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file attitude.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref AttitudeRaw @ref AttitudeState
*
* This module computes an attitude estimate from the sensor data
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "attitude.h"
#include "accelsensor.h"
#include "accelstate.h"
#include "airspeedsensor.h"
#include "airspeedstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "barosensor.h"
#include "flightstatus.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "gyrostate.h"
#include "gyrosensor.h"
#include "homelocation.h"
#include "magsensor.h"
#include "magstate.h"
#include "positionstate.h"
#include "ekfconfiguration.h"
#include "ekfstatevariance.h"
#include "revocalibration.h"
#include "revosettings.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 2048
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define FAILSAFE_TIMEOUT_MS 10
#define CALIBRATION_DELAY 4000
#define CALIBRATION_DURATION 6000
// low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
// exp(-(1/f) / tau ) ~=~ 0.9997
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// simple IAS to TAS aproximation - 2% increase per 1000ft
// since we do not have flowing air temperature information
#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
// Private types
// Private variables
static xTaskHandle attitudeTaskHandle;
static xQueueHandle gyroQueue;
static xQueueHandle accelQueue;
static xQueueHandle magQueue;
static xQueueHandle airspeedQueue;
static xQueueHandle baroQueue;
static xQueueHandle gpsQueue;
static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static EKFConfigurationData ekfConfiguration;
static RevoSettingsData revoSettings;
static FlightStatusData flightStatus;
const uint32_t SENSOR_QUEUE_SIZE = 10;
static bool volatile variance_error = true;
static bool volatile initialization_required = true;
static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
static float rollPitchBiasRate = 0;
// Accel filtering
static float accel_alpha = 0;
static bool accel_filter_enabled = false;
static float accels_filtered[3];
static float grot_filtered[3];
// Private functions
static void AttitudeTask(void *parameters);
static int32_t updateAttitudeComplementary(bool first_run);
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED);
static void magOffsetEstimation(MagSensorData *mag);
// check for invalid values
static inline bool invalid(float data)
{
if (isnan(data) || isinf(data)) {
return true;
}
return false;
}
// check for invalid variance values
static inline bool invalid_var(float data)
{
if (invalid(data)) {
return true;
}
if (data < 1e-15f) { // var should not be close to zero. And not negative either.
return true;
}
return false;
}
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
* Stores all the queues the algorithm will pull data from
* FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
* Update() -- queries queues and updates the attitude estiamte
*/
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeInitialize(void)
{
GyroSensorInitialize();
GyroStateInitialize();
AccelSensorInitialize();
AccelStateInitialize();
MagSensorInitialize();
MagStateInitialize();
AirspeedSensorInitialize();
AirspeedStateInitialize();
BaroSensorInitialize();
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
AttitudeSettingsInitialize();
AttitudeStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
// Initialize quaternion
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeStateSet(&attitude);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
/**
* Start the task. Expects all objects to be initialized by this point.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeStart(void)
{
// Create the queues for the sensors
gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent));
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
// Start main task
xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
#endif
GyroSensorConnectQueue(gyroQueue);
AccelSensorConnectQueue(accelQueue);
MagSensorConnectQueue(magQueue);
AirspeedSensorConnectQueue(airspeedQueue);
BaroSensorConnectQueue(baroQueue);
GPSPositionSensorConnectQueue(gpsQueue);
GPSVelocitySensorConnectQueue(gpsVelQueue);
return 0;
}
MODULE_INITCALL(AttitudeInitialize, AttitudeStart);
/**
* Module thread, should not return.
*/
static void AttitudeTask(__attribute__((unused)) void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(NULL);
// Wait for all the sensors be to read
vTaskDelay(100);
// Main task loop - TODO: make it run as delayed callback
while (1) {
int32_t ret_val = -1;
bool first_run = false;
if (initialization_required) {
initialization_required = false;
first_run = true;
}
// This function blocks on data queue
switch (running_algorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
ret_val = updateAttitudeINSGPS(first_run, true);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
ret_val = updateAttitudeINSGPS(first_run, false);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
if (ret_val != 0) {
initialization_required = true;
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
#endif
}
}
static inline void apply_accel_filter(const float *raw, float *filtered)
{
if (accel_filter_enabled) {
filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha);
filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha);
filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha);
} else {
filtered[0] = raw[0];
filtered[1] = raw[1];
filtered[2] = raw[2];
}
}
float accel_mag;
float qmag;
float attitudeDt;
float mag_err[3];
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyroSensorData gyroSensorData;
GyroStateData gyroStateData;
AccelSensorData accelSensorData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
static float gyro_bias[3] = { 0, 0, 0 };
static bool magCalibrated = true;
static uint32_t initStartupTime = 0;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) {
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelSensorGet(&accelSensorData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
// During initialization and
if (first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) {
return -1;
}
MagSensorData magData;
MagSensorGet(&magData);
#else
MagSensorData magData;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasArrayGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
magCalibrated = false;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
}
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
init = 0;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
timeval = PIOS_DELAY_GetRaw();
// wait calibration_delay only at powerup
if (xTaskGetTickCount() < 3000) {
initStartupTime = 0;
} else {
initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY;
}
// Zero gyro bias
// This is really needed after updating calibration settings.
gyro_bias[0] = 0.0f;
gyro_bias[1] = 0.0f;
gyro_bias[2] = 0.0f;
return 0;
}
if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) &&
(xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) {
// For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup
// Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate.
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
rollPitchBiasRate = 0.0f;
if (accel_alpha > 0.0f) {
accel_filter_enabled = true;
}
init = 1;
}
GyroSensorGet(&gyroSensorData);
gyroStateData.x = gyroSensorData.x;
gyroStateData.y = gyroSensorData.y;
gyroStateData.z = gyroSensorData.z;
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
float q[4];
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeState.q1, q);
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter((const float *)&accelSensorData.x, accels_filtered);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
apply_accel_filter(grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
accel_mag = accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2];
accel_mag = sqrtf(accel_mag);
float grot_mag;
if (accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
}
// TODO! check grot_mag & accel vector magnitude values for correctness.
accel_err[0] /= (accel_mag * grot_mag);
accel_err[1] /= (accel_mag * grot_mag);
accel_err[2] /= (accel_mag * grot_mag);
if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) {
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagSensorData mag;
Quaternion2R(q, Rbe);
MagSensorGet(&mag);
// TODO: separate filter!
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mag);
}
MagStateData mags;
mags.x = mag.x;
mags.y = mag.y;
mags.z = mag.z;
MagStateSet(&mags);
// If the mag is producing bad data don't use it (normally bad calibration)
if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) {
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1.0f || mag_len < 1.0f) {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
} else {
CrossProduct((const float *)&mag.x, (const float *)brot, mag_err);
}
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
// Correct rates based on integral coefficient
gyroStateData.x -= gyro_bias[0];
gyroStateData.y -= gyro_bias[1];
gyroStateData.z -= gyro_bias[2];
gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate;
gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate;
gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate;
// save gyroscope state
GyroStateSet(&gyroStateData);
// Correct rates based on proportional coefficient
gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if (q[0] < 0.0f) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1.0f;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 0.0f;
}
quat_copy(q, &attitudeState.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll);
AttitudeStateSet(&attitudeState);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = NED[0];
positionState.East = NED[1];
positionState.Down = NED[2];
PositionStateSet(&positionState);
}
if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) {
// Transform the GPS position into NED coordinates
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = gpsVelocity.North;
velocityState.East = gpsVelocity.East;
velocityState.Down = gpsVelocity.Down;
VelocityStateSet(&velocityState);
}
if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) {
// Calculate true airspeed from indicated airspeed
AirspeedSensorData airspeedSensor;
AirspeedSensorGet(&airspeedSensor);
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
PositionStateData positionState;
PositionStateGet(&positionState);
if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed);
}
}
if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (variance_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
return 0;
}
#include "insgps.h"
int32_t ins_failed = 0;
extern struct NavStruct Nav;
int32_t init_stage = 0;
/**
* @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS)
* @params[in] first_run This is the first run so trigger reinitialization
* @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)
* @return 0 for success, -1 for failure
*/
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
{
UAVObjEvent ev;
GyroSensorData gyroSensorData;
AccelSensorData accelSensorData;
MagStateData magData;
AirspeedSensorData airspeedData;
BaroSensorData baroData;
GPSPositionSensorData gpsData;
GPSVelocitySensorData gpsVelData;
static bool mag_updated = false;
static bool baro_updated;
static bool airspeed_updated;
static bool gps_updated;
static bool gps_vel_updated;
static bool value_error = false;
static float baroOffset = 0.0f;
static uint32_t ins_last_time = 0;
static bool inited;
float NED[3] = { 0.0f, 0.0f, 0.0f };
float vel[3] = { 0.0f, 0.0f, 0.0f };
float zeros[3] = { 0.0f, 0.0f, 0.0f };
// Perform the update
uint16_t sensors = 0;
float dT;
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
(xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) {
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
if (inited) {
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
}
if (first_run) {
inited = false;
init_stage = 0;
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
// Check if we are running simulation
if (!GPSPositionSensorReadOnly()) {
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_updated |= pdTRUE && outdoor_mode;
}
if (!GPSVelocitySensorReadOnly()) {
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_vel_updated |= pdTRUE && outdoor_mode;
}
// Get most recent data
GyroSensorGet(&gyroSensorData);
AccelSensorGet(&accelSensorData);
// TODO: separate filter!
if (mag_updated) {
MagSensorData mags;
MagSensorGet(&mags);
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mags);
}
magData.x = mags.x;
magData.y = mags.y;
magData.z = mags.z;
MagStateSet(&magData);
} else {
MagStateGet(&magData);
}
BaroSensorGet(&baroData);
AirspeedSensorGet(&airspeedData);
GPSPositionSensorGet(&gpsData);
GPSVelocitySensorGet(&gpsVelData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
value_error = false;
// safety checks
if (invalid(gyroSensorData.x) ||
invalid(gyroSensorData.y) ||
invalid(gyroSensorData.z) ||
invalid(accelSensorData.x) ||
invalid(accelSensorData.y) ||
invalid(accelSensorData.z)) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return 0;
}
if (invalid(magData.x) ||
invalid(magData.y) ||
invalid(magData.z)) {
// magnetometers can be ignored for a while
mag_updated = false;
value_error = true;
}
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) {
mag_updated = false;
value_error = true;
}
if (invalid(baroData.Altitude)) {
baro_updated = false;
value_error = true;
}
if (invalid(airspeedData.CalibratedAirspeed)) {
airspeed_updated = false;
value_error = true;
}
if (invalid(gpsData.Altitude)) {
gps_updated = false;
value_error = true;
}
if (invalid_var(ekfConfiguration.R.GPSPosNorth) ||
invalid_var(ekfConfiguration.R.GPSPosEast) ||
invalid_var(ekfConfiguration.R.GPSPosDown) ||
invalid_var(ekfConfiguration.R.GPSVelNorth) ||
invalid_var(ekfConfiguration.R.GPSVelEast) ||
invalid_var(ekfConfiguration.R.GPSVelDown)) {
gps_updated = false;
value_error = true;
}
if (invalid(gpsVelData.North) ||
invalid(gpsVelData.East) ||
invalid(gpsVelData.Down)) {
gps_vel_updated = false;
value_error = true;
}
// Discard airspeed if sensor not connected
if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
airspeed_updated = false;
}
// Have a minimum requirement for gps usage
if ((gpsData.Satellites < 7) ||
(gpsData.PDOP > 4.0f) ||
(gpsData.Latitude == 0 && gpsData.Longitude == 0) ||
(homeLocation.Set != HOMELOCATION_SET_TRUE)) {
gps_updated = false;
gps_vel_updated = false;
}
if (!inited) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (value_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (variance_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (outdoor_mode && gpsData.Satellites < 7) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
dT = 0.01f;
} else if (dT <= 0.001f) {
dT = 0.001f;
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) {
// Don't initialize until all sensors are read
if (init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar((float[3]) { ekfConfiguration.R.MagX,
ekfConfiguration.R.MagY,
ekfConfiguration.R.MagZ }
);
INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX,
ekfConfiguration.Q.AccelY,
ekfConfiguration.Q.AccelZ }
);
INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX,
ekfConfiguration.Q.GyroY,
ekfConfiguration.Q.GyroZ }
);
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX,
ekfConfiguration.Q.GyroDriftY,
ekfConfiguration.Q.GyroDriftZ }
);
INSSetBaroVar(ekfConfiguration.R.BaroZ);
// Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
INSSetGyroBias(gyro_bias);
float pos[3] = { 0.0f, 0.0f, 0.0f };
if (outdoor_mode) {
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, pos);
// Initialize barometric offset to current GPS NED coordinate
baroOffset = -pos[2] - baroData.Altitude;
} else {
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
}
// xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
// MagSensorGet(&magData);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 };
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1));
} else {
// Run prediction a bit before any corrections
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
INSStatePrediction(gyros, &accelSensorData.x, dT);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeStateSet(&attitude);
}
init_stage++;
if (init_stage > 10) {
inited = true;
}
return 0;
}
if (!inited) {
return 0;
}
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
// Advance the state estimate
INSStatePrediction(gyros, &accelSensorData.x, dT);
// Copy the attitude into the UAVO
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeStateSet(&attitude);
// Advance the covariance estimate
INSCovariancePrediction(dT);
if (mag_updated) {
sensors |= MAG_SENSORS;
}
if (baro_updated) {
sensors |= BARO_SENSOR;
}
INSSetMagNorth(homeLocation.Be);
if (gps_updated && outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.R.GPSPosNorth,
ekfConfiguration.R.GPSPosEast,
ekfConfiguration.R.GPSPosDown },
(float[3]) { ekfConfiguration.R.GPSVelNorth,
ekfConfiguration.R.GPSVelEast,
ekfConfiguration.R.GPSVelDown }
);
sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0.0f;
}
// Transform the GPS position into NED coordinates
getNED(&gpsData, NED);
// Track barometric altitude offset with a low pass filter
baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-NED[2] - baroData.Altitude);
} else if (!outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.FakeGPSVelIndoor }
);
vel[0] = vel[1] = vel[2] = 0.0f;
NED[0] = NED[1] = 0.0f;
NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS | VERT_SENSORS;
}
if (gps_vel_updated && outdoor_mode) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
vel[0] = gpsVelData.North;
vel[1] = gpsVelData.East;
vel[2] = gpsVelData.Down;
}
// Copy the position into the UAVO
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
// airspeed correction needs current positionState
if (airspeed_updated) {
// we have airspeed available
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed;
AirspeedStateSet(&airspeed);
if (!gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.FakeGPSVelAirspeed }
);
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q, R);
float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f };
rot_mult(R, vtas, vel);
}
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
if (sensors) {
INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors);
}
// Copy the velocity into the UAVO
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = Nav.Vel[0];
velocityState.East = Nav.Vel[1];
velocityState.Down = Nav.Vel[2];
VelocityStateSet(&velocityState);
GyroStateData gyroState;
gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0]));
gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1]));
gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2]));
GyroStateSet(&gyroState);
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1));
EKFStateVarianceSet(&vardata);
return 0;
}
/**
* @brief Convert the GPS LLA position into NED coordinates
* @note this method uses a taylor expansion around the home coordinates
* to convert to NED which allows it to be done with all floating
* calculations
* @param[in] Current GPS coordinates
* @param[out] NED frame coordinates
* @returns 0 for success, -1 for failure
*/
float T[3];
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED)
{
float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f),
DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f),
(gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) };
NED[0] = T[0] * dL[0];
NED[1] = T[1] * dL[1];
NED[2] = T[2] * dL[2];
return 0;
}
static void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev == NULL || ev->obj == FlightStatusHandle()) {
FlightStatusGet(&flightStatus);
}
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
}
// change of these settings require reinitialization of the EKF
// when an error flag has been risen, we also listen to flightStatus updates,
// since we are waiting for the system to get disarmed so we can reinitialize safely.
if (ev == NULL ||
ev->obj == EKFConfigurationHandle() ||
ev->obj == RevoSettingsHandle() ||
(variance_error == true && ev->obj == FlightStatusHandle())
) {
bool error = false;
EKFConfigurationGet(&ekfConfiguration);
int t;
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)[t])) {
error = true;
}
}
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.Q, ekfConfiguration.Q.AccelX)[t])) {
error = true;
}
}
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) {
error = true;
}
}
RevoSettingsGet(&revoSettings);
// Reinitialization of the EKF is not desired during flight.
// It will be delayed until the board is disarmed by raising the error flag.
// We will not prevent the initial initialization though, since the board could be in always armed mode.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) {
error = true;
}
if (error) {
variance_error = true;
} else {
// trigger reinitialization - possibly with new algorithm
running_algorithm = revoSettings.FusionAlgorithm;
variance_error = false;
initialization_required = true;
}
}
if (ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
// Compute matrix to convert deltaLLA to NED
float lat, alt;
lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
alt = homeLocation.Altitude;
T[0] = alt + 6.378137E6f;
T[1] = cosf(lat) * (alt + 6.378137E6f);
T[2] = -1.0f;
// TODO: convert positionState to new reference frame and gracefully update EKF state!
// needed for long range flights where the reference coordinate is adjusted in flight
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle()) {
AttitudeSettingsGet(&attitudeSettings);
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0015f;
if (attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false;
} else {
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
accel_filter_enabled = true;
}
}
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(MagSensorData *mag)
{
#if 0
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = { 0, 0, 0 };
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = { mag->x, mag->y, mag->z };
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else // if 0
static float magBias[3] = { 0 };
// Remove the current estimate of the bias
mag->x -= magBias[0];
mag->y -= magBias[1];
mag->z -= magBias[2];
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float rate = revoCalibration.MagBiasNullingRate;
float Rot[3][3];
float B_e[3];
float xy[2];
float delta[3];
// Get the rotation matrix
Quaternion2R(&attitude.q1, Rot);
// Rotate the mag into the NED frame
B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z;
B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z;
B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z;
float cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw));
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
magBias[0] += delta[0];
magBias[1] += delta[1];
magBias[2] += delta[2];
}
#endif // if 0
}
/**
* @}
* @}
*/

View File

@ -1,37 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Attitude Attitude Module
* @{
*
* @file attitude.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Acquires sensor data and fuses it into attitude estimate for CC
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ATTITUDE_H
#define ATTITUDE_H
#include "openpilot.h"
int32_t AttitudeInitialize(void);
#endif // ATTITUDE_H

View File

@ -64,7 +64,8 @@ static int32_t alt_ds_pres = 0;
static int alt_ds_count = 0;
#endif
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t mag_handle = 0;
int32_t mag_test;
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 1, 1, 1 };
@ -108,7 +109,7 @@ int32_t MagBaroInitialize()
#endif
if (magbaroEnabled) {
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorInitialize();
#endif
@ -127,15 +128,16 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart);
/**
* Module thread, should not return.
*/
#if defined(PIOS_INCLUDE_HMC5883)
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#ifdef PIOS_HMC5883_HAS_GPIOS
#if defined(PIOS_INCLUDE_HMC5X83)
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
#ifdef PIOS_HMC5X83_HAS_GPIOS
.exti_cfg = 0,
#endif
.M_ODR = PIOS_HMC5883_ODR_15,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
.M_ODR = PIOS_HMC5x83_ODR_15,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif
@ -148,9 +150,9 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
PIOS_BMP085_Init();
#endif
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
mag_handle = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0);
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
#endif
@ -197,10 +199,10 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
}
#endif /* if defined(PIOS_INCLUDE_BMP085) */
#if defined(PIOS_INCLUDE_HMC5883)
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
#if defined(PIOS_INCLUDE_HMC5X83)
if (PIOS_HMC5x83_NewDataAvailable(mag_handle) || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
PIOS_HMC5x83_ReadMag(mag_handle, values);
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0],
(float)values[0] * mag_scale[1] - mag_bias[1],
-(float)values[2] * mag_scale[2] - mag_bias[2] };

View File

@ -82,6 +82,11 @@ static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
AccelGyroSettingsData agcal;
#ifdef PIOS_INCLUDE_HMC5X83
#include <pios_hmc5x83.h>
extern pios_hmc5x83_dev_t onboard_mag;
#endif
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
@ -200,8 +205,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_DEBUG_Assert(0);
}
#if defined(PIOS_INCLUDE_HMC5883)
mag_test = PIOS_HMC5883_Test();
#if defined(PIOS_INCLUDE_HMC5X83)
mag_test = PIOS_HMC5x83_Test(onboard_mag);
#else
mag_test = 0;
#endif
@ -409,11 +414,11 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
PIOS_HMC5x83_ReadMag(onboard_mag, values);
float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] - mag_bias[1],
-(float)values[2] - mag_bias[2] };
@ -428,7 +433,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif /* if defined(PIOS_INCLUDE_HMC5883) */
#endif /* if defined(PIOS_INCLUDE_HMC5X83) */
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);

View File

@ -221,7 +221,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
// During initialization and
if (this->first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
// wait until mags have been updated
if (!this->magUpdated) {
return FILTERRESULT_ERROR;

View File

@ -1,425 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5883 HMC5883 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
* @file pios_hmc5883.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5883 Magnetic Sensor Functions from AHRS
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_HMC5883
/* Global Variables */
/* Local Types */
/* Local Variables */
volatile bool pios_hmc5883_data_ready;
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg);
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer);
static const struct pios_hmc5883_cfg *dev_cfg;
/**
* @brief Initialize the HMC5883 magnetometer sensor.
* @return none
*/
void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg)
{
dev_cfg = cfg; // store config before enabling interrupt
#ifdef PIOS_HMC5883_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
#endif
int32_t val = PIOS_HMC5883_Config(cfg);
PIOS_Assert(val == 0);
pios_hmc5883_data_ready = false;
}
/**
* @brief Initialize the HMC5883 magnetometer sensor
* \return none
* \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static uint8_t CTRLB = 0x00;
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg)
{
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
CTRLB = 0;
CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf);
CTRLB |= (uint8_t)(cfg->Gain);
MODE |= (uint8_t)(cfg->Mode);
// CRTL_REGA
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) {
return -1;
}
// CRTL_REGB
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) {
return -1;
}
// Mode register
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) {
return -1;
}
return 0;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \return 0 for success or -1 for failure
*/
int32_t PIOS_HMC5883_ReadMag(int16_t out[3])
{
pios_hmc5883_data_ready = false;
uint8_t buffer[6];
int32_t temp;
int32_t sensitivity;
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) {
return -1;
}
switch (CTRLB & 0xE0) {
case 0x00:
sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga;
break;
case 0x20:
sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga;
break;
case 0x40:
sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga;
break;
case 0x60:
sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga;
break;
case 0x80:
sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga;
break;
case 0xA0:
sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga;
break;
case 0xC0:
sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga;
break;
case 0xE0:
sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga;
break;
default:
PIOS_Assert(0);
}
for (int i = 0; i < 3; i++) {
temp = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / sensitivity;
out[i] = temp;
}
// Data reads out as X,Z,Y
temp = out[2];
out[2] = out[1];
out[1] = temp;
// This should not be necessary but for some reason it is coming out of continuous conversion mode
PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS);
return 0;
}
/**
* @brief Read the identification bytes from the HMC5883 sensor
* \param[out] uint8_t array of size 4 to store HMC5883 ID.
* \return 0 if successful, -1 if not
*/
uint8_t PIOS_HMC5883_ReadID(uint8_t out[4])
{
uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3);
out[3] = '\0';
return retval;
}
/**
* @brief Tells whether new magnetometer readings are available
* \return true if new data is available
* \return false if new data is not available
*/
bool PIOS_HMC5883_NewDataAvailable(void)
{
return pios_hmc5883_data_ready;
}
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5883 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len)
{
uint8_t addr_buffer[] = {
address,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Writes one or more bytes to the HMC5883
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer)
{
uint8_t data[] = {
address,
buffer,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(data),
.buf = data,
}
,
};
;
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Run self-test operation. Do not call this during operational use!!
* \return 0 if success, -1 if test failed
*/
int32_t PIOS_HMC5883_Test(void)
{
int32_t failed = 0;
uint8_t registers[3] = { 0, 0, 0 };
uint8_t status;
uint8_t ctrl_a_read;
uint8_t ctrl_b_read;
uint8_t mode_read;
int16_t values[3];
/* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */
char id[4];
PIOS_HMC5883_ReadID((uint8_t *)id);
if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43
return -1;
}
/* Backup existing configuration */
if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, registers, 3) != 0) {
return -1;
}
/* Stop the device and read out last value */
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) {
return -1;
}
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1) != 0) {
return -1;
}
if (PIOS_HMC5883_ReadMag(values) != 0) {
return -1;
}
/*
* Put HMC5883 into self test mode
* This is done by placing measurement config into positive (0x01) or negative (0x10) bias
* and then placing the mode register into single-measurement mode. This causes the HMC5883
* to create an artificial magnetic field of ~1.1 Gauss.
*
* If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB
* (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
*
* Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode.
*/
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) {
return -1;
}
/* Must wait for value to be updated */
PIOS_DELAY_WaitmS(200);
if (PIOS_HMC5883_ReadMag(values) != 0) {
return -1;
}
/*
if(abs(values[0] - 766) > 20)
failed |= 1;
if(abs(values[1] - 766) > 20)
failed |= 1;
if(abs(values[2] - 713) > 20)
failed |= 1;
*/
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1);
/* Restore backup configuration */
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) {
return -1;
}
return failed;
}
/**
* @brief IRQ Handler
*/
bool PIOS_HMC5883_IRQHandler(void)
{
pios_hmc5883_data_ready = true;
return false;
}
#endif /* PIOS_INCLUDE_HMC5883 */
/**
* @}
* @}
*/

View File

@ -0,0 +1,557 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5x83 HMC5x83 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
* @file pios_hmc5x83.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5x83 Magnetic Sensor Functions from AHRS
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#include <pios_hmc5x83.h>
#include <pios_mem.h>
#ifdef PIOS_INCLUDE_HMC5X83
#define PIOS_HMC5X83_MAGIC 0x4d783833
/* Global Variables */
/* Local Types */
typedef struct {
uint32_t magic;
const struct pios_hmc5x83_cfg *cfg;
uint32_t port_id;
uint8_t slave_num;
uint8_t CTRLB;
volatile bool data_ready;
} pios_hmc5x83_dev_data_t;
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
/**
* Allocate the device setting structure
* @return pios_hmc5x83_dev_data_t pointer to newly created structure
*/
pios_hmc5x83_dev_data_t *dev_alloc()
{
pios_hmc5x83_dev_data_t *dev = (pios_hmc5x83_dev_data_t *)pios_malloc(sizeof(pios_hmc5x83_dev_data_t));
PIOS_DEBUG_Assert(dev);
memset(dev, 0x00, sizeof(pios_hmc5x83_dev_data_t));
dev->magic = PIOS_HMC5X83_MAGIC;
return dev;
}
/**
* Validate a pios_hmc5x83_dev_t handler and return the related pios_hmc5x83_dev_data_t pointer
* @param dev device handler
* @return the device data structure
*/
pios_hmc5x83_dev_data_t *dev_validate(pios_hmc5x83_dev_t dev)
{
pios_hmc5x83_dev_data_t *dev_data = (pios_hmc5x83_dev_data_t *)dev;
PIOS_DEBUG_Assert(dev_data->magic == PIOS_HMC5X83_MAGIC);
return dev_data;
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor.
* @return none
*/
pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num)
{
pios_hmc5x83_dev_data_t *dev = dev_alloc();
dev->cfg = cfg; // store config before enabling interrupt
dev->port_id = port_id;
dev->slave_num = slave_num;
#ifdef PIOS_HMC5X83_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
#endif
int32_t val = PIOS_HMC5x83_Config(dev);
PIOS_Assert(val == 0);
dev->data_ready = false;
return (pios_hmc5x83_dev_t)dev;
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor
* \return none
* \param[in] pios_hmc5x83_dev_data_t device config to be used.
* \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
{
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
const struct pios_hmc5x83_cfg *cfg = dev->cfg;
dev->CTRLB = 0;
CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf);
CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0;
dev->CTRLB |= (uint8_t)(cfg->Gain);
MODE |= (uint8_t)(cfg->Mode);
// CRTL_REGA
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) {
return -1;
}
// CRTL_REGB
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_B, dev->CTRLB) != 0) {
return -1;
}
// Mode register
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_MODE_REG, MODE) != 0) {
return -1;
}
return 0;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[in] dev device handler
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \return 0 for success or -1 for failure
*/
int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
dev->data_ready = false;
uint8_t buffer[6];
int32_t temp;
int32_t sensitivity;
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) {
return -1;
}
switch (dev->CTRLB & 0xE0) {
case 0x00:
sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga;
break;
case 0x20:
sensitivity = PIOS_HMC5x83_Sensitivity_1_3Ga;
break;
case 0x40:
sensitivity = PIOS_HMC5x83_Sensitivity_1_9Ga;
break;
case 0x60:
sensitivity = PIOS_HMC5x83_Sensitivity_2_5Ga;
break;
case 0x80:
sensitivity = PIOS_HMC5x83_Sensitivity_4_0Ga;
break;
case 0xA0:
sensitivity = PIOS_HMC5x83_Sensitivity_4_7Ga;
break;
case 0xC0:
sensitivity = PIOS_HMC5x83_Sensitivity_5_6Ga;
break;
case 0xE0:
sensitivity = PIOS_HMC5x83_Sensitivity_8_1Ga;
break;
default:
PIOS_Assert(0);
}
for (int i = 0; i < 3; i++) {
temp = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / sensitivity;
out[i] = temp;
}
// Data reads out as X,Z,Y
temp = out[2];
out[2] = out[1];
out[1] = temp;
// This should not be necessary but for some reason it is coming out of continuous conversion mode
dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);
return 0;
}
/**
* @brief Read the identification bytes from the HMC5x83 sensor
* \param[out] uint8_t array of size 4 to store HMC5x83 ID.
* \return 0 if successful, -1 if not
*/
uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t retval = dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3);
out[3] = '\0';
return retval;
}
/**
* @brief Tells whether new magnetometer readings are available
* \return true if new data is available
* \return false if new data is not available
*/
bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
return dev->data_ready;
}
/**
* @brief Run self-test operation. Do not call this during operational use!!
* \return 0 if success, -1 if test failed
*/
int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler)
{
int32_t failed = 0;
uint8_t registers[3] = { 0, 0, 0 };
uint8_t status;
uint8_t ctrl_a_read;
uint8_t ctrl_b_read;
uint8_t mode_read;
int16_t values[3];
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
/* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */
char id[4];
PIOS_HMC5x83_ReadID(handler, (uint8_t *)id);
if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43
return -1;
}
/* Backup existing configuration */
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) {
return -1;
}
/* Stop the device and read out last value */
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) {
return -1;
}
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) {
return -1;
}
if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
return -1;
}
/*
* Put HMC5x83 into self test mode
* This is done by placing measurement config into positive (0x01) or negative (0x10) bias
* and then placing the mode register into single-measurement mode. This causes the HMC5x83
* to create an artificial magnetic field of ~1.1 Gauss.
*
* If gain were PIOS_HMC5x83_GAIN_2_5, for example, X and Y will read around +766 LSB
* (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
*
* Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode.
*/
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) {
return -1;
}
/* Must wait for value to be updated */
PIOS_DELAY_WaitmS(200);
if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
return -1;
}
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_MODE_REG, &mode_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1);
/* Restore backup configuration */
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, registers[2]) != 0) {
return -1;
}
return failed;
}
/**
* @brief IRQ Handler
*/
bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
dev->data_ready = true;
return false;
}
#ifdef PIOS_INCLUDE_SPI
int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = {
.Read = PIOS_HMC5x83_SPI_Read,
.Write = PIOS_HMC5x83_SPI_Write,
};
static int32_t pios_hmc5x83_spi_claim_bus(pios_hmc5x83_dev_data_t *dev)
{
if (PIOS_SPI_ClaimBus(dev->port_id) < 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0);
return 0;
}
static void pios_hmc5x83_spi_release_bus(pios_hmc5x83_dev_data_t *dev)
{
PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1);
PIOS_SPI_ReleaseBus(dev->port_id);
}
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5x83 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
return -1;
}
memset(buffer, 0xA5, len);
PIOS_SPI_TransferByte(dev->port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG);
// buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG;
/* Copy the transfer data to the buffer */
if (PIOS_SPI_TransferBlock(dev->port_id, NULL, buffer, len, NULL) < 0) {
pios_hmc5x83_spi_release_bus(dev);
return -3;
}
pios_hmc5x83_spi_release_bus(dev);
return 0;
}
/**
* @brief Writes one or more bytes to the HMC5x83
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim spi device
*/
int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
return -1;
}
uint8_t data[] = {
address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG,
buffer,
};
if (PIOS_SPI_TransferBlock(dev->port_id, data, NULL, sizeof(data), NULL) < 0) {
pios_hmc5x83_spi_release_bus(dev);
return -2;
}
pios_hmc5x83_spi_release_bus(dev);
return 0;
}
#endif /* PIOS_INCLUDE_SPI */
#ifdef PIOS_INCLUDE_I2C
int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = {
.Read = PIOS_HMC5x83_I2C_Read,
.Write = PIOS_HMC5x83_I2C_Write,
};
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5x83 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t addr_buffer[] = {
address,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Writes one or more bytes to the HMC5x83
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t data[] = {
address,
buffer,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(data),
.buf = data,
}
,
};
;
return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
}
#endif /* PIOS_INCLUDE_I2C */
#endif /* PIOS_INCLUDE_HMC5x83 */
/**
* @}
* @}
*/

View File

@ -1,116 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5883 HMC5883 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
*
* @file pios_hmc5883.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5883 functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_HMC5883_H
#define PIOS_HMC5883_H
/* HMC5883 Addresses */
#define PIOS_HMC5883_I2C_ADDR 0x1E
#define PIOS_HMC5883_I2C_READ_ADDR 0x3D
#define PIOS_HMC5883_I2C_WRITE_ADDR 0x3C
#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00
#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01
#define PIOS_HMC5883_MODE_REG (uint8_t)0x02
#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03
#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04
#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05
#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06
#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07
#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08
#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09
#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A
#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B
#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C
/* Output Data Rate */
#define PIOS_HMC5883_ODR_0_75 0x00
#define PIOS_HMC5883_ODR_1_5 0x04
#define PIOS_HMC5883_ODR_3 0x08
#define PIOS_HMC5883_ODR_7_5 0x0C
#define PIOS_HMC5883_ODR_15 0x10
#define PIOS_HMC5883_ODR_30 0x14
#define PIOS_HMC5883_ODR_75 0x18
/* Measure configuration */
#define PIOS_HMC5883_MEASCONF_NORMAL 0x00
#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01
#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02
/* Gain settings */
#define PIOS_HMC5883_GAIN_0_88 0x00
#define PIOS_HMC5883_GAIN_1_3 0x20
#define PIOS_HMC5883_GAIN_1_9 0x40
#define PIOS_HMC5883_GAIN_2_5 0x60
#define PIOS_HMC5883_GAIN_4_0 0x80
#define PIOS_HMC5883_GAIN_4_7 0xA0
#define PIOS_HMC5883_GAIN_5_6 0xC0
#define PIOS_HMC5883_GAIN_8_1 0xE0
/* Modes */
#define PIOS_HMC5883_MODE_CONTINUOUS 0x00
#define PIOS_HMC5883_MODE_SINGLE 0x01
#define PIOS_HMC5883_MODE_IDLE 0x02
#define PIOS_HMC5883_MODE_SLEEP 0x03
/* Sensitivity Conversion Values */
#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED
struct pios_hmc5883_cfg {
#ifdef PIOS_HMC5883_HAS_GPIOS
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
#endif
uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */
uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Mode;
};
/* Public Functions */
extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg);
extern bool PIOS_HMC5883_NewDataAvailable(void);
extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]);
extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]);
extern int32_t PIOS_HMC5883_Test(void);
extern bool PIOS_HMC5883_IRQHandler();
#endif /* PIOS_HMC5883_H */
/**
* @}
* @}
*/

View File

@ -0,0 +1,137 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5x83 HMC5x83 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
*
* @file pios_hmc5x83.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5x83 functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_HMC5x83_H
#define PIOS_HMC5x83_H
#include <stdint.h>
/* HMC5x83 Addresses */
#define PIOS_HMC5x83_I2C_ADDR 0x1E
#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D
#define PIOS_HMC5x83_I2C_WRITE_ADDR 0x3C
#define PIOS_HMC5x83_SPI_READ_FLAG 0x80
#define PIOS_HMC5x83_SPI_AUTOINCR_FLAG 0x40
#define PIOS_HMC5x83_CONFIG_REG_A (uint8_t)0x00
#define PIOS_HMC5x83_CONFIG_REG_B (uint8_t)0x01
#define PIOS_HMC5x83_MODE_REG (uint8_t)0x02
#define PIOS_HMC5x83_DATAOUT_XMSB_REG 0x03
#define PIOS_HMC5x83_DATAOUT_XLSB_REG 0x04
#define PIOS_HMC5x83_DATAOUT_ZMSB_REG 0x05
#define PIOS_HMC5x83_DATAOUT_ZLSB_REG 0x06
#define PIOS_HMC5x83_DATAOUT_YMSB_REG 0x07
#define PIOS_HMC5x83_DATAOUT_YLSB_REG 0x08
#define PIOS_HMC5x83_DATAOUT_STATUS_REG 0x09
#define PIOS_HMC5x83_DATAOUT_IDA_REG 0x0A
#define PIOS_HMC5x83_DATAOUT_IDB_REG 0x0B
#define PIOS_HMC5x83_DATAOUT_IDC_REG 0x0C
/* Output Data Rate */
#define PIOS_HMC5x83_ODR_0_75 0x00
#define PIOS_HMC5x83_ODR_1_5 0x04
#define PIOS_HMC5x83_ODR_3 0x08
#define PIOS_HMC5x83_ODR_7_5 0x0C
#define PIOS_HMC5x83_ODR_15 0x10
#define PIOS_HMC5x83_ODR_30 0x14
#define PIOS_HMC5x83_ODR_75 0x18
/* Measure configuration */
#define PIOS_HMC5x83_MEASCONF_NORMAL 0x00
#define PIOS_HMC5x83_MEASCONF_BIAS_POS 0x01
#define PIOS_HMC5x83_MEASCONF_BIAS_NEG 0x02
/* Gain settings */
#define PIOS_HMC5x83_GAIN_0_88 0x00
#define PIOS_HMC5x83_GAIN_1_3 0x20
#define PIOS_HMC5x83_GAIN_1_9 0x40
#define PIOS_HMC5x83_GAIN_2_5 0x60
#define PIOS_HMC5x83_GAIN_4_0 0x80
#define PIOS_HMC5x83_GAIN_4_7 0xA0
#define PIOS_HMC5x83_GAIN_5_6 0xC0
#define PIOS_HMC5x83_GAIN_8_1 0xE0
#define PIOS_HMC5x83_CTRLA_TEMP 0x40
/* Modes */
#define PIOS_HMC5x83_MODE_CONTINUOUS 0x00
#define PIOS_HMC5x83_MODE_SINGLE 0x01
#define PIOS_HMC5x83_MODE_IDLE 0x02
#define PIOS_HMC5x83_MODE_SLEEP 0x03
/* Sensitivity Conversion Values */
#define PIOS_HMC5x83_Sensitivity_0_88Ga 1370 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_1_3Ga 1090 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_1_9Ga 820 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_2_5Ga 660 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_4_0Ga 440 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_4_7Ga 390 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_5_6Ga 330 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED
typedef uintptr_t pios_hmc5x83_dev_t;
struct pios_hmc5x83_io_driver {
int32_t (*Write)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
int32_t (*Read)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
};
#ifdef PIOS_INCLUDE_SPI
extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER;
#endif
#ifdef PIOS_INCLUDE_I2C
extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER;
#endif
struct pios_hmc5x83_cfg {
#ifdef PIOS_HMC5X83_HAS_GPIOS
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
#endif
uint8_t M_ODR; // OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Meas_Conf; // Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */
uint8_t Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Mode;
bool TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation
const struct pios_hmc5x83_io_driver *Driver;
};
/* Public Functions */
extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num);
extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler);
extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]);
extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]);
extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler);
extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler);
#endif /* PIOS_HMC5x83_H */
/**
* @}
* @}
*/

View File

@ -204,10 +204,10 @@
#include <pios_hmc5843.h>
#endif
#ifdef PIOS_INCLUDE_HMC5883
/* HMC5883 3-Axis Digital Compass */
/* #define PIOS_HMC5883_HAS_GPIOS */
#include <pios_hmc5883.h>
#ifdef PIOS_INCLUDE_HMC5X83
/* HMC5883/HMC5983 3-Axis Digital Compass */
/* #define PIOS_HMC5x83_HAS_GPIOS */
#include <pios_hmc5x83.h>
#endif
#ifdef PIOS_INCLUDE_BMP085

View File

@ -84,7 +84,7 @@
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
/* #define PIOS_INCLUDE_HMC5883 */
/* #define PIOS_INCLUDE_HMC5X83 */
/* #define PIOS_HMC5883_HAS_GPIOS */
/* #define PIOS_INCLUDE_BMP085 */
/* #define PIOS_INCLUDE_MS5611 */

View File

@ -84,8 +84,8 @@
// #define PIOS_INCLUDE_MPU6000
// #define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
// #define PIOS_INCLUDE_HMC5883
// #define PIOS_HMC5883_HAS_GPIOS
// #define PIOS_INCLUDE_HMC5X83
// #define PIOS_HMC5X83_HAS_GPIOS
/* #define PIOS_INCLUDE_BMP085 */
// #define PIOS_INCLUDE_MS5611
// #define PIOS_INCLUDE_MPXV

View File

@ -90,10 +90,10 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5883)
#include "pios_hmc5883.h"
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
.vector = PIOS_HMC5883_IRQHandler,
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = PIOS_HMC5x83_IRQHandler,
.line = EXTI_Line7,
.pin = {
.gpio = GPIOB,
@ -123,14 +123,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
},
};
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.exti_cfg = &pios_exti_hmc5883_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif /* PIOS_INCLUDE_HMC5883 */
#endif /* PIOS_INCLUDE_HMC5X83 */
/**
* Configuration for the MS5611 chip
@ -929,8 +930,8 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#if defined(PIOS_INCLUDE_HMC5X83)
PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
#endif
#if defined(PIOS_INCLUDE_MS5611)

View File

@ -81,7 +81,7 @@
/* #define PIOS_INCLUDE_MPU6000 */
/* #define PIOS_MPU6000_ACCEL */
/* #define PIOS_INCLUDE_HMC5843 */
/* #define PIOS_INCLUDE_HMC5883 */
/* #define PIOS_INCLUDE_HMC5X83 */
/* #define PIOS_HMC5883_HAS_GPIOS */
/* #define PIOS_INCLUDE_BMP085 */
/* #define PIOS_INCLUDE_MS5611 */

View File

@ -81,8 +81,8 @@
/* #define PIOS_INCLUDE_MPU6000 */
/* #define PIOS_MPU6000_ACCEL */
/* #define PIOS_INCLUDE_HMC5843 */
#define PIOS_INCLUDE_HMC5883
/* #define PIOS_HMC5883_HAS_GPIOS */
#define PIOS_INCLUDE_HMC5X83
/* #define PIOS_HMC5X83_HAS_GPIOS */
#define PIOS_INCLUDE_BMP085
/* #define PIOS_INCLUDE_MS5611 */
/* #define PIOS_INCLUDE_MPXV */

View File

@ -84,8 +84,8 @@
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
#define PIOS_INCLUDE_HMC5883
#define PIOS_HMC5883_HAS_GPIOS
#define PIOS_INCLUDE_HMC5X83
#define PIOS_HMC5X83_HAS_GPIOS
/* #define PIOS_INCLUDE_BMP085 */
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_MPXV

View File

@ -57,6 +57,7 @@
*/
#if defined(PIOS_INCLUDE_ADC)
#include "pios_adc_priv.h"
void PIOS_ADC_DMC_irq_handler(void);
void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler")));
@ -91,10 +92,16 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5883)
#include "pios_hmc5883.h"
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
.vector = PIOS_HMC5883_IRQHandler,
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0;
bool pios_board_internal_mag_handler()
{
return PIOS_HMC5x83_IRQHandler(onboard_mag);
}
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = pios_board_internal_mag_handler,
.line = EXTI_Line7,
.pin = {
.gpio = GPIOB,
@ -124,14 +131,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
},
};
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.exti_cfg = &pios_exti_hmc5883_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif /* PIOS_INCLUDE_HMC5883 */
#endif /* PIOS_INCLUDE_HMC5X83 */
/**
* Configuration for the MS5611 chip
@ -944,8 +952,8 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
#endif
#if defined(PIOS_INCLUDE_MS5611)

View File

@ -81,8 +81,8 @@
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
#define PIOS_INCLUDE_HMC5883
#define PIOS_HMC5883_HAS_GPIOS
#define PIOS_INCLUDE_HMC5X83
#define PIOS_HMC5X83_HAS_GPIOS
/* #define PIOS_INCLUDE_BMP085 */
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_MPXV

View File

@ -81,10 +81,16 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5883)
#include "pios_hmc5883.h"
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
.vector = PIOS_HMC5883_IRQHandler,
#if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t onboard_mag = 0;
bool pios_board_internal_mag_handler()
{
return PIOS_HMC5x83_IRQHandler(onboard_mag);
}
#include "pios_hmc5x83.h"
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = pios_board_internal_mag_handler,
.line = EXTI_Line5,
.pin = {
.gpio = GPIOB,
@ -114,14 +120,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
},
};
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.exti_cfg = &pios_exti_hmc5883_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif /* PIOS_INCLUDE_HMC5883 */
#endif /* PIOS_INCLUDE_HMC5X83 */
/**
* Configuration for the MS5611 chip
@ -938,8 +945,8 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0);
#endif
#if defined(PIOS_INCLUDE_MS5611)

View File

@ -63,7 +63,7 @@
/* Select the sensors to include */
// #define PIOS_INCLUDE_BMA180
// #define PIOS_INCLUDE_HMC5883
// #define PIOS_INCLUDE_HMC5X83
// #define PIOS_INCLUDE_MPU6000
// #define PIOS_MPU6000_ACCEL
// #define PIOS_INCLUDE_L3GD20

View File

@ -64,7 +64,7 @@ SRC += $(PIOSCOMMON)/pios_etasv3.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_hmc5843.c
SRC += $(PIOSCOMMON)/pios_hmc5883.c
SRC += $(PIOSCOMMON)/pios_hmc5x83.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_l3gd20.c
SRC += $(PIOSCOMMON)/pios_mpu6000.c