Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
intermcu_fbw.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
3 * Copyright (C) 2022 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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 *
21 */
22
26
28#include "main_fbw.h"
29#include "pprzlink/intermcu_msg.h"
30#include "generated/modules.h"
33#if TELEMETRY_INTERMCU
35#endif
37#include "modules/core/abi.h"
38
39
41
42#include BOARD_CONFIG
43
44#if RADIO_CONTROL_NB_CHANNEL > 8
45//#undef RADIO_CONTROL_NB_CHANNEL
46//#define RADIO_CONTROL_NB_CHANNEL 8
47INFO("InterMCU UART will only send 8 radio channels!")
48#endif
49
50/* Main InterMCU structure */
52 .device = (&((INTERMCU_LINK).device)),
53 .enabled = true,
54 .msg_available = false
55};
56uint8_t imcu_msg_buf[256] __attribute__((aligned));
57
60
61
62// ABI callback for radio control
63#ifndef IMCU_RADIO_CONTROL_ID
64#define IMCU_RADIO_CONTROL_ID ABI_BROADCAST
65#endif
67static void rc_cb(uint8_t sender_id, struct RadioControl *rc);
68
69// ABI callback for GPS
70#ifdef USE_GPS
71#include "modules/gps/gps.h"
72#ifndef IMCU_GPS_ID
73#define IMCU_GPS_ID GPS_MULTI_ID
74#endif
76static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
77#endif
78
79void intermcu_init(void)
80{
81 pprz_transport_init(&intermcu.transport);
82
83 AbiBindMsgRADIO_CONTROL(IMCU_RADIO_CONTROL_ID, &rc_ev, rc_cb);
84#if USE_GPS
85 AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb);
86#endif
87}
88
90{
91 /* Check for interMCU loss */
92 if (intermcu.time_since_last_frame >= INTERMCU_LOST_CNT) {
93 intermcu.status = INTERMCU_LOST;
94 } else {
95 intermcu.time_since_last_frame++;
96 }
97}
98
100{
101 uint8_t i, c;
102
103 // Check if there are messages in the device
104 if (intermcu.device->char_available(intermcu.device->periph)) {
105 while (intermcu.device->char_available(intermcu.device->periph) && !intermcu.transport.trans_rx.msg_received) {
106 c = intermcu.device->get_byte(intermcu.device->periph);
107 parse_pprz(&intermcu.transport, c);
108 }
109
110 // If we have a message copy it
111 if (intermcu.transport.trans_rx.msg_received) {
112 for (i = 0; i < intermcu.transport.trans_rx.payload_len; i++) {
113 imcu_msg_buf[i] = intermcu.transport.trans_rx.payload[i];
114 }
115
116 intermcu.msg_available = true;
117 intermcu.transport.trans_rx.msg_received = false;
118 }
119 }
120
121 if (intermcu.msg_available) {
122 uint8_t class_id = pprzlink_get_msg_class_id(imcu_msg_buf);
123 if (class_id == DL_intermcu_CLASS_ID) {
124 // parse intermcu messages and call callbacks
125 dl_parse_msg(intermcu.device, &intermcu.transport.trans_tx, imcu_msg_buf);
126#if TELEMETRY_INTERMCU
127 } else {
128 // forward all other messages if needed
129 intermcu_dl_on_msg(imcu_msg_buf, intermcu.transport.trans_rx.payload_len);
130#endif
131 }
132 intermcu.msg_available = false;
133 }
134}
135
137{
138#ifdef RADIO_CONTROL
139 uint8_t rc_status = radio_control.status;
140 uint8_t rc_rate = radio_control.frame_rate;
141#else
142 uint8_t rc_status = 0;
143 uint8_t rc_rate = 0;
144#endif
145 // Send Status
146 pprz_msg_send_IMCU_FBW_STATUS(&(intermcu.transport.trans_tx), intermcu.device, AC_ID,
147 &fbw_mode, &rc_status, &rc_rate,
148 &electrical.vsupply, &electrical.current);
149}
150
152{
153 uint8_t size = DL_IMCU_COMMANDS_values_length(buf);
154 int16_t *new_commands = DL_IMCU_COMMANDS_values(buf);
155 intermcu.cmd_status |= DL_IMCU_COMMANDS_status(buf);
156
157 // Read the autopilot status and then clear it
160
161 for (int i = 0; i < size; i++) {
162 intermcu_commands[i] = new_commands[i];
163 }
164
165 intermcu.status = INTERMCU_OK;
166 intermcu.time_since_last_frame = 0;
167}
168
169void intermcu_parse_IMCU_SPEKTRUM_SOFT_BIND(uint8_t *buf __attribute__((unused)))
170{
171#if defined(SPEKTRUM_HAS_SOFT_BIND_PIN)
173#endif
174}
175
176void intermcu_forward_uplink(uint8_t *buf __attribute__((unused)))
177{
178 // forward all incoming messages to intermcu for AP side
179#if TELEMETRY_INTERMCU
180 uint8_t size = (DOWNLINK_TRANSPORT).trans_rx.payload_len; // FIXME is it always a valid length ?
181 intermcu_dl_repack(&(intermcu.transport.trans_tx), intermcu.device, buf, size);
182#endif
183}
184
185static void rc_cb(uint8_t sender_id __attribute__((unused)), struct RadioControl *rc __attribute__((unused)))
186{
187#if RADIO_CONTROL
188 pprz_t values[9];
189
194#ifdef RADIO_MODE
196#endif
197#ifdef RADIO_KILL_SWITCH
199#endif
200
201#ifdef RADIO_AUX1
202#ifdef RADIO_KILL_SWITCH
203#warning "RC AUX1 and KILL_SWITCH are on the same channel. AUX1 is discarded."
204#else
206#endif
207#endif
208#ifdef RADIO_AUX2
210#endif
211#ifdef RADIO_AUX3
213#endif
214
215 pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device,
216 INTERMCU_FBW, &fbw_mode, INTERMCU_RADIO_CONTROL_NB_CHANNEL, values);
217#endif
218}
219
220#if USE_GPS
221static void gps_cb(uint8_t sender_id __attribute__((unused)),
222 uint32_t stamp __attribute__((unused)),
223 struct GpsState *gps_s) {
224 // ECEF from the LLA position and NED velocity when the receiver doesn't send it
225 struct EcefCoor_i ecef_pos = ecef_int_from_gps(gps_s);
226 struct EcefCoor_i ecef_vel = ecef_vel_int_from_gps(gps_s);
227 pprz_msg_send_IMCU_REMOTE_GPS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW,
228 &ecef_pos.x,
229 &ecef_pos.y,
230 &ecef_pos.z,
231 &gps_s->lla_pos.alt,
232 &gps_s->hmsl,
233 &ecef_vel.x,
234 &ecef_vel.y,
235 &ecef_vel.z,
236 &gps_s->course,
237 &gps_s->gspeed,
238 &gps_s->pacc,
239 &gps_s->sacc,
240 &gps_s->num_sv,
241 &gps_s->fix);
242}
243#endif
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event gps_ev
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Definition autopilot.c:58
static abi_event rc_ev
Definition autopilot.c:56
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (integer) Converted on the fly if not available.
Definition gps.c:536
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Definition gps.c:507
Device independent GPS code (interface).
int32_t hmsl
height above mean sea level (MSL) in mm
Definition gps.h:97
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition gps.h:95
uint32_t sacc
speed accuracy in cm/s
Definition gps.h:106
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north).
Definition gps.h:102
uint32_t pacc
position accuracy in cm
Definition gps.h:103
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition gps.h:100
uint8_t num_sv
number of sat in fix
Definition gps.h:109
uint8_t fix
status of fix
Definition gps.h:110
data structure for GPS information
Definition gps.h:90
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
in centimeters
int32_t x
in centimeters
int32_t y
in centimeters
vector in EarthCenteredEarthFixed coordinates
#define INTERMCU_LOST_CNT
Definition intermcu.h:35
@ INTERMCU_LOST
No interMCU communication anymore.
Definition intermcu.h:43
@ INTERMCU_OK
InterMCU communication is OK.
Definition intermcu.h:42
bool enabled
If the InterMCU communication is enabled.
Definition intermcu.h:65
@ INTERMCU_CMD_MOTORS_ON
The status of intermcu_ap_motors_on.
Definition intermcu.h:48
struct intermcu_t intermcu
Definition intermcu_ap.c:45
bool msg_available
If we have an InterMCU message.
Definition intermcu.h:66
#define INTERMCU_CLR_CMD_STATUS(_bit)
Definition intermcu.h:56
#define INTERMCU_GET_CMD_STATUS(_bit)
Definition intermcu.h:57
uint8_t imcu_msg_buf[128]
The InterMCU message buffer.
Definition intermcu_ap.c:51
void intermcu_dl_repack(struct transport_tx *trans, struct link_device *dev, uint8_t *msg, uint8_t size)
Repack message with same header and send on selected link if possible.
Definition intermcu_dl.c:57
void intermcu_dl_on_msg(uint8_t *msg, uint8_t size)
function to forward telemetry from AP to the ground
Definition intermcu_dl.c:79
datalink forwarder for InterMCU
void intermcu_event(void)
#define IMCU_GPS_ID
void intermcu_periodic(void)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
void intermcu_send_status(void)
send fbw status message
#define IMCU_RADIO_CONTROL_ID
void intermcu_parse_IMCU_COMMANDS(uint8_t *buf)
Datalink event functions.
void intermcu_parse_IMCU_SPEKTRUM_SOFT_BIND(uint8_t *buf)
void intermcu_forward_uplink(uint8_t *buf)
pprz_t intermcu_commands[COMMANDS_NB]
void intermcu_init(void)
bool intermcu_ap_motors_on
Inter-MCU on FlyByWire side.
#define INTERMCU_RADIO_KILL_SWITCH
#define INTERMCU_RADIO_AUX2
#define INTERMCU_RADIO_AUX1
#define INTERMCU_RADIO_ROLL
#define INTERMCU_RADIO_YAW
#define INTERMCU_RADIO_MODE
#define INTERMCU_RADIO_CONTROL_NB_CHANNEL
#define INTERMCU_RADIO_THROTTLE
#define INTERMCU_RADIO_PITCH
#define INTERMCU_RADIO_AUX3
uint8_t fbw_mode
Fly by wire modes.
Definition main_fbw.c:41
Fly By Wire:
int16_t pprz_t
Definition paparazzi.h:6
struct RadioControl radio_control
Generic interface for radio control modules.
#define RADIO_AUX3
Definition rc_intermcu.h:43
void received_spektrum_soft_bind(void)
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.