Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 */
26
28
29#include "pprzlink/pprz_transport.h"
30#include "pprzlink/intermcu_msg.h"
31#include "mcu_periph/uart.h"
32#include "modules/core/abi.h"
33#include "mcu_periph/sys_time.h"
34#include "modules/imu/imu.h"
35#include "state.h"
36
37/* Main magneto structure */
38static struct mag_pitot_t mag_pitot = {
39 .device = (&((MAG_PITOT_PORT).device)),
40 .msg_available = false
41};
42static uint8_t mp_msg_buf[128] __attribute__((aligned));
43
44/* Initialize the magneto and pitot */
46 // Initialize transport protocol
47 pprz_transport_init(&mag_pitot.transport);
48
49 // Set the Imu to Magneto rotation
50 struct FloatEulers imu_to_mag_eulers =
52 orientationSetEulers_f(&mag_pitot.imu_to_mag, &imu_to_mag_eulers);
53}
54
55/* Parse the InterMCU message */
56static inline void mag_pitot_parse_msg(void)
57{
58 uint32_t now_ts = get_sys_time_usec();
59
60 /* Parse the mag-pitot message */
61 uint8_t msg_id = pprzlink_get_msg_id(mp_msg_buf);
62
63#if PPRZLINK_DEFAULT_VER == 2
64 // Check that the message is really a intermcu message
65 if (pprzlink_get_msg_class_id(mp_msg_buf) == DL_intermcu_CLASS_ID) {
66#endif
67 switch (msg_id) {
68 /* Got a magneto message */
69 case DL_IMCU_REMOTE_MAG: {
70 struct Int32Vect3 raw_mag, mag;
71
72 // Read the raw magneto information
73 raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
74 raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf);
75 raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf);
76
77 // Rotate the magneto
78 struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
79 int32_rmat_vmult(&mag, imu_to_mag_rmat, &raw_mag);
80
81 // Send and set the magneto IMU data
82 AbiSendMsgIMU_MAG_RAW(IMU_MAG_PITOT_ID, now_ts, &mag);
83 break;
84 }
85
86 /* Got a barometer message */
87 case DL_IMCU_REMOTE_BARO: {
88 float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf);
89 float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf);
90
91 AbiSendMsgBARO_ABS_RAW(IMU_MAG_PITOT_ID, now_ts, pitot_stat);
92 AbiSendMsgTEMPERATURE_RAW(IMU_MAG_PITOT_ID, now_ts, pitot_temp, TEMPERATURE_TYPE_DIFF);
93 break;
94 }
95
96 /* Got an airspeed message */
97 case DL_IMCU_REMOTE_AIRSPEED: {
98 // Should be updated to differential pressure
99 float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf);
100 AbiSendMsgAIRSPEED_RAW(IMU_MAG_PITOT_ID, now_ts, pitot_ias);
101 break;
102 }
103
104 default:
105 break;
106 }
107#if PPRZLINK_DEFAULT_VER == 2
108 }
109#endif
110}
111
112/* We need to wait for incomming messages */
114 // Check if we got some message from the Magneto or Pitot
115 pprz_check_and_parse(mag_pitot.device, &mag_pitot.transport, mp_msg_buf, &mag_pitot.msg_available);
116
117 // If we have a message we should parse it
118 if (mag_pitot.msg_available) {
120 mag_pitot.msg_available = false;
121 }
122}
123
124
Main include for ABI (AirBorneInterface).
#define TEMPERATURE_TYPE_DIFF
differential pressure (pitot) sensor
Definition abi_common.h:90
#define IMU_MAG_PITOT_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
euler angles
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
rotation matrix
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Inertial Measurement Unit interface.
void mag_pitot_init()
static void mag_pitot_parse_msg(void)
static uint8_t mp_msg_buf[128]
The message buffer for the Magneto and pitot.
static struct mag_pitot_t mag_pitot
void mag_pitot_event()
bool msg_available
If we received a message.
#define IMU_TO_MAG_THETA
#define IMU_TO_MAG_PSI
#define IMU_TO_MAG_PHI
API to get/set the generic vehicle states.
Architecture independent timing functions.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.