Peter Barker
6 years ago
committed by
Andrew Tridgell
10 changed files with 2 additions and 525 deletions
@ -1,447 +0,0 @@
@@ -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
|
||||
Loading…
Reference in new issue