Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
21#include "modules/imu/imu_nps.h"
22#include "modules/imu/imu.h"
23#include "modules/core/abi.h"
24#include "generated/airframe.h"
25#include "modules/imu/imu_nps.h"
26#include "nps_sensors.h"
27
29
30void imu_nps_init(void)
31{
32
33 imu_nps.gyro_available = false;
34 imu_nps.mag_available = false;
35 imu_nps.accel_available = false;
36
37// Set the default scaling
38 const struct FloatRates gyro_scale_f = {RATE_FLOAT_OF_BFP(NPS_GYRO_SENSITIVITY_PP), RATE_FLOAT_OF_BFP(NPS_GYRO_SENSITIVITY_QQ), RATE_FLOAT_OF_BFP(NPS_GYRO_SENSITIVITY_RR)};
39 const struct Int32Rates gyro_neutral = {NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R};
40 const struct FloatVect3 accel_scale_f = {ACCEL_FLOAT_OF_BFP(NPS_ACCEL_SENSITIVITY_XX), ACCEL_FLOAT_OF_BFP(NPS_ACCEL_SENSITIVITY_YY), ACCEL_FLOAT_OF_BFP(NPS_ACCEL_SENSITIVITY_ZZ)};
41 const struct Int32Vect3 accel_neutral = {NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z};
42 const struct FloatVect3 mag_scale_f = {MAG_FLOAT_OF_BFP(NPS_MAG_SENSITIVITY_XX), MAG_FLOAT_OF_BFP(NPS_MAG_SENSITIVITY_YY), MAG_FLOAT_OF_BFP(NPS_MAG_SENSITIVITY_ZZ)};
43 const struct Int32Vect3 mag_neutral = {NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z};
44 imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, &gyro_scale_f);
45 imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, &accel_scale_f);
46 imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, &mag_scale_f);
47}
48
49
51{
52 RATES_ASSIGN(imu_nps.gyro, sensors.gyro.value.x, sensors.gyro.value.y, sensors.gyro.value.z);
53 VECT3_COPY(imu_nps.accel, sensors.accel.value);
54
55 // set availability flags...
56 imu_nps.accel_available = true;
57 imu_nps.gyro_available = true;
58}
59
60
61void imu_feed_mag(void)
62{
63 VECT3_COPY(imu_nps.mag, sensors.mag.value);
64 imu_nps.mag_available = true;
65}
66
67
68void imu_nps_event(void)
69{
70 uint32_t now_ts = get_sys_time_usec();
71 if (imu_nps.gyro_available) {
72 AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1, NPS_PROPAGATE, NAN);
73 imu_nps.gyro_available = false;
74 }
75 if (imu_nps.accel_available) {
76 AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1, NPS_PROPAGATE, NAN);
77 imu_nps.accel_available = false;
78 }
79 if (imu_nps.mag_available) {
80 AbiSendMsgIMU_MAG_RAW(IMU_NPS_ID, now_ts, &imu_nps.mag);
81 imu_nps.mag_available = false;
82 }
83}
Main include for ABI (AirBorneInterface).
#define IMU_NPS_ID
#define sensors(...)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
angular rates
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define VECT3_COPY(_a, _b)
#define RATE_FLOAT_OF_BFP(_ai)
#define ACCEL_FLOAT_OF_BFP(_ai)
#define MAG_FLOAT_OF_BFP(_ai)
angular rates
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition imu.c:842
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition imu.c:871
void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to e...
Definition imu.c:900
Inertial Measurement Unit interface.
void imu_feed_mag(void)
Definition imu_nps.c:61
void imu_feed_gyro_accel(void)
Definition imu_nps.c:50
void imu_nps_event(void)
Definition imu_nps.c:68
struct ImuNps imu_nps
Definition imu_nps.c:28
void imu_nps_init(void)
Definition imu_nps.c:30
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.