diff --git a/flight/libraries/auxmagsupport.c b/flight/libraries/auxmagsupport.c index 3d27e336a..d8011fae4 100644 --- a/flight/libraries/auxmagsupport.c +++ b/flight/libraries/auxmagsupport.c @@ -27,6 +27,30 @@ #include #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); } diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 800d8a94c..d1d271d17 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -57,6 +57,8 @@ #include #include +#include +#include #include #include @@ -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); diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index 0a16c4d64..68d4013f0 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/flight/pios/common/pios_hmc5x83.c @@ -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); diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index 490168017..92070d4c6 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -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]); diff --git a/flight/pios/inc/pios_sensors.h b/flight/pios/inc/pios_sensors.h index eb565c14a..085fe96f2 100644 --- a/flight/pios/inc/pios_sensors.h +++ b/flight/pios/inc/pios_sensors.h @@ -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; diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index 171669ac4..60811fa13 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -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) { diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 5bfc0a16c..0c0c67bfe 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -36,6 +36,7 @@ #include #include #include +#include #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 +*/ diff --git a/shared/uavobjectdefinition/auxmagsettings.xml b/shared/uavobjectdefinition/auxmagsettings.xml index fda802031..d2ea3dcb2 100644 --- a/shared/uavobjectdefinition/auxmagsettings.xml +++ b/shared/uavobjectdefinition/auxmagsettings.xml @@ -5,8 +5,10 @@ - - + +