Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: setup mixer in the one second loop when disarmed

Browse Source
This commit is contained in:
Michael du Breuil 2016-06-05 15:47:55 -07:00 committed by Andrew Tridgell
parent fc3ab7b691
commit bb9086b87f
3 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/ArduPlane.cpp
View File

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

11
ArduPlane/control_modes.cpp
View File

@ -73,17 +73,14 @@ void Plane::read_control_switch()
// if the user has configured an override channel then check it // if the user has configured an override channel then check it
bool override_requested = (hal.rcin->read(g.override_channel-1) >= PX4IO_OVERRIDE_PWM); bool override_requested = (hal.rcin->read(g.override_channel-1) >= PX4IO_OVERRIDE_PWM);
if (override_requested && !px4io_override_enabled) { if (override_requested && !px4io_override_enabled) {
// we only update the mixer if we are not armed. This is if (hal.util->get_soft_armed() || (last_mixer_crc != -1)) {
// important as otherwise we will need to temporarily
// disarm to change the mixer
if (hal.util->get_soft_armed() || setup_failsafe_mixing()) {
px4io_override_enabled = true; px4io_override_enabled = true;
// disable output channels to force PX4IO override // disable output channels to force PX4IO override
gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enabled"); gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enabled");
} else { } else {
// we'll try again next loop. The PX4IO code sometimes // we'll let the one second loop reconfigure the mixer. The
// rejects a mixer, probably due to it being busy in // PX4IO code sometimes rejects a mixer, probably due to it
// some way? // being busy in some way?
gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enable failed"); gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override enable failed");
} }
} else if (!override_requested && px4io_override_enabled) { } else if (!override_requested && px4io_override_enabled) {

21
ArduPlane/px4_mixer.cpp
View File

@ -235,20 +235,23 @@ bool Plane::setup_failsafe_mixing(void)
bool ret = false; bool ret = false;
char *buf = NULL; char *buf = NULL;
const uint16_t buf_size = 2048; const uint16_t buf_size = 2048;
uint16_t fileSize, new_crc;
int px4io_fd = -1;
enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
buf = (char *)malloc(buf_size); buf = (char *)malloc(buf_size);
if (buf == NULL) { if (buf == NULL) {
return false; goto failed;
} }
uint16_t fileSize = create_mixer(buf, buf_size, mixer_filename); fileSize = create_mixer(buf, buf_size, mixer_filename);
if (!fileSize) { if (!fileSize) {
hal.console->printf("Unable to create mixer\n"); hal.console->printf("Unable to create mixer\n");
free(buf); goto failed;
return false;
} }
uint16_t new_crc = crc_calculate((uint8_t *)buf, fileSize); new_crc = crc_calculate((uint8_t *)buf, fileSize);
if ((int32_t)new_crc == last_mixer_crc) { if ((int32_t)new_crc == last_mixer_crc) {
return true; return true;
@ -256,14 +259,10 @@ bool Plane::setup_failsafe_mixing(void)
last_mixer_crc = new_crc; last_mixer_crc = new_crc;
} }
enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state(); px4io_fd = open("/dev/px4io", 0);
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
int px4io_fd = open("/dev/px4io", 0);
if (px4io_fd == -1) { if (px4io_fd == -1) {
// px4io isn't started, no point in setting up a mixer // px4io isn't started, no point in setting up a mixer
free(buf); goto failed;
return false;
} }
if (old_state == AP_HAL::Util::SAFETY_ARMED) { if (old_state == AP_HAL::Util::SAFETY_ARMED) {