From 50d4a97c0cf1a6482c7def9bc5c1a2ac8a3f491f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 7 Sep 2014 22:43:53 +0200 Subject: [PATCH] OP-1477 - GPS SYS: for multi device support of HMC5x driver --- flight/modules/gpsp/gpsdsysmod.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/flight/modules/gpsp/gpsdsysmod.c b/flight/modules/gpsp/gpsdsysmod.c index 9997af14d..815b9aaf3 100644 --- a/flight/modules/gpsp/gpsdsysmod.c +++ b/flight/modules/gpsp/gpsdsysmod.c @@ -62,13 +62,13 @@ extern uint32_t pios_com_main_id; static bool getLastSentence(uint8_t *data, uint16_t bufferCount, uint8_t * *lastSentence, uint16_t *lenght); extern struct pios_flash_driver pios_jedec_flash_driver; +extern pios_hmc5x83_dev_t onboard_mag; extern uintptr_t flash_id; // Private constants #define SYSTEM_UPDATE_PERIOD_MS 2 #define STACK_SIZE_BYTES 450 -#define MAG_RATE 50 #define STAT_RATE 1 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define BUFFER_SIZE 200 @@ -333,19 +333,16 @@ void vApplicationMallocFailedHook(void) void ReadMag() { - static uint32_t lastUpdate; - - if (PIOS_DELAY_DiffuS(lastUpdate) < 1000 * configTICK_RATE_HZ / MAG_RATE) { + if (!PIOS_HMC5x83_NewDataAvailable(onboard_mag)) { return; } - lastUpdate = PIOS_DELAY_GetRaw(); static int16_t mag[3]; - if (PIOS_HMC5x83_ReadMag(mag) == 0) { + if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) { MagUbxPkt magPkt; // swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned - magPkt.fragments.data.X = mag[0]; - magPkt.fragments.data.Y = - mag[1]; + magPkt.fragments.data.X = -mag[1]; + magPkt.fragments.data.Y = mag[0]; magPkt.fragments.data.Z = mag[2]; magPkt.fragments.data.status = 1; ubx_build_packet(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData)); @@ -421,7 +418,6 @@ typedef struct { } ubx_init_sentence; - const ubx_init_sentence gps_config[] = { [0] = { .sentence = (uint8_t *)cfg_settings,