Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 */
99  if (intermcu.time_since_last_frame >= INTERMCU_LOST_CNT) {
100  intermcu.status = INTERMCU_LOST;
101  } else {
102  intermcu.time_since_last_frame++;
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,
136  INTERMCU_FBW, &fbw_mode, RADIO_CONTROL_NB_CHANNEL, values);
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
164  autopilot_motors_on = INTERMCU_GET_CMD_STATUS(INTERMCU_CMD_MOTORS_ON);
166 
167  for (i = 0; i < size; i++) {
168  intermcu_commands[i] = new_commands[i];
169  }
170 
171  intermcu.status = INTERMCU_OK;
172  intermcu.time_since_last_frame = 0;
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);
180  telemetry_intermcu_on_msg(msg, size);
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 
253 void gps_periodic_check(struct GpsState *gps_s) {
254  if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
255  gps_s->fix = GPS_FIX_NONE;
256  }
257 }
258 #endif
259 
260 /* SOME STUFF FOR PX4IO BOOTLOADER (TODO: move this code) */
261 #ifdef BOARD_PX4IO
262 static void checkPx4RebootCommand(uint8_t b)
263 {
264  if (intermcu.stable_px4_baud == CHANGING_BAUD && sys_time_check_and_ack_timer(px4bl_tid)) {
265  //to prevent a short intermcu comm loss, give some time to changing the baud
266  sys_time_cancel_timer(px4bl_tid);
267  intermcu.stable_px4_baud = PPRZ_BAUD;
268  } else if (intermcu.stable_px4_baud == PX4_BAUD) {
269 
270  if (sys_time_check_and_ack_timer(px4bl_tid)) {
271  //time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight
272  sys_time_cancel_timer(px4bl_tid);
273  //for unknown reasons, 1500000 baud does not work reliably after prolonged times.
274  //I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
275  //After an initial period on 1500000, revert to 230400
276  //We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
277  uart_periph_set_baudrate(intermcu.device->periph, B230400);
278  intermcu.stable_px4_baud = CHANGING_BAUD;
279  px4bl_tid = sys_time_register_timer(1.0, NULL);
280  return;
281  }
282 
283 #ifdef SYS_TIME_LED
284  LED_ON(SYS_TIME_LED);
285 #endif
286 
287  if (b == px4RebootSequence[px4RebootSequenceCount]) {
288  px4RebootSequenceCount++;
289  } else {
290  px4RebootSequenceCount = 0;
291  }
292 
293  if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1
294  px4RebootSequenceCount = 0; // should not be necessary...
295 
296  //send some magic back
297  //this is the same as the Pixhawk IO code would send
298  intermcu.device->put_byte(intermcu.device->periph, 0, 0x00);
299  intermcu.device->put_byte(intermcu.device->periph, 0, 0xe5);
300  intermcu.device->put_byte(intermcu.device->periph, 0, 0x32);
301  intermcu.device->put_byte(intermcu.device->periph, 0, 0x0a);
302  intermcu.device->put_byte(intermcu.device->periph, 0,
303  0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side...
304 
305  while (((struct uart_periph *)(intermcu.device->periph))->tx_running) {
306  // tx_running is volatile now, so LED_TOGGLE not necessary anymore
307 #ifdef SYS_TIME_LED
308  LED_TOGGLE(SYS_TIME_LED);
309 #endif
310  }
311 
312 #ifdef SYS_TIME_LED
313  LED_OFF(SYS_TIME_LED);
314 #endif
315  scb_reset_system();
316  }
317  }
318 }
319 #endif
struct intermcu_t intermcu
Definition: intermcu_fbw.c:56
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
int32_t z
in centimeters
void intermcu_send_status(uint8_t mode)
Definition: intermcu_fbw.c:139
bool autopilot_motors_on
Definition: intermcu_fbw.c:64
#define RADIO_ROLL
Definition: intermcu_ap.h:41
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define RADIO_AUX2
Definition: intermcu_ap.h:47
void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud)
Set baudrate.
Definition: uart_arch.c:861
#define INTERMCU_LOST_CNT
Definition: intermcu.h:38
uint8_t imcu_msg_buf[128]
The InterMCU message buffer.
Definition: intermcu_fbw.c:61
void InterMcuEvent(void(*frame_handler)(void))
Definition: intermcu_fbw.c:199
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
static void intermcu_parse_msg(void(*commands_frame_handler)(void))
Definition: intermcu_fbw.c:148
struct link_device * device
Device used for communication.
Definition: intermcu.h:71
#define INTERMCU_RADIO_AUX3
Definition: intermcu_fbw.h:48
pprz_t intermcu_commands[COMMANDS_NB]
Definition: intermcu_fbw.c:63
uint8_t cmd_status
Command status information that is transfered (intermcu_cmd_status)
Definition: intermcu.h:77
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition: intermcu_fbw.c:253
uint8_t status
Definition: radio_control.h:64
int32_t y
in centimeters
#define LED_OFF(i)
Definition: led_hw.h:52
void telemetry_intermcu_on_msg(uint8_t *msg, uint8_t size)
Telemetry through InterMCU.
void sys_time_cancel_timer(tid_t id)
Cancel a system timer by id.
Definition: sys_time.c:60
int16_t pprz_t
Definition: paparazzi.h:6
Main include for ABI (AirBorneInterface).
enum intermcu_status status
Status of the INTERMCU.
Definition: intermcu.h:73
float vsupply
supply voltage in V
Definition: electrical.h:45
InterMCU communication is OK.
Definition: intermcu.h:45
#define INTERMCU_RADIO_PITCH
Definition: intermcu_fbw.h:42
#define INTERMCU_RADIO_MODE
Definition: intermcu_fbw.h:44
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
bool msg_available
If we have an InterMCU message.
Definition: intermcu.h:76
int32_t alt
in millimeters above WGS84 reference ellipsoid
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
#define B230400
Definition: uart_arch.h:49
Rotorcraft Inter-MCU on FlyByWire.
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:49
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
bool enabled
If the InterMCU communication is enabled.
Definition: intermcu.h:75
#define INTERMCU_RADIO_ROLL
Definition: intermcu_fbw.h:41
No interMCU communication anymore.
Definition: intermcu.h:46
#define INTERMCU_RADIO_AUX1
Definition: intermcu_fbw.h:46
#define RADIO_MODE
Definition: intermcu_ap.h:44
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
Interface for electrical status: supply voltage, current, battery status, etc.
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:87
The status of autopilot_motors_on.
Definition: intermcu.h:58
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
#define INTERMCU_RADIO_YAW
Definition: intermcu_fbw.h:43
UART peripheral.
Definition: uart.h:70
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
#define INTERMCU_GET_CMD_STATUS(_bit)
Definition: intermcu.h:67
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
signed short int16_t
Definition: types.h:17
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
struct RadioControl radio_control
Definition: radio_control.c:30
struct pprz_transport transport
Transport over communication line (PPRZ)
Definition: intermcu.h:72
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
void intermcu_on_rc_frame(uint8_t fbw_mode)
Definition: intermcu_fbw.c:106
#define LED_TOGGLE(i)
Definition: led_hw.h:53
#define INTERMCU_RADIO_KILL_SWITCH
Definition: intermcu_fbw.h:45
#define INTERMCU_RADIO_THROTTLE
Definition: intermcu_fbw.h:40
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: intermcu_fbw.c:233
static abi_event gps_ev
Definition: intermcu_fbw.c:79
#define IMCU_GPS_ID
Definition: intermcu_fbw.c:74
#define INTERMCU_CLR_CMD_STATUS(_bit)
Definition: intermcu.h:66
uint8_t fbw_mode
Definition: main_fbw.c:63
#define INTERMCU_RADIO_AUX2
Definition: intermcu_fbw.h:47
unsigned char uint8_t
Definition: types.h:14
void intermcu_init(void)
Definition: intermcu_fbw.c:83
void received_spektrum_soft_bind(void)
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
uint8_t time_since_last_frame
Time since last frame.
Definition: intermcu.h:74
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
#define RADIO_PITCH
Definition: intermcu_ap.h:42
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:114
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
#define RADIO_AUX3
Definition: intermcu_ap.h:48
#define RADIO_AUX1
Definition: intermcu_ap.h:46
#define RADIO_YAW
Definition: intermcu_ap.h:43
#define INTERMCU_FBW
Definition: intermcu.h:35
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
arch independent LED (Light Emitting Diodes) API
int32_t x
in centimeters
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
#define LED_ON(i)
Definition: led_hw.h:51
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
struct Electrical electrical
Definition: electrical.c:66
#define GPS_TIMEOUT
GPS timeout in seconds.
Definition: gps.h:176
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
#define RADIO_KILL_SWITCH
Definition: intermcu_ap.h:45
float current
current in A
Definition: electrical.h:46
uint8_t fix
status of fix
Definition: gps.h:107
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
void intermcu_periodic(void)
Definition: intermcu_fbw.c:96
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
uint8_t frame_rate
Definition: radio_control.h:67