rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-73 External Mags on I2C initial coding

Browse Source
This commit is contained in:
Cliff Geerdes 2015-09-12 18:16:27 -04:00
parent 06517dcd0b
commit d4b0d106f8
8 changed files with 522 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
flight/libraries/auxmagsupport.c
View File

@ -27,6 +27,30 @@
#include <stdint.h>
#include "inc/auxmagsupport.h"
#include "CoordinateConversions.h"
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
#endif
#define assumptions \
( \
((int) PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP == \
(int) AUXMAGSETTINGS_ORIENTATION_EAST_NORTH_UP) && \
((int) PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP == \
(int) AUXMAGSETTINGS_ORIENTATION_SOUTH_EAST_UP) && \
((int) PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP == \
(int) AUXMAGSETTINGS_ORIENTATION_WEST_SOUTH_UP) && \
((int) PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP == \
(int) AUXMAGSETTINGS_ORIENTATION_NORTH_WEST_UP) && \
((int) PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN == \
(int) AUXMAGSETTINGS_ORIENTATION_EAST_SOUTH_DOWN) && \
((int) PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN == \
(int) AUXMAGSETTINGS_ORIENTATION_SOUTH_WEST_DOWN) && \
((int) PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN == \
(int) AUXMAGSETTINGS_ORIENTATION_WEST_NORTH_DOWN) && \
((int) PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN == \
(int) AUXMAGSETTINGS_ORIENTATION_NORTH_EAST_DOWN) && \
((int) PIOS_HMC5X83_ORIENTATION_UNCHANGED == \
(int) AUXMAGSETTINGS_ORIENTATION_UNCHANGED) )
static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
@ -38,14 +62,11 @@ AuxMagSettingsTypeOptions option;
void auxmagsupport_reload_settings()
{
AuxMagSettingsTypeGet(&option);
float a[3][3];
float b[3][3];
float rotz;
AuxMagSettingsmag_transformArrayGet((float *)a);
AuxMagSettingsOrientationGet(&rotz);
rotz = DEG2RAD(rotz);
rot_about_axis_z(rotz, b);
matrix_mult_3x3f(a, b, mag_transform);
AuxMagSettingsmag_transformArrayGet((float *)mag_transform);
AuxMagSettingsOrientationOptions orientation;
AuxMagSettingsOrientationGet(&orientation);
PIOS_STATIC_ASSERT(assumptions);
PIOS_HMC5x83_Ext_Orientation_Set((enum PIOS_HMC5X83_ORIENTATION) orientation);
AuxMagSettingsmag_biasArrayGet(mag_bias);
}

105
flight/modules/Sensors/sensors.c
View File

@ -57,6 +57,8 @@
#include <attitudesettings.h>
#include <revocalibration.h>
#include <auxmagsettings.h>
#include <auxmagsensor.h>
#include <accelgyrosettings.h>
#include <revosettings.h>
@ -81,7 +83,8 @@
#define REGISTER_WDG()
#endif
static const TickType_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS;
static const TickType_t sensor_period_ticks = ((uint32_t) (1000.0f / PIOS_SENSOR_RATE / (float) portTICK_RATE_MS));
#define AUX_MAG_SKIP ((int) ((((PIOS_SENSOR_RATE < 76) ? 76 : PIOS_SENSOR_RATE) + 74) / 75)) /* (AMS at least 2) 75 is mag ODR output data rate in pios_board.c */
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30
@ -99,8 +102,8 @@ static const float temp_alpha_gyro_accel = LPF_ALPHA(TEMP_DT_GYRO_ACCEL, TEMP_LP
#define TEMP_LPF_FC_BARO 5.0f
static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC_BARO));
#define ZERO_ROT_ANGLE 0.00001f
// Private types
typedef struct {
// used to accumulate all samples in a task iteration
@ -138,6 +141,7 @@ static void clearContext(sensor_fetch_context *sensor_context);
static void handleAccel(float *samples, float temperature);
static void handleGyro(float *samples, float temperature);
static void handleMag(float *samples, float temperature);
static void handleAuxMag(float *samples);
static void handleBaro(float sample, float temperature);
static void updateAccelTempBias(float temperature);
@ -148,14 +152,17 @@ static void updateBaroTempBias(float temperature);
static sensor_data *source_data;
static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
AuxMagSettingsData auxmagcal;
AccelGyroSettingsData agcal;
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
static float auxmag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
// Variables used to handle accel/gyro temperature bias
static volatile bool gyro_temp_calibrated = false;
@ -168,9 +175,11 @@ static float gyro_temp_bias[3] = { 0 };
static uint8_t accel_temp_calibration_count = 0;
static uint8_t gyro_temp_calibration_count = 0;
// The user specified "Rotate virtual attitude relative to board"
static float R[3][3] = {
{ 0 }
};
// Variables used to handle baro temperature bias
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
@ -179,6 +188,8 @@ static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t baro_temp_calibration_count = 0;
// this is set, but not used
// it was intended to be a flag to avoid rotation calculation if the rotation was zero
static int8_t rotate = 0;
/**
@ -193,6 +204,7 @@ int32_t SensorsInitialize(void)
MagSensorInitialize();
BaroSensorInitialize();
RevoCalibrationInitialize();
AuxMagSettingsInitialize();
RevoSettingsInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
@ -201,6 +213,7 @@ int32_t SensorsInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AuxMagSettingsConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
@ -242,6 +255,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
bool error = false;
const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList();
PIOS_SENSORS_Instance *sensor;
uint8_t aux_mag_skip = 0;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
settingsUpdatedCb(NULL);
@ -258,6 +272,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
bool sensors_test = true;
uint8_t count = 0;
LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
RELOAD_WDG(); // mag tests on I2C have 200+(7x10)ms delay calls in them
sensors_test &= PIOS_SENSORS_Test(sensor);
count++;
}
@ -290,36 +305,39 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
// reset the fetch context
clearContext(&sensor_context);
aux_mag_skip = (aux_mag_skip + 1) % AUX_MAG_SKIP;
LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
// we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
if (!sensor->driver->is_polled) {
const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
while (xQueueReceive(queue,
(void *)source_data,
(is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
accumulateSamples(&sensor_context, source_data);
}
if (sensor_context.count) {
processSamples3d(&sensor_context, sensor);
clearContext(&sensor_context);
} else if (is_primary) {
PIOS_SENSOR_Reset(sensor);
reset_counter++;
PERF_TRACK_VALUE(counterSensorResets, reset_counter);
error = true;
}
} else {
if (PIOS_SENSORS_Poll(sensor)) {
PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
if (sensor->type & PIOS_SENSORS_TYPE_3D) {
if (sensor->type != PIOS_SENSORS_TYPE_3AXIS_AUXMAG || aux_mag_skip == 0) {
if (!sensor->driver->is_polled) {
const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
while (xQueueReceive(queue,
(void *)source_data,
(is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
accumulateSamples(&sensor_context, source_data);
processSamples3d(&sensor_context, sensor);
} else {
processSamples1d(&source_data->sensorSample1Axis, sensor);
}
clearContext(&sensor_context);
if (sensor_context.count) {
processSamples3d(&sensor_context, sensor);
clearContext(&sensor_context);
} else if (is_primary) {
PIOS_SENSOR_Reset(sensor);
reset_counter++;
PERF_TRACK_VALUE(counterSensorResets, reset_counter);
error = true;
}
} else {
if (PIOS_SENSORS_Poll(sensor)) {
PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
if (sensor->type & PIOS_SENSORS_TYPE_3D) {
accumulateSamples(&sensor_context, source_data);
processSamples3d(&sensor_context, sensor);
} else {
processSamples1d(&source_data->sensorSample1Axis, sensor);
}
clearContext(&sensor_context);
}
}
}
}
@ -361,20 +379,27 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE
PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE);
float inv_count = 1.0f / (float)sensor_context->count;
if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) ||
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)) {
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) ||
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG)) {
float t = inv_count * scales[0];
samples[0] = ((float)sensor_context->accum[0].x * t);
samples[1] = ((float)sensor_context->accum[0].y * t);
samples[2] = ((float)sensor_context->accum[0].z * t);
temperature = (float)sensor_context->temperature * inv_count * 0.01f;
if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) {
switch (sensor->type) {
case PIOS_SENSORS_TYPE_3AXIS_MAG:
handleMag(samples, temperature);
PERF_MEASURE_PERIOD(counterMagPeriod);
return;
} else {
case PIOS_SENSORS_TYPE_3AXIS_AUXMAG:
handleAuxMag(samples);
PERF_MEASURE_PERIOD(counterMagPeriod);
return;
default:
PERF_TRACK_VALUE(counterAccelSamples, sensor_context->count);
PERF_MEASURE_PERIOD(counterAccelPeriod);
handleAccel(samples, temperature);
break;
}
}
@ -458,6 +483,23 @@ static void handleMag(float *samples, float temperature)
MagSensorSet(&mag);
}
static void handleAuxMag(float *samples)
{
AuxMagSensorData mag;
float mags[3] = { (float)samples[0] - mag_bias[0],
(float)samples[1] - mag_bias[1],
(float)samples[2] - mag_bias[2] };
rot_mult(auxmag_transform, mags, samples);
mag.x = samples[0];
mag.y = samples[1];
mag.z = samples[2];
mag.Status = AUXMAGSENSOR_STATUS_OK;
AuxMagSensorSet(&mag);
}
static void handleBaro(float sample, float temperature)
{
updateBaroTempBias(temperature);
@ -535,12 +577,14 @@ static void updateBaroTempBias(float temperature)
}
baro_temp_calibration_count--;
}
/**
* Locally cache some variables from the AtttitudeSettings object
*/
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
RevoCalibrationGet(&cal);
AuxMagSettingsGet(&auxmagcal);
AccelGyroSettingsGet(&agcal);
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
@ -588,6 +632,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
Quaternion2R(rotationQuat, R);
}
matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
matrix_mult_3x3f((float(*)[3])AuxMagSettingsmag_transformToArray(auxmagcal.mag_transform), R, auxmag_transform);
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);

156
flight/pios/common/pios_hmc5x83.c
View File

@ -42,6 +42,7 @@
typedef struct {
uint32_t magic;
const struct pios_hmc5x83_cfg *cfg;
enum PIOS_HMC5X83_ORIENTATION Orientation;
uint32_t port_id;
uint8_t slave_num;
uint8_t CTRLB;
@ -66,6 +67,7 @@ const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver = {
.get_scale = PIOS_HMC5x83_driver_get_scale,
.is_polled = true,
};
/**
* Allocate the device setting structure
* @return pios_hmc5x83_dev_data_t pointer to newly created structure
@ -101,11 +103,15 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
{
pios_hmc5x83_dev_data_t *dev = dev_alloc();
dev->cfg = cfg; // store config before enabling interrupt
dev->cfg = cfg; // store config before enabling interrupt
dev->port_id = port_id;
dev->slave_num = slave_num;
dev->Orientation = cfg->Orientation; // make a read/write copy so we can update it at run time.
#ifdef PIOS_HMC5X83_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
if (cfg->exti_cfg) {
PIOS_EXTI_Init(cfg->exti_cfg);
}
#endif
int32_t val = PIOS_HMC5x83_Config(dev);
@ -115,9 +121,9 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
return (pios_hmc5x83_dev_t)dev;
}
void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler)
void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler, PIOS_SENSORS_TYPE sensortype)
{
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, handler);
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, sensortype, handler);
}
/**
@ -212,6 +218,64 @@ static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
return 0;
}
void PIOS_HMC5x83_Ext_Orientation_Set(enum PIOS_HMC5X83_ORIENTATION orientation)
{
if (external_mag) {
((pios_hmc5x83_dev_data_t *) external_mag)->Orientation = orientation;
}
}
void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3], int16_t out[3])
{
switch (orientation) {
case PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP:
out[0] = in[2];
out[1] = in[0];
out[2] = -in[1];
break;
case PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP:
out[0] = -in[0];
out[1] = in[2];
out[2] = -in[1];
break;
case PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP:
out[0] = -in[2];
out[1] = -in[0];
out[2] = -in[1];
break;
case PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP:
out[0] = in[0];
out[1] = -in[2];
out[2] = -in[1];
break;
case PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN:
out[0] = in[2];
out[1] = -in[0];
out[2] = in[1];
break;
case PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN:
out[0] = -in[0];
out[1] = -in[2];
out[2] = in[1];
break;
case PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN:
out[0] = -in[2];
out[1] = in[0];
out[2] = in[1];
break;
case PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN:
out[0] = in[0];
out[1] = in[2];
out[2] = in[1];
break;
case PIOS_HMC5X83_ORIENTATION_UNCHANGED:
out[0] = in[0]; // N
out[1] = in[1]; // D
out[2] = in[2]; // E
break;
}
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[in] dev device handler
@ -265,50 +329,14 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
temp[i] = v;
}
switch (dev->cfg->Orientation) {
case PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP:
out[0] = temp[2];
out[1] = temp[0];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP:
out[0] = -temp[0];
out[1] = temp[2];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP:
out[0] = -temp[2];
out[1] = -temp[0];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP:
out[0] = temp[0];
out[1] = -temp[2];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN:
out[0] = temp[2];
out[1] = -temp[0];
out[2] = temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN:
out[0] = -temp[0];
out[1] = -temp[2];
out[2] = temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN:
out[0] = -temp[2];
out[1] = temp[0];
out[2] = temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN:
out[0] = temp[0];
out[1] = temp[2];
out[2] = temp[1];
break;
}
PIOS_HMC5x83_Orient(dev->Orientation, temp, out);
// This should not be necessary but for some reason it is coming out of continuous conversion mode
// "This should not be necessary but for some reason it is coming out of continuous conversion mode"
//
// By default the chip is in single read mode meaning after reading from it once, it will go idle to save power.
// Once idle, we have write to it to turn it on before we can read from it again.
// To conserve current between measurements, the device is placed in a state similar to idle mode, but the
// Mode Register is not changed to Idle Mode. That is, MD[n] bits are unchanged.
dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);
return 0;
@ -329,16 +357,40 @@ uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
return retval;
}
// define this to simply return true when asking if data is available on non-GPIO devices
// we just set the polling rate elsewhere
// this is more efficient, but has more data time lag
#define HMC5X83_POLLED_STATUS_RETURNS_TRUE
/**
* @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)
bool PIOS_HMC5x83_NewDataAvailable(__attribute__((unused)) pios_hmc5x83_dev_t handler)
{
#if ( defined(PIOS_HMC5X83_HAS_GPIOS) || !defined(HMC5X83_POLLED_STATUS_RETURNS_TRUE) )
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
#endif
return dev->data_ready;
#ifdef PIOS_HMC5X83_HAS_GPIOS
if (dev->cfg->exti_cfg) { // if this device has an interrupt line attached, then wait for interrupt to say data is ready
return dev->data_ready;
}
else
#endif /* PIOS_HMC5X83_HAS_GPIOS */
{ // else poll to see if data is ready or just say "true" and set polling interval elsewhere
#ifdef HMC5X83_POLLED_STATUS_RETURNS_TRUE
return true;
#else
// poll SR0 (RDY) here. 1 -> data ready.
uint8_t rdy;
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &rdy, 1) != 0) {
return false;
}
return ((rdy & PIOS_HMC5x83_DATAOUT_STATUS_RDY) != 0);
#endif /* POLLED_STATUS_RETURNS_TRUE */
}
}
/**
@ -435,16 +487,18 @@ int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler)
return failed;
}
#ifdef PIOS_HMC5X83_HAS_GPIOS
/**
* @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;
}
#endif /* PIOS_HMC5X83_HAS_GPIOS */
#ifdef PIOS_INCLUDE_SPI
int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
@ -529,8 +583,8 @@ int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint
return 0;
}
#endif /* PIOS_INCLUDE_SPI */
#ifdef PIOS_INCLUDE_I2C
#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);

9
flight/pios/inc/pios_hmc5x83.h
View File

@ -95,7 +95,11 @@
#define PIOS_HMC5x83_Sensitivity_5_6Ga 330 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED
/* Status Register */
#define PIOS_HMC5x83_DATAOUT_STATUS_RDY 0x01
typedef uintptr_t pios_hmc5x83_dev_t;
extern pios_hmc5x83_dev_t external_mag;
struct pios_hmc5x83_io_driver {
int32_t (*Write)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
@ -119,6 +123,7 @@ enum PIOS_HMC5X83_ORIENTATION {
PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN,
PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN,
PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN,
PIOS_HMC5X83_ORIENTATION_UNCHANGED,
};
@ -137,8 +142,10 @@ struct pios_hmc5x83_cfg {
/* 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 void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler);
extern void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler, PIOS_SENSORS_TYPE sensortype);
extern void PIOS_HMC5x83_Ext_Orientation_Set(enum PIOS_HMC5X83_ORIENTATION orientation);
extern void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3], int16_t out[3]);
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]);

11
flight/pios/inc/pios_sensors.h
View File

@ -54,15 +54,16 @@ typedef struct PIOS_SENSORS_Driver {
} PIOS_SENSORS_Driver;
typedef enum PIOS_SENSORS_TYPE {
PIOS_SENSORS_TYPE_3AXIS_ACCEL = 0x01,
PIOS_SENSORS_TYPE_3AXIS_GYRO = 0x02,
PIOS_SENSORS_TYPE_3AXIS_ACCEL = 0x01,
PIOS_SENSORS_TYPE_3AXIS_GYRO = 0x02,
PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL = 0x03,
PIOS_SENSORS_TYPE_3AXIS_MAG = 0x04,
PIOS_SENSORS_TYPE_1AXIS_BARO = 0x08,
PIOS_SENSORS_TYPE_3AXIS_MAG = 0x04,
PIOS_SENSORS_TYPE_3AXIS_AUXMAG = 0x08,
PIOS_SENSORS_TYPE_1AXIS_BARO = 0x10,
} PIOS_SENSORS_TYPE;
#define PIOS_SENSORS_TYPE_1D (PIOS_SENSORS_TYPE_1AXIS_BARO)
#define PIOS_SENSORS_TYPE_3D (PIOS_SENSORS_TYPE_3AXIS_ACCEL | PIOS_SENSORS_TYPE_3AXIS_GYRO | PIOS_SENSORS_TYPE_3AXIS_MAG)
#define PIOS_SENSORS_TYPE_3D (PIOS_SENSORS_TYPE_3AXIS_ACCEL | PIOS_SENSORS_TYPE_3AXIS_GYRO | PIOS_SENSORS_TYPE_3AXIS_MAG | PIOS_SENSORS_TYPE_3AXIS_AUXMAG)
typedef struct PIOS_SENSORS_Instance {
const PIOS_SENSORS_Driver *driver;

50
flight/pios/stm32f4xx/pios_i2c.c
View File

@ -390,6 +390,7 @@ static const struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM
},
};
static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter)
{
#if defined(I2C_HALT_ON_ERRORS)
@ -401,6 +402,7 @@ static void go_fsm_fault(struct pios_i2c_adapter *i2c_adapter)
i2c_adapter_reset_bus(i2c_adapter);
}
static void go_bus_error(struct pios_i2c_adapter *i2c_adapter)
{
/* Note that this transfer has hit a bus error */
@ -409,6 +411,7 @@ static void go_bus_error(struct pios_i2c_adapter *i2c_adapter)
i2c_adapter_reset_bus(i2c_adapter);
}
static void go_stopping(struct pios_i2c_adapter *i2c_adapter)
{
#ifdef USE_FREERTOS
@ -431,12 +434,14 @@ static void go_stopping(struct pios_i2c_adapter *i2c_adapter)
}
}
static void go_stopped(struct pios_i2c_adapter *i2c_adapter)
{
I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE);
}
static void go_starting(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_DEBUG_Assert(i2c_adapter->active_txn);
@ -465,6 +470,7 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter)
}
}
/* Common to 'more' and 'last' transaction */
static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter)
{
@ -477,24 +483,28 @@ static void go_r_any_txn_addr(struct pios_i2c_adapter *i2c_adapter)
I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Receiver);
}
static void go_r_more_txn_pre_one(struct pios_i2c_adapter *i2c_adapter)
{
I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE);
I2C_GenerateSTART(i2c_adapter->cfg->regs, ENABLE);
}
static void go_r_last_txn_pre_one(struct pios_i2c_adapter *i2c_adapter)
{
I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE);
I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE);
}
/* Common to 'more' and 'last' transaction */
static void go_r_any_txn_pre_first(struct pios_i2c_adapter *i2c_adapter)
{
I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, ENABLE);
}
/* Common to 'more' and 'last' transaction */
static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter)
{
@ -508,6 +518,7 @@ static void go_r_any_txn_pre_middle(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte);
}
static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_DEBUG_Assert(i2c_adapter->active_byte);
@ -526,6 +537,7 @@ static void go_r_more_txn_pre_last(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte);
}
static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_DEBUG_Assert(i2c_adapter->active_byte);
@ -544,6 +556,7 @@ static void go_r_last_txn_pre_last(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte);
}
/* Common to 'more' and 'last' transaction */
static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter)
{
@ -562,6 +575,7 @@ static void go_r_any_txn_post_last(struct pios_i2c_adapter *i2c_adapter)
i2c_adapter->active_txn++;
}
/* Common to 'more' and 'last' transaction */
static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter)
{
@ -574,6 +588,7 @@ static void go_w_any_txn_addr(struct pios_i2c_adapter *i2c_adapter)
I2C_Send7bitAddress(i2c_adapter->cfg->regs, (i2c_adapter->active_txn->addr) << 1, I2C_Direction_Transmitter);
}
static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_DEBUG_Assert(i2c_adapter->active_byte);
@ -589,6 +604,7 @@ static void go_w_any_txn_middle(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_byte <= i2c_adapter->last_byte);
}
static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_DEBUG_Assert(i2c_adapter->active_byte);
@ -607,6 +623,7 @@ static void go_w_more_txn_last(struct pios_i2c_adapter *i2c_adapter)
PIOS_DEBUG_Assert(i2c_adapter->active_txn <= i2c_adapter->last_txn);
}
static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_DEBUG_Assert(i2c_adapter->active_byte);
@ -625,6 +642,7 @@ static void go_w_last_txn_last(struct pios_i2c_adapter *i2c_adapter)
i2c_adapter->active_byte++;
}
static void go_nack(struct pios_i2c_adapter *i2c_adapter)
{
i2c_adapter->nack = true;
@ -633,6 +651,7 @@ static void go_nack(struct pios_i2c_adapter *i2c_adapter)
I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE);
}
static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event)
{
PIOS_IRQ_Disable();
@ -668,6 +687,7 @@ static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum
PIOS_IRQ_Enable();
}
static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter)
{
PIOS_IRQ_Disable();
@ -684,12 +704,14 @@ static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter)
PIOS_IRQ_Enable();
}
static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter)
{
i2c_adapter_reset_bus(i2c_adapter);
i2c_adapter->curr_state = I2C_STATE_STOPPED;
}
static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter)
{
uint32_t guard;
@ -712,8 +734,15 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter)
return true;
}
static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter)
{
// retry with wait code from
// TauLabs 20150718 - Prevent F3 I2C Init Lockup #1728
uint8_t retry_count;
uint8_t retry_count_clk;
static const uint8_t MAX_I2C_RETRY_COUNT = 10;
/* Reset the I2C block */
I2C_DeInit(i2c_adapter->cfg->regs);
@ -734,11 +763,13 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter)
/* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */
/* ESC */
// bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET;
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) {
retry_count_clk = 0;
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET && (retry_count_clk++ < MAX_I2C_RETRY_COUNT)) {
retry_count = 0;
/* Set clock high and wait for any clock stretching to finish. */
GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin);
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) {
;
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET && (retry_count++ < MAX_I2C_RETRY_COUNT)) {
PIOS_DELAY_WaituS(1);
}
PIOS_DELAY_WaituS(2);
@ -762,12 +793,14 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter)
/* Set data and clock high and wait for any clock stretching to finish. */
GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin);
GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin);
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) {
;
retry_count = 0;
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET && (retry_count++ < MAX_I2C_RETRY_COUNT)) {
PIOS_DELAY_WaituS(1);
}
/* Wait for data to be high */
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) {
;
retry_count = 0;
while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET && (retry_count++ < MAX_I2C_RETRY_COUNT)) {
PIOS_DELAY_WaituS(1);
}
@ -818,6 +851,7 @@ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter)
}
}
uint32_t i2c_cb_count = 0;
static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter)
{
@ -872,6 +906,7 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter)
return (!i2c_adapter->bus_error) && semaphore_success;
}
/**
* Logs the last N state transitions and N IRQ events due to
* an error condition
@ -936,6 +971,7 @@ static bool PIOS_I2C_validate(struct pios_i2c_adapter *i2c_adapter)
return i2c_adapter->magic == PIOS_I2C_DEV_MAGIC;
}
#if defined(PIOS_INCLUDE_FREERTOS) && 0
static struct pios_i2c_dev *PIOS_I2C_alloc(void)
{

269
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -36,6 +36,7 @@
#include <pios_oplinkrcvr_priv.h>
#include <taskinfo.h>
#include <pios_ws2811.h>
#include <auxmagsettings.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
@ -55,12 +56,11 @@
/**
* Sensor configurations
*/
#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")));
struct pios_adc_cfg pios_adc_cfg = {
.adc_dev = ADC1,
.dma = {
@ -84,22 +84,25 @@ struct pios_adc_cfg pios_adc_cfg = {
.half_flag = DMA_IT_HTIF4,
.full_flag = DMA_IT_TCIF4,
};
void PIOS_ADC_DMC_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_ADC_DMA_Handler();
}
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0;
pios_hmc5x83_dev_t external_mag = 0;
#ifdef PIOS_HMC5X83_HAS_GPIOS
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,
@ -130,13 +133,30 @@ static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
},
},
};
#endif /* PIOS_HMC5X83_HAS_GPIOS */
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = {
#ifdef PIOS_HMC5X83_HAS_GPIOS
.exti_cfg = &pios_exti_hmc5x83_cfg,
#endif
.M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.TempCompensation = false,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
};
static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = {
#ifdef PIOS_HMC5X83_HAS_GPIOS
.exti_cfg = NULL,
#endif
.M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.TempCompensation = false,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
};
@ -247,7 +267,6 @@ uint32_t pios_com_rf_id = 0;
uint32_t pios_com_bridge_id = 0;
uint32_t pios_com_overo_id = 0;
uint32_t pios_com_hkosd_id = 0;
uint32_t pios_com_vcp_id = 0;
#if defined(PIOS_INCLUDE_RFM22B)
@ -368,7 +387,6 @@ void PIOS_Board_Init(void)
PIOS_LED_Init(led_cfg);
#endif /* PIOS_INCLUDE_LED */
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS);
#endif
@ -400,6 +418,7 @@ void PIOS_Board_Init(void)
#if defined(PIOS_INCLUDE_RTC)
PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif
/* IAP System Setup */
PIOS_IAP_Init();
// check for safe mode commands from gcs
@ -411,8 +430,15 @@ void PIOS_Board_Init(void)
PIOS_IAP_WriteBootCmd(1, 0);
PIOS_IAP_WriteBootCmd(2, 0);
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_Init();
/* From TauLabs
* Initialize watchdog as early as possible to catch faults during init
* but do it only if there is no debugger connected
*/
if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) {
PIOS_WDG_Init();
}
#endif
/* Initialize the task monitor */
@ -471,11 +497,21 @@ void PIOS_Board_Init(void)
break;
case HWSETTINGS_RM_FLEXIPORT_I2C:
#if defined(PIOS_INCLUDE_I2C)
{
if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) {
PIOS_Assert(0);
}
if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) {
PIOS_Assert(0);
}
PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too
#if defined(PIOS_INCLUDE_HMC5X83)
// get auxmag type
AuxMagSettingsTypeOptions option;
AuxMagSettingsInitialize();
AuxMagSettingsTypeGet(&option);
// if the aux mag type is FlexiPort then set it up
if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
}
#endif /* PIOS_INCLUDE_HMC5X83 */
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_RM_FLEXIPORT_GPS:
@ -662,7 +698,6 @@ void PIOS_Board_Init(void)
}
#endif /* PIOS_INCLUDE_USB */
/* Configure main USART port */
uint8_t hwsettings_mainport;
HwSettingsRM_MainPortGet(&hwsettings_mainport);
@ -724,7 +759,6 @@ void PIOS_Board_Init(void)
GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable);
}
/* Initalize the RFM22B radio COM device. */
#if defined(PIOS_INCLUDE_RFM22B)
@ -838,7 +872,6 @@ void PIOS_Board_Init(void)
#endif /* PIOS_INCLUDE_RFM22B */
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM)
const struct pios_servo_cfg *pios_servo_cfg;
// default to servo outputs only
pios_servo_cfg = &pios_servo_cfg_out;
@ -933,10 +966,10 @@ void PIOS_Board_Init(void)
};
GPIO_Init(GPIOA, &gpioA8);
// init I2C1 for use with the internal mag and baro
if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
PIOS_DELAY_WaitmS(50);
#if defined(PIOS_INCLUDE_ADC)
@ -950,8 +983,10 @@ void PIOS_Board_Init(void)
#endif
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
PIOS_HMC5x83_Register(onboard_mag);
// attach the 5x83 mag to the previously inited I2C1
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_internal_cfg, pios_i2c_mag_pressure_adapter_id, 0);
// add this sensor to the sensor task's list
PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG);
#endif
#if defined(PIOS_INCLUDE_MS5611)
@ -985,3 +1020,203 @@ void PIOS_Board_Init(void)
* @}
* @}
*/
/*
things to try:
--------------
tau uses single mode on external
REMEMBER that to change this, we need to change the hack that always sets it continuous after each read?/write?
static const struct pios_hmc5883_cfg pios_hmc5883_internal_cfg = {
.exti_cfg = &pios_exti_hmc5883_internal_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
.Default_Orientation = PIOS_HMC5883_TOP_90DEG,
};
static const struct pios_hmc5883_cfg pios_hmc5883_external_cfg = {
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_SINGLE,
.Default_Orientation = PIOS_HMC5883_TOP_0DEG,
};
tau does as many i2c_init()s as are enabled
but only does PIOS_HMC5883_Init() for the one mag that is in use
try these in this order:
------------------------
current setup with tau fixes
single mode
only PIOS_HMC5883_Init() and register one mag
increase several stacks
sensors.c sensor task
callback
hardware interrupt stack
research tau's PIOS_I2C_Init() and PIOS_HMC5883_Init()
RELOAD_WDG();
things to do:
-------------
make an ASSERT() for enum PIOS_HMC5X83_ORIENTATION must match AuxMagSettingsOrientationOptions
fix i2c reset in all processors, not just F4
move all I2C stuff to a lower priority sensor task
Things to do now:
-----------------
fix AUX_MAG_SKIP
set AMS lower and see some duplicates
set AMS to exactly what it needs to be for highest rate with no dups
figure out mag orientation and set correct default and document how to determine what to use
I think that chip upside down and facing forward is the best default.
back up into a tar
clean up code 90% at least
examine git diff
back up into a tar
merge latest next into it
test calibration
test fly
push
announce
code and TL fixes on all other boards
push
for a test I moved PIOS_WDG_Init() to the end of PIOS_Board_Init()
four work arounds:
------------------
disable ext mag with either UAVO setting or code
disable int mag with code
remove just registration
PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG);
disable watchdog
either comment out #define PIOS_INCLUDE_WDG or comment out PIOS_WDG_Init();
sometimes: pause in PIOS_Board_Init() with the debugger, then some next/step then cont, can't reproduce
does not fix it:
----------------
disable baro registration
some RELOAD_WDG(); in sensors.c that I tried
neo6 gps/mag:
-------------
east south down is right side up
south west down locks up the GCS, no it just reds the mag and reinits and so atti and stab are offline
mag facts:
Revo front pointed north gives high positive X
Revo right (servo pins) pointed north gives high Y
Revo bottom pointed north gives high positive Z
neo6 bottom pointed north gives high
neo6 bottom pointed north gives high
neo6 bottom pointed north gives high
steps:
send UNCHANGED
point front north (and very importantly down 64 degrees for me)
flip 180 degrees
which axis moved the most? was it positive (then negative)? X yes
that is N channel
point bottom north
flip 180 degrees
which axis moved the most? was it positive (then negative)? Y yes
that is D channel
point right side north
flip 180 degrees
which axis moved the most? was it positive (then negative)? Z yes
that is E channel
Revo does X yes, Z yes, Y yes
so to get that from this mag we need all positive
NED
maybe ask which axis moves Z
which one makes X move, that is north (south if negative)
which one makes Y move, that is east (west if negative)
which one makes Z move, that is down (up if negative)
find that exact combination in some order
a test with upside down battery the desired orientation
N channel Z neg is S
D channel Y neg is U
E channel X neg is W
arrange in XZY order and that is WSU
should be WSU
that is correct
What must we do to get NDE XZY
=================================
rephrase question
what gives us XZY
NED
WSU
=================================
send UNCHANGED
point front north (and very importantly down 64 degrees for me)
flip 180 degrees
which axis moved the most? was it positive (then negative)? X yes
that is N channel
point bottom north
flip 180 degrees
which axis moved the most? was it positive (then negative)? Y yes
that is D channel
point right side north
flip 180 degrees
which axis moved the most? was it positive (then negative)? Z yes
that is E channel
negative N is S and comes from Z
negative D is U and comes from Y
negative E is W and comes from X
where did XZY come from? WSU
wires forward and top side up
Note that it is the largest difference.
-100 to -900 is larger than 200 to -200
-100 to -900 started more positive
you can find a more accurate north after you know which axis is pointing that way
make small rotations in yaw and pitch to change where it is pointing a little and watch for the strongest signal ON THAT AXIS
send UNCHANGED
point front north (and very importantly down 64 degrees for me)
flip 180 degrees to point the back north
which axis moved the most? did it start positive (then negative)? X neg
that is N channel
point bottom north
flip 180 degrees to point the top north
which axis moved the most? did it start positive (then negative)? Y pos
that is D channel
point right side north
flip 180 degrees to point the left north
which axis moved the most? did it start positive (then negative)? Z neg
that is E channel
X-N = S
Y+D = D
Z-E = W
where did XZY come from? SWD
looks pointed south, not west after hours
--- or? ---
debug and hit continue when it traps
It appears that the 0x08000bcc address is the boot code where it goes after watchdog
Neo6
7-8 sats upside down, indoors
*/

6
shared/uavobjectdefinition/auxmagsettings.xml
View File

@ -5,8 +5,10 @@
<field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
defaultvalue="1,0,0,0,1,0,0,0,1"/>
<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
<field name="Orientation" units="degrees" type="float" elements="1" defaultvalue="0"/>
<field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext" defaultvalue="GPSV9"/>
<field name="Orientation" units="" type="enum" elements="1"
options="EAST_NORTH_UP,SOUTH_EAST_UP,WEST_SOUTH_UP,NORTH_WEST_UP,EAST_SOUTH_DOWN,SOUTH_WEST_DOWN,WEST_NORTH_DOWN,NORTH_EAST_DOWN,UNCHANGED"
defaultvalue="UNCHANGED"/>
<field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext,Flexi" defaultvalue="GPSV9"/>
<field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>