Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu_hbmini.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Paparazzi Team
3  * Derived from Aspirin, NavGo and ppzuavimu drivers
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
33 #include <math.h>
34 #include "imu_hbmini.h"
35 #include "mcu_periph/i2c.h"
36 #include "led.h"
37 #include "subsystems/abi.h"
38 
39 // Downlink
40 #include "mcu_periph/uart.h"
41 #include "pprzlink/messages.h"
43 
44 
45 #if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined IMU_GYRO_R_CHAN
46 #define IMU_GYRO_P_CHAN 3
47 #define IMU_GYRO_Q_CHAN 4
48 #define IMU_GYRO_R_CHAN 5
49 #endif
50 #if !defined IMU_ACCEL_X_CHAN & !defined IMU_ACCEL_Y_CHAN & !defined IMU_ACCEL_Z_CHAN
51 #define IMU_ACCEL_X_CHAN 0
52 #define IMU_ACCEL_Y_CHAN 1
53 #define IMU_ACCEL_Z_CHAN 2
54 #endif
55 #if !defined IMU_MAG_X_CHAN & !defined IMU_MAG_Y_CHAN & !defined IMU_MAG_Z_CHAN
56 #define IMU_MAG_X_CHAN 2
57 #define IMU_MAG_Y_CHAN 0
58 #define IMU_MAG_Z_CHAN 1
59 #endif
60 
62 
63 void imu_hbmini_init(void)
64 {
65  max1168_init();
66 
68  // HMC58XX
69  hmc58xx_init(&imu_hbmini.hmc, &(IMU_HBMINI_I2C_DEV), HMC58XX_ADDR);
70 }
71 
73 {
74 
76 
77  // Read HMC58XX at 100Hz (main loop for rotorcraft: 512Hz)
78  RunOnceEvery(5, hmc58xx_periodic(&imu_hbmini.hmc));
79 
80  //RunOnceEvery(20,imu_hbmini_downlink_raw());
81 }
82 
83 
85 {
86  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q,
88  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y,
90  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z);
91 }
92 
93 void imu_hbmini_event(void)
94 {
95  uint32_t now_ts = get_sys_time_usec();
96 
97  max1168_event();
98 
109  AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
110  AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
111  }
112 
113  // HMC58XX event task
119 
120  imu_hbmini.hmc.data_available = false;
121  imu_scale_mag(&imu);
122  AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
123  }
124 
125 }
126 
void max1168_init(void)
Definition: max1168.c:40
void max1168_event(void)
Definition: max1168.c:101
#define IMU_MAG_Y_CHAN
Definition: imu_hbmini.c:57
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
volatile bool data_available
data ready flag
Definition: hmc58xx.h:65
uint16_t max1168_values[MAX1168_NB_CHAN]
Definition: max1168.c:28
#define IMU_BOARD_ID
#define IMU_GYRO_P_CHAN
Definition: imu_hbmini.c:46
Main include for ABI (AirBorneInterface).
void imu_hbmini_event(void)
Definition: imu_hbmini.c:93
struct Imu imu
global IMU state
Definition: imu.c:108
union Hmc58xx::@303 data
void imu_hbmini_periodic(void)
Definition: imu_hbmini.c:72
#define MAX1168_IDLE
Definition: max1168.h:59
#define HMC58XX_ADDR
Definition: hmc58xx_regs.h:31
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
int32_t r
in rad/s with INT32_RATE_FRAC
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
struct Hmc58xx hmc
Definition: imu_hbmini.h:62
#define IMU_ACCEL_Y_CHAN
Definition: imu_hbmini.c:52
struct ImuHbmini imu_hbmini
Definition: imu_hbmini.c:61
void imu_hbmini_downlink_raw(void)
Definition: imu_hbmini.c:84
#define IMU_GYRO_Q_CHAN
Definition: imu_hbmini.c:47
unsigned long uint32_t
Definition: types.h:18
#define IMU_GYRO_R_CHAN
Definition: imu_hbmini.c:48
#define Max1168Periodic()
Definition: max1168.h:68
#define IMU_MAG_Z_CHAN
Definition: imu_hbmini.c:58
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
#define IMU_ACCEL_Z_CHAN
Definition: imu_hbmini.c:53
volatile uint8_t max1168_status
Definition: max1168.c:27
void imu_hbmini_init(void)
Definition: imu_hbmini.c:63
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:351
#define MAX1168_DATA_AVAILABLE
Definition: max1168.h:63
static void hmc58xx_periodic(struct Hmc58xx *hmc)
convenience function: read or start configuration if not already initialized
Definition: hmc58xx.h:85
#define IMU_MAG_X_CHAN
Definition: imu_hbmini.c:56
int32_t p
in rad/s with INT32_RATE_FRAC
arch independent LED (Light Emitting Diodes) API
void hmc58xx_init(struct Hmc58xx *hmc, struct i2c_periph *i2c_p, uint8_t addr)
Initialize Hmc58xx struct and set default config options.
Definition: hmc58xx.c:75
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
void imu_scale_accel(struct Imu *_imu)
Definition: imu_vectornav.c:44
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
void hmc58xx_event(struct Hmc58xx *hmc)
Definition: hmc58xx.c:152
#define IMU_ACCEL_X_CHAN
Definition: imu_hbmini.c:51
int32_t q
in rad/s with INT32_RATE_FRAC
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
Architecture independent I2C (Inter-Integrated Circuit Bus) API.