Plane: fixed mixer arming error with FMUv4

thanks to Tim Gold for reporting this
This commit is contained in:
Andrew Tridgell 2016-09-11 08:19:22 +10:00
parent cc28c8e6ee
commit 9b83781b4f
8 changed files with 15 additions and 8 deletions

View File

@ -308,7 +308,7 @@ void Plane::one_second_loop()
// make it possible to change control channel ordering at runtime
set_control_channels();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if HAVE_PX4_MIXER
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();

View File

@ -1009,7 +1009,7 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Advanced
GSCALAR(land_flap_percent, "LAND_FLAP_PERCNT", 0),
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if HAVE_PX4_MIXER
// @Param: OVERRIDE_CHAN
// @DisplayName: PX4IO override channel
// @Description: If set to a non-zero value then this is an RC input channel number to use for giving PX4IO manual control in case the main FMU microcontroller on a PX4 or Pixhawk fails. When this RC input channel goes above 1750 the FMU microcontroller will no longer be involved in controlling the servos and instead the PX4IO microcontroller will directly control the servos. Note that PX4IO manual control will be automatically activated if the FMU crashes for any reason. This parameter allows you to test for correct manual behaviour without actually crashing the FMU. This parameter is can be set to a non-zero value either for ground testing purposes or for giving the effect of an external override control board. Please also see the docs on OVERRIDE_SAFETY. Note that you may set OVERRIDE_CHAN to the same channel as FLTMODE_CH to get PX4IO based override when in flight mode 6. Note that when override is triggered due to a FMU crash the 6 auxiliary output channels on Pixhawk will no longer be updated, so all the flight controls you need must be assigned to the first 8 channels.

View File

@ -83,7 +83,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool report)
ret = false;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if HAVE_PX4_MIXER
if (plane.last_mixer_crc == -1) {
if (report) {
// if you ever get this error, a reboot is recommended.

View File

@ -412,3 +412,10 @@
#ifndef PARACHUTE
#define PARACHUTE ENABLED
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
# define HAVE_PX4_MIXER 1
#else
# define HAVE_PX4_MIXER 0
#endif

View File

@ -68,7 +68,7 @@ void Plane::read_control_switch()
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if HAVE_PX4_MIXER
if (g.override_channel > 0) {
// if the user has configured an override channel then check it
bool override_requested = (hal.rcin->read(g.override_channel-1) >= PX4IO_OVERRIDE_PWM);
@ -97,7 +97,7 @@ void Plane::read_control_switch()
hal.rcout->force_safety_off();
}
}
#endif // CONFIG_HAL_BOARD
#endif // HAVE_PX4_MIXER
}
uint8_t Plane::readSwitch(void)

View File

@ -12,7 +12,7 @@
that include flaps, landing gear, ignition cut etc
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if HAVE_PX4_MIXER
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

View File

@ -462,7 +462,7 @@ bool QuadPlane::setup(void)
}
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if HAVE_PX4_MIXER
// redo failsafe mixing on px4
plane.setup_failsafe_mixing();
#endif

View File

@ -102,7 +102,7 @@ void Plane::init_ardupilot()
set_control_channels();
init_rc_out_main();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if HAVE_PX4_MIXER
if (!quadplane.enable) {
// this must be before BoardConfig.init() so if
// BRD_SAFETYENABLE==0 then we don't have safety off yet. For