Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
sensors_hitl.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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
22#include "modules/imu/imu.h"
23#include "modules/gps/gps.h"
24#include "modules/core/abi.h"
25#include "mcu_periph/sys_time.h"
27#include "generated/airframe.h"
30#include "nps_sensors_params_common.h"
31#if USE_BATTERY_MONITOR
33#endif
34
44
46struct GpsState gps_hitl = {0};
48
49static bool sensors_hitl_msg_available = false;
50static uint8_t sensors_hitl_dl_buffer[DATALINK_MSG_SIZE] __attribute__((aligned));
51static struct pprz_transport sensors_hitl_tp;
52
54{
55 pprz_transport_init(&sensors_hitl_tp); // for receiving only
56
57 gps_has_fix = true;
58
59 imu_hitl.gyro_available = false;
60 imu_hitl.mag_available = false;
61 imu_hitl.accel_available = false;
62
63// Set the default scaling
64 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)};
65 const struct Int32Rates gyro_neutral = {NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R};
66 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)};
67 const struct Int32Vect3 accel_neutral = {NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z};
68 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)};
69 const struct Int32Vect3 mag_neutral = {NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z};
70
71 imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, &gyro_scale_f);
72 imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, &accel_scale_f);
73 imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, &mag_scale_f);
74}
75
77#if USE_BATTERY_MONITOR
78#ifdef MAX_BAT_LEVEL
79 // init vsupply to MAX_BAT in case battery voltage is not available
80 electrical.vsupply = MAX_BAT_LEVEL;
81#else
82 electrical.vsupply = 12.f; // 3S battery
83#endif
84#endif
85}
86
88{
89 if (DL_HITL_IMU_ac_id(buf) != AC_ID) {
90 return;
91 }
92
94 NPS_GYRO_SIGN_P * DL_HITL_IMU_gp(buf),
95 NPS_GYRO_SIGN_Q * DL_HITL_IMU_gq(buf),
96 NPS_GYRO_SIGN_R * DL_HITL_IMU_gr(buf));
98 NPS_ACCEL_SIGN_X * DL_HITL_IMU_ax(buf),
99 NPS_ACCEL_SIGN_Y * DL_HITL_IMU_ay(buf),
100 NPS_ACCEL_SIGN_Z * DL_HITL_IMU_az(buf));
102 NPS_MAG_SIGN_X * DL_HITL_IMU_mx(buf),
103 NPS_MAG_SIGN_Y * DL_HITL_IMU_my(buf),
104 NPS_MAG_SIGN_Z * DL_HITL_IMU_mz(buf));
105
106 imu_hitl.accel_available = true;
107 imu_hitl.gyro_available = true;
108 imu_hitl.mag_available = true;
109}
110
112{
113 if (DL_HITL_GPS_ac_id(buf) != AC_ID) {
114 return;
115 }
116
117 gps_hitl.week = 1794;
118 gps_hitl.tow = DL_HITL_GPS_time(buf) * 1000;
119
120 gps_hitl.ecef_vel.x = DL_HITL_GPS_ecef_vel_x(buf) * 100.;
121 gps_hitl.ecef_vel.y = DL_HITL_GPS_ecef_vel_y(buf) * 100.;
122 gps_hitl.ecef_vel.z = DL_HITL_GPS_ecef_vel_z(buf) * 100.;
123 SetBit(gps_hitl.valid_fields, GPS_VALID_VEL_ECEF_BIT);
124
125 gps_hitl.lla_pos.lat = DL_HITL_GPS_lat(buf) * 1e7;
126 gps_hitl.lla_pos.lon = DL_HITL_GPS_lon(buf) * 1e7;
127 gps_hitl.lla_pos.alt = DL_HITL_GPS_alt(buf) * 1000.;
128 SetBit(gps_hitl.valid_fields, GPS_VALID_POS_LLA_BIT);
129
130 gps_hitl.hmsl = DL_HITL_GPS_hmsl(buf) * 1000.;
131 SetBit(gps_hitl.valid_fields, GPS_VALID_HMSL_BIT);
132
133 /* calc NED speed from ECEF */
134 struct LtpDef_i ref_ltp;
135 ltp_def_from_lla_i(&ref_ltp, &gps_hitl.lla_pos);
136 struct NedCoor_i ned_vel_i;
137 ned_of_ecef_vect_i(&ned_vel_i, &ref_ltp, &gps_hitl.ecef_vel);
138 gps_hitl.ned_vel.x = ned_vel_i.x;
139 gps_hitl.ned_vel.y = ned_vel_i.y;
140 gps_hitl.ned_vel.z = ned_vel_i.z;
141 SetBit(gps_hitl.valid_fields, GPS_VALID_VEL_NED_BIT);
142 struct NedCoor_f ned_vel_f;
143 VECT3_FLOAT_OF_CM(ned_vel_f, gps_hitl.ned_vel);
144
145 /* horizontal and 3d ground speed in cm/s */
146 gps_hitl.gspeed = sqrtf(ned_vel_f.x * ned_vel_f.x + ned_vel_f.y * ned_vel_f.y) * 100;
147 gps_hitl.speed_3d = sqrtf(ned_vel_f.x * ned_vel_f.x + ned_vel_f.y * ned_vel_f.y + ned_vel_f.z * ned_vel_f.z) * 100;
148
149 /* ground course in radians * 1e7 */
150 gps_hitl.course = atan2f(ned_vel_f.y, ned_vel_f.x) * 1e7;
151 SetBit(gps_hitl.valid_fields, GPS_VALID_COURSE_BIT);
152
153 gps_hitl.pacc = 650;
154 gps_hitl.hacc = 450;
155 gps_hitl.vacc = 200;
156 gps_hitl.sacc = 100;
157 gps_hitl.pdop = 650;
158
159 if (gps_has_fix) {
160 gps_hitl.num_sv = 11;
161 gps_hitl.fix = GPS_FIX_3D;
162 } else {
163 gps_hitl.num_sv = 1;
165 }
166
167 // publish gps data
168 uint32_t now_ts = get_sys_time_usec();
169 gps_hitl.last_msg_ticks = sys_time.nb_sec_rem;
170 gps_hitl.last_msg_time = sys_time.nb_sec;
171 if (gps_hitl.fix == GPS_FIX_3D) {
172 gps_hitl.last_3dfix_ticks = sys_time.nb_sec_rem;
173 gps_hitl.last_3dfix_time = sys_time.nb_sec;
174 }
175 AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps_hitl);
176}
177
179 if (DL_HITL_AIR_DATA_ac_id(buf) != AC_ID) {
180 return;
181 }
182
183 uint8_t flag = DL_HITL_AIR_DATA_update_flag(buf);
184 if (bit_is_set(flag, 0)) {
185 uint32_t now_ts = get_sys_time_usec();
186 float pressure = DL_HITL_AIR_DATA_baro(buf);
187 AbiSendMsgBARO_ABS_RAW(BARO_SIM_SENDER_ID, now_ts, pressure);
188 }
189 if (bit_is_set(flag, 1)) {
190 AbiSendMsgAIRSPEED_RAW(AIRSPEED_NPS_ID, get_sys_time_usec(), DL_HITL_AIR_DATA_airspeed(buf));
191 }
192 if (bit_is_set(flag, 2) || bit_is_set(flag, 3)) {
193 uint8_t incidence_flag = (flag >> 2) & (0x3);
194 float aoa = DL_HITL_AIR_DATA_aoa(buf);
195 float sideslip = DL_HITL_AIR_DATA_sideslip(buf);
196 AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, incidence_flag, aoa, sideslip);
197 }
198}
199
200
202{
203 uint32_t now_ts = get_sys_time_usec();
204 if (imu_hitl.gyro_available) {
205 AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_hitl.gyro, 1, NPS_PROPAGATE, NAN);
206 imu_hitl.gyro_available = false;
207 }
208 if (imu_hitl.accel_available) {
209 AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_hitl.accel, 1, NPS_PROPAGATE, NAN);
210 imu_hitl.accel_available = false;
211 }
212 if (imu_hitl.mag_available) {
213 AbiSendMsgIMU_MAG_RAW(IMU_NPS_ID, now_ts, &imu_hitl.mag);
214 imu_hitl.mag_available = false;
215 }
216
217 // parse incoming messages
218 pprz_check_and_parse(&HITL_DEVICE.device, &sensors_hitl_tp, sensors_hitl_dl_buffer, &sensors_hitl_msg_available);
220}
221
223void imu_feed_mag(void) {}
224void gps_feed_value(void) {}
225
Main include for ABI (AirBorneInterface).
#define BARO_SIM_SENDER_ID
#define IMU_NPS_ID
#define INCIDENCE_NPS_ID
#define GPS_SIM_ID
#define AIRSPEED_NPS_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
Device independent GPS code (interface).
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
data structure for GPS information
Definition gps.h:90
bool gps_has_fix
Definition gps_sim_nps.c:31
angular rates
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define RATE_FLOAT_OF_BFP(_ai)
#define ACCEL_FLOAT_OF_BFP(_ai)
#define MAG_FLOAT_OF_BFP(_ai)
angular rates
int32_t z
Down.
int32_t y
East.
int32_t x
North.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
#define VECT3_FLOAT_OF_CM(_o, _i)
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
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.
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
static uint8_t sensors_hitl_dl_buffer[DATALINK_MSG_SIZE]
static bool sensors_hitl_msg_available
void imu_feed_mag(void)
uint8_t accel_available
void sensors_hitl_event(void)
struct ImuHitl imu_hitl
void sensors_hitl_periodic(void)
void imu_feed_gyro_accel(void)
void gps_feed_value(void)
uint8_t mag_available
struct Int32Vect3 accel
uint8_t gyro_available
void sensors_hitl_parse_HITL_AIR_DATA(uint8_t *buf)
void sensors_hitl_parse_HITL_GPS(uint8_t *buf)
struct Int32Vect3 mag
struct Int32Rates gyro
struct GpsState gps_hitl
void sensors_hitl_parse_HITL_IMU(uint8_t *buf)
void sensors_hitl_init(void)
static struct pprz_transport sensors_hitl_tp
Architecture independent timing functions.
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:81
Periodic telemetry system header (includes downlink utility and generated code).
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.