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 changes for pull request comments

Browse Source
This commit is contained in:
Cliff Geerdes 2015-09-19 01:04:12 -04:00
parent a110eeda21
commit 7de55812dd
8 changed files with 80 additions and 184 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
flight/libraries/auxmagsupport.c
View File

@ -29,27 +29,6 @@
#include "CoordinateConversions.h"
#include "pios_hmc5x83.h" // this is needed for mag orientation even for other mag types
#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] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
@ -59,13 +38,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);
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);
}
void auxmagsupport_publish_samples(float mags[3], uint8_t status)

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

@ -85,9 +85,6 @@
#endif
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 */
#define AUX_MAG_LOCAL_SUPPORT
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30
@ -155,9 +152,6 @@ static void updateBaroTempBias(float temperature);
static sensor_data *source_data;
static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
#ifdef AUX_MAG_LOCAL_SUPPORT
AuxMagSettingsData auxmagcal;
#endif
AccelGyroSettingsData agcal;
// These values are initialized by settings but can be updated by the attitude algorithm
@ -165,12 +159,6 @@ static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
#ifdef AUX_MAG_LOCAL_SUPPORT
static float auxmag_bias[3] = { 0, 0, 0 };
static float auxmag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
#endif
// Variables used to handle accel/gyro temperature bias
static volatile bool gyro_temp_calibrated = false;
@ -196,10 +184,6 @@ 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;
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
@ -212,20 +196,16 @@ int32_t SensorsInitialize(void)
MagSensorInitialize();
BaroSensorInitialize();
RevoCalibrationInitialize();
#ifdef AUX_MAG_LOCAL_SUPPORT
AuxMagSettingsInitialize();
#endif
RevoSettingsInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
rotate = 0;
// for auxmagsupport.c helpers
AuxMagSettingsInitialize();
AuxMagSensorInitialize();
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
#ifdef AUX_MAG_LOCAL_SUPPORT
AuxMagSettingsConnectCallback(&settingsUpdatedCb);
#endif
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
@ -267,7 +247,6 @@ 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);
@ -317,39 +296,35 @@ 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->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) {
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) {
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) {
accumulateSamples(&sensor_context, source_data);
processSamples3d(&sensor_context, sensor);
} else {
processSamples1d(&source_data->sensorSample1Axis, sensor);
}
clearContext(&sensor_context);
} else {
processSamples1d(&source_data->sensorSample1Axis, sensor);
}
clearContext(&sensor_context);
}
}
}
@ -497,23 +472,7 @@ static void handleMag(float *samples, float temperature)
static void handleAuxMag(float *samples)
{
#ifdef AUX_MAG_LOCAL_SUPPORT
AuxMagSensorData mag;
float mags[3] = { (float)samples[0] - auxmag_bias[0],
(float)samples[1] - auxmag_bias[1],
(float)samples[2] - auxmag_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);
#else
auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
#endif
}
static void handleBaro(float sample, float temperature)
@ -595,7 +554,7 @@ static void updateBaroTempBias(float temperature)
}
/**
* Locally cache some variables from the AtttitudeSettings object
* Locally cache some variables from the AttitudeSettings object
*/
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
@ -603,59 +562,38 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
#ifdef AUX_MAG_LOCAL_SUPPORT
AuxMagSettingsGet(&auxmagcal);
auxmag_bias[0] = auxmagcal.mag_bias.X;
auxmag_bias[1] = auxmagcal.mag_bias.Y;
auxmag_bias[2] = auxmagcal.mag_bias.Z;
#endif
AccelGyroSettingsGet(&agcal);
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);
#ifdef AUX_MAG_LOCAL_SUPPORT
matrix_mult_3x3f((float(*)[3])AuxMagSettingsmag_transformToArray(auxmagcal.mag_transform), R, auxmag_transform);
#endif
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);

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

@ -42,7 +42,6 @@
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;
@ -103,10 +102,9 @@ 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
if (cfg->exti_cfg) {
@ -114,14 +112,9 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
}
#endif
#if 0
int32_t val = PIOS_HMC5x83_Config(dev);
PIOS_Assert(val == 0);
#else
if (PIOS_HMC5x83_Config(dev) != 0) {
return ((pios_hmc5x83_dev_t) NULL);
}
#endif
dev->data_ready = false;
return (pios_hmc5x83_dev_t)dev;
@ -226,14 +219,7 @@ 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])
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:
@ -276,11 +262,6 @@ void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3
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;
}
}
@ -337,7 +318,7 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
temp[i] = v;
}
PIOS_HMC5x83_Orient(dev->Orientation, temp, out);
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"
//
@ -368,7 +349,7 @@ uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
// 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_STATUS_POLL_RETURNS_TRUE
//#define HMC5X83_STATUS_POLL_RETURNS_TRUE
/**
* @brief Tells whether new magnetometer readings are available

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

@ -99,7 +99,6 @@
#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);
@ -123,7 +122,6 @@ 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,
};
@ -144,8 +142,6 @@ struct pios_hmc5x83_cfg {
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, 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]);

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

@ -155,7 +155,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = {
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.TempCompensation = false,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag
};
#endif /* PIOS_INCLUDE_HMC5X83 */
@ -422,13 +422,7 @@ void PIOS_Board_Init(void)
PIOS_IAP_WriteBootCmd(2, 0);
}
#ifdef PIOS_INCLUDE_WDG
/* 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();
}
PIOS_WDG_Init();
#endif
/* Initialize the task monitor */
@ -699,6 +693,9 @@ void PIOS_Board_Init(void)
// attach the 5x83 mag to the previously inited I2C2
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
// 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);

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

@ -157,7 +157,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = {
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.TempCompensation = false,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag
};
#endif /* PIOS_INCLUDE_HMC5X83 */
@ -431,13 +431,7 @@ void PIOS_Board_Init(void)
}
#ifdef PIOS_INCLUDE_WDG
/* 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();
}
PIOS_WDG_Init();
#endif
/* Initialize the task monitor */
@ -510,6 +504,9 @@ void PIOS_Board_Init(void)
// attach the 5x83 mag to the previously inited I2C2
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
// 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);

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

@ -109,7 +109,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = {
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.TempCompensation = false,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag
};
#endif /* PIOS_INCLUDE_HMC5X83 */
@ -380,13 +380,7 @@ void PIOS_Board_Init(void)
}
#ifdef PIOS_INCLUDE_WDG
/* 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();
}
PIOS_WDG_Init();
#endif
/* Initialize the task monitor */
@ -656,6 +650,9 @@ void PIOS_Board_Init(void)
// attach the 5x83 mag to the previously inited I2C2
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
// 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);

4
shared/uavobjectdefinition/auxmagsettings.xml
View File

@ -5,9 +5,7 @@
<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="" 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="NORTH_EAST_DOWN"/>
<field name="BoardRotation" units="deg" type="float" 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"/>