/// map a function to a servo channel and output it
void SRV_Channel::output_ch(void)
{
int8_t passthrough_from = -1;
// take care of special function cases
switch(function)
{
case k_manual: // manual
passthrough_from = ch_num;
break;
case k_rcin1 ... k_rcin16: // rc pass-thru
passthrough_from = int8_t(function - k_rcin1);
break;
}
if (passthrough_from != -1) {
// we are doing passthrough from input to output for this channel
RC_Channel *rc = RC_Channels::rc_channel(passthrough_from);
if (rc) {
if (SRV_Channels::passthrough_disabled()) {
output_pwm = rc->get_radio_trim();
} else {
output_pwm = rc->get_radio_in();
}
}
}
if (!(SRV_Channels::disabled_mask & (1U<<ch_num))) {
hal.rcout->write(ch_num, output_pwm);
}
}
// convert input in -1 to +1 range to pwm output
int16_t AP_Motors::calc_pwm_output_1to1(float input, const RC_Channel& servo)
{
int16_t ret;
input = constrain_float(input, -1.0f, 1.0f);
if (servo.get_reverse()) {
input = -input;
}
if (input >= 0.0f) {
ret = ((input * (servo.get_radio_max() - servo.get_radio_trim())) + servo.get_radio_trim());
} else {
ret = ((input * (servo.get_radio_trim() - servo.get_radio_min())) + servo.get_radio_trim());
}
return constrain_int16(ret, servo.get_radio_min(), servo.get_radio_max());
}
/*
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 = NULL;
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);
if (buf == NULL) {
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
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_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_Channel::rc_channel(i);
if (ch == NULL) {
continue;
}
struct pwm_output_rc_config config;
/*
we use a min/max of 900/2100 to allow for pass-thru of
larger values than the RC min/max range. This mimics the APM
behaviour of pass-thru in manual, which allows for dual-rate
transmitter setups in manual mode to go beyond the ranges
used in stabilised modes
*/
config.channel = i;
config.rc_min = 900;
config.rc_max = 2100;
if (rcmap.throttle()-1 == i) {
// throttle uses a trim of 1500, so we don't get division
// by small numbers near RC3_MIN
config.rc_trim = 1500;
} 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
// we set reverse as false, as users of ArduPilot will have
// input reversed on transmitter, so from the point of view of
// the mixer the input is never reversed. The one exception is
// the 2nd channel, which is reversed inside the PX4IO code,
// so needs to be unreversed here to give sane behaviour.
if (i == 1) {
config.rc_reverse = true;
} else {
config.rc_reverse = false;
}
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 {
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++) {
pwm_values.values[i] = 900;
}
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++) {
pwm_values.values[i] = 2100;
}
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;
}
// setup for immediate manual control if FMU dies
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1) != 0) {
hal.console->printf("SET_OVERRIDE_IMMEDIATE failed\n");
goto failed;
}
ret = true;
failed:
if (buf != NULL) {
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();
}
if (!ret) {
// clear out the mixer CRC so that we will attempt to send it again
last_mixer_crc = -1;
}
return ret;
}
