Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
mag_pitot_uart.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) C. De Wagter
3  * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
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, see
19  * <http://www.gnu.org/licenses/>.
20  */
28 
29 #include "pprzlink/pprz_transport.h"
30 #include "pprzlink/intermcu_msg.h"
31 #include "mcu_periph/uart.h"
32 #include "subsystems/abi.h"
33 #include "subsystems/imu.h"
34 #include "state.h"
35 
36 /* Main magneto structure */
37 static struct mag_pitot_t mag_pitot = {
38  .device = (&((MAG_PITOT_PORT).device)),
39  .msg_available = false
40 };
41 static uint8_t mp_msg_buf[128] __attribute__((aligned));
42 
43 
44 #if PERIODIC_TELEMETRY
46 
47 static void mag_pitot_raw_downlink(struct transport_tx *trans, struct link_device *dev)
48 {
49  pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mag_unscaled.x, &imu.mag_unscaled.y,
50  &imu.mag_unscaled.z);
51 }
52 #endif
53 
54 /* Initialize the magneto and pitot */
56  // Initialize transport protocol
57  pprz_transport_init(&mag_pitot.transport);
58 
59  // Set the Imu to Magneto rotation
60  struct FloatEulers imu_to_mag_eulers =
62  orientationSetEulers_f(&mag_pitot.imu_to_mag, &imu_to_mag_eulers);
63 
64 #if PERIODIC_TELEMETRY
66 #endif
67 }
68 
69 /* Parse the InterMCU message */
70 static inline void mag_pitot_parse_msg(void)
71 {
72  uint32_t now_ts = get_sys_time_usec();
73 
74  /* Parse the mag-pitot message */
75  uint8_t msg_id = pprzlink_get_msg_id(mp_msg_buf);
76 
77 #if PPRZLINK_DEFAULT_VER == 2
78  // Check that the message is really a intermcu message
79  if (pprzlink_get_msg_class_id(mp_msg_buf) == DL_intermcu_CLASS_ID) {
80 #endif
81  switch (msg_id) {
82  /* Got a magneto message */
83  case DL_IMCU_REMOTE_MAG: {
84  struct Int32Vect3 raw_mag;
85 
86  // Read the raw magneto information
87  raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
88  raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf);
89  raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf);
90 
91  // Rotate the magneto
92  struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
93  int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag);
94 
95  // Send and set the magneto IMU data
97  AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag);
98  break;
99  }
100 
101  /* Got a barometer message */
102  case DL_IMCU_REMOTE_BARO: {
103  float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf);
104  float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf);
105 
106  AbiSendMsgBARO_ABS(IMU_MAG_PITOT_ID, now_ts, pitot_stat);
107  AbiSendMsgTEMPERATURE(IMU_MAG_PITOT_ID, pitot_temp);
108  break;
109  }
110 
111  /* Got an airspeed message */
112  case DL_IMCU_REMOTE_AIRSPEED: {
113  // Should be updated to differential pressure
114  float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf);
115  AbiSendMsgAIRSPEED(IMU_MAG_PITOT_ID, pitot_ias);
116  break;
117  }
118 
119  default:
120  break;
121  }
122 #if PPRZLINK_DEFAULT_VER == 2
123  }
124 #endif
125 }
126 
127 /* We need to wait for incomming messages */
129  // Check if we got some message from the Magneto or Pitot
131 
132  // If we have a message we should parse it
133  if (mag_pitot.msg_available) {
135  mag_pitot.msg_available = false;
136  }
137 }
138 
139 
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
mag_pitot_t::transport
struct pprz_transport transport
The transport layer (PPRZ)
Definition: mag_pitot_uart.h:45
mag_pitot_t::msg_available
bool msg_available
If we received a message.
Definition: mag_pitot_uart.h:47
mp_msg_buf
static uint8_t mp_msg_buf[128]
The message buffer for the Magneto and pitot.
Definition: mag_pitot_uart.c:41
abi.h
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
mag_pitot_t
Definition: mag_pitot_uart.h:43
mag_pitot_parse_msg
static void mag_pitot_parse_msg(void)
Definition: mag_pitot_uart.c:70
uint32_t
unsigned long uint32_t
Definition: types.h:18
mag_pitot_t::imu_to_mag
struct OrientationReps imu_to_mag
IMU to magneto translation.
Definition: mag_pitot_uart.h:46
IMU_TO_MAG_THETA
#define IMU_TO_MAG_THETA
Definition: mag_pitot_uart.h:38
IMU_MAG_PITOT_ID
#define IMU_MAG_PITOT_ID
Definition: abi_sender_ids.h:205
mag_pitot_t::device
struct link_device * device
The device which is uses for communication.
Definition: mag_pitot_uart.h:44
IMU_TO_MAG_PHI
#define IMU_TO_MAG_PHI
Definition: mag_pitot_uart.h:37
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
telemetry.h
imu.h
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
mag_pitot_init
void mag_pitot_init()
Definition: mag_pitot_uart.c:55
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Int32Vect3
Definition: pprz_algebra_int.h:88
uint8_t
unsigned char uint8_t
Definition: types.h:14
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
orientationSetEulers_f
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
Definition: pprz_orientation_conversion.h:189
imu
struct Imu imu
global IMU state
Definition: imu.c:108
int32_rmat_vmult
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_int.c:107
IMU_TO_MAG_PSI
#define IMU_TO_MAG_PSI
Definition: mag_pitot_uart.h:39
mag_pitot_raw_downlink
static void mag_pitot_raw_downlink(struct transport_tx *trans, struct link_device *dev)
Definition: mag_pitot_uart.c:47
imu_scale_mag
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
orientationGetRMat_i
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Definition: pprz_orientation_conversion.h:207
mag_pitot_event
void mag_pitot_event()
Definition: mag_pitot_uart.c:128
Imu::mag
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Imu::mag_unscaled
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
state.h
mag_pitot
static struct mag_pitot_t mag_pitot
Definition: mag_pitot_uart.c:37
mag_pitot_uart.h
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66