Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
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 #ifdef BOARD_PX4IO
44 #include "libopencm3/cm3/scb.h"
45 #include "led.h"
46 #include "mcu_periph/sys_time.h"
47 static uint8_t px4RebootSequence[] = {0x41, 0xd7, 0x32, 0x0a, 0x46, 0x39};
48 static uint8_t px4RebootSequenceCount = 0;
49 tid_t px4bl_tid;
50 #endif
51 
52 #if RADIO_CONTROL_NB_CHANNEL > 8
53 //#undef RADIO_CONTROL_NB_CHANNEL
54 //#define RADIO_CONTROL_NB_CHANNEL 8
55 INFO("InterMCU UART will only send 8 radio channels!")
56 #endif
57 
58 /* Main InterMCU structure */
59 struct intermcu_t intermcu = {
60  .device = (&((INTERMCU_LINK).device)),
61  .enabled = true,
62  .msg_available = false
63 };
64 uint8_t imcu_msg_buf[256] __attribute__((aligned));
65 
67 bool intermcu_ap_motors_on = false;
68 
69 
70 #ifdef BOARD_PX4IO
71 static void checkPx4RebootCommand(unsigned char b);
72 #endif
73 
74 // ABI callback for radio control
75 #ifndef IMCU_RADIO_CONTROL_ID
76 #define IMCU_RADIO_CONTROL_ID ABI_BROADCAST
77 #endif
79 static void rc_cb(uint8_t sender_id, struct RadioControl *rc);
80 
81 // ABI callback for GPS
82 #ifdef USE_GPS
83 #include "modules/gps/gps.h"
84 #ifndef IMCU_GPS_ID
85 #define IMCU_GPS_ID GPS_MULTI_ID
86 #endif
88 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
89 #endif
90 
91 void intermcu_init(void)
92 {
93  pprz_transport_init(&intermcu.transport);
94 
95  AbiBindMsgRADIO_CONTROL(IMCU_RADIO_CONTROL_ID, &rc_ev, rc_cb);
96 #if USE_GPS
97  AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb);
98 #endif
99 
100 #ifdef BOARD_PX4IO
101  px4bl_tid = sys_time_register_timer(20.0, NULL); //bootloader time out. After this intermcu will be set to slow baud.
102 #endif
103 }
104 
106 {
107  /* Check for interMCU loss */
110  } else {
112  }
113 }
114 
115 void intermcu_event(void)
116 {
117  uint8_t i, c;
118 
119  // Check if there are messages in the device
120  if (intermcu.device->char_available(intermcu.device->periph)) {
121  while (intermcu.device->char_available(intermcu.device->periph) && !intermcu.transport.trans_rx.msg_received) {
122  c = intermcu.device->get_byte(intermcu.device->periph);
123  parse_pprz(&intermcu.transport, c);
124 
125 #ifdef BOARD_PX4IO
126  // TODO: create a hook
127  checkPx4RebootCommand(c);
128 #endif
129  }
130 
131  // If we have a message copy it
132  if (intermcu.transport.trans_rx.msg_received) {
133  for (i = 0; i < intermcu.transport.trans_rx.payload_len; i++) {
134  imcu_msg_buf[i] = intermcu.transport.trans_rx.payload[i];
135  }
136 
137  intermcu.msg_available = true;
138  intermcu.transport.trans_rx.msg_received = false;
139  }
140  }
141 
142  if (intermcu.msg_available) {
143  uint8_t class_id = pprzlink_get_msg_class_id(imcu_msg_buf);
144  if (class_id == DL_intermcu_CLASS_ID) {
145  // parse intermcu messages and call callbacks
147 #if TELEMETRY_INTERMCU
148  } else {
149  // forward all other messages if needed
150  intermcu_dl_on_msg(imcu_msg_buf, intermcu.transport.trans_rx.payload_len);
151 #endif
152  }
153  intermcu.msg_available = false;
154  }
155 }
156 
158 {
159 #ifdef RADIO_CONTROL
160  uint8_t rc_status = radio_control.status;
161  uint8_t rc_rate = radio_control.frame_rate;
162 #else
163  uint8_t rc_status = 0;
164  uint8_t rc_rate = 0;
165 #endif
166  // Send Status
167  pprz_msg_send_IMCU_FBW_STATUS(&(intermcu.transport.trans_tx), intermcu.device, AC_ID,
168  &fbw_mode, &rc_status, &rc_rate,
170 }
171 
173 {
174  uint8_t size = DL_IMCU_COMMANDS_values_length(buf);
175  int16_t *new_commands = DL_IMCU_COMMANDS_values(buf);
176  intermcu.cmd_status |= DL_IMCU_COMMANDS_status(buf);
177 
178  // Read the autopilot status and then clear it
181 
182  for (int i = 0; i < size; i++) {
183  intermcu_commands[i] = new_commands[i];
184  }
185 
188 }
189 
190 void intermcu_parse_IMCU_SPEKTRUM_SOFT_BIND(uint8_t *buf __attribute__((unused)))
191 {
192 #if defined(SPEKTRUM_HAS_SOFT_BIND_PIN)
194 #endif
195 }
196 
197 void intermcu_forward_uplink(uint8_t *buf __attribute__((unused)))
198 {
199  // forward all incoming messages to intermcu for AP side
200 #if TELEMETRY_INTERMCU
201  uint8_t size = (DOWNLINK_TRANSPORT).trans_rx.payload_len; // FIXME is it always a valid length ?
202  intermcu_dl_repack(&(intermcu.transport.trans_tx), intermcu.device, buf, size);
203 #endif
204 }
205 
206 static void rc_cb(uint8_t sender_id __attribute__((unused)), struct RadioControl *rc __attribute__((unused)))
207 {
208 #if RADIO_CONTROL
209  pprz_t values[9];
210 
215 #ifdef RADIO_MODE
217 #endif
218 #ifdef RADIO_KILL_SWITCH
220 #endif
221 
222 #ifdef RADIO_AUX1
223 #ifdef RADIO_KILL_SWITCH
224 #warning "RC AUX1 and KILL_SWITCH are on the same channel. AUX1 is discarded."
225 #else
227 #endif
228 #endif
229 #ifdef RADIO_AUX2
231 #endif
232 #ifdef RADIO_AUX3
234 #endif
235 
236  pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device,
237  INTERMCU_FBW, &fbw_mode, INTERMCU_RADIO_CONTROL_NB_CHANNEL, values);
238 #endif
239 }
240 
241 #if USE_GPS
242 static void gps_cb(uint8_t sender_id __attribute__((unused)),
243  uint32_t stamp __attribute__((unused)),
244  struct GpsState *gps_s) {
245  pprz_msg_send_IMCU_REMOTE_GPS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW,
246  &gps_s->ecef_pos.x,
247  &gps_s->ecef_pos.y,
248  &gps_s->ecef_pos.z,
249  &gps_s->lla_pos.alt,
250  &gps_s->hmsl,
251  &gps_s->ecef_vel.x,
252  &gps_s->ecef_vel.y,
253  &gps_s->ecef_vel.z,
254  &gps_s->course,
255  &gps_s->gspeed,
256  &gps_s->pacc,
257  &gps_s->sacc,
258  &gps_s->num_sv,
259  &gps_s->fix);
260 }
261 #endif
262 
263 /* SOME STUFF FOR PX4IO BOOTLOADER (TODO: move this code) */
264 #ifdef BOARD_PX4IO
265 static void checkPx4RebootCommand(uint8_t b)
266 {
267  if (intermcu.stable_px4_baud == CHANGING_BAUD && sys_time_check_and_ack_timer(px4bl_tid)) {
268  //to prevent a short intermcu comm loss, give some time to changing the baud
269  sys_time_cancel_timer(px4bl_tid);
270  intermcu.stable_px4_baud = PPRZ_BAUD;
271  } else if (intermcu.stable_px4_baud == PX4_BAUD) {
272 
273  if (sys_time_check_and_ack_timer(px4bl_tid)) {
274  //time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight
275  sys_time_cancel_timer(px4bl_tid);
276  //for unknown reasons, 1500000 baud does not work reliably after prolonged times.
277  //I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
278  //After an initial period on 1500000, revert to 230400
279  //We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
281  intermcu.stable_px4_baud = CHANGING_BAUD;
282  px4bl_tid = sys_time_register_timer(1.0, NULL);
283  return;
284  }
285 
286 #ifdef SYS_TIME_LED
287  LED_ON(SYS_TIME_LED);
288 #endif
289 
290  if (b == px4RebootSequence[px4RebootSequenceCount]) {
291  px4RebootSequenceCount++;
292  } else {
293  px4RebootSequenceCount = 0;
294  }
295 
296  if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1
297  px4RebootSequenceCount = 0; // should not be necessary...
298 
299  //send some magic back
300  //this is the same as the Pixhawk IO code would send
301  intermcu.device->put_byte(intermcu.device->periph, 0, 0x00);
302  intermcu.device->put_byte(intermcu.device->periph, 0, 0xe5);
303  intermcu.device->put_byte(intermcu.device->periph, 0, 0x32);
304  intermcu.device->put_byte(intermcu.device->periph, 0, 0x0a);
305  intermcu.device->put_byte(intermcu.device->periph, 0,
306  0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side...
307 
308  while (((struct uart_periph *)(intermcu.device->periph))->tx_running) {
309  // tx_running is volatile now, so LED_TOGGLE not necessary anymore
310 #ifdef SYS_TIME_LED
311  LED_TOGGLE(SYS_TIME_LED);
312 #endif
313  }
314 
315 #ifdef SYS_TIME_LED
316  LED_OFF(SYS_TIME_LED);
317 #endif
318  scb_reset_system();
319  }
320  }
321 }
322 #endif
radio_control.h
electrical.h
intermcu_event
void intermcu_event(void)
Definition: intermcu_fbw.c:115
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
intermcu
struct intermcu_t intermcu
Definition: intermcu_fbw.c:59
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
INTERMCU_RADIO_AUX3
#define INTERMCU_RADIO_AUX3
Definition: intermcu_fbw.h:57
LED_OFF
#define LED_OFF(i)
Definition: led_hw.h:52
b
float b
Definition: wedgebug.c:202
abi.h
imcu_msg_buf
uint8_t imcu_msg_buf[256]
The InterMCU message buffer.
Definition: intermcu_fbw.c:64
intermcu_t::device
struct link_device * device
Device used for communication.
Definition: intermcu.h:70
intermcu_fbw.h
Inter-MCU on FlyByWire side.
RADIO_KILL_SWITCH
#define RADIO_KILL_SWITCH
Definition: rc_intermcu.h:40
INTERMCU_CLR_CMD_STATUS
#define INTERMCU_CLR_CMD_STATUS(_bit)
Definition: intermcu.h:65
GpsState
data structure for GPS information
Definition: gps.h:87
intermcu_commands
pprz_t intermcu_commands[COMMANDS_NB]
Definition: intermcu_fbw.c:66
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
GpsState::pacc
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
INTERMCU_LOST_CNT
#define INTERMCU_LOST_CNT
Definition: intermcu.h:35
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
intermcu_forward_uplink
void intermcu_forward_uplink(uint8_t *buf)
Definition: intermcu_fbw.c:197
RADIO_AUX1
#define RADIO_AUX1
Definition: rc_intermcu.h:41
LED_ON
#define LED_ON(i)
Definition: led_hw.h:51
B230400
#define B230400
Definition: uart_arch.h:49
intermcu_t::status
enum intermcu_status status
Status of the INTERMCU.
Definition: intermcu.h:72
intermcu_t::time_since_last_frame
uint8_t time_since_last_frame
Time since last frame.
Definition: intermcu.h:73
INTERMCU_RADIO_AUX1
#define INTERMCU_RADIO_AUX1
Definition: intermcu_fbw.h:55
LED_TOGGLE
#define LED_TOGGLE(i)
Definition: led_hw.h:53
intermcu_t
Definition: intermcu.h:69
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
INTERMCU_RADIO_YAW
#define INTERMCU_RADIO_YAW
Definition: intermcu_fbw.h:52
INTERMCU_RADIO_MODE
#define INTERMCU_RADIO_MODE
Definition: intermcu_fbw.h:53
intermcu_t::enabled
bool enabled
If the InterMCU communication is enabled.
Definition: intermcu.h:74
main_fbw.h
RadioControl::frame_rate
uint8_t frame_rate
Definition: radio_control.h:64
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
intermcu_dl.h
datalink forwarder for InterMCU
INTERMCU_LOST
@ INTERMCU_LOST
No interMCU communication anymore.
Definition: intermcu.h:43
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
INTERMCU_RADIO_CONTROL_NB_CHANNEL
#define INTERMCU_RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_fbw.h:59
uart_periph_set_baudrate
void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud)
Set baudrate.
Definition: uart_arch.c:984
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
pprz_t
int16_t pprz_t
Definition: paparazzi.h:6
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
INTERMCU_RADIO_PITCH
#define INTERMCU_RADIO_PITCH
Definition: intermcu_fbw.h:51
INTERMCU_CMD_MOTORS_ON
@ INTERMCU_CMD_MOTORS_ON
The status of intermcu_ap_motors_on.
Definition: intermcu.h:57
INTERMCU_OK
@ INTERMCU_OK
InterMCU communication is OK.
Definition: intermcu.h:42
gps_ev
static abi_event gps_ev
Definition: intermcu_fbw.c:87
sys_time.h
Architecture independent timing functions.
INTERMCU_RADIO_AUX2
#define INTERMCU_RADIO_AUX2
Definition: intermcu_fbw.h:56
fbw_mode
uint8_t fbw_mode
Fly by wire modes.
Definition: main_fbw.c:41
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
RadioControl::status
uint8_t status
Definition: radio_control.h:61
dl_parse_msg
void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
Should be called when chars are available in dl_buffer.
Definition: setup_actuators.c:113
INTERMCU_RADIO_ROLL
#define INTERMCU_RADIO_ROLL
Definition: intermcu_fbw.h:50
INTERMCU_RADIO_KILL_SWITCH
#define INTERMCU_RADIO_KILL_SWITCH
Definition: intermcu_fbw.h:54
RadioControl
Definition: radio_control.h:60
intermcu_dl_on_msg
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
tid_t
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
led.h
arch independent LED (Light Emitting Diodes) API
IMCU_GPS_ID
#define IMCU_GPS_ID
Definition: intermcu_fbw.c:85
intermcu_parse_IMCU_COMMANDS
void intermcu_parse_IMCU_COMMANDS(uint8_t *buf)
Datalink event functions.
Definition: intermcu_fbw.c:172
intermcu_t::msg_available
bool msg_available
If we have an InterMCU message.
Definition: intermcu.h:75
intermcu_dl_repack
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
intermcu_ap_motors_on
bool intermcu_ap_motors_on
Definition: intermcu_fbw.c:67
INTERMCU_GET_CMD_STATUS
#define INTERMCU_GET_CMD_STATUS(_bit)
Definition: intermcu.h:66
received_spektrum_soft_bind
void received_spektrum_soft_bind(void)
Definition: spektrum_soft_bind_fbw.c:39
intermcu_t::cmd_status
uint8_t cmd_status
Command status information that is transfered (intermcu_cmd_status)
Definition: intermcu.h:76
RADIO_AUX2
#define RADIO_AUX2
Definition: rc_intermcu.h:42
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: intermcu_fbw.c:242
intermcu_parse_IMCU_SPEKTRUM_SOFT_BIND
void intermcu_parse_IMCU_SPEKTRUM_SOFT_BIND(uint8_t *buf)
Definition: intermcu_fbw.c:190
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
uart_periph
UART peripheral.
Definition: uart.h:72
intermcu_init
void intermcu_init(void)
Definition: intermcu_fbw.c:91
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
intermcu_t::transport
struct pprz_transport transport
Transport over communication line (PPRZ)
Definition: intermcu.h:71
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
intermcu_periodic
void intermcu_periodic(void)
Definition: intermcu_fbw.c:105
INTERMCU_RADIO_THROTTLE
#define INTERMCU_RADIO_THROTTLE
Definition: intermcu_fbw.h:49
sys_time_register_timer
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
sys_time_cancel_timer
void sys_time_cancel_timer(tid_t id)
Cancel a system timer by id.
Definition: sys_time.c:76
IMCU_RADIO_CONTROL_ID
#define IMCU_RADIO_CONTROL_ID
Definition: intermcu_fbw.c:76
rc_cb
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Definition: intermcu_fbw.c:206
electrical
struct Electrical electrical
Definition: electrical.c:66
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
intermcu_send_status
void intermcu_send_status(void)
send fbw status message
Definition: intermcu_fbw.c:157
RADIO_AUX3
#define RADIO_AUX3
Definition: rc_intermcu.h:43
spektrum_soft_bind_fbw.h
sys_time_check_and_ack_timer
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:123
rc_ev
static abi_event rc_ev
Definition: intermcu_fbw.c:78
Electrical::current
float current
current in A
Definition: electrical.h:46
radio_control
struct RadioControl radio_control
Definition: radio_control.c:33
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67