diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 030fb587a5..4d6c3b820d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -761,7 +761,8 @@ private: void update_cruise(); void update_fbwb_speed_height(void); void setup_turn_angle(void); - bool create_mixer_file(const char *filename); + bool print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...); + bool create_mixer(char *buf, uint16_t buf_size, const char *filename); bool setup_failsafe_mixing(void); void set_control_channels(void); void init_rc_in(); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 66e1771408..ef4a427509 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -105,7 +105,12 @@ void Plane::init_ardupilot() #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 // this must be before BoardConfig.init() so if // BRD_SAFETYENABLE==0 then we don't have safety off yet - setup_failsafe_mixing(); + for (uint8_t tries=0; tries<10; tries++) { + if (setup_failsafe_mixing()) { + break; + } + hal.scheduler->delay(10); + } #endif BoardConfig.init();