/* For interrupt handling we add a new function which is called
* when recieve interrupts happen. The name (usart1_isr) is created
* by the irq.json file in libopencm3 calling this interrupt for
* USART2 'usart2', adding the suffix '_isr', and then weakly binding
* it to the 'do nothing' interrupt function in vec.c.
*
* By defining it in this file the linker will override that weak
* binding and instead bind it here, but you have to get the name
* right or it won't work. And you'll wonder where your interrupts
* are going.
*/
void usart1_isr(void)
{
uint32_t reg;
int i;
do {
reg = USART_SR(CONSOLE_UART);
if (reg & USART_SR_RXNE) {
recv_buf[recv_ndx_nxt] = USART_DR(CONSOLE_UART);
#ifdef RESET_ON_CTRLC
/*
* This bit of code will jump to the ResetHandler if you
* hit ^C
*/
if (recv_buf[recv_ndx_nxt] == '\003') {
scb_reset_system();
return; /* never actually reached */
}
#endif
/* Check for "overrun" */
i = (recv_ndx_nxt + 1) % RECV_BUF_SIZE;
if (i != recv_ndx_cur) {
recv_ndx_nxt = i;
}
}
/* can read back-to-back interrupts */
} while ((reg & USART_SR_RXNE) != 0);
}
static void checkPx4RebootCommand(uint8_t b)
{
if (intermcu.stable_px4_baud == CHANGING_BAUD && sys_time_check_and_ack_timer(px4bl_tid)) {
//to prevent a short intermcu comm loss, give some time to changing the baud
sys_time_cancel_timer(px4bl_tid);
intermcu.stable_px4_baud = PPRZ_BAUD;
} else if (intermcu.stable_px4_baud == PX4_BAUD) {
if (sys_time_check_and_ack_timer(px4bl_tid)) {
//time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight
sys_time_cancel_timer(px4bl_tid);
//for unknown reasons, 1500000 baud does not work reliably after prolonged times.
//I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
//After a initial period on 1500000, revert to 230400
//We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
uart_periph_set_baudrate(intermcu.device->periph, B230400);
intermcu.stable_px4_baud = CHANGING_BAUD;
px4bl_tid = sys_time_register_timer(1.0, NULL);
return;
}
#ifdef SYS_TIME_LED
LED_ON(SYS_TIME_LED);
#endif
if (b == px4RebootSequence[px4RebootSequenceCount]) {
px4RebootSequenceCount++;
} else {
px4RebootSequenceCount = 0;
}
if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1
px4RebootSequenceCount = 0; // should not be necessary...
//send some magic back
//this is the same as the Pixhawk IO code would send
intermcu.device->put_byte(intermcu.device->periph, 0, 0x00);
intermcu.device->put_byte(intermcu.device->periph, 0, 0xe5);
intermcu.device->put_byte(intermcu.device->periph, 0, 0x32);
intermcu.device->put_byte(intermcu.device->periph, 0, 0x0a);
intermcu.device->put_byte(intermcu.device->periph, 0,
0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side...
while (((struct uart_periph *)(intermcu.device->periph))->tx_running) {
// tx_running is volatile now, so LED_TOGGLE not necessary anymore
#ifdef SYS_TIME_LED
LED_TOGGLE(SYS_TIME_LED);
#endif
}
#ifdef SYS_TIME_LED
LED_OFF(SYS_TIME_LED);
#endif
scb_reset_system();
}
}
}
void dfu_detach(void)
{
/* Disconnect USB cable by resetting USB Device
and pulling USB_DP low*/
rcc_peripheral_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
rcc_peripheral_clear_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USBEN);
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
gpio_clear(GPIOA, GPIO12);
gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
scb_reset_system();
}
void stfub_protect_pages_and_reset(u8 mask)
{
/*
Fixme what would happen if Vcc dissapears in
the middle of this code's execution
*/
struct option_bytes *ob = (struct option_bytes *)&_option_bytes_start;
flash_unlock();
flash_program_option_bytes((u32)&ob->wrp0, ob->wrp0 & (~mask));
flash_lock();
scb_reset_system();
}
static void checkPx4RebootCommand(unsigned char b)
{
if (!px4RebootTimeout) {
if (sys_time_check_and_ack_timer(px4bl_tid)) {
//time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight
px4RebootTimeout = true;
sys_time_cancel_timer(px4bl_tid);
return;
}
#ifdef SYS_TIME_LED
LED_ON(SYS_TIME_LED);
#endif
if (b == px4RebootSequence[px4RebootSequenceCount]) {
px4RebootSequenceCount++;
} else {
px4RebootSequenceCount = 0;
}
if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1
px4RebootSequenceCount = 0; // should not be necessary...
//send some magic back
//this is the same as the Pixhawk IO code would send
intermcu_device->put_byte(intermcu_device->periph, 0x00);
intermcu_device->put_byte(intermcu_device->periph, 0xe5);
intermcu_device->put_byte(intermcu_device->periph, 0x32);
intermcu_device->put_byte(intermcu_device->periph, 0x0a);
intermcu_device->put_byte(intermcu_device->periph,
0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side...
while (((struct uart_periph *)(intermcu_device->periph))->tx_running) {
// tx_running is volatile now, so LED_TOGGLE not necessary anymore
#ifdef SYS_TIME_LED
LED_TOGGLE(SYS_TIME_LED);
#endif
}
#ifdef SYS_TIME_LED
LED_OFF(SYS_TIME_LED);
#endif
scb_reset_system();
}
}
}
/* For interrupt handling we add a new function which is called
* when receive interrupts happen. The name (usart1_isr) is created
* by the irq.json file in libopencm3 calling this interrupt for
* USART1 'usart1', adding the suffix '_isr', and then weakly binding
* it to the 'do nothing' interrupt function in vec.c.
*
* By defining it in this file the linker will override that weak
* binding and instead bind it here, but you have to get the name
* right or it won't work. And you'll wonder where your interrupts
* are going.
*/
void usart1_isr(void)
{
uint32_t reg;
int i;
do {
reg = USART_SR(CONSOLE_UART);
if (reg & USART_SR_RXNE) {
recv_buf[recv_ndx_nxt] = USART_DR(CONSOLE_UART);
#ifdef RESET_ON_CTRLC
/* Check for "reset" */
if (recv_buf[recv_ndx_nxt] == '\003') {
scb_reset_system();
}
#endif
/* Check for "overrun" */
i = (recv_ndx_nxt + 1) % RECV_BUF_SIZE;
if (i != recv_ndx_cur) {
recv_ndx_nxt = i;
}
}
} while ((reg & USART_SR_RXNE) != 0);
/* can read back-to-back interrupts */
}
void PWR_JumpToProgrammer()
{
scb_reset_system();
}
void handle_message(const Command &msg) {
if(msg.dest != dest) return;
Response resp; memset(&resp, 0, sizeof(resp));
resp.id = msg.id;
switch(msg.command) {
case CommandID::Reset: {
// action happens later, after response is sent
} break;
case CommandID::GetStatus: {
resp.resp.GetStatus.magic = GetStatusResponse::MAGIC_VALUE;
} break;
case CommandID::GetProgramHash: {
if(msg.args.GetProgramHash.length > static_cast<int64_t>(client_flash_end - client_flash_start)) {
resp.resp.GetProgramHash.error_number = 1;
break;
}
uf_subbus_protocol::sha256_state md; uf_subbus_protocol::sha256_init(md);
uf_subbus_protocol::sha256_process(md, client_flash_start, msg.args.GetProgramHash.length);
resp.resp.GetProgramHash.error_number = 0;
uf_subbus_protocol::sha256_done(md, resp.resp.GetProgramHash.hash);
} break;
case CommandID::Erase: {
if(msg.args.Erase.min_length > client_flash_end - client_flash_start) {
resp.resp.Erase.error_number = 1;
break;
}
flash_unlock();
uint8_t *to_erase = client_flash_start;
bool failed = false;
while(to_erase - client_flash_start < msg.args.Erase.min_length) {
if(!flash_erase(to_erase)) {
failed = true;
break;
}
to_erase += 2048;
}
flash_lock();
if(failed) {
resp.resp.Erase.error_number = 2;
break;
}
resp.resp.Erase.error_number = 0;
} break;
case CommandID::Flash: {
if(msg.args.Flash.length > FlashCommand::MAX_LENGTH) {
resp.resp.Flash.error_number = 1;
break;
}
if(static_cast<int64_t>(msg.args.Flash.start_offset) +
msg.args.Flash.length > client_flash_end - client_flash_start) {
resp.resp.Flash.error_number = 2;
break;
}
flash_unlock();
bool success = flash_write(
client_flash_start + msg.args.Flash.start_offset,
msg.args.Flash.data, msg.args.Flash.length);
flash_lock();
if(!success) {
resp.resp.Flash.error_number = 3;
break;
}
resp.resp.Flash.error_number = 0;
} break;
case CommandID::RunProgram: {
// action happens later, after response is sent
} break;
default: {
return; // send nothing back if command is invalid
} break;
}
if(resp.id) {
sender.write_object(resp);
}
switch(msg.command) {
case CommandID::Reset: {
scb_reset_system();
} break;
case CommandID::RunProgram: {
SCB_VTOR = reinterpret_cast<uint32_t>(client_flash_start);
vector_table_t *new_vector_table = reinterpret_cast<vector_table_t*>(client_flash_start);
asm("mov SP, %0;bx %1;" ::
"r"(new_vector_table->initial_sp_value),
"r"(new_vector_table->reset));
} break;
default: {
} break;
}
}
void dfu_detach(void)
{
/* USB device must detach, we just reset... */
scb_reset_system();
}
inline void
reboot(void) {
scb_reset_system();
}
/*
* board_reset() - Request board reset
*
* INPUT
* none
* OUTPUT
* none
*/
void board_reset(void)
{
#ifndef EMULATOR
scb_reset_system();
#endif
}
void process_cmd(struct cmd_frame_t *cmd)
{
int res;
reply.status = ACK; // change to NACK as necessary
reply.cmd = cmd->cmd;
switch (cmd->cmd) {
case CMD_ECHO:
reply.data[0] = cmd->echo.length;
memcpy(&reply.data[1], cmd->echo.data, cmd->echo.length);
send_reply(&reply, cmd->echo.length+3);
break;
case CMD_RESET:
if (cmd->reset_magic == RESET_MAGIC) {
usb_peripheral_reset();
CREG_M4MEMMAP = 0x10400000;
scb_reset_system();
} else {
send_nack();
}
break;
case CMD_GET_EVENT_COUNTERS:
{
struct event_counters counters = get_last_event_counters();
memcpy(reply.data, &counters, sizeof(struct event_counters));
send_reply(&reply, 2+sizeof(struct event_counters));
break;
}
case CMD_GET_STAGE_GAINS:
memcpy(reply.data, stage_fb_gains, sizeof(stage_fb_gains));
send_reply(&reply, 2+sizeof(stage_fb_gains));
break;
case CMD_SET_STAGE_GAINS:
memcpy(stage_fb_gains, cmd->set_stage_gains, sizeof(stage_fb_gains));
send_ack();
break;
case CMD_GET_STAGE_SETPOINT:
memcpy(reply.data, stage_fb_setpoint, sizeof(stage_fb_setpoint));
send_reply(&reply, 2+sizeof(stage_fb_setpoint));
break;
case CMD_SET_STAGE_SETPOINT:
memcpy(stage_fb_setpoint, cmd->set_stage_setpoint, sizeof(stage_fb_setpoint));
send_ack();
break;
case CMD_GET_PSD_GAINS:
memcpy(reply.data, psd_fb_gains, sizeof(psd_fb_gains));
send_reply(&reply, 2+sizeof(psd_fb_gains));
break;
case CMD_SET_PSD_GAINS:
memcpy(psd_fb_gains, cmd->set_psd_gains, sizeof(psd_fb_gains));
send_ack();
break;
case CMD_GET_PSD_SETPOINT:
memcpy(reply.data, psd_fb_setpoint, sizeof(psd_fb_setpoint));
send_reply(&reply, 2+sizeof(psd_fb_setpoint));
break;
case CMD_SET_PSD_SETPOINT:
memcpy(psd_fb_setpoint, cmd->set_psd_setpoint, sizeof(psd_fb_setpoint));
send_ack();
break;
case CMD_GET_MAX_ERROR:
memcpy(reply.data, &max_error, sizeof(max_error));
send_reply(&reply, 2+sizeof(max_error));
break;
case CMD_SET_MAX_ERROR:
max_error = cmd->set_max_error;
send_ack();
break;
case CMD_GET_OUTPUT_GAINS:
{
fixed16_t* tmp = (fixed16_t*) reply.data;
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
*tmp = stage_outputs[i].p_gain; tmp++;
*tmp = stage_outputs[i].i_gain; tmp++;
}
send_reply(&reply, 2+2*STAGE_OUTPUTS*sizeof(fixed16_t));
break;
}
case CMD_SET_OUTPUT_GAINS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
stage_outputs[i].p_gain = cmd->set_output_gains[i][0];
stage_outputs[i].i_gain = cmd->set_output_gains[i][1];
}
send_ack();
break;
}
case CMD_GET_OUTPUT_TAUS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
reply.data[i] = pi_get_tau(&stage_outputs[i]);
}
send_reply(&reply, 2+3);
break;
}
case CMD_SET_OUTPUT_TAUS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
pi_set_tau(&stage_outputs[i], cmd->set_output_taus[i]);
}
send_ack();
break;
}
case CMD_GET_ADC_FREQ:
reply.data32[0] = 0; //TODO adc_get_trigger_freq();
send_reply(&reply, 2+4);
break;
case CMD_SET_ADC_FREQ:
adc_set_trigger_freq(cmd->set_adc_freq);
send_ack();
break;
case CMD_GET_ADC_TRIGGER_MODE:
reply.data[0] = adc_get_trigger_mode();
send_reply(&reply, 2+1);
break;
case CMD_SET_ADC_TRIGGER_MODE:
adc_set_trigger_mode(cmd->set_adc_trigger_mode);
send_ack();
break;
case CMD_START_ADC_STREAM:
adc_streaming = true;
send_ack();
break;
case CMD_STOP_ADC_STREAM:
adc_streaming = false;
adc_flush();
send_ack();
break;
case CMD_FLUSH_ADC_STREAM:
if (adc_flush() == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_GET_ADC_DECIMATION:
{
reply.data32[0] = adc_get_decimation();
send_reply(&reply, 2+4);
break;
}
case CMD_SET_ADC_DECIMATION:
if (adc_set_decimation(cmd->set_adc_decimation) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_GET_FEEDBACK_FREQ:
{
unsigned int freq = feedback_get_loop_freq();
reply.data32[0] = freq;
send_reply(&reply, 2+4);
break;
}
case CMD_SET_FEEDBACK_FREQ:
feedback_set_loop_freq(cmd->set_feedback_freq);
send_ack();
break;
case CMD_GET_FEEDBACK_MODE:
reply.data[0] = feedback_get_mode();
send_reply(&reply, 3);
break;
case CMD_SET_FEEDBACK_MODE:
feedback_set_mode(cmd->set_feedback_mode);
send_ack();
break;
case CMD_SET_RAW_POS:
if (feedback_set_position(cmd->set_raw_pos) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_CLEAR_PATH:
clear_path();
send_ack();
break;
case CMD_ENQUEUE_POINTS:
res = 0;
if (cmd->enqueue_points.npts > 0)
res = enqueue_points((uint16_t*) &cmd->enqueue_points.points, cmd->enqueue_points.npts);
if (res == -1 || res == 0) {
reply.data[0] = res != -1; // were the points added?
reply.data[1] = is_path_running();
send_reply(&reply, 4);
} else {
send_nack();
}
break;
case CMD_START_PATH:
if (start_path(cmd->start_path.freq, cmd->start_path.synchronous_adc) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_SET_EXCITATION:
{
struct set_excitation* const se = &cmd->set_excitation;
struct excitation_buffer* const exc = &excitations[se->channel];
if ((se->length + se->offset > MAX_EXCITATION_LENGTH)
|| (se->channel >= 3))
{
send_nack();
break;
}
exc->length = 0;
exc->offset = 0;
if (se->length != 0) {
memcpy(&exc->samples[se->offset],
&se->samples,
sizeof(uint16_t) * se->length);
}
exc->length = se->total_length;
send_ack();
break;
}
case CMD_GET_SEARCH_STEP:
memcpy(reply.data, search_fb_step,
sizeof(search_fb_step));
send_reply(&reply, 2+sizeof(search_fb_step));
break;
case CMD_SET_SEARCH_STEP:
memcpy(search_fb_step, cmd->set_search_step,
sizeof(search_fb_step));
send_ack();
break;
case CMD_GET_SEARCH_OBJ_GAINS:
memcpy(reply.data, search_obj_gains,
sizeof(search_obj_gains));
send_reply(&reply, 2+sizeof(search_obj_gains));
break;
case CMD_SET_SEARCH_OBJ_GAINS:
memcpy(search_obj_gains, cmd->set_search_obj_gains,
sizeof(search_obj_gains));
send_ack();
break;
case CMD_GET_SEARCH_OBJ_THRESH:
memcpy(reply.data, &search_obj_thresh,
sizeof(search_obj_thresh));
send_reply(&reply, 2+sizeof(search_obj_thresh));
break;
case CMD_SET_SEARCH_OBJ_THRESH:
search_obj_thresh = cmd->set_search_obj_thresh,
send_ack();
break;
case CMD_GET_COARSE_FB_PARAMS:
memcpy(reply.data, &coarse_fb_channels,
sizeof(coarse_fb_channels));
send_reply(&reply, 2+sizeof(coarse_fb_channels));
break;
case CMD_SET_COARSE_FB_PARAMS:
memcpy(coarse_fb_channels, cmd->set_coarse_fb_params,
sizeof(coarse_fb_channels));
send_ack();
break;
default:
send_nack();
return;
}
}
void target_manifest_app(void) {
scb_reset_system();
}
void px4flash_event(void)
{
if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
if (!setToBootloaderMode) {
//ignore anything coming from IO if not in bootloader mode (which should be nothing)
} else {
//relay everything from IO to the laptop
while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
FLASH_PORT->put_byte(FLASH_PORT->periph, b);
}
}
}
//TODO: check if bootloader timeout was surpassed
if (FLASH_PORT->char_available(FLASH_PORT->periph) && !setToBootloaderMode) {
// TMP TEST
// while (FLASH_PORT->char_available(FLASH_PORT->periph)) {
// unsigned char bla = FLASH_PORT->get_byte(FLASH_PORT->periph);
// FLASH_PORT->put_byte(FLASH_PORT->periph,bla);
// }
// return;
//check whether this is flash related communication, and for who (ap/fbw)
int state = 0;
while (state < 4 && FLASH_PORT->char_available(FLASH_PORT->periph)) {
unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph);
switch (state) {
case (0) :
if (b == 'p') { state++; } else { return; }
break;
case (1) :
if (b == 'p') { state++; } else { return; }
break;
case (2) :
if (b == 'r') { state++; } else { return; }
break;
case (3) :
if (b == 'z') { state++; } else { return; }
break;
default :
break;
}
}
if (state != 4) {return;}
//TODO: check if/how this interferes with flashing original PX4 firmware
unsigned char target = FLASH_PORT->get_byte(FLASH_PORT->periph);
if (target == '1') { //target ap
//the target is the ap, so reboot to PX4 bootloader
scb_reset_system();
} else { // target fbw
//the target is the fbw, so reboot the fbw and switch to relay mode
//first check if the bootloader has not timeout:
if (sys_time_check_and_ack_timer(px4iobl_tid) || px4ioRebootTimeout) {
px4ioRebootTimeout = TRUE;
sys_time_cancel_timer(px4iobl_tid);
FLASH_PORT->put_byte(FLASH_PORT->periph, 'T');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'I');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'M');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'E');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'U');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'T'); // use 7 chars as answer
return;
} { // FBW OK OK hollay hollay :)
FLASH_PORT->put_byte(FLASH_PORT->periph, 'F');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'B');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'W');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'K');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'K'); // use 7 chars as answer
}
//stop all intermcu communication:
disable_inter_comm(true);
/*
* The progdieshit define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct.
* In that case:
* 1. enable this define
* 2. build and upload the fmu f4 chip (ap target in pprz center)
* 3. build the io code, and convert the firmware using the following command:
* /home/houjebek/paparazzi/sw/tools/px4/px_mkfw.py --prototype "/home/houjebek/px4/Firmware/Images/px4io-v2.prototype" --image /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.bin > /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
* 4. Start the following command:
* /home/houjebek/paparazzi/sw/tools/px4/px_uploader.py --port "/dev/ttyACM0" /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
* 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)
* 5b Or, press the IO reset button on the pixhawk
* 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.
* 7. Don forget to disable the define and upload the ap again :)
*/
// #define progdieshit
#ifndef progdieshit
//send the reboot to bootloader command:
static struct IOPacket dma_packet;
dma_packet.count_code = 0x40 + 0x01;
dma_packet.crc = 0;
dma_packet.page = PX4IO_PAGE_SETUP;
dma_packet.offset = PX4IO_P_SETUP_REBOOT_BL;
dma_packet.regs[0] = PX4IO_REBOOT_BL_MAGIC;
dma_packet.crc = crc_packet(&dma_packet);
struct IOPacket *pkt = &dma_packet;
uint8_t *p = (uint8_t *)pkt;
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[0]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[1]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[2]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[3]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[4]);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[5]);
sys_time_usleep(5000); // this seems to be close to the minimum delay necessary to process this packet at the IO side
//the pixhawk IO chip should respond with:
// 0x00 ( PKT_CODE_SUCCESS )
// 0xe5
// 0x32
// 0x0a
//After that, the IO chips reboots into bootloader mode, in which it will stay for a short period
//The baudrate in bootloader mode ic changed to 115200 (normal operating baud is 1500000, at least for original pixhawk fmu firmware)
//state machine
state = 0;
while (state < 4 && PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
switch (state) {
case (0) :
if (b == PKT_CODE_SUCCESS) { state++; } else { state = 0; }
break;
case (1) :
if (b == 0xe5) { state++; } else { state = 0; }
break;
case (2) :
if (b == 0x32) { state++; } else { state = 0; }
break;
case (3) :
if (b == 0x0a) { state++; } else { state = 0; }
break;
default :
break;
}
}
#else
int state = 4;
#endif
if (state == 4) {
uart_periph_set_baudrate(PX4IO_PORT->periph, B115200);
/* look for the bootloader for 150 ms */
int ret = 0;
for (int i = 0; i < 15 && !ret ; i++) {
sys_time_usleep(10000);
//send a get_sync command in order to keep the io in bootloader mode
PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_GET_SYNC);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_EOC);
//get_sync should be replied with, so check if that happens and
//all other bytes are discarded, hopefully those were not important
//(they may be caused by sending multiple syncs)
while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
if (b == PROTO_INSYNC) {
setToBootloaderMode = true;
ret = 1;
break;
}
}
}
if (setToBootloaderMode) {
//if successfully entered bootloader mode, clear any remaining bytes (which may have a function, but I did not check)
while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {PX4IO_PORT->get_byte(PX4IO_PORT->periph);}
}
} else {
FLASH_PORT->put_byte(FLASH_PORT->periph, 'E'); //TODO: find out what the PX4 protocol for error feedback is...
FLASH_PORT->put_byte(FLASH_PORT->periph, 'R');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'R');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'O');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'R');
FLASH_PORT->put_byte(FLASH_PORT->periph, '!');
FLASH_PORT->put_byte(FLASH_PORT->periph, ' '); // use 7 chars as answer
}
}
} else if (FLASH_PORT->char_available(FLASH_PORT->periph)) {
//already in bootloader mode, just directly relay data
while (FLASH_PORT->char_available(FLASH_PORT->periph)) {
unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph);
PX4IO_PORT->put_byte(PX4IO_PORT->periph, b);
}
}
}
