flight/modules/gpsp/gps9maghandler.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       gps9maghandler.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @brief      handles GPSV9 onboard magnetometer and sends its data.
   7  *             --
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26 #include <openpilot.h>
  27 #include <pios_struct_helper.h>
  28 #include <pios_helpers.h>
  29 #include <ubx_utils.h>
  30 #include <pios_hmc5x83.h>
  31 #include "inc/gps9protocol.h"
  32 #define MAG_RATE_HZ 30
  33 extern pios_hmc5x83_dev_t onboard_mag;
  34
  35 void handleMag()
  36 {
  37 #ifdef PIOS_HMC5X83_HAS_GPIOS
  38     if (!PIOS_HMC5x83_NewDataAvailable(onboard_mag)) {
  39         return;
  40     }
  41 #else
  42     static uint32_t lastUpdate = 0;
  43     if (PIOS_DELAY_DiffuS(lastUpdate) < (1000000 / MAG_RATE_HZ)) {
  44         return;
  45     }
  46     lastUpdate = PIOS_DELAY_GetRaw();
  47 #endif
  48     static int16_t mag[3];
  49
  50     if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
  51         MagUbxPkt magPkt;
  52         magPkt.fragments.data.X = mag[0];
  53         magPkt.fragments.data.Y = mag[1];
  54         magPkt.fragments.data.Z = mag[2];
  55         magPkt.fragments.data.status = 1;
  56         ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData));
  57         PIOS_COM_SendBuffer(pios_com_main_id, magPkt.packet.binarystream, sizeof(MagUbxPkt));
  58         return;
  59     }
  60 }