Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: remove vestiges of old PX4_MIXER code

This commit is contained in:
Peter Barker 2019-01-19 18:46:40 +11:00 committed by Andrew Tridgell
parent cd2182453a
commit 40856fef56
10 changed files with 2 additions and 525 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/AP_Arming.cpp
View File

@ -74,13 +74,6 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}
#if HAVE_PX4_MIXER
if (plane.last_mixer_crc == -1) {
check_failed(ARMING_CHECK_NONE, display_failure, "Mixer error");
ret = false;
}
#endif // CONFIG_HAL_BOARD
return ret;
}

7
ArduPlane/ArduPlane.cpp
View File

@ -257,13 +257,6 @@ void Plane::one_second_loop()
// make it possible to change control channel ordering at runtime
set_control_channels();
#if HAVE_PX4_MIXER
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();
}
#endif // CONFIG_HAL_BOARD
#if HAL_WITH_IO_MCU
iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
#endif

2
ArduPlane/Parameters.cpp
View File

@ -807,7 +807,7 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Advanced
GSCALAR(flap_2_speed, "FLAP_2_SPEED", FLAP_2_SPEED),
#if HAVE_PX4_MIXER || HAL_WITH_IO_MCU
#if HAL_WITH_IO_MCU
// @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.

2
ArduPlane/Parameters.h
View File

@ -487,7 +487,7 @@ public:
AP_Int8 fbwa_tdrag_chan;
AP_Int8 rangefinder_landing;
AP_Int8 flap_slewrate;
#if HAVE_PX4_MIXER || HAL_WITH_IO_MCU
#if HAL_WITH_IO_MCU
AP_Int8 override_channel;
AP_Int8 override_safety;
#endif

6
ArduPlane/Plane.h
View File

@ -791,11 +791,6 @@ private:
AP_Tuning_Plane tuning;
static const struct LogStructure log_structure[];
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// the crc of the last created PX4Mixer
int32_t last_mixer_crc = -1;
#endif // CONFIG_HAL_BOARD
// rudder mixing gain for differential thrust (0 - 1)
float rudder_dt;
@ -927,7 +922,6 @@ private:
bool mix_two_channels(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan1, uint8_t in_chan2, bool left_channel);
bool mix_passthrough(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan);
bool mix_trim_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan);
bool setup_failsafe_mixing(void);
void set_control_channels(void);
void init_rc_in();
void init_rc_out_main();

6
ArduPlane/config.h
View File

@ -352,12 +352,6 @@
#endif
#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
#ifndef STATS_ENABLED
# define STATS_ENABLED ENABLED
#endif

31
ArduPlane/control_modes.cpp
View File

@ -59,37 +59,6 @@ void Plane::read_control_switch()
}
}
#endif
#if HAVE_PX4_MIXER
if (g.override_channel > 0) {
// if the user has configured an override channel then check it
bool override_requested = (RC_Channels::get_radio_in(g.override_channel-1) >= PX4IO_OVERRIDE_PWM);
if (override_requested && !px4io_override_enabled) {
if (hal.util->get_soft_armed() || (last_mixer_crc != -1)) {
px4io_override_enabled = true;
// disable output channels to force PX4IO override
gcs().send_text(MAV_SEVERITY_WARNING, "PX4IO override enabled");
} else {
// we'll let the one second loop reconfigure the mixer. The
// PX4IO code sometimes rejects a mixer, probably due to it
// being busy in some way?
gcs().send_text(MAV_SEVERITY_WARNING, "PX4IO override enable failed");
}
} else if (!override_requested && px4io_override_enabled) {
px4io_override_enabled = false;
SRV_Channels::enable_aux_servos();
gcs().send_text(MAV_SEVERITY_WARNING, "PX4IO override disabled");
}
if (px4io_override_enabled &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED &&
g.override_safety == 1) {
// we force safety off, so that if this override is used
// with a in-flight reboot it gives a way for the pilot to
// re-arm and take manual control
hal.rcout->force_safety_off();
}
}
#endif // HAVE_PX4_MIXER
}
uint8_t Plane::readSwitch(void)

447
ArduPlane/px4_mixer.cpp
View File

@ -1,447 +0,0 @@
#include "Plane.h"
/*
handle creation of PX4 mixer file, for failover to direct RC control
on failure of FMU
This will create APM/MIXER.MIX on the microSD card. The user may
also create APM/CUSTOM.MIX, and if it exists that will be used
instead. That allows the user to setup more complex failsafe mixes
that include flaps, landing gear, ignition cut etc
*/
#if HAVE_PX4_MIXER
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
#include <modules/px4iofirmware/protocol.h>
#include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
#include <utility>
#define PX4_LIM_RC_MIN 900
#define PX4_LIM_RC_MAX 2100
/*
formatted print to a buffer with buffer advance. Returns true on
success, false on fail
*/
bool Plane::print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...)
{
va_list arg_list;
va_start(arg_list, fmt);
int n = ::vsnprintf(buf, buf_size, fmt, arg_list);
va_end(arg_list);
if (n <= 0 || n >= buf_size) {
return false;
}
buf += n;
buf_size -= n;
return true;
}
/*
create a mixer for a normal angle channel
*/
bool Plane::mix_one_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan)
{
const float limit = 10000;
const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan);
bool is_throttle = in_chan==rcmap.throttle()-1;
int16_t outch_trim = is_throttle?1500:outch->get_trim();
outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1);
if (!print_buffer(buf, buf_size, "M: 1\n")) {
return false;
}
int32_t out_min = limit*(outch_trim - outch->get_output_min()) / (1500 - PX4_LIM_RC_MIN);
int32_t out_max = limit*(outch->get_output_max() - outch_trim) / (PX4_LIM_RC_MAX - 1500);
int32_t out_trim = limit*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2);
int32_t reverse = outch->get_reversed()?-1:1;
if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
int(out_min*reverse),
int(out_max*reverse),
int(out_trim),
int(-limit), int(limit))) {
return false;
}
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
in_chan,
int(limit), int(limit),
0,
int(-limit), int(limit))) {
return false;
}
return true;
}
/*
mix two channels using elevon style mixer
*/
bool Plane::mix_two_channels(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan1, uint8_t in_chan2, bool left_channel)
{
const float limit = 10000;
const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan);
int16_t outch_trim = outch->get_trim();
outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1);
if (!print_buffer(buf, buf_size, "M: 2\n")) {
return false;
}
int32_t out_min = limit*(outch->get_trim() - outch->get_output_min()) / (1500 - PX4_LIM_RC_MIN);
int32_t out_max = limit*(outch->get_output_max() - outch->get_trim()) / (PX4_LIM_RC_MAX - 1500);
int32_t out_trim = limit*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2);
int32_t in_mul2 = left_channel?-1:1;
float in_gain = g.mixing_gain;
int32_t reverse = outch->get_reversed()?-1:1;
if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
int(out_min*reverse),
int(out_max*reverse),
int(out_trim),
int(-limit*2), int(limit*2))) {
return false;
}
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
in_chan1,
int(limit*in_gain), int(limit*in_gain),
0,
int(-limit), int(limit))) {
return false;
}
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
in_chan2,
int(limit*in_gain*in_mul2), int(limit*in_gain*in_mul2),
0,
int(-limit), int(limit))) {
return false;
}
return true;
}
/*
create a mixer for k_manual and k_rcin*
*/
bool Plane::mix_passthrough(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan)
{
const float limit = 10000;
if (!print_buffer(buf, buf_size, "M: 1\n")) {
return false;
}
if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
int(limit),
int(limit),
0,
int(-limit), int(limit))) {
return false;
}
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
in_chan,
int(limit), int(limit),
0,
int(-limit), int(limit))) {
return false;
}
return true;
}
/*
create a mixer for outputting trim only
*/
bool Plane::mix_trim_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan)
{
const float limit = 10000;
const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan);
int16_t outch_trim = outch->get_trim();
outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1);
if (!print_buffer(buf, buf_size, "M: 0\n")) {
return false;
}
int32_t out_trim = limit*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2);
if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
int(limit),
int(limit),
int(out_trim),
int(-limit), int(limit))) {
return false;
}
return true;
}
/*
create a PX4 mixer buffer given the current fixed wing parameters, returns the size of the buffer used
*/
uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
{
char *buf0 = buf;
uint16_t buf_size0 = buf_size;
uint16_t manual_mask = uint16_t(g2.manual_rc_mask.get());
for (uint8_t i=0; i<8; i++) {
if ((1U<<i) & manual_mask) {
// handle MANUAL_RCMASK channels
mix_passthrough(buf, buf_size, i, i);
continue;
}
SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(i);
switch (function) {
case SRV_Channel::k_aileron:
case SRV_Channel::k_flaperon_left:
case SRV_Channel::k_flaperon_right:
mix_one_channel(buf, buf_size, i, rcmap.roll()-1);
break;
case SRV_Channel::k_elevator:
mix_one_channel(buf, buf_size, i, rcmap.pitch()-1);
break;
case SRV_Channel::k_throttle:
mix_one_channel(buf, buf_size, i, rcmap.throttle()-1);
break;
case SRV_Channel::k_rudder:
case SRV_Channel::k_steering:
mix_one_channel(buf, buf_size, i, rcmap.yaw()-1);
break;
case SRV_Channel::k_elevon_left:
case SRV_Channel::k_dspoilerLeft1:
case SRV_Channel::k_dspoilerLeft2:
mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.roll()-1, true);
break;
case SRV_Channel::k_elevon_right:
case SRV_Channel::k_dspoilerRight1:
case SRV_Channel::k_dspoilerRight2:
mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.roll()-1, false);
break;
case SRV_Channel::k_vtail_left:
mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.yaw()-1, true);
break;
case SRV_Channel::k_vtail_right:
mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.yaw()-1, false);
break;
case SRV_Channel::k_manual:
mix_passthrough(buf, buf_size, i, i);
break;
case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16:
mix_passthrough(buf, buf_size, i, uint8_t(function - SRV_Channel::k_rcin1));
break;
default:
mix_trim_channel(buf, buf_size, i);
break;
}
}
/*
if possible, also write to a file for debugging purposes
*/
int mix_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (mix_fd != -1) {
write(mix_fd, buf0, buf_size0 - buf_size);
close(mix_fd);
}
return buf_size0 - buf_size;
}
/*
setup mixer on PX4 so that if FMU dies the pilot gets manual control
*/
bool Plane::setup_failsafe_mixing(void)
{
const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX";
bool ret = false;
char *buf = nullptr;
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};
unsigned mixer_status = 0;
uint16_t manual_mask = uint16_t(g2.manual_rc_mask.get());
buf = (char *)calloc(1, buf_size);
if (buf == nullptr) {
goto failed;
}
fileSize = create_mixer(buf, buf_size, mixer_filename);
if (!fileSize) {
hal.console->printf("Unable to create mixer\n");
goto failed;
}
new_crc = crc_calculate((uint8_t *)buf, fileSize);
if ((int32_t)new_crc == last_mixer_crc) {
free(buf);
return true;
} else {
last_mixer_crc = new_crc;
}
px4io_fd = open("/dev/px4io", 0);
if (px4io_fd == -1) {
// px4io isn't started, no point in setting up a mixer
goto failed;
}
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
// make sure the throttle has a non-zero failsafe value before we
// disable safety. This prevents sending zero PWM during switch over
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
// we need to force safety on to allow us to load a mixer. We call
// it twice as there have been reports that this call can fail
// with a small probability
hal.rcout->force_safety_on();
hal.rcout->force_safety_no_wait();
/* reset any existing mixer in px4io. This shouldn't be needed,
* but is good practice */
if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) {
hal.console->printf("Unable to reset mixer\n");
goto failed;
}
/* pass the buffer to the device */
if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) {
hal.console->printf("Unable to send mixer to IO\n");
goto failed;
}
// setup RC config for each channel based on user specified
// mix/max/trim. We only do the first 8 channels due to
// a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS
for (uint8_t i=0; i<8; i++) {
RC_Channel *ch = RC_Channels::rc_channel(i);
if (ch == nullptr) {
continue;
}
struct pwm_output_rc_config config;
config.channel = i;
// use high rc limits to allow for correct pass-thru channels
// without limits
config.rc_min = ch->get_radio_min();
config.rc_max = ch->get_radio_max();
if (rcmap.throttle()-1 == i) {
// throttle uses a trim between min and max, so we don't get division
// by small numbers near RC3_MIN
config.rc_trim = (config.rc_min + config.rc_max)/2;
} else {
config.rc_trim = constrain_int16(ch->get_radio_trim(), config.rc_min+1, config.rc_max-1);
}
config.rc_dz = 0; // zero for the purposes of manual takeover
config.rc_reverse = ch->get_reverse();
if (i == 1) {
// undo the reversal of channel2 in px4io
config.rc_reverse = !config.rc_reverse;
}
if (i+1 == g.override_channel.get()) {
/*
This is an OVERRIDE_CHAN channel. We want IO to trigger
override with a channel input of over 1750. The px4io
code is setup for triggering below 80% of the range below
trim. To map this to values above 1750 we need to reverse
the direction and set the rc range for this channel to 1000
to 1813 (1812.5 = 1500 + 250/0.8)
*/
config.rc_assignment = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
config.rc_reverse = true;
config.rc_max = 1813; // round 1812.5 up to grant > 1750
config.rc_min = 1000;
config.rc_trim = 1500;
} else if (manual_mask & (1U<<i)) {
// use fixed limits for manual_mask channels
config.rc_assignment = i;
config.rc_reverse = i==1?true:false;
config.rc_max = PX4_LIM_RC_MAX;
config.rc_min = PX4_LIM_RC_MIN;
config.rc_trim = 1500;
} else {
config.rc_assignment = i;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_RC_CONFIG, (unsigned long)&config) != 0) {
hal.console->printf("SET_RC_CONFIG failed\n");
goto failed;
}
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
if (SRV_Channel::is_motor(SRV_Channels::channel_function(i))) {
pwm_values.values[i] = quadplane.thr_min_pwm;
} else {
pwm_values.values[i] = PX4_LIM_RC_MIN;
}
}
if (ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values) != 0) {
hal.console->printf("SET_MIN_PWM failed\n");
goto failed;
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
if (SRV_Channel::is_motor(SRV_Channels::channel_function(i))) {
hal.rcout->write(i, quadplane.thr_min_pwm);
pwm_values.values[i] = quadplane.thr_min_pwm;
} else {
pwm_values.values[i] = PX4_LIM_RC_MAX;
}
}
if (ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values) != 0) {
hal.console->printf("SET_MAX_PWM failed\n");
goto failed;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_OK, 0) != 0) {
hal.console->printf("SET_OVERRIDE_OK failed\n");
goto failed;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1) != 0) {
hal.console->printf("SET_OVERRIDE_IMMEDIATE failed\n");
goto failed;
}
if (ioctl(px4io_fd, PWM_IO_GET_STATUS, (unsigned long)&mixer_status) != 0 ||
(mixer_status & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
hal.console->printf("Mixer failed: 0x%04x\n", mixer_status);
goto failed;
}
ret = true;
failed:
if (buf != nullptr) {
free(buf);
}
if (px4io_fd != -1) {
close(px4io_fd);
}
// restore safety state if it was previously armed
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
hal.rcout->force_safety_off();
hal.rcout->force_safety_no_wait();
}
if (!ret) {
// clear out the mixer CRC so that we will attempt to send it again
last_mixer_crc = -1;
}
return ret;
}
#endif // CONFIG_HAL_BOARD

5
ArduPlane/quadplane.cpp
View File

@ -631,11 +631,6 @@ bool QuadPlane::setup(void)
SRV_Channels::set_failsafe_pwm(func, thr_min_pwm);
}
#if HAVE_PX4_MIXER
// redo failsafe mixing on px4
plane.setup_failsafe_mixing();
#endif
transition_state = TRANSITION_DONE;
if (tilt.tilt_mask != 0 && tilt.tilt_type == TILT_TYPE_VECTORED_YAW) {

14
ArduPlane/system.cpp
View File

@ -51,20 +51,6 @@ void Plane::init_ardupilot()
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
set_control_channels();
#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
// quadplanes we wait till AP_Motors is initialised
for (uint8_t tries=0; tries<10; tries++) {
if (setup_failsafe_mixing()) {
break;
}
hal.scheduler->delay(10);
}
}
#endif
mavlink_system.sysid = g.sysid_this_mav;