mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Bootloader: added more protection against line noisy triggering bootloader
this is in reponse to a report from Ryan of the Hybrid project who found that mavlink on telem1 could make bootloader get stuck
This commit is contained in:
parent
3901471eaf
commit
436662c0d7
@ -462,7 +462,9 @@ bootloader(unsigned timeout)
|
|||||||
uint32_t first_words[RESERVE_LEAD_WORDS];
|
uint32_t first_words[RESERVE_LEAD_WORDS];
|
||||||
bool done_sync = false;
|
bool done_sync = false;
|
||||||
bool done_get_device = false;
|
bool done_get_device = false;
|
||||||
|
bool done_erase = false;
|
||||||
static bool done_timer_init;
|
static bool done_timer_init;
|
||||||
|
unsigned original_timeout = timeout;
|
||||||
|
|
||||||
memset(first_words, 0xFF, sizeof(first_words));
|
memset(first_words, 0xFF, sizeof(first_words));
|
||||||
|
|
||||||
@ -596,12 +598,17 @@ bootloader(unsigned timeout)
|
|||||||
// lower chance of random data on a uart triggering erase
|
// lower chance of random data on a uart triggering erase
|
||||||
goto cmd_bad;
|
goto cmd_bad;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* expect EOC */
|
/* expect EOC */
|
||||||
if (!wait_for_eoc(2)) {
|
if (!wait_for_eoc(2)) {
|
||||||
goto cmd_bad;
|
goto cmd_bad;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// once erase is done there is no going back, set timeout
|
||||||
|
// to zero
|
||||||
|
done_erase = true;
|
||||||
|
timeout = 0;
|
||||||
|
|
||||||
flash_set_keep_unlocked(true);
|
flash_set_keep_unlocked(true);
|
||||||
|
|
||||||
// clear the bootloader LED while erasing - it stops blinking at random
|
// clear the bootloader LED while erasing - it stops blinking at random
|
||||||
@ -911,7 +918,11 @@ bootloader(unsigned timeout)
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case PROTO_SET_BAUD: {
|
case PROTO_SET_BAUD: {
|
||||||
/* expect arg then EOC */
|
if (!done_sync || !done_get_device) {
|
||||||
|
// prevent timeout going to zero on noise
|
||||||
|
goto cmd_bad;
|
||||||
|
}
|
||||||
|
/* expect arg then EOC */
|
||||||
uint32_t baud = 0;
|
uint32_t baud = 0;
|
||||||
|
|
||||||
if (cin_word(&baud, 100)) {
|
if (cin_word(&baud, 100)) {
|
||||||
@ -957,6 +968,12 @@ bootloader(unsigned timeout)
|
|||||||
sync_response();
|
sync_response();
|
||||||
continue;
|
continue;
|
||||||
cmd_bad:
|
cmd_bad:
|
||||||
|
// if we get a bad command it could be line noise on a
|
||||||
|
// uart. Set timeout back to original timeout so we don't get
|
||||||
|
// stuck in the bootloader
|
||||||
|
if (!done_erase) {
|
||||||
|
timeout = original_timeout;
|
||||||
|
}
|
||||||
// send an 'invalid' response but don't kill the timeout - could be garbage
|
// send an 'invalid' response but don't kill the timeout - could be garbage
|
||||||
invalid_response();
|
invalid_response();
|
||||||
continue;
|
continue;
|
||||||
|
Loading…
Reference in New Issue
Block a user