diff --git a/conf/airframes/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml
index 6b7ff13814..60e681616a 100644
--- a/conf/airframes/TUDELFT/tudelft_conf.xml
+++ b/conf/airframes/TUDELFT/tudelft_conf.xml
@@ -116,7 +116,7 @@
radio="radios/FrSky3dr.xml"
telemetry="telemetry/default_rotorcraft_slow.xml"
flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml settings/estimation/body_to_imu.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="#ffffcccaccca"
/>
diff --git a/conf/airframes/TUDELFT/tudelft_iris_indi.xml b/conf/airframes/TUDELFT/tudelft_iris_indi.xml
index dfef0ebf6b..c12ad51d0a 100644
--- a/conf/airframes/TUDELFT/tudelft_iris_indi.xml
+++ b/conf/airframes/TUDELFT/tudelft_iris_indi.xml
@@ -35,7 +35,7 @@
-
+
@@ -58,7 +58,7 @@
-
+
diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/firmwares/rotorcraft/main_fbw.c
index a47037c7af..566aef1186 100644
--- a/sw/airborne/firmwares/rotorcraft/main_fbw.c
+++ b/sw/airborne/firmwares/rotorcraft/main_fbw.c
@@ -37,19 +37,17 @@
#include "subsystems/actuators/motor_mixing.h"
#endif
-
#include "subsystems/electrical.h"
-
#include "subsystems/radio_control.h"
-
#include "subsystems/intermcu/intermcu_fbw.h"
-
#include "firmwares/rotorcraft/main_fbw.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#define MODULES_C
#include "generated/modules.h"
+
+
/** Fly by wire modes */
typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum;
fbw_mode_enum fbw_mode;
@@ -178,18 +176,31 @@ STATIC_INLINE void main_periodic(void)
}
}
+#ifdef BOARD_PX4IO
+ //due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
+ //to prevent this situation:
+ if (inter_mcu.stable_px4_baud != PPRZ_BAUD) {
+ fbw_mode = FBW_MODE_FAILSAFE;
+ autopilot_motors_on = false;
+ //signal to user whether fbw can be flashed:
+#ifdef FBW_MODE_LED
+ LED_OFF(FBW_MODE_LED); // causes really fast blinking
+#endif
+ }
+#endif
static uint16_t dv = 0;
// TODO make module out of led blink?
/* set failsafe commands */
if (fbw_mode == FBW_MODE_FAILSAFE) {
+ autopilot_motors_on = false;
SetCommands(commands_failsafe);
#ifdef FBW_MODE_LED
if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_MANUAL) {
if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_AUTO) {
- LED_TOGGLE(FBW_MODE_LED); // toggle instead of on, because then it is still visible when fbw_mode switches very fast
+ intermcu_blink_fbw_led(dv++);
#endif // FWB_MODE_LED
}
@@ -208,7 +219,10 @@ static void autopilot_on_rc_frame(void)
{
/* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
if (radio_control.values[RADIO_FBW_MODE] < (MIN_PPRZ / 2)) {
- fbw_mode = FBW_MODE_MANUAL;
+ //TODO, check whether the aircraft can actually be flown in manual mode
+ //most rotory aircraft can't, at least not without additional IMU aid
+ //for now, just turn set to failsafe instead of manual mode.
+ fbw_mode = FBW_MODE_FAILSAFE;
} else {
fbw_mode = FBW_MODE_AUTO;
}
@@ -222,6 +236,7 @@ static void autopilot_on_rc_frame(void)
/* if manual */
if (fbw_mode == FBW_MODE_MANUAL) {
autopilot_motors_on = true;
+ SetCommands(commands_failsafe);
#ifdef SetCommandsFromRC
SetCommandsFromRC(commands, radio_control.values);
#else
@@ -237,8 +252,10 @@ static void autopilot_on_ap_command(void)
{
if (fbw_mode != FBW_MODE_MANUAL) {
SetCommands(intermcu_commands);
- } else {
+ } else if (fbw_mode == FBW_MODE_AUTO) {
autopilot_motors_on = true;
+ } else {
+ autopilot_motors_on = false;
}
}
diff --git a/sw/airborne/modules/px4_flash/px4_flash.c b/sw/airborne/modules/px4_flash/px4_flash.c
index 7aa81f17e2..0eee092b42 100644
--- a/sw/airborne/modules/px4_flash/px4_flash.c
+++ b/sw/airborne/modules/px4_flash/px4_flash.c
@@ -74,6 +74,15 @@ void px4flash_init(void)
void px4flash_event(void)
{
+ if (sys_time_check_and_ack_timer(px4iobl_tid)) {
+ px4ioRebootTimeout = TRUE;
+ sys_time_cancel_timer(px4iobl_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(PX4IO_PORT->periph, B230400);
+ }
if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
if (!setToBootloaderMode) {
//ignore anything coming from IO if not in bootloader mode (which should be nothing)
@@ -128,9 +137,7 @@ void px4flash_event(void)
//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);
+ if (px4ioRebootTimeout) {
FLASH_PORT->put_byte(FLASH_PORT->periph, 'T');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'I');
FLASH_PORT->put_byte(FLASH_PORT->periph, 'M');
@@ -153,23 +160,22 @@ void px4flash_event(void)
//stop all intermcu communication:
disable_inter_comm(true);
+ px4iobl_tid = sys_time_register_timer(5.0, NULL); //10 (fbw pprz bl timeout)-5 (px4 fmu bl timeout)
/*
- * The progdieshit define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct.
+ * The forceprog 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
+ * Optional 5&6:
* 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
+ //#define forceprog
-#ifndef progdieshit
+#ifndef forceprog
//send the reboot to bootloader command:
static struct IOPacket dma_packet;
dma_packet.count_code = 0x40 + 0x01;
diff --git a/sw/airborne/subsystems/intermcu.h b/sw/airborne/subsystems/intermcu.h
index eff1a9fcee..031ad03a5a 100644
--- a/sw/airborne/subsystems/intermcu.h
+++ b/sw/airborne/subsystems/intermcu.h
@@ -37,17 +37,30 @@
#define INTERMCU_LOST_CNT 25 /* 50ms with a 512Hz timer TODO fixed value */
#endif
+#include BOARD_CONFIG
+
enum intermcu_status {
INTERMCU_OK,
INTERMCU_LOST
};
+enum intermcu_PX4_baud_status {
+ PX4_BAUD,
+ CHANGING_BAUD,
+ PPRZ_BAUD
+};
+
struct intermcu_t {
enum intermcu_status status;
uint8_t time_since_last_frame;
+#ifdef BOARD_PX4IO
+ enum intermcu_PX4_baud_status stable_px4_baud;
+#endif
};
extern struct intermcu_t inter_mcu;
+
+
void intermcu_init(void);
void intermcu_periodic(void);
diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c
index b4bebed41f..d641d184d7 100644
--- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c
+++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c
@@ -31,13 +31,13 @@
#include "pprzlink/pprz_transport.h"
#include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h"
+#include BOARD_CONFIG
#ifdef BOARD_PX4IO
#include "libopencm3/cm3/scb.h"
#include "led.h"
#include "mcu_periph/sys_time.h"
static uint8_t px4RebootSequence[] = {0x41, 0xd7, 0x32, 0x0a, 0x46, 0x39};
static uint8_t px4RebootSequenceCount = 0;
-static bool px4RebootTimeout = false;
uint8_t autopilot_motors_on = false;
tid_t px4bl_tid; ///< id for time out of the px4 bootloader reset
#endif
@@ -63,7 +63,7 @@ void intermcu_init(void)
{
pprz_transport_init(&intermcu_transport);
#ifdef BOARD_PX4IO
- px4bl_tid = sys_time_register_timer(20.0, NULL);
+ px4bl_tid = sys_time_register_timer(10.0, NULL);
#endif
}
@@ -119,6 +119,21 @@ void intermcu_send_status(uint8_t mode)
//FIXME
}
+
+void intermcu_blink_fbw_led(uint16_t dv)
+{
+#ifdef FBW_MODE_LED
+ static uint16_t idv = 0;
+ if (!autopilot_motors_on) {
+ if (!(dv % (PERIODIC_FREQUENCY))) {
+ if (!(idv++ % 3)) { LED_OFF(FBW_MODE_LED);} else {LED_TOGGLE(FBW_MODE_LED);}
+ }
+ } else {
+ LED_TOGGLE(FBW_MODE_LED); // toggle makes random blinks if intermcu comm problem!
+ }
+#endif
+}
+
static void intermcu_parse_msg(struct transport_rx *trans, void (*commands_frame_handler)(void))
{
/* Parse the Inter MCU message */
@@ -174,13 +189,22 @@ void InterMcuEvent(void (*frame_handler)(void))
#ifdef BOARD_PX4IO
static void checkPx4RebootCommand(unsigned char b)
{
- if (!px4RebootTimeout) {
-
+ if (inter_mcu.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);
+ inter_mcu.stable_px4_baud = PPRZ_BAUD;
+ } else if (inter_mcu.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
- px4RebootTimeout = true;
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);
+ inter_mcu.stable_px4_baud = CHANGING_BAUD;
+ px4bl_tid = sys_time_register_timer(1.0, NULL);
return;
}
diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.h b/sw/airborne/subsystems/intermcu/intermcu_fbw.h
index 88d065bd00..05ea25a7f7 100644
--- a/sw/airborne/subsystems/intermcu/intermcu_fbw.h
+++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.h
@@ -34,6 +34,7 @@ extern pprz_t intermcu_commands[COMMANDS_NB];
void intermcu_on_rc_frame(uint8_t fbw_mode);
void intermcu_send_status(uint8_t mode);
void InterMcuEvent(void (*frame_handler)(void));
+void intermcu_blink_fbw_led(uint16_t dv);
/* We need radio defines for the Autopilot */