/// 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); } }
// setup void setup() { hal.console->println("AP_Motors library test ver 1.0"); // motor initialisation motors.set_update_rate(490); motors.set_frame_orientation(AP_MOTORS_X_FRAME); motors.Init(); motors.set_throttle_range(130,1000,2000); motors.set_hover_throttle(500); motors.enable(); motors.output_min(); // setup radio if (rc3.radio_min == 0) { // cope with AP_Param not being loaded rc3.radio_min = 1000; } if (rc3.radio_max == 0) { // cope with AP_Param not being loaded rc3.radio_max = 2000; } // set rc channel ranges rc1.set_angle(4500); rc2.set_angle(4500); rc3.set_range(130, 1000); rc4.set_angle(4500); hal.scheduler->delay(1000); }
// reset_swash_servo void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo) { servo.set_range_out(0, 1000); // swash servos always use full endpoints as restricting them would lead to scaling errors servo.set_radio_min(1000); servo.set_radio_max(2000); }
/* copy radio_in to radio_out for a channel mask */ void SRV_Channels::copy_radio_in_out_mask(uint16_t mask) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { if ((1U<<i) & mask) { RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num); if (rc == nullptr) { continue; } channels[i].set_output_pwm(rc->get_radio_in()); } } }
void Sub::init_rc_in() { channel_pitch = RC_Channels::rc_channel(0); channel_roll = RC_Channels::rc_channel(1); channel_throttle = RC_Channels::rc_channel(2); channel_yaw = RC_Channels::rc_channel(3); channel_forward = RC_Channels::rc_channel(4); channel_lateral = RC_Channels::rc_channel(5); // set rc channel ranges channel_roll->set_angle(ROLL_PITCH_INPUT_MAX); channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX); channel_yaw->set_angle(ROLL_PITCH_INPUT_MAX); channel_throttle->set_range(1000); channel_forward->set_angle(ROLL_PITCH_INPUT_MAX); channel_lateral->set_angle(ROLL_PITCH_INPUT_MAX); // set default dead zones channel_roll->set_default_dead_zone(30); channel_pitch->set_default_dead_zone(30); channel_throttle->set_default_dead_zone(30); channel_yaw->set_default_dead_zone(40); channel_forward->set_default_dead_zone(30); channel_lateral->set_default_dead_zone(30); #if CONFIG_HAL_BOARD != HAL_BOARD_SITL // initialize rc input to 1500 on control channels (rather than 0) for (int i = 0; i < 6; i++) { hal.rcin->set_override(i, 1500); } hal.rcin->set_override(6, 1500); // camera pan channel hal.rcin->set_override(7, 1500); // camera tilt channel RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); hal.rcin->set_override(8, min); // lights 1 channel chan = RC_Channels::rc_channel(9); min = chan->get_radio_min(); hal.rcin->set_override(9, min); // lights 2 channel hal.rcin->set_override(10, 1100); // video switch #endif }
// 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()); }
/* set radio output value for an auxiliary function type to a LimitValue */ void SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { SRV_Channel &ch = channels[i]; if (ch.function.get() == function) { uint16_t pwm = ch.get_limit_pwm(limit); ch.set_output_pwm(pwm); if (ch.function.get() == SRV_Channel::k_manual) { RC_Channel *rc = RC_Channels::rc_channel(ch.ch_num); if (rc != nullptr) { // in order for output_ch() to work for k_manual we // also have to override radio_in rc->set_radio_in(pwm); } } } } }
/* copy radio_in to radio_out for a given function */ void SRV_Channels::copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { if (channels[i].function.get() == function) { RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num); if (rc == nullptr) { continue; } if (do_input_output) { rc->read(); } channels[i].set_output_pwm(rc->get_radio_in()); if (do_input_output) { channels[i].output_ch(); } } } }
// convert input in 0 to +1 range to pwm output int16_t AP_Motors::calc_pwm_output_0to1(float input, const RC_Channel& servo) { int16_t ret; input = constrain_float(input, 0.0f, 1.0f); if (servo.get_reverse()) { input = 1.0f-input; } ret = input * (servo.get_radio_max() - servo.get_radio_min()) + servo.get_radio_min(); return constrain_int16(ret, servo.get_radio_min(), servo.get_radio_max()); }
void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) { // Act based on the function assigned to this button switch (get_button(button)->function(shift)) { case JSButton::button_function_t::k_arm_toggle: if (motors.armed()) { init_disarm_motors(); } else { init_arm_motors(true); } break; case JSButton::button_function_t::k_arm: init_arm_motors(true); break; case JSButton::button_function_t::k_disarm: init_disarm_motors(); break; case JSButton::button_function_t::k_mode_manual: set_mode(MANUAL, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_stabilize: set_mode(STABILIZE, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_depth_hold: set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_auto: set_mode(AUTO, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_guided: set_mode(GUIDED, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_circle: set_mode(CIRCLE, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_acro: set_mode(ACRO, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mode_poshold: set_mode(POSHOLD, MODE_REASON_TX_COMMAND); break; case JSButton::button_function_t::k_mount_center: camera_mount.set_angle_targets(0, 0, 0); // for some reason the call to set_angle_targets changes the mode to mavlink targeting! camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING); break; case JSButton::button_function_t::k_mount_tilt_up: cam_tilt = 1900; break; case JSButton::button_function_t::k_mount_tilt_down: cam_tilt = 1100; break; case JSButton::button_function_t::k_camera_trigger: break; case JSButton::button_function_t::k_camera_source_toggle: if (!held) { static bool video_toggle = false; video_toggle = !video_toggle; if (video_toggle) { video_switch = 1900; gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2"); } else { video_switch = 1100; gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1"); } } break; case JSButton::button_function_t::k_mount_pan_right: // Not implemented break; case JSButton::button_function_t::k_mount_pan_left: // Not implemented break; case JSButton::button_function_t::k_lights1_cycle: if (!held) { static bool increasing = true; RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; if (increasing) { lights1 = constrain_float(lights1 + step, min, max); } else { lights1 = constrain_float(lights1 - step, min, max); } if (lights1 >= max || lights1 <= min) { increasing = !increasing; } } break; case JSButton::button_function_t::k_lights1_brighter: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights1 = constrain_float(lights1 + step, min, max); } break; case JSButton::button_function_t::k_lights1_dimmer: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(8); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights1 = constrain_float(lights1 - step, min, max); } break; case JSButton::button_function_t::k_lights2_cycle: if (!held) { static bool increasing = true; RC_Channel* chan = RC_Channels::rc_channel(9); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; if (increasing) { lights2 = constrain_float(lights2 + step, min, max); } else { lights2 = constrain_float(lights2 - step, min, max); } if (lights2 >= max || lights2 <= min) { increasing = !increasing; } } break; case JSButton::button_function_t::k_lights2_brighter: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(9); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights2 = constrain_float(lights2 + step, min, max); } break; case JSButton::button_function_t::k_lights2_dimmer: if (!held) { RC_Channel* chan = RC_Channels::rc_channel(9); uint16_t min = chan->get_radio_min(); uint16_t max = chan->get_radio_max(); uint16_t step = (max - min) / g.lights_steps; lights2 = constrain_float(lights2 - step, min, max); } break; case JSButton::button_function_t::k_gain_toggle: if (!held) { static bool lowGain = false; lowGain = !lowGain; if (lowGain) { gain = 0.5f; } else { gain = 1.0f; } gcs().send_text(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100); } break; case JSButton::button_function_t::k_gain_inc: if (!held) { // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80)); g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0)); g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10)); if (g.numGainSettings == 1) { gain = constrain_float(g.gain_default, g.minGain, g.maxGain); } else { gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain); } gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100); } break; case JSButton::button_function_t::k_gain_dec: if (!held) { // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80)); g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0)); g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10)); if (g.numGainSettings == 1) { gain = constrain_float(g.gain_default, g.minGain, g.maxGain); } else { gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain); } gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100); } break; case JSButton::button_function_t::k_trim_roll_inc: rollTrim = constrain_float(rollTrim+10,-200,200); break; case JSButton::button_function_t::k_trim_roll_dec: rollTrim = constrain_float(rollTrim-10,-200,200); break; case JSButton::button_function_t::k_trim_pitch_inc: pitchTrim = constrain_float(pitchTrim+10,-200,200); break; case JSButton::button_function_t::k_trim_pitch_dec: pitchTrim = constrain_float(pitchTrim-10,-200,200); break; case JSButton::button_function_t::k_input_hold_set: if(!motors.armed()) { break; } if (!held) { zTrim = abs(z_last-500) > 50 ? z_last-500 : 0; xTrim = abs(x_last) > 50 ? x_last : 0; yTrim = abs(y_last) > 50 ? y_last : 0; bool input_hold_engaged_last = input_hold_engaged; input_hold_engaged = zTrim || xTrim || yTrim; if (input_hold_engaged) { gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Set"); } else if (input_hold_engaged_last) { gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Disabled"); } } break; case JSButton::button_function_t::k_relay_1_on: relay.on(0); break; case JSButton::button_function_t::k_relay_1_off: relay.off(0); break; case JSButton::button_function_t::k_relay_1_toggle: if (!held) { relay.toggle(0); } break; case JSButton::button_function_t::k_relay_2_on: relay.on(1); break; case JSButton::button_function_t::k_relay_2_off: relay.off(1); break; case JSButton::button_function_t::k_relay_2_toggle: if (!held) { relay.toggle(1); } break; case JSButton::button_function_t::k_relay_3_on: relay.on(2); break; case JSButton::button_function_t::k_relay_3_off: relay.off(2); break; case JSButton::button_function_t::k_relay_3_toggle: if (!held) { relay.toggle(2); } break; case JSButton::button_function_t::k_relay_4_on: relay.on(3); break; case JSButton::button_function_t::k_relay_4_off: relay.off(3); break; case JSButton::button_function_t::k_relay_4_toggle: if (!held) { relay.toggle(3); } break; //////////////////////////////////////////////// // Servo functions // TODO: initialize case JSButton::button_function_t::k_servo_1_inc: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_dec: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_min: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_max: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed } break; case JSButton::button_function_t::k_servo_1_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_inc: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_dec: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_min: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_max: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed } break; case JSButton::button_function_t::k_servo_2_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_inc: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_dec: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max()); ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_min: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_max: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed } break; case JSButton::button_function_t::k_servo_3_center: { SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed } break; case JSButton::button_function_t::k_roll_pitch_toggle: if (!held) { roll_pitch_flag = !roll_pitch_flag; } break; case JSButton::button_function_t::k_custom_1: // Not implemented break; case JSButton::button_function_t::k_custom_2: // Not implemented break; case JSButton::button_function_t::k_custom_3: // Not implemented break; case JSButton::button_function_t::k_custom_4: // Not implemented break; case JSButton::button_function_t::k_custom_5: // Not implemented break; case JSButton::button_function_t::k_custom_6: // Not implemented break; } }
/* setup flap outputs */ void Plane::set_servos_flaps(void) { // Auto flap deployment int8_t auto_flap_percent = 0; int8_t manual_flap_percent = 0; // work out any manual flap input RC_Channel *flapin = RC_Channels::rc_channel(g.flapin_channel-1); if (flapin != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) { manual_flap_percent = flapin->percent_input(); } if (auto_throttle_mode) { int16_t flapSpeedSource = 0; if (ahrs.airspeed_sensor_enabled()) { flapSpeedSource = target_airspeed_cm * 0.01f; } else { flapSpeedSource = aparm.throttle_cruise; } if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) { auto_flap_percent = g.flap_2_percent; } else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) { auto_flap_percent = g.flap_1_percent; } //else flaps stay at default zero deflection if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && landing.get_flap_percent() != 0) { auto_flap_percent = landing.get_flap_percent(); } /* special flap levels for takeoff and landing. This works better than speed based flaps as it leads to less possibility of oscillation */ if (control_mode == AUTO) { switch (flight_stage) { case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: if (g.takeoff_flap_percent != 0) { auto_flap_percent = g.takeoff_flap_percent; } break; case AP_Vehicle::FixedWing::FLIGHT_NORMAL: if (g.takeoff_flap_percent != 0 && in_preLaunch_flight_stage()) { // TODO: move this to a new FLIGHT_PRE_TAKEOFF stage auto_flap_percent = g.takeoff_flap_percent; } break; default: break; } } } // manual flap input overrides auto flap input if (abs(manual_flap_percent) > auto_flap_percent) { auto_flap_percent = manual_flap_percent; } SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, auto_flap_percent); SRV_Channels::set_output_scaled(SRV_Channel::k_flap, manual_flap_percent); if (g.flap_slewrate) { SRV_Channels::limit_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate, G_Dt); SRV_Channels::limit_slew_rate(SRV_Channel::k_flap, g.flap_slewrate, G_Dt); } // output to flaperons, if any flaperon_update(auto_flap_percent); }
/***************************************** * Set the flight control servos based on the current calculated values *****************************************/ void Plane::set_servos(void) { int16_t last_throttle = channel_throttle->get_radio_out(); // do any transition updates for quadplane quadplane.update(); if (control_mode == AUTO && auto_state.idle_mode) { // special handling for balloon launch set_servos_idle(); return; } /* see if we are doing ground steering. */ if (!steering_control.ground_steering) { // we are not at an altitude for ground steering. Set the nose // wheel to the rudder just in case the barometer has drifted // a lot steering_control.steering = steering_control.rudder; } else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) { // we are within the ground steering altitude but don't have a // dedicated steering channel. Set the rudder to the ground // steering output steering_control.rudder = steering_control.steering; } channel_rudder->set_servo_out(steering_control.rudder); // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run steering_control.ground_steering = false; RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, steering_control.steering); if (control_mode == MANUAL) { // do a direct pass through of radio values if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) { channel_roll->set_radio_out(channel_roll->get_radio_in()); channel_pitch->set_radio_out(channel_pitch->get_radio_in()); } else { channel_roll->set_radio_out(channel_roll->read()); channel_pitch->set_radio_out(channel_pitch->read()); } channel_throttle->set_radio_out(channel_throttle->get_radio_in()); channel_rudder->set_radio_out(channel_rudder->get_radio_in()); // setup extra channels. We want this to come from the // main input channel, but using the 2nd channels dead // zone, reverse and min/max settings. We need to use // pwm_to_angle_dz() to ensure we don't trim the value for the // deadzone of the main aileron channel, otherwise the 2nd // aileron won't quite follow the first one RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0)); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0)); // this variant assumes you have the corresponding // input channel setup in your transmitter for manual control // of the 2nd aileron RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input); if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) { // set any differential spoilers to follow the elevons in // manual mode. RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->get_radio_out()); RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->get_radio_out()); } } else { if (g.mix_mode == 0) { // both types of secondary aileron are slaved to the roll servo out RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out()); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out()); // both types of secondary elevator are slaved to the pitch servo out RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out()); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out()); }else{ /*Elevon mode*/ float ch1; float ch2; ch1 = channel_pitch->get_servo_out() - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out()); ch2 = channel_pitch->get_servo_out() + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out()); /* Differential Spoilers If differential spoilers are setup, then we translate rudder control into splitting of the two ailerons on the side of the aircraft where we want to induce additional drag. */ if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) { float ch3 = ch1; float ch4 = ch2; if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->get_servo_out() < 0) { ch1 += abs(channel_rudder->get_servo_out()); ch3 -= abs(channel_rudder->get_servo_out()); } else { ch2 += abs(channel_rudder->get_servo_out()); ch4 -= abs(channel_rudder->get_servo_out()); } RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, ch3); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, ch4); } // directly set the radio_out values for elevon mode channel_roll->set_radio_out(elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX))); channel_pitch->set_radio_out(elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX))); } // push out the PWM values if (g.mix_mode == 0) { channel_roll->calc_pwm(); channel_pitch->calc_pwm(); } channel_rudder->calc_pwm(); #if THROTTLE_OUT == 0 channel_throttle->set_servo_out(0); #else // convert 0 to 100% (or -100 to +100) into PWM int8_t min_throttle = aparm.throttle_min.get(); int8_t max_throttle = aparm.throttle_max.get(); if (min_throttle < 0 && !allow_reverse_thrust()) { // reverse thrust is available but inhibited. min_throttle = 0; } if (control_mode == AUTO) { if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { min_throttle = 0; } if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) { if(aparm.takeoff_throttle_max != 0) { max_throttle = aparm.takeoff_throttle_max; } else { max_throttle = aparm.throttle_max; } } } uint32_t now = millis(); if (battery.overpower_detected()) { // overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value // throttle limit will attack by 10% per second if (channel_throttle->get_servo_out() > 0 && // demanding too much positive thrust throttle_watt_limit_max < max_throttle - 25 && now - throttle_watt_limit_timer_ms >= 1) { // always allow for 25% throttle available regardless of battery status throttle_watt_limit_timer_ms = now; throttle_watt_limit_max++; } else if (channel_throttle->get_servo_out() < 0 && min_throttle < 0 && // reverse thrust is available throttle_watt_limit_min < -(min_throttle) - 25 && now - throttle_watt_limit_timer_ms >= 1) { // always allow for 25% throttle available regardless of battery status throttle_watt_limit_timer_ms = now; throttle_watt_limit_min++; } } else if (now - throttle_watt_limit_timer_ms >= 1000) { // it has been 1 second since last over-current, check if we can resume higher throttle. // this throttle release is needed to allow raising the max_throttle as the battery voltage drains down // throttle limit will release by 1% per second if (channel_throttle->get_servo_out() > throttle_watt_limit_max && // demanding max forward thrust throttle_watt_limit_max > 0) { // and we're currently limiting it throttle_watt_limit_timer_ms = now; throttle_watt_limit_max--; } else if (channel_throttle->get_servo_out() < throttle_watt_limit_min && // demanding max negative thrust throttle_watt_limit_min > 0) { // and we're limiting it throttle_watt_limit_timer_ms = now; throttle_watt_limit_min--; } } max_throttle = constrain_int16(max_throttle, 0, max_throttle - throttle_watt_limit_max); if (min_throttle < 0) { min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0); } channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), min_throttle, max_throttle)); if (!hal.util->get_soft_armed()) { channel_throttle->set_servo_out(0); channel_throttle->calc_pwm(); } else if (suppress_throttle()) { // throttle is suppressed in auto mode channel_throttle->set_servo_out(0); if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed channel_throttle->set_radio_out(channel_throttle->get_radio_in()); } else { channel_throttle->calc_pwm(); } } else if (g.throttle_passthru_stabilize && (control_mode == STABILIZE || control_mode == TRAINING || control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == AUTOTUNE) && !failsafe.ch3_counter) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set channel_throttle->set_radio_out(channel_throttle->get_radio_in()); } else if (control_mode == GUIDED && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED channel_throttle->set_radio_out(channel_throttle->get_radio_in()); } else if (quadplane.in_vtol_mode()) { // ask quadplane code for forward throttle channel_throttle->set_servo_out(quadplane.forward_throttle_pct()); channel_throttle->calc_pwm(); } else { // normal throttle calculation based on servo_out channel_throttle->calc_pwm(); } #endif } // Auto flap deployment int8_t auto_flap_percent = 0; int8_t manual_flap_percent = 0; static int8_t last_auto_flap; static int8_t last_manual_flap; // work out any manual flap input RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1); if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) { flapin->input(); manual_flap_percent = flapin->percent_input(); } if (auto_throttle_mode) { int16_t flapSpeedSource = 0; if (ahrs.airspeed_sensor_enabled()) { flapSpeedSource = target_airspeed_cm * 0.01f; } else { flapSpeedSource = aparm.throttle_cruise; } if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) { auto_flap_percent = g.flap_2_percent; } else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) { auto_flap_percent = g.flap_1_percent; } //else flaps stay at default zero deflection /* special flap levels for takeoff and landing. This works better than speed based flaps as it leads to less possibility of oscillation */ if (control_mode == AUTO) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: case AP_SpdHgtControl::FLIGHT_LAND_ABORT: if (g.takeoff_flap_percent != 0) { auto_flap_percent = g.takeoff_flap_percent; } break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: if (g.land_flap_percent != 0) { auto_flap_percent = g.land_flap_percent; } break; default: break; } } } // manual flap input overrides auto flap input if (abs(manual_flap_percent) > auto_flap_percent) { auto_flap_percent = manual_flap_percent; } flap_slew_limit(last_auto_flap, auto_flap_percent); flap_slew_limit(last_manual_flap, manual_flap_percent); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent); if (control_mode >= FLY_BY_WIRE_B || quadplane.in_assisted_flight() || quadplane.in_vtol_mode()) { /* only do throttle slew limiting in modes where throttle * control is automatic */ throttle_slew_limit(last_throttle); } if (control_mode == TRAINING) { // copy rudder in training mode channel_rudder->set_radio_out(channel_rudder->get_radio_in()); } if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) { flaperon_update(auto_flap_percent); } if (g.vtail_output != MIXING_DISABLED) { channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder); } else if (g.elevon_output != MIXING_DISABLED) { channel_output_mixer(g.elevon_output, channel_pitch, channel_roll); } if (!arming.is_armed()) { //Some ESCs get noisy (beep error msgs) if PWM == 0. //This little segment aims to avoid this. switch (arming.arming_required()) { case AP_Arming::NO: //keep existing behavior: do nothing to radio_out //(don't disarm throttle channel even if AP_Arming class is) break; case AP_Arming::YES_ZERO_PWM: channel_throttle->set_radio_out(0); break; case AP_Arming::YES_MIN_PWM: default: channel_throttle->set_radio_out(throttle_min()); break; } } #if OBC_FAILSAFE == ENABLED // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules obc.check_crash_plane(); #endif #if HIL_SUPPORT if (g.hil_mode == 1) { // get the servos to the GCS immediately for HIL if (HAVE_PAYLOAD_SPACE(MAVLINK_COMM_0, RC_CHANNELS_SCALED)) { send_servo_out(MAVLINK_COMM_0); } if (!g.hil_servos) { return; } } #endif if (g.land_then_servos_neutral > 0 && control_mode == AUTO && g.land_disarm_delay > 0 && auto_state.land_complete && !arming.is_armed()) { // after an auto land and auto disarm, set the servos to be neutral just // in case we're upside down or some crazy angle and straining the servos. if (g.land_then_servos_neutral == 1) { channel_roll->set_radio_out(channel_roll->get_radio_trim()); channel_pitch->set_radio_out(channel_pitch->get_radio_trim()); channel_rudder->set_radio_out(channel_rudder->get_radio_trim()); } else if (g.land_then_servos_neutral == 2) { channel_roll->disable_out(); channel_pitch->disable_out(); channel_rudder->disable_out(); } } // send values to the PWM timers for output // ---------------------------------------- if (g.rudder_only == 0) { // when we RUDDER_ONLY mode we don't send the channel_roll // output and instead rely on KFF_RDDRMIX. That allows the yaw // damper to operate. channel_roll->output(); } channel_pitch->output(); channel_throttle->output(); channel_rudder->output(); RC_Channel_aux::output_ch_all(); }
/* setup flap outputs */ void Plane::set_servos_flaps(void) { // Auto flap deployment int8_t auto_flap_percent = 0; int8_t manual_flap_percent = 0; static int8_t last_auto_flap; static int8_t last_manual_flap; // work out any manual flap input RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1); if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) { flapin->input(); manual_flap_percent = flapin->percent_input(); } if (auto_throttle_mode) { int16_t flapSpeedSource = 0; if (ahrs.airspeed_sensor_enabled()) { flapSpeedSource = target_airspeed_cm * 0.01f; } else { flapSpeedSource = aparm.throttle_cruise; } if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) { auto_flap_percent = g.flap_2_percent; } else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) { auto_flap_percent = g.flap_1_percent; } //else flaps stay at default zero deflection /* special flap levels for takeoff and landing. This works better than speed based flaps as it leads to less possibility of oscillation */ if (control_mode == AUTO) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: case AP_SpdHgtControl::FLIGHT_LAND_ABORT: if (g.takeoff_flap_percent != 0) { auto_flap_percent = g.takeoff_flap_percent; } break; case AP_SpdHgtControl::FLIGHT_NORMAL: if (auto_flap_percent != 0 && in_preLaunch_flight_stage()) { // TODO: move this to a new FLIGHT_PRE_TAKEOFF stage auto_flap_percent = g.takeoff_flap_percent; } break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: if (g.land_flap_percent != 0) { auto_flap_percent = g.land_flap_percent; } break; default: break; } } } // manual flap input overrides auto flap input if (abs(manual_flap_percent) > auto_flap_percent) { auto_flap_percent = manual_flap_percent; } flap_slew_limit(last_auto_flap, auto_flap_percent); flap_slew_limit(last_manual_flap, manual_flap_percent); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent); RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent); if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) { flaperon_update(auto_flap_percent); } }
/***************************************** * Set the flight control servos based on the current calculated values *****************************************/ void Plane::set_servos(void) { int16_t last_throttle = channel_throttle->radio_out; if (control_mode == AUTO && auto_state.idle_mode) { // special handling for balloon launch set_servos_idle(); return; } /* see if we are doing ground steering. */ if (!steering_control.ground_steering) { // we are not at an altitude for ground steering. Set the nose // wheel to the rudder just in case the barometer has drifted // a lot steering_control.steering = steering_control.rudder; } else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) { // we are within the ground steering altitude but don't have a // dedicated steering channel. Set the rudder to the ground // steering output steering_control.rudder = steering_control.steering; } channel_rudder->servo_out = steering_control.rudder; // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run steering_control.ground_steering = false; RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, steering_control.rudder); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, steering_control.steering); if (control_mode == MANUAL) { // do a direct pass through of radio values if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) { channel_roll->radio_out = channel_roll->radio_in; channel_pitch->radio_out = channel_pitch->radio_in; } else { channel_roll->radio_out = channel_roll->read(); channel_pitch->radio_out = channel_pitch->read(); } channel_throttle->radio_out = channel_throttle->radio_in; channel_rudder->radio_out = channel_rudder->radio_in; // setup extra channels. We want this to come from the // main input channel, but using the 2nd channels dead // zone, reverse and min/max settings. We need to use // pwm_to_angle_dz() to ensure we don't trim the value for the // deadzone of the main aileron channel, otherwise the 2nd // aileron won't quite follow the first one RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0)); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0)); // this variant assumes you have the corresponding // input channel setup in your transmitter for manual control // of the 2nd aileron RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input); if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) { // set any differential spoilers to follow the elevons in // manual mode. RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->radio_out); RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->radio_out); } } else { if (g.mix_mode == 0) { // both types of secondary aileron are slaved to the roll servo out RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->servo_out); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out); // both types of secondary elevator are slaved to the pitch servo out RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out); } else { /*Elevon mode*/ float ch1; float ch2; ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); /* Differential Spoilers If differential spoilers are setup, then we translate rudder control into splitting of the two ailerons on the side of the aircraft where we want to induce additional drag. */ if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) { float ch3 = ch1; float ch4 = ch2; if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) { ch1 += abs(channel_rudder->servo_out); ch3 -= abs(channel_rudder->servo_out); } else { ch2 += abs(channel_rudder->servo_out); ch4 -= abs(channel_rudder->servo_out); } RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4); } // directly set the radio_out values for elevon mode channel_roll->radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)); channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)); } // push out the PWM values if (g.mix_mode == 0) { channel_roll->calc_pwm(); channel_pitch->calc_pwm(); } channel_rudder->calc_pwm(); #if THROTTLE_OUT == 0 channel_throttle->servo_out = 0; #else // convert 0 to 100% into PWM uint8_t min_throttle = aparm.throttle_min.get(); uint8_t max_throttle = aparm.throttle_max.get(); if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { min_throttle = 0; } if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) { if(aparm.takeoff_throttle_max != 0) { max_throttle = aparm.takeoff_throttle_max; } else { max_throttle = aparm.throttle_max; } } channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, min_throttle, max_throttle); if (!hal.util->get_soft_armed()) { channel_throttle->servo_out = 0; channel_throttle->calc_pwm(); } else if (suppress_throttle()) { // throttle is suppressed in auto mode channel_throttle->servo_out = 0; if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed channel_throttle->radio_out = channel_throttle->radio_in; } else { channel_throttle->calc_pwm(); } } else if (g.throttle_passthru_stabilize && (control_mode == STABILIZE || control_mode == TRAINING || control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == AUTOTUNE)) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set channel_throttle->radio_out = channel_throttle->radio_in; } else if (control_mode == GUIDED && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED channel_throttle->radio_out = channel_throttle->radio_in; } else { // normal throttle calculation based on servo_out channel_throttle->calc_pwm(); } #endif } // Auto flap deployment int8_t auto_flap_percent = 0; int8_t manual_flap_percent = 0; static int8_t last_auto_flap; static int8_t last_manual_flap; // work out any manual flap input RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1); if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) { flapin->input(); manual_flap_percent = flapin->percent_input(); } if (auto_throttle_mode) { int16_t flapSpeedSource = 0; if (ahrs.airspeed_sensor_enabled()) { flapSpeedSource = target_airspeed_cm * 0.01f; } else { flapSpeedSource = aparm.throttle_cruise; } if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) { auto_flap_percent = g.flap_2_percent; } else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) { auto_flap_percent = g.flap_1_percent; } //else flaps stay at default zero deflection /* special flap levels for takeoff and landing. This works better than speed based flaps as it leads to less possibility of oscillation */ if (control_mode == AUTO) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: if (g.takeoff_flap_percent != 0) { auto_flap_percent = g.takeoff_flap_percent; } break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: if (g.land_flap_percent != 0) { auto_flap_percent = g.land_flap_percent; } break; default: break; } } } // manual flap input overrides auto flap input if (abs(manual_flap_percent) > auto_flap_percent) { auto_flap_percent = manual_flap_percent; } flap_slew_limit(last_auto_flap, auto_flap_percent); flap_slew_limit(last_manual_flap, manual_flap_percent); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, auto_flap_percent); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent); if (control_mode >= FLY_BY_WIRE_B) { /* only do throttle slew limiting in modes where throttle * control is automatic */ throttle_slew_limit(last_throttle); } if (control_mode == TRAINING) { // copy rudder in training mode channel_rudder->radio_out = channel_rudder->radio_in; } if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) { flaperon_update(auto_flap_percent); } if (g.vtail_output != MIXING_DISABLED) { channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); } else if (g.elevon_output != MIXING_DISABLED) { channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); } //send throttle to 0 or MIN_PWM if not yet armed if (!arming.is_armed()) { //Some ESCs get noisy (beep error msgs) if PWM == 0. //This little segment aims to avoid this. switch (arming.arming_required()) { case AP_Arming::YES_MIN_PWM: channel_throttle->radio_out = channel_throttle->radio_min; break; case AP_Arming::YES_ZERO_PWM: channel_throttle->radio_out = 0; break; default: //keep existing behavior: do nothing to radio_out //(don't disarm throttle channel even if AP_Arming class is) break; } } #if OBC_FAILSAFE == ENABLED // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules obc.check_crash_plane(); #endif #if HIL_SUPPORT if (g.hil_mode == 1) { // get the servos to the GCS immediately for HIL if (comm_get_txspace(MAVLINK_COMM_0) >= MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { send_servo_out(MAVLINK_COMM_0); } if (!g.hil_servos) { return; } } #endif // send values to the PWM timers for output // ---------------------------------------- if (g.rudder_only == 0) { // when we RUDDER_ONLY mode we don't send the channel_roll // output and instead rely on KFF_RDDRMIX. That allows the yaw // damper to operate. channel_roll->output(); } channel_pitch->output(); channel_throttle->output(); channel_rudder->output(); RC_Channel_aux::output_ch_all(); }
/* check for changed tuning input */ void Tuning::check_input(void) { if (channel <= 0 || parm <= 0) { // disabled return; } // only adjust values at 4Hz uint32_t now = AP_HAL::millis(); if (now - last_check_ms < 250) { return; } last_check_ms = now; if (channel > hal.rcin->num_channels()) { return; } RC_Channel *chan = RC_Channel::rc_channel(channel-1); if (chan == nullptr) { return; } uint8_t input = chan->percent_input(); if (input == last_input_pct) { // no change return; } last_input_pct = input; float tuning_value = minimum + (maximum-minimum)*(input*0.01f); Log_Write_Parameter_Tuning((uint8_t)parm.get(), tuning_value, minimum, maximum); switch((enum tuning_func)parm.get()) { case TUNING_RLL_P: plane.rollController.kP(tuning_value); break; case TUNING_RLL_I: plane.rollController.kI(tuning_value); break; case TUNING_RLL_D: plane.rollController.kD(tuning_value); break; case TUNING_RLL_FF: plane.rollController.kFF(tuning_value); break; case TUNING_PIT_P: plane.pitchController.kP(tuning_value); break; case TUNING_PIT_I: plane.pitchController.kI(tuning_value); break; case TUNING_PIT_D: plane.pitchController.kD(tuning_value); break; case TUNING_PIT_FF: plane.pitchController.kFF(tuning_value); break; default: break; } if (!plane.quadplane.available()) { // quadplane tuning options not available return; } switch((enum tuning_func)parm.get()) { case TUNING_Q_RATE_ROLL_PITCH_KPI: plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); break; case TUNING_Q_RATE_ROLL_PITCH_KP: plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); break; case TUNING_Q_RATE_ROLL_PITCH_KI: plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); break; case TUNING_Q_RATE_ROLL_PITCH_KD: plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value); plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value); break; case TUNING_Q_RATE_ROLL_KPI: plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); break; case TUNING_Q_RATE_ROLL_KP: plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); break; case TUNING_Q_RATE_ROLL_KI: plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); break; case TUNING_Q_RATE_ROLL_KD: plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value); break; case TUNING_Q_RATE_PITCH_KPI: plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); break; case TUNING_Q_RATE_PITCH_KP: plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); break; case TUNING_Q_RATE_PITCH_KI: plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); break; case TUNING_Q_RATE_PITCH_KD: plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value); break; case TUNING_Q_RATE_YAW_KPI: plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value); plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value); break; case TUNING_Q_RATE_YAW_KP: plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value); break; case TUNING_Q_RATE_YAW_KI: plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value); break; case TUNING_Q_RATE_YAW_KD: plane.quadplane.attitude_control->get_rate_yaw_pid().kD(tuning_value); break; case TUNING_Q_ANG_ROLL_KP: plane.quadplane.attitude_control->get_angle_roll_p().kP(tuning_value); break; case TUNING_Q_ANG_PITCH_KP: plane.quadplane.attitude_control->get_angle_pitch_p().kP(tuning_value); break; case TUNING_Q_ANG_YAW_KP: plane.quadplane.attitude_control->get_angle_yaw_p().kP(tuning_value); break; case TUNING_Q_PXY_P: plane.quadplane.p_pos_xy.kP(tuning_value); break; case TUNING_Q_PZ_P: plane.quadplane.p_alt_hold.kP(tuning_value); break; case TUNING_Q_VXY_P: plane.quadplane.pi_vel_xy.kP(tuning_value); break; case TUNING_Q_VXY_I: plane.quadplane.pi_vel_xy.kI(tuning_value); break; case TUNING_Q_VZ_P: plane.quadplane.p_vel_z.kP(tuning_value); break; case TUNING_Q_AZ_P: plane.quadplane.pid_accel_z.kP(tuning_value); break; case TUNING_Q_AZ_I: plane.quadplane.pid_accel_z.kI(tuning_value); break; case TUNING_Q_AZ_D: plane.quadplane.pid_accel_z.kD(tuning_value); break; default: return; } }
/* 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; }