mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Sometimes the mag stops updating so kickstart it
This commit is contained in:
parent
ab8b71a6c1
commit
91a226fc12
@ -192,6 +192,7 @@ static void SensorsTask(void *parameters)
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
bool error = false;
|
||||
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
|
||||
while (1) {
|
||||
// TODO: add timeouts to the sensor reads and set an error if the fail
|
||||
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
|
||||
@ -347,7 +348,8 @@ static void SensorsTask(void *parameters)
|
||||
// and make it average zero (weakly)
|
||||
MagnetometerData mag;
|
||||
bool mag_updated = false;
|
||||
if (PIOS_HMC5883_NewDataAvailable()) {
|
||||
|
||||
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
|
||||
mag_updated = true;
|
||||
int16_t values[3];
|
||||
PIOS_HMC5883_ReadMag(values);
|
||||
@ -355,6 +357,7 @@ static void SensorsTask(void *parameters)
|
||||
mag.y = values[0] * mag_scale[1] - mag_bias[1];
|
||||
mag.z = -values[2] * mag_scale[2] - mag_bias[2];
|
||||
MagnetometerSet(&mag);
|
||||
mag_update_time = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
// For debugging purposes here we can output all of the sensors. Do it as a single transaction
|
||||
|
Loading…
x
Reference in New Issue
Block a user