From 40856fef560af885309f0eb163d3af11c5b37a94 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 19 Jan 2019 18:46:40 +1100 Subject: [PATCH] Plane: remove vestiges of old PX4_MIXER code --- ArduPlane/AP_Arming.cpp | 7 - ArduPlane/ArduPlane.cpp | 7 - ArduPlane/Parameters.cpp | 2 +- ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 6 - ArduPlane/config.h | 6 - ArduPlane/control_modes.cpp | 31 --- ArduPlane/px4_mixer.cpp | 447 ------------------------------------ ArduPlane/quadplane.cpp | 5 - ArduPlane/system.cpp | 14 -- 10 files changed, 2 insertions(+), 525 deletions(-) delete mode 100644 ArduPlane/px4_mixer.cpp diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index affaaa7ea1..692010bc3f 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -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; } diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ab99a16d4c..730e1b1e2b 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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 diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index aa94e5c6fb..ff82939f7b 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -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. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 4f539f2d87..b9cafad1a2 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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 diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1c5fcc1fe6..6ff52521a3 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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(); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index cea595e844..65ef02ebe8 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -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 diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 63110b89c5..b6f2e8b3e6 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -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) diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp deleted file mode 100644 index a7049e866d..0000000000 --- a/ArduPlane/px4_mixer.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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<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<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 diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c547e4986a..8bcff86fe0 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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) { diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 82a1f08924..7e5c386020 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -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;