Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
px4_flash.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Kevin van Hecke
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  */
29 #include "mcu_periph/sys_time_arch.h"
31 
32 // Serial Port
33 #include "mcu_periph/uart.h"
34 #include "mcu_periph/usb_serial.h"
35 
36 #include "mcu.h"
37 #include "led.h"
38 #include "mcu_arch.h"
39 
40 #include "mcu_periph/sys_time.h"
41 #ifdef INTER_MCU_AP
42 tid_t px4iobl_tid;
43 // define coms link for px4io f1
44 #define PX4IO_PORT (&((PX4IO_UART).device))
45 #endif
46 
47 #define FLASH_PORT (&((FLASH_UART).device))
48 
49 #ifndef PX4_IO_FORCE_PROG
50 #define PX4_IO_FORCE_PROG FALSE
51 #elif (PX4_IO_FORCE_PROG == TRUE)
52 PRINT_CONFIG_MSG("Using PX4_IO force program. Don't forget to re-program AP with this option off!");
53 #endif
54 
55 // weird that these below are not in protocol.h, which is from the firmware px4 repo
56 // below is copied from qgroundcontrol:
57 #define PROTO_INSYNC 0x12
58 #define PROTO_EOC 0x20
59 // Reply bytes
60 #define PROTO_OK 0x10
61 #define PROTO_FAILED 0x11
62 #define PROTO_INVALID 0x13
63 // Command bytes
64 #define PROTO_GET_SYNC 0x21
65 #define PROTO_GET_DEVICE 0x22
66 #define PROTO_CHIP_ERASE 0x23
67 #define PROTO_CHIP_VERIFY 0x24
68 #define PROTO_PROG_MULTI 0x27
69 #define PROTO_GET_CRC 0x29
70 #define PROTO_BOOT 0x30
71 
74 
75 void px4flash_init(void)
76 {
77  setToBootloaderMode = false;
78 #ifdef INTER_MCU_AP
79  px4ioRebootTimeout = false;
80  px4iobl_tid = sys_time_register_timer(12.0, NULL); //20 (fbw pprz bl timeout)-5 (px4 fmu bl timeout) - 3 (uncertainty and random delays)
81 #endif
82 }
83 
84 void px4flash_event(void)
85 {
86 #ifdef INTER_MCU_AP
87  if (sys_time_check_and_ack_timer(px4iobl_tid)) {
89  sys_time_cancel_timer(px4iobl_tid);
90  //for unknown reasons, 1500000 baud does not work reliably after prolonged times.
91  //I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
92  //After a initial period on 1500000, revert to 230400
93  //We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
94  uart_periph_set_baudrate(PX4IO_PORT->periph, B230400);
95  }
96  if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
97  if (!setToBootloaderMode) {
98  //ignore anything coming from IO if not in bootloader mode (which should be nothing)
99  } else {
100  //relay everything from IO to the laptop
101  while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
102  unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
103  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, b);
104  }
105  }
106  }
107 #endif
108 
109  //TODO: check if bootloader timeout was surpassed
110  if (FLASH_PORT->char_available(FLASH_PORT->periph) && !setToBootloaderMode) {
111  //check whether this is flash related communication, and for who (ap/fbw)
112  int state = 0;
113  while (state < 4 && FLASH_PORT->char_available(FLASH_PORT->periph)) {
114  unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph);
115  switch (state) {
116  case (0) :
117  if (b == 'p') { state++; } else { return; }
118  break;
119  case (1) :
120  if (b == 'p') { state++; } else { return; }
121  break;
122  case (2) :
123  if (b == 'r') { state++; } else { return; }
124  break;
125  case (3) :
126  if (b == 'z') { state++; } else { return; }
127  break;
128  default :
129  break;
130  }
131  }
132 
133  if (state != 4) {return;}
134  //TODO: check if/how this interferes with flashing original PX4 firmware
135  unsigned char target = FLASH_PORT->get_byte(FLASH_PORT->periph);
136  if (target == '1') { //target ap
137  //the target is the ap, so reboot to PX4 bootloader
138 #if defined(CHIBIOS_MCU_ARCH_H)
140 #elif defined(STM32_MCU_ARCH_H)
141  scb_reset_system();
142 #endif
143  } else { // target fbw
144 #ifdef INTER_MCU_AP
145  //the target is the fbw, so reboot the fbw and switch to relay mode
146 
147  //first check if the bootloader has not timeout:
148  if (px4ioRebootTimeout) {
149  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'T');
150  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'I');
151  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'M');
152  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'E');
153  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'O');
154  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'U');
155  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'T'); // use 7 chars as answer
156  return;
157  }
158 
159  //stop all intermcu communication:
161 
162  sys_time_cancel_timer(px4iobl_tid);
163 
164  /*
165  * The PX4_IO_FORCE_PROG define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct.
166  * In that case:
167  * 1. enable this define
168  * 2. build and upload the fmu f4 chip (ap target in pprz center)
169  * 3. build the io code, and convert the firmware using the following command:
170  * $(PAPARAZZI_SRC)/sw/tools/px4/px_mkfw.py --prototype $(PAPARAZZI_SRC)/sw/tools/px4/px4io-v2.prototype --image $(PAPARAZZI_HOME)/var/aircrafts/<AC_NAME>/fbw/fbw.bin > $(PAPARAZZI_HOME)/var/aircrafts/<AC_NAME>/fbw/fbw.px4
171  * 4. Start the following command:
172  * $(PAPARAZZI_SRC)/sw/tools/px4/px_uploader.py --port "/dev/ttyACM0" $(PAPARAZZI_SRC)/var/aircrafts/<AC_NAME>/fbw/fbw.px4
173  * Optional 5&6:
174  * 5a. Either, boot the Pixhawk (reconnect usb) holding the IO reset button until the FMU led stops blinking fast (i.e. exits its own bootloader)
175  * 5b Or, press the IO reset button on the pixhawk
176  * 6. Watch the output of the command of step 4, it should recognize the IO bootloader and start flashing. If not try repeating step 5a.
177  * 7. Don forget to disable the define and upload the ap again :)
178  */
179 
180 #if !PX4_IO_FORCE_PROG
181  //send the reboot to bootloader command:
182  static struct IOPacket dma_packet;
183  dma_packet.count_code = 0x40 + 0x01;
184  dma_packet.crc = 0;
185  dma_packet.page = PX4IO_PAGE_SETUP;
186  dma_packet.offset = PX4IO_P_SETUP_REBOOT_BL;
187  dma_packet.regs[0] = PX4IO_REBOOT_BL_MAGIC;
188  dma_packet.crc = crc_packet(&dma_packet);
189  struct IOPacket *pkt = &dma_packet;
190  uint8_t *p = (uint8_t *)pkt;
191  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, p[0]);
192  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, p[1]);
193  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, p[2]);
194  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, p[3]);
195  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, p[4]);
196  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, p[5]);
197 
198  sys_time_usleep(5000); // this seems to be close to the minimum delay necessary to process this packet at the IO side
199  //the pixhawk IO chip should respond with:
200  // 0x00 ( PKT_CODE_SUCCESS )
201  // 0xe5
202  // 0x32
203  // 0x0a
204  //After that, the IO chips reboots into bootloader mode, in which it will stay for a short period
205  //The baudrate in bootloader mode ic changed to 115200 (normal operating baud is 1500000, at least for original pixhawk fmu firmware)
206 
207  //state machine
208  state = 0;
209  while (state < 4 && PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
210 
211  unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
212  switch (state) {
213  case (0) :
214  if (b == PKT_CODE_SUCCESS) { state++; } else { state = 0; }
215  break;
216  case (1) :
217  if (b == 0xe5) { state++; } else { state = 0; }
218  break;
219  case (2) :
220  if (b == 0x32) { state++; } else { state = 0; }
221  break;
222  case (3) :
223  if (b == 0x0a) { state++; } else { state = 0; }
224  break;
225  default :
226  break;
227  }
228  }
229 #else
230  state = 4;
231 #endif
232  if (state == 4) {
233  uart_periph_set_baudrate(PX4IO_PORT->periph, B115200);
234  /* look for the bootloader for 150 ms */
235  int ret = 0;
236  for (int i = 0; i < 15 && !ret ; i++) {
237  sys_time_usleep(10000);
238 
239  //send a get_sync command in order to keep the io in bootloader mode
240  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, PROTO_GET_SYNC);
241  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, PROTO_EOC);
242 
243  //get_sync should be replied with, so check if that happens and
244  //all other bytes are discarded, hopefully those were not important
245  //(they may be caused by sending multiple syncs)
246  while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
247  unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
248 
249  if (b == PROTO_INSYNC) {
250  sys_time_usleep(10000);
251  if(PX4IO_PORT->char_available(PX4IO_PORT->periph)){
252  b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
253  if (b == PROTO_OK)
254  {
255  setToBootloaderMode = true;
256  ret = 1;
257  break;
258  }
259  }
260  }
261  }
262  }
263  if (setToBootloaderMode) {
264  //if successfully entered bootloader mode, clear any remaining bytes (which may have a function, but I did not check)
265  while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {PX4IO_PORT->get_byte(PX4IO_PORT->periph);}
266  // FBW OK OK hollay hollay :)
267  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'F');
268  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'B');
269  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'W');
270  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'O');
271  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'K');
272  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'O');
273  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'K'); // use 7 chars as answer
274  } else {
275  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'E'); //TODO: find out what the PX4 protocol for error feedback is...
276  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'R');
277  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'R');
278  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'O');
279  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'R');
280  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, ':');
281  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, '2'); // use 7 chars as answer
282  }
283  } else {
284  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'E'); //TODO: find out what the PX4 protocol for error feedback is...
285  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'R');
286  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'R');
287  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'O');
288  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, 'R');
289  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, ':');
290  FLASH_PORT->put_byte(FLASH_PORT->periph, 0, '1'); // use 7 chars as answer
291  }
292 #endif
293  }
294  } else if (FLASH_PORT->char_available(FLASH_PORT->periph)) {
295 #ifdef INTER_MCU_AP
296  //already in bootloader mode, just directly relay data
297  while (FLASH_PORT->char_available(FLASH_PORT->periph)) {
298  unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph);
299  PX4IO_PORT->put_byte(PX4IO_PORT->periph, 0, b);
300  }
301 #endif
302  }
303 }
304 
void sys_time_usleep(uint32_t us)
sys_time_usleep(uint32_t us)
void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud)
Set baudrate.
Definition: uart_arch.c:984
#define B230400
Definition: uart_arch.h:49
#define B115200
Definition: uart_arch.h:48
void mcu_reboot(enum reboot_state_t reboot_state)
Reboot the MCU.
Definition: mcu_arch.c:171
@ MCU_REBOOT_BOOTLOADER
Go to bootloader.
Definition: mcu.h:46
struct State state
Definition: state.c:36
static float p[2][2]
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
void intermcu_set_enabled(bool value)
enable/disable intermcu link
Definition: intermcu_ap.c:123
Inter-MCU on the AP side.
arch independent LED (Light Emitting Diodes) API
Arch independent mcu ( Micro Controller Unit ) utilities.
PX4IO interface protocol.
uint8_t offset
Definition: iomcu.c:40
#define PKT_CODE_SUCCESS
Definition: protocol.h:234
#define PX4IO_REBOOT_BL_MAGIC
Definition: protocol.h:182
uint8_t page
Definition: iomcu.c:39
#define PX4IO_P_SETUP_REBOOT_BL
Definition: protocol.h:181
#define PX4IO_PAGE_SETUP
Definition: protocol.h:151
uint8_t count_code
Definition: protocol.h:220
static uint8_t crc_packet(struct IOPacket *pkt)
Definition: protocol.h:282
uint8_t crc
Definition: iomcu.c:38
uint16_t regs[PKT_MAX_REGS]
Definition: iomcu.c:41
Definition: iomcu.c:35
#define PROTO_GET_SYNC
NOP for re-establishing sync.
Definition: px4_flash.c:64
void px4flash_event(void)
Definition: px4_flash.c:84
bool px4ioRebootTimeout
Definition: px4_flash.c:73
#define PROTO_OK
INSYNC/OK - 'ok' response.
Definition: px4_flash.c:60
void px4flash_init(void)
Definition: px4_flash.c:75
#define PROTO_EOC
end of command
Definition: px4_flash.c:58
bool setToBootloaderMode
Definition: px4_flash.c:72
#define PROTO_INSYNC
'in sync' byte sent before status
Definition: px4_flash.c:57
#define FLASH_PORT
Definition: px4_flash.c:47
#define TRUE
Definition: std.h:4
#define FALSE
Definition: std.h:5
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
void sys_time_cancel_timer(tid_t id)
Cancel a system timer by id.
Definition: sys_time.c:76
Architecture independent timing functions.
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:123
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
struct target_t target
Definition: target_pos.c:64
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
arch independent USB API
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float b
Definition: wedgebug.c:202