Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
mag_uavcan.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2025 FabienB <fabien-b@github.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
25
27#include "uavcan/uavcan.h"
28#include "core/abi.h"
29#include "uavcan.equipment.ahrs.MagneticFieldStrength.h"
30#include "uavcan.equipment.ahrs.MagneticFieldStrength2.h"
31#include "imu/imu.h"
33
36
37static struct FloatVect3 default_uavcan_mag_scale = {1,1,1};
38
39
40static void mag_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
41 // current timestamp
42 uint32_t now_ts = get_sys_time_usec();
43
44 struct uavcan_equipment_ahrs_MagneticFieldStrength msg;
45 if(uavcan_equipment_ahrs_MagneticFieldStrength_decode(transfer, &msg)) {
46 return; // decode error
47 }
48
49 // The field is in gauss, scaled with 8192 (2**13) for the int32 raw message (the airframe calibration sets the real scale)
50 struct Int32Vect3 mag = {
51 msg.magnetic_field_ga[0] * 8192,
52 msg.magnetic_field_ga[1] * 8192,
53 msg.magnetic_field_ga[2] * 8192,
54 };
55
56 AbiSendMsgIMU_MAG_RAW(MAG_UAVCAN_SENDER_ID, now_ts, &mag);
57
58#if MAG_UAVCAN_SYNC_SEND
60 DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &abi_id, &mag.x, &mag.y, &mag.z);
61#endif
62
63}
64
65static void mag_uavcan_cb2(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
66 // current timestamp
67 uint32_t now_ts = get_sys_time_usec();
68
69 struct uavcan_equipment_ahrs_MagneticFieldStrength2 msg;
70 if(uavcan_equipment_ahrs_MagneticFieldStrength2_decode(transfer, &msg)) {
71 return; // decode error
72 }
73
74 // The field is in gauss, scaled with 8192 (2**13) for the int32 raw message (the airframe calibration sets the real scale)
75 struct Int32Vect3 mag = {
76 msg.magnetic_field_ga[0] * 8192,
77 msg.magnetic_field_ga[1] * 8192,
78 msg.magnetic_field_ga[2] * 8192,
79 };
80
81 // Same ABI id as MagneticFieldStrength: the node sensor_id (0, 1, ...) would be ABI_DISABLE or collide with other mags
82 AbiSendMsgIMU_MAG_RAW(MAG_UAVCAN_SENDER_ID, now_ts, &mag);
83
84#if MAG_UAVCAN_SYNC_SEND
86 DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &abi_id, &mag.x, &mag.y, &mag.z);
87#endif
88
89}
90
91
92
94{
95 struct Int32RMat mag_to_imu;
96 struct Int32Eulers mag_to_imu_eulers = {0,0,0};
97 int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
98
100
102 UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID,
103 UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE,
105
107 UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH2_ID,
108 UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH2_SIGNATURE,
110
111}
Main include for ABI (AirBorneInterface).
#define MAG_UAVCAN_SENDER_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
euler angles
rotation matrix
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 mag_uavcan_init(void)
Definition mag_uavcan.c:93
static uavcan_event mag_uavcan_ev
Definition mag_uavcan.c:34
static struct FloatVect3 default_uavcan_mag_scale
Definition mag_uavcan.c:37
static void mag_uavcan_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Definition mag_uavcan.c:40
static uavcan_event mag_uavcan_ev2
Definition mag_uavcan.c:35
static void mag_uavcan_cb2(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Definition mag_uavcan.c:65
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
Bind to a receiving message from uavcan.
Definition uavcan.c:272
struct uavcan_event_t uavcan_event
Definition uavcan.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
int transfer(const Mat *from, const image_t *to)