forked from Archive/PX4-Autopilot
Merged PX4IO crc checks and force update
This commit is contained in:
parent
8f90efa312
commit
6016fbe55d
|
@ -95,6 +95,7 @@ extern device::Device *PX4IO_serial_interface() weak_function;
|
|||
|
||||
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
|
||||
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
|
||||
#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2)
|
||||
|
||||
#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz
|
||||
#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz
|
||||
|
@ -2146,6 +2147,16 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
|
|||
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
|
||||
break;
|
||||
|
||||
case PX4IO_REBOOT_BOOTLOADER:
|
||||
if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
|
||||
return -EINVAL;
|
||||
|
||||
/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
|
||||
// we don't expect a reply from this operation
|
||||
ret = OK;
|
||||
break;
|
||||
|
||||
case PX4IO_INAIR_RESTART_ENABLE:
|
||||
|
||||
/* set/clear the 'in-air restart' bit */
|
||||
|
@ -2673,6 +2684,39 @@ px4io_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "forceupdate")) {
|
||||
/*
|
||||
force update of the IO firmware without requiring
|
||||
the user to hold the safety switch down
|
||||
*/
|
||||
if (argc <= 3) {
|
||||
printf("usage: px4io forceupdate MAGIC filename\n");
|
||||
exit(1);
|
||||
}
|
||||
if (g_dev == nullptr) {
|
||||
printf("px4io is not started\n");
|
||||
exit(1);
|
||||
}
|
||||
uint16_t arg = atol(argv[2]);
|
||||
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
|
||||
if (ret != OK) {
|
||||
printf("reboot failed - %d\n", ret);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// tear down the px4io instance
|
||||
delete g_dev;
|
||||
|
||||
// upload the specified firmware
|
||||
const char *fn[2];
|
||||
fn[0] = argv[3];
|
||||
fn[1] = nullptr;
|
||||
PX4IO_Uploader *up = new PX4IO_Uploader;
|
||||
up->upload(&fn[0]);
|
||||
delete up;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "rx_dsm") ||
|
||||
!strcmp(argv[1], "rx_dsm_10bit") ||
|
||||
!strcmp(argv[1], "rx_dsm_11bit") ||
|
||||
|
@ -2690,5 +2734,5 @@ px4io_main(int argc, char *argv[])
|
|||
bind(argc, argv);
|
||||
|
||||
out:
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
|
||||
}
|
||||
|
|
|
@ -274,7 +274,10 @@ PX4IO_Uploader::drain()
|
|||
int ret;
|
||||
|
||||
do {
|
||||
ret = recv(c, 1000);
|
||||
// the small recv timeout here is to allow for fast
|
||||
// drain when rebooting the io board for a forced
|
||||
// update of the fw without using the safety switch
|
||||
ret = recv(c, 40);
|
||||
|
||||
#ifdef UDEBUG
|
||||
if (ret == OK) {
|
||||
|
|
|
@ -186,6 +186,9 @@ enum { /* DSM bind states */
|
|||
/* 8 */
|
||||
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
|
||||
|
||||
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
|
||||
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
|
||||
|
||||
/* autopilot control values, -10000..10000 */
|
||||
#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
|
||||
|
||||
|
|
|
@ -45,6 +45,8 @@
|
|||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <stm32_pwr.h>
|
||||
|
||||
#include "px4io.h"
|
||||
#include "protocol.h"
|
||||
|
@ -154,6 +156,7 @@ volatile uint16_t r_page_setup[] =
|
|||
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
|
||||
#endif
|
||||
[PX4IO_P_SETUP_SET_DEBUG] = 0,
|
||||
[PX4IO_P_SETUP_REBOOT_BL] = 0,
|
||||
};
|
||||
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (0)
|
||||
|
@ -501,6 +504,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_REBOOT_BL:
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
// don't allow reboot while armed
|
||||
break;
|
||||
}
|
||||
|
||||
// check the magic value
|
||||
if (value != PX4IO_REBOOT_BL_MAGIC)
|
||||
break;
|
||||
|
||||
// note that we don't set BL_WAIT_MAGIC in
|
||||
// BKP_DR1 as that is not necessary given the
|
||||
// timing of the forceupdate command. The
|
||||
// bootloader on px4io waits for enough time
|
||||
// anyway, and this method works with older
|
||||
// bootloader versions (tested with both
|
||||
// revision 3 and revision 4).
|
||||
|
||||
up_systemreset();
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_DSM:
|
||||
dsm_bind(value & 0x0f, (value >> 4) & 7);
|
||||
break;
|
||||
|
|
Loading…
Reference in New Issue