Paparazzi UAS  v5.18.0_stable
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  *
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
27 #define ABI_C
28 
29 #include "intermcu_fbw.h"
30 #include "pprzlink/intermcu_msg.h"
32 #include "subsystems/electrical.h"
33 #include "mcu_periph/uart.h"
35 
36 
38 
39 #include BOARD_CONFIG
40 #ifdef BOARD_PX4IO
41 #include "libopencm3/cm3/scb.h"
42 #include "led.h"
43 #include "mcu_periph/sys_time.h"
44 static uint8_t px4RebootSequence[] = {0x41, 0xd7, 0x32, 0x0a, 0x46, 0x39};
45 static uint8_t px4RebootSequenceCount = 0;
46 tid_t px4bl_tid;
47 #endif
48 
49 #if RADIO_CONTROL_NB_CHANNEL > 8
50 #undef RADIO_CONTROL_NB_CHANNEL
51 #define RADIO_CONTROL_NB_CHANNEL 8
52 INFO("InterMCU UART will only send 8 radio channels!")
53 #endif
54 
55 /* Main InterMCU structure */
56 struct intermcu_t intermcu = {
57  .device = (&((INTERMCU_LINK).device)),
58  .enabled = true,
59  .msg_available = false
60 };
61 uint8_t imcu_msg_buf[128] __attribute__((aligned));
62 
64 bool autopilot_motors_on = false;
65 static void intermcu_parse_msg(void (*commands_frame_handler)(void));
66 
67 #ifdef BOARD_PX4IO
68 static void checkPx4RebootCommand(unsigned char b);
69 #endif
70 
71 #ifdef USE_GPS
72 
73 #ifndef IMCU_GPS_ID
74 #define IMCU_GPS_ID GPS_MULTI_ID
75 #endif
76 
77 #include "subsystems/abi.h"
78 #include "subsystems/gps.h"
80 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
81 #endif
82 
83 void intermcu_init(void)
84 {
85  pprz_transport_init(&intermcu.transport);
86 
87 #if USE_GPS
88  AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb);
89 #endif
90 
91 #ifdef BOARD_PX4IO
92  px4bl_tid = sys_time_register_timer(20.0, NULL); //bootloader time out. After this intermcu will be set to slow baud.
93 #endif
94 }
95 
97 {
98  /* Check for interMCU loss */
101  } else {
103  }
104 }
105 
107 {
108  pprz_t values[9];
109 
114 #ifdef RADIO_MODE
116 #endif
117 #ifdef RADIO_KILL_SWITCH
119 #endif
120 
121 #ifdef RADIO_AUX1
122 #ifdef RADIO_KILL_SWITCH
123 #warning "RC AUX1 and KILL_SWITCH are on the same channel. AUX1 is discarded."
124 #else
126 #endif
127 #endif
128 #ifdef RADIO_AUX2
130 #endif
131 #ifdef RADIO_AUX3
133 #endif
134 
135  pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device,
137 }
138 
140 {
141  // Send Status
142  pprz_msg_send_IMCU_FBW_STATUS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW,
145 }
146 
147 #pragma GCC diagnostic ignored "-Wcast-align"
148 static void intermcu_parse_msg(void (*commands_frame_handler)(void))
149 {
150  /* Parse the Inter MCU message */
151  uint8_t msg_id = pprzlink_get_msg_id(imcu_msg_buf);
152 #if PPRZLINK_DEFAULT_VER == 2
153  // Check that the message is really a intermcu message
154  if (pprzlink_get_msg_class_id(imcu_msg_buf) == DL_intermcu_CLASS_ID) {
155 #endif
156  switch (msg_id) {
157  case DL_IMCU_COMMANDS: {
158  uint8_t i;
159  uint8_t size = DL_IMCU_COMMANDS_values_length(imcu_msg_buf);
160  int16_t *new_commands = DL_IMCU_COMMANDS_values(imcu_msg_buf);
161  intermcu.cmd_status |= DL_IMCU_COMMANDS_status(imcu_msg_buf);
162 
163  // Read the autopilot status and then clear it
166 
167  for (i = 0; i < size; i++) {
168  intermcu_commands[i] = new_commands[i];
169  }
170 
173  commands_frame_handler();
174  break;
175  }
176  #if defined(TELEMETRY_INTERMCU_DEV)
177  case DL_IMCU_TELEMETRY: {
178  uint8_t size = DL_IMCU_TELEMETRY_msg_length(imcu_msg_buf);
179  uint8_t *msg = DL_IMCU_TELEMETRY_msg(imcu_msg_buf);
181  break;
182  }
183  #endif
184  #if defined(SPEKTRUM_HAS_SOFT_BIND_PIN) //TODO: make subscribable module parser
185  case DL_IMCU_SPEKTRUM_SOFT_BIND:
187  break;
188  #endif
189 
190  default:
191  break;
192  }
193 #if PPRZLINK_DEFAULT_VER == 2
194  }
195 #endif
196 }
197 #pragma GCC diagnostic pop
198 
199 void InterMcuEvent(void (*frame_handler)(void))
200 {
201  uint8_t i, c;
202 
203  // Check if there are messages in the device
204  if (intermcu.device->char_available(intermcu.device->periph)) {
205  while (intermcu.device->char_available(intermcu.device->periph) && !intermcu.transport.trans_rx.msg_received) {
206  c = intermcu.device->get_byte(intermcu.device->periph);
207  parse_pprz(&intermcu.transport, c);
208 
209 #ifdef BOARD_PX4IO
210  // TODO: create a hook
211  checkPx4RebootCommand(c);
212 #endif
213  }
214 
215  // If we have a message copy it
216  if (intermcu.transport.trans_rx.msg_received) {
217  for (i = 0; i < intermcu.transport.trans_rx.payload_len; i++) {
218  imcu_msg_buf[i] = intermcu.transport.trans_rx.payload[i];
219  }
220 
221  intermcu.msg_available = true;
222  intermcu.transport.trans_rx.msg_received = false;
223  }
224  }
225 
226  if (intermcu.msg_available) {
227  intermcu_parse_msg(frame_handler);
228  intermcu.msg_available = false;
229  }
230 }
231 
232 #if USE_GPS
233 static void gps_cb(uint8_t sender_id __attribute__((unused)),
234  uint32_t stamp __attribute__((unused)),
235  struct GpsState *gps_s) {
236  pprz_msg_send_IMCU_REMOTE_GPS(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_FBW,
237  &gps_s->ecef_pos.x,
238  &gps_s->ecef_pos.y,
239  &gps_s->ecef_pos.z,
240  &gps_s->lla_pos.alt,
241  &gps_s->hmsl,
242  &gps_s->ecef_vel.x,
243  &gps_s->ecef_vel.y,
244  &gps_s->ecef_vel.z,
245  &gps_s->course,
246  &gps_s->gspeed,
247  &gps_s->pacc,
248  &gps_s->sacc,
249  &gps_s->num_sv,
250  &gps_s->fix);
251 }
252 #endif
253 
254 /* SOME STUFF FOR PX4IO BOOTLOADER (TODO: move this code) */
255 #ifdef BOARD_PX4IO
256 static void checkPx4RebootCommand(uint8_t b)
257 {
258  if (intermcu.stable_px4_baud == CHANGING_BAUD && sys_time_check_and_ack_timer(px4bl_tid)) {
259  //to prevent a short intermcu comm loss, give some time to changing the baud
260  sys_time_cancel_timer(px4bl_tid);
261  intermcu.stable_px4_baud = PPRZ_BAUD;
262  } else if (intermcu.stable_px4_baud == PX4_BAUD) {
263 
264  if (sys_time_check_and_ack_timer(px4bl_tid)) {
265  //time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight
266  sys_time_cancel_timer(px4bl_tid);
267  //for unknown reasons, 1500000 baud does not work reliably after prolonged times.
268  //I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
269  //After an initial period on 1500000, revert to 230400
270  //We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
272  intermcu.stable_px4_baud = CHANGING_BAUD;
273  px4bl_tid = sys_time_register_timer(1.0, NULL);
274  return;
275  }
276 
277 #ifdef SYS_TIME_LED
278  LED_ON(SYS_TIME_LED);
279 #endif
280 
281  if (b == px4RebootSequence[px4RebootSequenceCount]) {
282  px4RebootSequenceCount++;
283  } else {
284  px4RebootSequenceCount = 0;
285  }
286 
287  if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1
288  px4RebootSequenceCount = 0; // should not be necessary...
289 
290  //send some magic back
291  //this is the same as the Pixhawk IO code would send
292  intermcu.device->put_byte(intermcu.device->periph, 0, 0x00);
293  intermcu.device->put_byte(intermcu.device->periph, 0, 0xe5);
294  intermcu.device->put_byte(intermcu.device->periph, 0, 0x32);
295  intermcu.device->put_byte(intermcu.device->periph, 0, 0x0a);
296  intermcu.device->put_byte(intermcu.device->periph, 0,
297  0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side...
298 
299  while (((struct uart_periph *)(intermcu.device->periph))->tx_running) {
300  // tx_running is volatile now, so LED_TOGGLE not necessary anymore
301 #ifdef SYS_TIME_LED
302  LED_TOGGLE(SYS_TIME_LED);
303 #endif
304  }
305 
306 #ifdef SYS_TIME_LED
307  LED_OFF(SYS_TIME_LED);
308 #endif
309  scb_reset_system();
310  }
311  }
312 }
313 #endif
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
radio_control.h
electrical.h
intermcu
struct intermcu_t intermcu
Definition: intermcu_fbw.c:56
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:48
autopilot_motors_on
bool autopilot_motors_on
Definition: intermcu_fbw.c:64
RADIO_ROLL
#define RADIO_ROLL
Definition: intermcu_ap.h:41
imcu_msg_buf
uint8_t imcu_msg_buf[128]
The InterMCU message buffer.
Definition: intermcu_fbw.c:61
LED_OFF
#define LED_OFF(i)
Definition: led_hw.h:52
PX4_BAUD
@ PX4_BAUD
Definition: intermcu.h:51
b
float b
Definition: wedgebug.c:202
abi.h
telemetry_intermcu_on_msg
void telemetry_intermcu_on_msg(uint8_t *msg, uint8_t size)
Definition: telemetry_intermcu_ap.c:72
telemetry_intermcu.h
Telemetry through InterMCU.
intermcu_t::device
struct link_device * device
Device used for communication.
Definition: intermcu.h:71
intermcu_fbw.h
Rotorcraft Inter-MCU on FlyByWire.
INTERMCU_CLR_CMD_STATUS
#define INTERMCU_CLR_CMD_STATUS(_bit)
Definition: intermcu.h:66
fbw_mode
uint8_t fbw_mode
Definition: main_fbw.c:63
intermcu_send_status
void intermcu_send_status(uint8_t mode)
Definition: intermcu_fbw.c:139
GpsState
data structure for GPS information
Definition: gps.h:87
intermcu_commands
pprz_t intermcu_commands[COMMANDS_NB]
Definition: intermcu_fbw.c:63
InterMcuEvent
void InterMcuEvent(void(*frame_handler)(void))
Definition: intermcu_fbw.c:199
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:65
GpsState::pacc
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
RADIO_MODE
#define RADIO_MODE
Definition: intermcu_ap.h:44
INTERMCU_LOST_CNT
#define INTERMCU_LOST_CNT
Definition: intermcu.h:38
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
uint32_t
unsigned long uint32_t
Definition: types.h:18
intermcu_parse_msg
static void intermcu_parse_msg(void(*commands_frame_handler)(void))
Definition: intermcu_fbw.c:148
INTERMCU_FBW
#define INTERMCU_FBW
Definition: intermcu.h:35
LED_ON
#define LED_ON(i)
Definition: led_hw.h:51
RADIO_AUX3
#define RADIO_AUX3
Definition: intermcu_ap.h:48
B230400
#define B230400
Definition: uart_arch.h:49
intermcu_t::status
enum intermcu_status status
Status of the INTERMCU.
Definition: intermcu.h:73
intermcu_t::time_since_last_frame
uint8_t time_since_last_frame
Time since last frame.
Definition: intermcu.h:74
INTERMCU_RADIO_AUX1
#define INTERMCU_RADIO_AUX1
Definition: intermcu_fbw.h:46
LED_TOGGLE
#define LED_TOGGLE(i)
Definition: led_hw.h:53
intermcu_t
Definition: intermcu.h:70
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
INTERMCU_RADIO_YAW
#define INTERMCU_RADIO_YAW
Definition: intermcu_fbw.h:43
INTERMCU_RADIO_MODE
#define INTERMCU_RADIO_MODE
Definition: intermcu_fbw.h:44
intermcu_t::enabled
bool enabled
If the InterMCU communication is enabled.
Definition: intermcu.h:75
RadioControl::frame_rate
uint8_t frame_rate
Definition: radio_control.h:67
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
msg
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
Definition: high_speed_logger_direct_memory.c:134
INTERMCU_LOST
@ INTERMCU_LOST
No interMCU communication anymore.
Definition: intermcu.h:46
RADIO_AUX2
#define RADIO_AUX2
Definition: intermcu_ap.h:47
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
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:42
intermcu_on_rc_frame
void intermcu_on_rc_frame(uint8_t fbw_mode)
Definition: intermcu_fbw.c:106
PPRZ_BAUD
@ PPRZ_BAUD
Definition: intermcu.h:53
INTERMCU_CMD_MOTORS_ON
@ INTERMCU_CMD_MOTORS_ON
The status of autopilot_motors_on.
Definition: intermcu.h:58
INTERMCU_OK
@ INTERMCU_OK
InterMCU communication is OK.
Definition: intermcu.h:45
gps_ev
static abi_event gps_ev
Definition: intermcu_fbw.c:79
int16_t
signed short int16_t
Definition: types.h:17
sys_time.h
Architecture independent timing functions.
INTERMCU_RADIO_AUX2
#define INTERMCU_RADIO_AUX2
Definition: intermcu_fbw.h:47
uint8_t
unsigned char uint8_t
Definition: types.h:14
RADIO_CONTROL_NB_CHANNEL
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:49
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:64
INTERMCU_RADIO_ROLL
#define INTERMCU_RADIO_ROLL
Definition: intermcu_fbw.h:41
INTERMCU_RADIO_KILL_SWITCH
#define INTERMCU_RADIO_KILL_SWITCH
Definition: intermcu_fbw.h:45
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:74
intermcu_t::msg_available
bool msg_available
If we have an InterMCU message.
Definition: intermcu.h:76
INTERMCU_GET_CMD_STATUS
#define INTERMCU_GET_CMD_STATUS(_bit)
Definition: intermcu.h:67
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:77
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:233
RADIO_THROTTLE
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
CHANGING_BAUD
@ CHANGING_BAUD
Definition: intermcu.h:52
uart_periph
UART peripheral.
Definition: uart.h:70
intermcu_init
void intermcu_init(void)
Definition: intermcu_fbw.c:83
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:72
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:96
INTERMCU_RADIO_THROTTLE
#define INTERMCU_RADIO_THROTTLE
Definition: intermcu_fbw.h:40
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:60
RADIO_AUX1
#define RADIO_AUX1
Definition: intermcu_ap.h:46
RADIO_YAW
#define RADIO_YAW
Definition: intermcu_ap.h:43
mode
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
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
RADIO_PITCH
#define RADIO_PITCH
Definition: intermcu_ap.h:42
RADIO_KILL_SWITCH
#define RADIO_KILL_SWITCH
Definition: intermcu_ap.h:45
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:114
Electrical::current
float current
current in A
Definition: electrical.h:46
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69