rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Merged in TheOtherCliff/librepilot/theothercliff/LP-73_External_Mags_on_I2C (pull request #59)

Browse Source 
LP-73 External Mags on I2C Flexiport
This commit is contained in:
Fredrik Arvidsson 2015-10-28 10:04:30 +01:00
commit 3b2795e2b4
19 changed files with 593 additions and 190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/libraries/auxmagsupport.c
View File

@ -37,16 +37,26 @@ AuxMagSettingsTypeOptions option;
void auxmagsupport_reload_settings()
{
AuxMagSettingsData cal;
float magQuat[4];
float R[3][3];
AuxMagSettingsGet(&cal);
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
// convert the RPY mag board rotation to into a rotation matrix
// rotate the vector into the level hover frame (the attitude frame)
const float magRpy[3] = { cal.BoardRotation.Roll, cal.BoardRotation.Pitch, cal.BoardRotation.Yaw };
RPY2Quaternion(magRpy, magQuat);
Quaternion2R(magQuat, R);
// the mag transform only scales the raw mag values
matrix_mult_3x3f((float(*)[3])AuxMagSettingsmag_transformToArray(cal.mag_transform), R, mag_transform);
// GPSV9, Ext (unused), and Flexi
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_biasArrayGet(mag_bias);
}
void auxmagsupport_publish_samples(float mags[3], uint8_t status)

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

@ -57,6 +57,9 @@
#include <attitudesettings.h>
#include <revocalibration.h>
#include <auxmagsettings.h>
#include <auxmagsensor.h>
#include <auxmagsupport.h>
#include <accelgyrosettings.h>
#include <revosettings.h>
@ -81,7 +84,7 @@
#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));
// 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);
@ -151,7 +155,6 @@ RevoCalibrationData cal;
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 }
@ -168,9 +171,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,8 +184,6 @@ static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t baro_temp_calibration_count = 0;
static int8_t rotate = 0;
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
@ -197,7 +200,9 @@ int32_t SensorsInitialize(void)
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
rotate = 0;
// for auxmagsupport.c helpers
AuxMagSettingsInitialize();
AuxMagSensorInitialize();
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
@ -258,6 +263,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++;
}
@ -361,20 +367,29 @@ 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 +473,11 @@ static void handleMag(float *samples, float temperature)
MagSensorSet(&mag);
}
static void handleAuxMag(float *samples)
{
auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
}
static void handleBaro(float sample, float temperature)
{
updateBaroTempBias(temperature);
@ -535,58 +555,47 @@ static void updateBaroTempBias(float temperature)
}
baro_temp_calibration_count--;
}
/**
* Locally cache some variables from the AtttitudeSettings object
* Locally cache some variables from the AttitudeSettings object
*/
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
RevoCalibrationGet(&cal);
AccelGyroSettingsGet(&agcal);
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
AccelGyroSettingsGet(&agcal);
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
(fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
gyro_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
(fabsf(agcal.gyro_temp_coeff.X) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Y) > 1e-9f ||
fabsf(agcal.gyro_temp_coeff.Z) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Z2) > 1e-9f);
// convert BoardRotation ("rotate virtual") into a quaternion
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation
if (fabsf(attitudeSettings.BoardRotation.Roll) < ZERO_ROT_ANGLE
&& fabsf(attitudeSettings.BoardRotation.Pitch) < ZERO_ROT_ANGLE &&
fabsf(attitudeSettings.BoardRotation.Yaw) < ZERO_ROT_ANGLE) {
rotate = 0;
} else {
rotate = 1;
}
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
float rotationQuat[4];
RPY2Quaternion(rpy, rotationQuat);
if (fabsf(attitudeSettings.BoardLevelTrim.Roll) > ZERO_ROT_ANGLE ||
fabsf(attitudeSettings.BoardLevelTrim.Pitch) > ZERO_ROT_ANGLE) {
float trimQuat[4];
float sumQuat[4];
rotate = 1;
// convert BoardLevelTrim ("board level calibration") into a quaternion
float trimQuat[4];
float sumQuat[4];
const float trimRpy[3] = { attitudeSettings.BoardLevelTrim.Roll, attitudeSettings.BoardLevelTrim.Pitch, 0.0f };
// do we actually want to include BoardLevelTrim in the mag rotation? BoardRotation yes, but BoardLevelTrim?
// and is BoardLevelTrim done at the correct point in the sequence of rotations?
RPY2Quaternion(trimRpy, trimQuat);
const float trimRpy[3] = { attitudeSettings.BoardLevelTrim.Roll, attitudeSettings.BoardLevelTrim.Pitch, 0.0f };
RPY2Quaternion(trimRpy, trimQuat);
// add the boardrotation and boardtrim rotations and put them into a rotation matrix
quat_mult(rotationQuat, trimQuat, sumQuat);
Quaternion2R(sumQuat, R);
quat_mult(rotationQuat, trimQuat, sumQuat);
Quaternion2R(sumQuat, R);
} else {
Quaternion2R(rotationQuat, R);
}
// mag_transform is only a scaling
// so add the scaling, and store the result in mag_transform for run time use
matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);

4
flight/modules/System/systemmod.c
View File

@ -183,6 +183,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
vTaskDelay(10);
}
#ifndef PIOS_INCLUDE_WDG
// if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
#define PIOS_WDG_Clear()
#endif
/* create all modules thread */
MODULE_TASKCREATE_ALL;

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

@ -45,6 +45,8 @@ typedef struct {
uint32_t port_id;
uint8_t slave_num;
uint8_t CTRLB;
uint16_t magCountMax;
uint16_t magCount;
volatile bool data_ready;
} pios_hmc5x83_dev_data_t;
@ -66,6 +68,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
@ -104,20 +107,73 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
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);
#ifdef PIOS_HMC5X83_HAS_GPIOS
if (cfg->exti_cfg) {
PIOS_EXTI_Init(cfg->exti_cfg);
} else
#endif /* PIOS_HMC5X83_HAS_GPIOS */
{
// if PIOS_SENSOR_RATE is defined, there is a sensor loop that is called at that frequency
// and "is data available" can simply return false a few times to save some CPU
#ifdef PIOS_SENSOR_RATE
// for external mags that have no interrupt line, just poll them with a timer
// use the configured Output Data Rate to calculate the number of interations (of the main sensor task loop)
// to return false, before returning true
uint16_t rate100;
switch (cfg->M_ODR) {
case PIOS_HMC5x83_ODR_0_75:
rate100 = 75;
break;
case PIOS_HMC5x83_ODR_1_5:
rate100 = 150;
break;
case PIOS_HMC5x83_ODR_3:
rate100 = 300;
break;
case PIOS_HMC5x83_ODR_7_5:
rate100 = 750;
break;
case PIOS_HMC5x83_ODR_15:
rate100 = 1500;
break;
case PIOS_HMC5x83_ODR_30:
rate100 = 3000;
break;
default:
case PIOS_HMC5x83_ODR_75:
rate100 = 7500;
break;
}
// if the application sensor rate is fast enough to warrant skipping some slow hardware sensor reads
if ((PIOS_SENSOR_RATE * 100.0f / 3.0f) > rate100) {
// count the number of "return false" up to this number
dev->magCountMax = ((uint16_t)PIOS_SENSOR_RATE * 100 / rate100) + 1;
} else {
// return true every time (do a hardware sensor poll every time)
dev->magCountMax = 1;
}
#else /* PIOS_SENSOR_RATE */
// return true every time (do a hardware sensor poll every time)
dev->magCountMax = 1;
#endif /* PIOS_SENSOR_RATE */
// with this counter
dev->magCount = 0;
}
if (PIOS_HMC5x83_Config(dev) != 0) {
return (pios_hmc5x83_dev_t)NULL;
}
dev->data_ready = false;
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);
if (handler) {
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, sensortype, handler);
}
}
/**
@ -212,6 +268,52 @@ static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
return 0;
}
static 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;
}
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[in] dev device handler
@ -265,50 +367,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->cfg->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;
@ -334,11 +400,23 @@ uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
* \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)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
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
if (++(dev->magCount) >= dev->magCountMax) {
dev->magCount = 0;
return true;
} else {
return false;
}
}
}
/**
@ -435,6 +513,7 @@ int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler)
return failed;
}
#ifdef PIOS_HMC5X83_HAS_GPIOS
/**
* @brief IRQ Handler
*/
@ -443,8 +522,10 @@ 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 +610,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);

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

@ -95,6 +95,9 @@
#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;
struct pios_hmc5x83_io_driver {
@ -137,7 +140,7 @@ 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 bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler);
extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]);

8
flight/pios/inc/pios_initcall.h
View File

@ -100,10 +100,12 @@ extern void StartModules();
initTaskDone = 1; \
}
#define MODULE_TASKCREATE_ALL \
#define MODULE_TASKCREATE_ALL \
{ for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \
if (fn->fn_tinit) { \
(fn->fn_tinit)(); } \
if (fn->fn_tinit) { \
(fn->fn_tinit)(); \
PIOS_WDG_Clear(); \
} \
} \
}

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;

24
flight/pios/stm32f10x/pios_i2c.c
View File

@ -711,6 +711,12 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter)
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);
@ -731,11 +737,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);
@ -759,12 +767,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);
}

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)
{

2
flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
View File

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

99
flight/targets/boards/discoveryf4bare/firmware/pios_board.c
View File

@ -36,6 +36,7 @@
#include <pios_oplinkrcvr_priv.h>
#include <taskinfo.h>
#include <pios_callbackscheduler.h>
#include <auxmagsettings.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
@ -92,8 +93,16 @@ void PIOS_ADC_DMC_irq_handler(void)
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0;
pios_hmc5x83_dev_t external_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_HMC5x83_IRQHandler,
.vector = pios_board_internal_mag_handler,
.line = EXTI_Line7,
.pin = {
.gpio = GPIOB,
@ -123,14 +132,30 @@ static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
},
};
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,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
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, // ENU for GPSV9, WND for typical I2C mag
};
#endif /* PIOS_INCLUDE_HMC5X83 */
@ -654,11 +679,40 @@ 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
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
// to avoid making something else fail when HMC5X83 is removed
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
#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) {
// attach the 5x83 mag to the previously inited I2C2
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
// add this sensor to the sensor task's list
// be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
// and before registering some other fast and important sensor
// as that would cause delay and time jitter for the second fast sensor
PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
// mag alarm is cleared later, so use I2C
AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
}
#endif /* PIOS_INCLUDE_HMC5X83 */
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_RM_FLEXIPORT_GPS:
@ -882,6 +936,7 @@ 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);
}
@ -892,8 +947,24 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
// to avoid making something else fail when HMC5X83 is removed
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
#if defined(PIOS_INCLUDE_HMC5X83)
PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
// 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);
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
// 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)

2
flight/targets/boards/osd/firmware/inc/pios_config.h
View File

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

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

@ -36,7 +36,7 @@
#include <pios_oplinkrcvr_priv.h>
#include <taskinfo.h>
#include <pios_ws2811.h>
#include <auxmagsettings.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
@ -55,12 +55,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 +83,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 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,15 +132,32 @@ 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 = {
.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,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
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, // ENU for GPSV9, WND for typical I2C mag
};
#endif /* PIOS_INCLUDE_HMC5X83 */
@ -247,7 +266,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 +386,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 +417,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,6 +429,7 @@ void PIOS_Board_Init(void)
PIOS_IAP_WriteBootCmd(1, 0);
PIOS_IAP_WriteBootCmd(2, 0);
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_Init();
#endif
@ -471,11 +490,40 @@ 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
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
// to avoid making something else fail when HMC5X83 is removed
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
#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) {
// attach the 5x83 mag to the previously inited I2C2
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
// add this sensor to the sensor task's list
// be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
// and before registering some other fast and important sensor
// as that would cause delay and time jitter for the second fast sensor
PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
// mag alarm is cleared later, so use I2C
AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
}
#endif /* PIOS_INCLUDE_HMC5X83 */
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_RM_FLEXIPORT_GPS:
@ -662,7 +710,6 @@ void PIOS_Board_Init(void)
}
#endif /* PIOS_INCLUDE_USB */
/* Configure main USART port */
uint8_t hwsettings_mainport;
HwSettingsRM_MainPortGet(&hwsettings_mainport);
@ -724,7 +771,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 +884,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 +978,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)
@ -949,9 +994,24 @@ void PIOS_Board_Init(void)
PIOS_MPU6000_Register();
#endif
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
// to avoid making something else fail when HMC5X83 is removed
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
#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);
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
// 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)

2
flight/targets/boards/revonano/firmware/inc/pios_config.h
View File

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

58
flight/targets/boards/revonano/firmware/pios_board.c
View File

@ -38,6 +38,7 @@
#include <pios_ws2811.h>
#include <sanitycheck.h>
#include <actuatorsettings.h>
#include <auxmagsettings.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
@ -92,9 +93,26 @@ 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 external_mag = 0;
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, // ENU for GPSV9, WND for typical I2C mag
};
#endif /* PIOS_INCLUDE_HMC5X83 */
/**
* Configuration for the MS5611 chip
*/
@ -360,6 +378,7 @@ void PIOS_Board_Init(void)
PIOS_IAP_WriteBootCmd(1, 0);
PIOS_IAP_WriteBootCmd(2, 0);
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_Init();
#endif
@ -617,11 +636,40 @@ 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
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
// to avoid making something else fail when HMC5X83 is removed
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
#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) {
// attach the 5x83 mag to the previously inited I2C2
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
// add this sensor to the sensor task's list
// be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
// and before registering some other fast and important sensor
// as that would cause delay and time jitter for the second fast sensor
PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
// mag alarm is cleared later, so use I2C
AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
}
#endif /* PIOS_INCLUDE_HMC5X83 */
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_RM_FLEXIPORT_GPS:

94
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -31,6 +31,7 @@
#include <hwsettings.h>
#include <manualcontrolsettings.h>
#include <taskinfo.h>
#include <auxmagsettings.h>
/*
* Pull in the board-specific static HW definitions.
@ -84,8 +85,10 @@ void PIOS_ADC_DMC_irq_handler(void)
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0;
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);
@ -120,15 +123,32 @@ 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 = {
.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,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
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,
};
#endif /* PIOS_INCLUDE_HMC5X83 */
@ -753,14 +773,39 @@ void PIOS_Board_Init(void)
break;
case HWSETTINGS_RV_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
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
// to avoid making something else fail when HMC5X83 is removed
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
#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) {
// attach the 5x83 mag to the previously inited I2C2
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
// add this sensor to the sensor task's list
PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
// mag alarm is cleared later, so use I2C
AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
}
#endif /* PIOS_INCLUDE_HMC5X83 */
#endif /* PIOS_INCLUDE_I2C */
break;
case HWSETTINGS_RV_FLEXIPORT_DSM:
// TODO: Define the various Channelgroup for Revo dsm inputs and handle here
PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg,
@ -879,10 +924,12 @@ void PIOS_Board_Init(void)
PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins));
#endif
// init I2C1 for use with the internal mag
if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
// init I2C1 for use with the internal baro
if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
@ -893,9 +940,24 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
// leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
// to avoid making something else fail when HMC5X83 is removed
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_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_adapter_id, 0);
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
// this is not in a loop, so it is safe
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
// 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)

12
ground/gcs/src/plugins/config/configrevowidget.cpp
View File

@ -246,10 +246,14 @@ void ConfigRevoWidget::storeAndClearBoardRotation()
AuxMagSettings *auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
Q_ASSERT(auxMagSettings);
AuxMagSettings::DataFields auxMagData = auxMagSettings->getData();
auxMagStoredBoardRotation = auxMagData.Orientation;
auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_YAW] = auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_YAW];
auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_ROLL] = auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_ROLL];
auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_PITCH] = auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_PITCH];
// Set aux mag board rotation to no rotation
auxMagData.Orientation = 0.0f;
auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_YAW] = 0;
auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_ROLL] = 0;
auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_PITCH] = 0;
auxMagSettings->setData(auxMagData);
}
}
@ -273,7 +277,9 @@ void ConfigRevoWidget::recallBoardRotation()
AuxMagSettings *auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
Q_ASSERT(auxMagSettings);
AuxMagSettings::DataFields auxMagData = auxMagSettings->getData();
auxMagData.Orientation = auxMagStoredBoardRotation;
auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_YAW] = auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_YAW];
auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_ROLL] = auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_ROLL];
auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_PITCH] = auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_PITCH];
auxMagSettings->setData(auxMagData);
}
}

2
ground/gcs/src/plugins/config/configrevowidget.h
View File

@ -64,7 +64,7 @@ private:
// Board rotation store/recall for FC and for aux mag
qint16 storedBoardRotation[3];
float auxMagStoredBoardRotation;
qint16 auxMagStoredBoardRotation[3];
bool isBoardRotationStored;
private slots:

4
shared/uavobjectdefinition/auxmagsettings.xml
View File

@ -5,8 +5,8 @@
<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="BoardRotation" units="deg" type="int16" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<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"/>