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

448 lines
15 KiB
C++
Raw Blame History

#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, aparm.throttle_min<0?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
Reference in New Issue View Git Blame Copy Permalink