/// 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; }