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

Merged in mindnever/librepilot/LP-328_hmc5x83_init_fix (pull request #257)

Browse Source 
LP-328_hmc5x83_init_fix
This commit is contained in:
Lalanne Laurent 2016-06-04 16:50:51 +02:00
commit b1cc1332ec
1 changed files with 61 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,6 +48,9 @@ typedef struct {
uint16_t magCountMax;
uint16_t magCount;
volatile bool data_ready;
int16_t magData[3];
bool hw_error;
uint32_t lastConfigTime;
} pios_hmc5x83_dev_data_t;
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
@ -162,7 +165,7 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
}
if (PIOS_HMC5x83_Config(dev) != 0) {
return (pios_hmc5x83_dev_t)NULL;
dev->hw_error = true;
}
dev->data_ready = false;
@ -241,6 +244,8 @@ static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
dev->lastConfigTime = PIOS_DELAY_GetRaw();
const struct pios_hmc5x83_cfg *cfg = dev->cfg;
dev->CTRLB = 0;
@ -265,6 +270,15 @@ static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
return -1;
}
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
if (PIOS_HMC5x83_Test((pios_hmc5x83_dev_t)dev) != 0) {
return -1;
}
return 0;
}
@ -434,6 +448,12 @@ int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler)
int16_t values[3];
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
#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 */
/* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */
char id[4];
@ -690,9 +710,9 @@ int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint
#endif /* PIOS_INCLUDE_I2C */
/* PIOS sensor driver implementation */
bool PIOS_HMC5x83_driver_Test(uintptr_t context)
bool PIOS_HMC5x83_driver_Test(__attribute__((unused)) uintptr_t context)
{
return !PIOS_HMC5x83_Test((pios_hmc5x83_dev_t)context);
return true; // Do not do tests now, Sensors module takes this rather too seriously.
}
void PIOS_HMC5x83_driver_Reset(__attribute__((unused)) uintptr_t context) {}
@ -706,19 +726,51 @@ void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, __attribute__((u
void PIOS_HMC5x83_driver_fetch(void *data, uint8_t size, uintptr_t context)
{
PIOS_Assert(size > 0);
int16_t mag[3];
PIOS_HMC5x83_ReadMag((pios_hmc5x83_dev_t)context, mag);
pios_hmc5x83_dev_data_t *dev = dev_validate((pios_hmc5x83_dev_t)context);
PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data;
tmp->count = 1;
tmp->sample[0].x = mag[0];
tmp->sample[0].y = mag[1];
tmp->sample[0].z = mag[2];
tmp->sample[0].x = dev->magData[0];
tmp->sample[0].y = dev->magData[1];
tmp->sample[0].z = dev->magData[2];
tmp->temperature = 0;
}
bool PIOS_HMC5x83_driver_poll(uintptr_t context)
{
return PIOS_HMC5x83_NewDataAvailable((pios_hmc5x83_dev_t)context);
pios_hmc5x83_dev_data_t *dev = dev_validate((pios_hmc5x83_dev_t)context);
if (dev->hw_error) {
if (PIOS_DELAY_DiffuS(dev->lastConfigTime) < 1000000) {
return false;
}
#ifdef PIOS_INCLUDE_WDG
// give HMC5x83 on I2C some extra time
PIOS_WDG_Clear();
#endif /* PIOS_INCLUDE_WDG */
if (PIOS_HMC5x83_Config(dev) == 0) {
dev->hw_error = false;
}
}
if (dev->hw_error) {
return false;
}
if (!PIOS_HMC5x83_NewDataAvailable((pios_hmc5x83_dev_t)context)) {
return false;
}
if (PIOS_HMC5x83_ReadMag((pios_hmc5x83_dev_t)context, dev->magData) != 0) {
dev->hw_error = true;
return false;
}
return true;
}
#endif /* PIOS_INCLUDE_HMC5x83 */