Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
imu_nps.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Felix Ruess <felix.ruess@gmail.com
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
22 #include "modules/imu/imu_nps.h"
23 #include "modules/imu/imu.h"
24 #include "modules/core/abi.h"
25 #include "generated/airframe.h"
26 #include "modules/imu/imu_nps.h"
27 #include "nps_sensors.h"
28 
29 struct ImuNps imu_nps;
30 
31 void imu_nps_init(void)
32 {
33 
34  imu_nps.gyro_available = false;
35  imu_nps.mag_available = false;
36  imu_nps.accel_available = false;
37 
38  // Set the default scaling
39  const struct Int32Rates gyro_scale[2] = {
40  {NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM},
41  {NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN}
42  };
43  const struct Int32Rates gyro_neutral = {
44  NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R
45  };
46  const struct Int32Vect3 accel_scale[2] = {
47  {NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM},
48  {NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN}
49  };
50  const struct Int32Vect3 accel_neutral = {
51  NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z
52  };
53  const struct Int32Vect3 mag_scale[2] = {
54  {NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM},
55  {NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN}
56  };
57  const struct Int32Vect3 mag_neutral = {
58  NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z
59  };
60  imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, gyro_scale);
62  imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, mag_scale);
63 }
64 
65 
67 {
68 
69  RATES_ASSIGN(imu_nps.gyro, NPS_GYRO_SIGN_P * sensors.gyro.value.x, NPS_GYRO_SIGN_Q * sensors.gyro.value.y, NPS_GYRO_SIGN_R * sensors.gyro.value.z);
70  VECT3_ASSIGN(imu_nps.accel, NPS_ACCEL_SIGN_X * sensors.accel.value.x, NPS_ACCEL_SIGN_Y * sensors.accel.value.y, NPS_ACCEL_SIGN_Z * sensors.accel.value.z);
71 
72  // set availability flags...
73  imu_nps.accel_available = true;
74  imu_nps.gyro_available = true;
75 
76 }
77 
78 
79 void imu_feed_mag(void)
80 {
81 
82  VECT3_ASSIGN(imu_nps.mag, NPS_MAG_SIGN_X * sensors.mag.value.x, NPS_MAG_SIGN_Y * sensors.mag.value.y, NPS_MAG_SIGN_Z * sensors.mag.value.z);
83  imu_nps.mag_available = true;
84 
85 }
86 
87 void imu_nps_event(void)
88 {
89  uint32_t now_ts = get_sys_time_usec();
90  if (imu_nps.gyro_available) {
91  AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1, NAN);
92  imu_nps.gyro_available = false;
93  }
95  AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1, NAN);
96  imu_nps.accel_available = false;
97  }
98  if (imu_nps.mag_available) {
99  AbiSendMsgIMU_MAG_RAW(IMU_NPS_ID, now_ts, &imu_nps.mag);
100  imu_nps.mag_available = false;
101  }
102 }
Main include for ABI (AirBorneInterface).
#define IMU_NPS_ID
#define sensors(...)
Definition: cc2500_compat.h:68
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
angular rates
void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to e...
Definition: imu.c:470
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition: imu.c:440
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition: imu.c:410
Inertial Measurement Unit interface.
void imu_feed_mag(void)
Definition: imu_nps.c:79
void imu_feed_gyro_accel(void)
Definition: imu_nps.c:66
void imu_nps_event(void)
Definition: imu_nps.c:87
struct ImuNps imu_nps
Definition: imu_nps.c:29
void imu_nps_init(void)
Definition: imu_nps.c:31
struct Int32Rates gyro
Definition: imu_nps.h:34
struct Int32Vect3 accel
Definition: imu_nps.h:35
uint8_t gyro_available
Definition: imu_nps.h:32
struct Int32Vect3 mag
Definition: imu_nps.h:36
uint8_t mag_available
Definition: imu_nps.h:30
uint8_t accel_available
Definition: imu_nps.h:31
Definition: imu_nps.h:29
static const struct Int32Vect3 accel_neutral
Definition: navdata.c:104
static const struct Int32Vect3 mag_scale[2]
Default mag scale.
Definition: navdata.c:111
static const struct Int32Rates gyro_scale[2]
Default gyro scale.
Definition: navdata.c:92
static const struct Int32Vect3 accel_scale[2]
Default accel scale/neutral.
Definition: navdata.c:100
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78