From b83f50be0f3b2d3333dd3f5c1ce7ffecf77b3a73 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 22 Oct 2016 21:27:57 +1100 Subject: [PATCH] Plane: use new SRV_Channels API --- ArduPlane/ArduPlane.cpp | 12 +- ArduPlane/Attitude.cpp | 78 ++++----- ArduPlane/GCS_Mavlink.cpp | 8 +- ArduPlane/Log.cpp | 4 +- ArduPlane/Parameters.cpp | 86 ++-------- ArduPlane/Parameters.h | 78 +++------ ArduPlane/Plane.h | 15 +- ArduPlane/afs_plane.cpp | 58 ++----- ArduPlane/control_modes.cpp | 2 +- ArduPlane/failsafe.cpp | 39 ++--- ArduPlane/px4_mixer.cpp | 44 ++--- ArduPlane/quadplane.cpp | 19 +-- ArduPlane/radio.cpp | 57 +++---- ArduPlane/sensors.cpp | 2 +- ArduPlane/servos.cpp | 321 ++++++++++++++---------------------- ArduPlane/system.cpp | 28 +--- ArduPlane/test.cpp | 254 ---------------------------- ArduPlane/tiltrotor.cpp | 6 +- 18 files changed, 309 insertions(+), 802 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index e6c65e9a4d..311c9ed115 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -311,7 +311,7 @@ void Plane::afs_fs_check(void) */ void Plane::update_aux(void) { - RC_Channel_aux::enable_aux_servos(); + SRV_Channels::enable_aux_servos(); } void Plane::one_second_loop() @@ -558,7 +558,7 @@ void Plane::handle_auto_mode(void) // we are in the final stage of a landing - force // zero throttle - channel_throttle->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); } else { calc_throttle(); } @@ -685,7 +685,7 @@ void Plane::update_flight_mode(void) // FBWA failsafe glide nav_roll_cd = 0; nav_pitch_cd = 0; - channel_throttle->set_servo_out(0); + SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } if (g.fbwa_tdrag_chan > 0) { // check for the user enabling FBWA taildrag takeoff mode @@ -748,10 +748,8 @@ void Plane::update_flight_mode(void) break; case MANUAL: - // servo_out is for Sim control only - // --------------------------------- - channel_roll->set_servo_out(channel_roll->pwm_to_angle()); - channel_pitch->set_servo_out(channel_pitch->pwm_to_angle()); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->pwm_to_angle()); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->pwm_to_angle()); steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 4d9a332303..996b931961 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -19,8 +19,8 @@ float Plane::get_speed_scaler(void) } speed_scaler = constrain_float(speed_scaler, 0.5f, 2.0f); } else { - if (channel_throttle->get_servo_out() > 0) { - speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->get_servo_out() / 2.0f); // First order taylor expansion of square root + if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) { + speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 2.0f); // First order taylor expansion of square root // Should maybe be to the 2/7 power, but we aren't going to implement that... }else{ speed_scaler = 1.67f; @@ -80,9 +80,9 @@ void Plane::stabilize_roll(float speed_scaler) if (control_mode == STABILIZE && channel_roll->get_control_in() != 0) { disable_integrator = true; } - channel_roll->set_servo_out(rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, - speed_scaler, - disable_integrator)); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, + speed_scaler, + disable_integrator)); } /* @@ -96,17 +96,17 @@ void Plane::stabilize_pitch(float speed_scaler) if (force_elevator != 0) { // we are holding the tail down during takeoff. Just convert // from a percentage to a -4500..4500 centidegree angle - channel_pitch->set_servo_out(45*force_elevator); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator); return; } - int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->get_servo_out() * g.kff_throttle_to_pitch; + int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; bool disable_integrator = false; if (control_mode == STABILIZE && channel_pitch->get_control_in() != 0) { disable_integrator = true; } - channel_pitch->set_servo_out(pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, - speed_scaler, - disable_integrator)); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, + speed_scaler, + disable_integrator)); } /* @@ -127,17 +127,6 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) servo_out += channel->pwm_to_angle(); } -/* - One argument version for when the servo out in the rc channel - is the target - */ -void Plane::stick_mix_channel(RC_Channel * channel) -{ - int16_t servo_out = channel->get_servo_out(); - stick_mix_channel(channel,servo_out); - channel->set_servo_out(servo_out); -} - /* this gives the user control of the aircraft in stabilization modes */ @@ -157,8 +146,13 @@ void Plane::stabilize_stick_mixing_direct() control_mode == TRAINING) { return; } - stick_mix_channel(channel_roll); - stick_mix_channel(channel_pitch); + int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); + stick_mix_channel(channel_roll, aileron); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); + + int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); + stick_mix_channel(channel_pitch, elevator); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); } /* @@ -268,25 +262,25 @@ void Plane::stabilize_yaw(float speed_scaler) void Plane::stabilize_training(float speed_scaler) { if (training_manual_roll) { - channel_roll->set_servo_out(channel_roll->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in()); } else { // calculate what is needed to hold stabilize_roll(speed_scaler); - if ((nav_roll_cd > 0 && channel_roll->get_control_in() < channel_roll->get_servo_out()) || - (nav_roll_cd < 0 && channel_roll->get_control_in() > channel_roll->get_servo_out())) { + if ((nav_roll_cd > 0 && channel_roll->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) || + (nav_roll_cd < 0 && channel_roll->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) { // allow user to get out of the roll - channel_roll->set_servo_out(channel_roll->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in()); } } if (training_manual_pitch) { - channel_pitch->set_servo_out(channel_pitch->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in()); } else { stabilize_pitch(speed_scaler); - if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < channel_pitch->get_servo_out()) || - (nav_pitch_cd < 0 && channel_pitch->get_control_in() > channel_pitch->get_servo_out())) { + if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) || + (nav_pitch_cd < 0 && channel_pitch->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) { // allow user to get back to level - channel_pitch->set_servo_out(channel_pitch->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in()); } } @@ -321,16 +315,16 @@ void Plane::stabilize_acro(float speed_scaler) nav_roll_cd = ahrs.roll_sensor + roll_error_cd; // try to reduce the integrated angular error to zero. We set // 'stabilze' to true, which disables the roll integrator - channel_roll->set_servo_out(rollController.get_servo_out(roll_error_cd, - speed_scaler, - true)); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd, + speed_scaler, + true)); } else { /* aileron stick is non-zero, use pure rate control until the user releases the stick */ acro_state.locked_roll = false; - channel_roll->set_servo_out(rollController.get_rate_out(roll_rate, speed_scaler)); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_rate_out(roll_rate, speed_scaler)); } if (g.acro_locking && is_zero(pitch_rate)) { @@ -345,15 +339,15 @@ void Plane::stabilize_acro(float speed_scaler) // try to hold the locked pitch. Note that we have the pitch // integrator enabled, which helps with inverted flight nav_pitch_cd = acro_state.locked_pitch_cd; - channel_pitch->set_servo_out(pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, - speed_scaler, - false)); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, + speed_scaler, + false)); } else { /* user has non-zero pitch input, use a pure rate controller */ acro_state.locked_pitch = false; - channel_pitch->set_servo_out( pitchController.get_rate_out(pitch_rate, speed_scaler)); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_rate_out(pitch_rate, speed_scaler)); } /* @@ -423,7 +417,7 @@ void Plane::calc_throttle() // user has asked for zero throttle - this may be done by a // mission which wants to turn off the engine for a parachute // landing - channel_throttle->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); return; } @@ -436,7 +430,7 @@ void Plane::calc_throttle() commanded_throttle = plane.guided_state.forced_throttle; } - channel_throttle->set_servo_out(commanded_throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle); } /***************************************** @@ -464,7 +458,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler) commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator); // add in rudder mixing from roll - commanded_rudder += channel_roll->get_servo_out() * g.kff_rudder_mix; + commanded_rudder += SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * g.kff_rudder_mix; commanded_rudder += rudder_input; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 03090c214b..a0e6dc3748 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -216,10 +216,10 @@ void Plane::send_servo_out(mavlink_channel_t chan) chan, millis(), 0, // port 0 - 10000 * channel_roll->norm_output() * (channel_roll->get_reverse()?-1:1), - 10000 * channel_pitch->norm_output() * (channel_pitch->get_reverse()?-1:1), - 10000 * channel_throttle->norm_output() * (channel_throttle->get_reverse()?-1:1), - 10000 * channel_rudder->norm_output() * (channel_rudder->get_reverse()?-1:1), + 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) / 4500.0f), + 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0f), + 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f), + 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / 4500.0f), 0, 0, 0, diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 84dd99c762..41ee4dea63 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -263,8 +263,8 @@ void Plane::Log_Write_Control_Tuning() roll : (int16_t)ahrs.roll_sensor, nav_pitch_cd : (int16_t)nav_pitch_cd, pitch : (int16_t)ahrs.pitch_sensor, - throttle_out : (int16_t)channel_throttle->get_servo_out(), - rudder_out : (int16_t)channel_rudder->get_servo_out(), + throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), + rudder_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_rudder), throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 9c58b49e0d..042ffec599 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1047,72 +1047,6 @@ const AP_Param::Info Plane::var_info[] = { (const void *)&plane.quadplane.attitude_control, {group_info : AC_AttitudeControl_Multi::var_info}, AP_PARAM_FLAG_POINTER }, - // RC channel - //----------- - // @Group: RC1_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GGROUP(rc_1, "RC1_", RC_Channel), - - // @Group: RC2_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GGROUP(rc_2, "RC2_", RC_Channel), - - // @Group: RC3_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GGROUP(rc_3, "RC3_", RC_Channel), - - // @Group: RC4_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GGROUP(rc_4, "RC4_", RC_Channel), - - // @Group: RC5_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_5, "RC5_", RC_Channel_aux), - - // @Group: RC6_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_6, "RC6_", RC_Channel_aux), - - // @Group: RC7_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_7, "RC7_", RC_Channel_aux), - - // @Group: RC8_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_8, "RC8_", RC_Channel_aux), - - // @Group: RC9_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_9, "RC9_", RC_Channel_aux), - - // @Group: RC10_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_10, "RC10_", RC_Channel_aux), - - // @Group: RC11_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_11, "RC11_", RC_Channel_aux), - - // @Group: RC12_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_12, "RC12_", RC_Channel_aux), - - // @Group: RC13_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_13, "RC13_", RC_Channel_aux), - - // @Group: RC14_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_14, "RC14_", RC_Channel_aux), - - // @Group: RC15_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_15, "RC15_", RC_Channel_aux), - - // @Group: RC16_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_16, "RC16_", RC_Channel_aux), - // @Group: RLL2SRV_ // @Path: ../libraries/APM_Control/AP_RollController.cpp GOBJECT(rollController, "RLL2SRV_", AP_RollController), @@ -1267,10 +1201,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine), - // @Group: SERVO_ - // @Path: ../libraries/RC_Channel/SRV_Channel.cpp - AP_SUBGROUPINFO(servo_channels, "SERVO", 3, ParametersG2, SRV_Channels), - + // 3 was used by prototype for servo_channels + // @Group: SYSID_ENFORCE // @DisplayName: GCS sysid enforcement // @Description: This controls whether packets from other than the expected GCS system ID will be accepted @@ -1282,6 +1214,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_Stats/AP_Stats.cpp AP_SUBGROUPINFO(stats, "STAT", 5, ParametersG2, AP_Stats), + // @Group: SERVO + // @Path: ../libraries/SRV_Channel/SRV_Channel.cpp + AP_SUBGROUPINFO(servo_channels, "SERVO", 6, ParametersG2, SRV_Channels), + + // @Group: RC + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + AP_SUBGROUPINFO(rc_channels, "RC", 7, ParametersG2, RC_Channels), + AP_GROUPEND }; @@ -1387,6 +1327,12 @@ void Plane::load_parameters(void) // quadplanes needs a higher loop rate AP_Param::set_default_by_name("SCHED_LOOP_RATE", 300); } + + // setup defaults in SRV_Channels + g2.servo_channels.set_default_function(CH_1, SRV_Channel::k_aileron); + g2.servo_channels.set_default_function(CH_2, SRV_Channel::k_elevator); + g2.servo_channels.set_default_function(CH_3, SRV_Channel::k_throttle); + g2.servo_channels.set_default_function(CH_4, SRV_Channel::k_rudder); cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 627aac1f33..5f97779b6f 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -244,19 +244,19 @@ class Parameters { k_param_battery_curr_pin, // unused - 169 // - // 170: Radio settings + // 170: Radio settings - all unused now // - k_param_rc_1 = 170, - k_param_rc_2, - k_param_rc_3, - k_param_rc_4, - k_param_rc_5, - k_param_rc_6, - k_param_rc_7, - k_param_rc_8, - k_param_rc_9, - k_param_rc_10, - k_param_rc_11, + k_param_rc_1_old = 170, + k_param_rc_2_old, + k_param_rc_3_old, + k_param_rc_4_old, + k_param_rc_5_old, + k_param_rc_6_old, + k_param_rc_7_old, + k_param_rc_8_old, + k_param_rc_9_old, + k_param_rc_10_old, + k_param_rc_11_old, k_param_throttle_min, k_param_throttle_max, @@ -270,13 +270,13 @@ class Parameters { k_param_throttle_slewrate, k_param_throttle_suppress_manual, k_param_throttle_passthru_stabilize, - k_param_rc_12, + k_param_rc_12_old, k_param_fs_batt_voltage, k_param_fs_batt_mah, k_param_short_fs_timeout, k_param_long_fs_timeout, - k_param_rc_13, - k_param_rc_14, + k_param_rc_13_old, + k_param_rc_14_old, k_param_tuning, // @@ -290,8 +290,8 @@ class Parameters { k_param_quadplane, k_param_rtl_radius, k_param_land_then_servos_neutral, // unused - moved to AP_Landing - k_param_rc_15, - k_param_rc_16, + k_param_rc_15_old, + k_param_rc_16_old, // // 210: flight modes @@ -515,47 +515,6 @@ class Parameters { #endif AP_Int16 gcs_pid_mask; AP_Int8 parachute_channel; - - // RC channels - RC_Channel rc_1; - RC_Channel rc_2; - RC_Channel rc_3; - RC_Channel rc_4; - RC_Channel_aux rc_5; - RC_Channel_aux rc_6; - RC_Channel_aux rc_7; - RC_Channel_aux rc_8; - RC_Channel_aux rc_9; - RC_Channel_aux rc_10; - RC_Channel_aux rc_11; - RC_Channel_aux rc_12; - RC_Channel_aux rc_13; - RC_Channel_aux rc_14; - RC_Channel_aux rc_15; - RC_Channel_aux rc_16; - uint8_t _dummy; - - Parameters() : - // variable default - //---------------------------------------- - rc_1 (CH_1), - rc_2 (CH_2), - rc_3 (CH_3), - rc_4 (CH_4), - rc_5 (CH_5), - rc_6 (CH_6), - rc_7 (CH_7), - rc_8 (CH_8), - rc_9 (CH_9), - rc_10 (CH_10), - rc_11 (CH_11), - rc_12 (CH_12), - rc_13 (CH_13), - rc_14 (CH_14), - rc_15 (CH_15), - rc_16 (CH_16), - _dummy(0) - {} }; /* @@ -577,6 +536,9 @@ class ParametersG2 { // internal combustion engine control AP_ICEngine ice_control; + // RC input channels + RC_Channels rc_channels; + // control over servo output ranges SRV_Channels servo_channels; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 690b7d4bf3..4ec640ae78 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -43,7 +43,7 @@ #include // interface and maths for accelerometer calibration #include // ArduPilot Mega DCM Library #include // RC Channel Library -#include +#include #include // Range finder library #include // Filter library #include // APM FIFO Buffer @@ -189,7 +189,7 @@ class Plane : public AP_HAL::HAL::Callbacks { // board specific config AP_BoardConfig BoardConfig; - // primary control channels + // primary input channels RC_Channel *channel_roll; RC_Channel *channel_pitch; RC_Channel *channel_throttle; @@ -983,7 +983,6 @@ class Plane : public AP_HAL::HAL::Callbacks { void resetPerfData(void); void check_usb_mux(void); void print_comma(void); - void servo_write(uint8_t ch, uint16_t pwm); bool should_log(uint32_t mask); int8_t throttle_percentage(void); void change_arm_state(void); @@ -1047,7 +1046,6 @@ class Plane : public AP_HAL::HAL::Callbacks { bool stick_mixing_enabled(void); void stabilize_roll(float speed_scaler); void stabilize_pitch(float speed_scaler); - static void stick_mix_channel(RC_Channel *channel); static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); void stabilize_stick_mixing_direct(); void stabilize_stick_mixing_fbw(); @@ -1057,11 +1055,10 @@ class Plane : public AP_HAL::HAL::Callbacks { void calc_nav_yaw_coordinated(float speed_scaler); void calc_nav_yaw_course(void); void calc_nav_yaw_ground(void); - void throttle_slew_limit(int16_t last_throttle); - void flap_slew_limit(int8_t &last_value, int8_t &new_value); + void throttle_slew_limit(void); bool suppress_throttle(void); - void channel_output_mixer(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; - void channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const; + void channel_output_mixer_pwm(uint8_t mixing_type, uint16_t & chan1, uint16_t & chan2)const; + void channel_output_mixer(uint8_t mixing_type, SRV_Channel::Aux_servo_function_t servo1, SRV_Channel::Aux_servo_function_t servo2); void flaperon_update(int8_t flap_percent); bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); @@ -1091,7 +1088,6 @@ class Plane : public AP_HAL::HAL::Callbacks { void log_init(); void init_capabilities(void); void dataflash_periodic(void); - uint16_t throttle_min(void) const; void parachute_check(); #if PARACHUTE == ENABLED void do_parachute(const AP_Mission::Mission_Command& cmd); @@ -1120,7 +1116,6 @@ class Plane : public AP_HAL::HAL::Callbacks { int8_t reboot_board(uint8_t argc, const Menu::arg *argv); int8_t main_menu_help(uint8_t argc, const Menu::arg *argv); int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); - int8_t test_passthru(uint8_t argc, const Menu::arg *argv); int8_t test_radio(uint8_t argc, const Menu::arg *argv); int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); int8_t test_relay(uint8_t argc, const Menu::arg *argv); diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 13f5de0f19..d2f039e155 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -15,30 +15,19 @@ AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Ba */ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) { - // we are terminating. Setup primary output channels radio_out values - RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1); - RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1); - RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1); - RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); - - ch_roll->set_radio_out(ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN)); - ch_pitch->set_radio_out(ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX)); - ch_yaw->set_radio_out(ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX)); - ch_throttle->set_radio_out(ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN)); - - RC_Channel_aux::disable_passthrough(true); + plane.g2.servo_channels.disable_passthrough(true); plane.servos_output(); // and all aux channels - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN); - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM); - RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_output_limit(SRV_Channel::k_flap_auto, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_output_limit(SRV_Channel::k_flap, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); + SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_output_limit(SRV_Channel::k_elevator_with_input, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); plane.quadplane.afs_terminate(); @@ -48,26 +37,15 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void) { - const RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1); - const RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1); - const RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1); - const RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); - - // setup primary channel output values - hal.rcout->set_failsafe_pwm(1U<<(rcmap.roll()-1), ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN)); - hal.rcout->set_failsafe_pwm(1U<<(rcmap.pitch()-1), ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX)); - hal.rcout->set_failsafe_pwm(1U<<(rcmap.yaw()-1), ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX)); - hal.rcout->set_failsafe_pwm(1U<<(rcmap.throttle()-1), ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN)); - - // and all aux channels - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN); - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX); - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM); - RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM); + // all aux channels + SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap_auto, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator_with_input, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); if (plane.quadplane.available()) { // setup AP_Motors outputs for failsafe diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 1bff875390..926cb975ec 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -83,7 +83,7 @@ void Plane::read_control_switch() } } else if (!override_requested && px4io_override_enabled) { px4io_override_enabled = false; - RC_Channel_aux::enable_aux_servos(); + SRV_Channels::enable_aux_servos(); gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override disabled"); } if (px4io_override_enabled && diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index b6185e4e42..5c18dfc99c 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -53,26 +53,23 @@ void Plane::failsafe_check(void) // pass RC inputs to outputs every 20ms hal.rcin->clear_overrides(); - channel_roll->set_radio_out(channel_roll->read()); - channel_pitch->set_radio_out(channel_pitch->read()); - if (hal.util->get_soft_armed()) { - channel_throttle->set_radio_out(channel_throttle->read()); - } else { - channel_throttle->set_servo_out(0); - channel_throttle->calc_pwm(); - } - channel_rudder->set_radio_out(channel_rudder->read()); - int16_t roll = channel_roll->pwm_to_angle_dz(0); - int16_t pitch = channel_pitch->pwm_to_angle_dz(0); - int16_t rudder = channel_rudder->pwm_to_angle_dz(0); + int16_t roll = channel_roll->get_control_in_zero_dz(); + int16_t pitch = channel_pitch->get_control_in_zero_dz(); + int16_t throttle = channel_throttle->get_control_in_zero_dz(); + int16_t rudder = channel_rudder->get_control_in_zero_dz(); + if (!hal.util->get_soft_armed()) { + throttle = 0; + } + // setup secondary output channels that don't have // corresponding input channels - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, roll); - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, pitch); - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, rudder); - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the @@ -84,11 +81,11 @@ void Plane::failsafe_check(void) // setup secondary output channels that do have // corresponding input channels - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true); - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input, true); - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, 0); - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, 0); + SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true); + SRV_Channels::copy_radio_in_out(SRV_Channel::k_aileron_with_input, true); + SRV_Channels::copy_radio_in_out(SRV_Channel::k_elevator_with_input, true); + SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 0); + SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0); servos_output(); diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp index 039428485a..bb1e8ba528 100644 --- a/ArduPlane/px4_mixer.cpp +++ b/ArduPlane/px4_mixer.cpp @@ -71,7 +71,7 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) for (uint8_t i=0; i<8; i++) { int32_t c1, c2, mix=0; bool rev = false; - RC_Channel_aux::Aux_servo_function_t function = RC_Channel_aux::channel_function(i); + SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(i); if (i == rcmap.pitch()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) { // first channel of VTAIL mix c1 = rcmap.yaw()-1; @@ -98,27 +98,27 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) c2 = rcmap.roll()-1; rev = false; mix = mix_max*mixmul[g.elevon_output][0]; - } else if (function == RC_Channel_aux::k_aileron || - function == RC_Channel_aux::k_flaperon1 || - function == RC_Channel_aux::k_flaperon2) { + } else if (function == SRV_Channel::k_aileron || + function == SRV_Channel::k_flaperon1 || + function == SRV_Channel::k_flaperon2) { // a secondary aileron. We don't mix flap input in yet for flaperons c1 = rcmap.roll()-1; - } else if (function == RC_Channel_aux::k_elevator) { + } else if (function == SRV_Channel::k_elevator) { // a secondary elevator c1 = rcmap.pitch()-1; - } else if (function == RC_Channel_aux::k_rudder || - function == RC_Channel_aux::k_steering) { + } else if (function == SRV_Channel::k_rudder || + function == SRV_Channel::k_steering) { // a secondary rudder or wheel c1 = rcmap.yaw()-1; } else if (g.flapin_channel > 0 && - (function == RC_Channel_aux::k_flap || - function == RC_Channel_aux::k_flap_auto)) { + (function == SRV_Channel::k_flap || + function == SRV_Channel::k_flap_auto)) { // a flap output channel, and we have a manual flap input channel c1 = g.flapin_channel-1; } else if (i < 4 || - function == RC_Channel_aux::k_elevator_with_input || - function == RC_Channel_aux::k_aileron_with_input || - function == RC_Channel_aux::k_manual) { + function == SRV_Channel::k_elevator_with_input || + function == SRV_Channel::k_aileron_with_input || + function == SRV_Channel::k_manual) { // a pass-thru channel c1 = i; } else { @@ -132,8 +132,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) // pass through channel, possibly with reversal. We also // adjust the gain based on the range of input and output // channels and adjust for trims - const RC_Channel *chan1 = RC_Channel::rc_channel(i); - const RC_Channel *chan2 = RC_Channel::rc_channel(c1); + const RC_Channel *chan1 = RC_Channels::rc_channel(i); + const RC_Channel *chan2 = RC_Channels::rc_channel(c1); int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim()); int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim()); chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); @@ -171,8 +171,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) return 0; } } else { - const RC_Channel *chan1 = RC_Channel::rc_channel(c1); - const RC_Channel *chan2 = RC_Channel::rc_channel(c2); + const RC_Channel *chan1 = RC_Channels::rc_channel(c1); + const RC_Channel *chan2 = RC_Channels::rc_channel(c2); int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim()); int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim()); chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); @@ -268,7 +268,7 @@ bool Plane::setup_failsafe_mixing(void) 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()); + 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 @@ -294,7 +294,7 @@ bool Plane::setup_failsafe_mixing(void) // 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); + RC_Channel *ch = RC_Channels::rc_channel(i); if (ch == nullptr) { continue; } @@ -354,8 +354,8 @@ bool Plane::setup_failsafe_mixing(void) } for (uint8_t i = 0; i < pwm_values.channel_count; i++) { - if (RC_Channel_aux::channel_function(i) >= RC_Channel_aux::k_motor1 && - RC_Channel_aux::channel_function(i) <= RC_Channel_aux::k_motor8) { + if (SRV_Channels::channel_function(i) >= SRV_Channel::k_motor1 && + SRV_Channels::channel_function(i) <= SRV_Channel::k_motor8) { pwm_values.values[i] = quadplane.thr_min_pwm; } else { pwm_values.values[i] = 900; @@ -367,8 +367,8 @@ bool Plane::setup_failsafe_mixing(void) } for (uint8_t i = 0; i < pwm_values.channel_count; i++) { - if (RC_Channel_aux::channel_function(i) >= RC_Channel_aux::k_motor1 && - RC_Channel_aux::channel_function(i) <= RC_Channel_aux::k_motor8) { + if (SRV_Channels::channel_function(i) >= SRV_Channel::k_motor1 && + SRV_Channels::channel_function(i) <= SRV_Channel::k_motor8) { hal.rcout->write(i, quadplane.thr_min_pwm); pwm_values.values[i] = quadplane.thr_min_pwm; } else { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1d40b67cbb..edafc16e65 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -354,7 +354,7 @@ QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) : void QuadPlane::setup_default_channels(uint8_t num_motors) { for (uint8_t i=0; iset_radio_trim(thr_min_pwm); - } + SRV_Channel::Aux_servo_function_t func = (SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+i); + SRV_Channels::set_failsafe_pwm(func, thr_min_pwm); + SRV_Channels::set_trim_to_pwm_for(func, thr_min_pwm); } #if HAVE_PX4_MIXER diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 0e6cb77ade..9199e9e693 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -11,13 +11,13 @@ void Plane::set_control_channels(void) if (g.rudder_only) { // in rudder only mode the roll and rudder channels are the // same. - channel_roll = RC_Channel::rc_channel(rcmap.yaw()-1); + channel_roll = RC_Channels::rc_channel(rcmap.yaw()-1); } else { - channel_roll = RC_Channel::rc_channel(rcmap.roll()-1); + channel_roll = RC_Channels::rc_channel(rcmap.roll()-1); } - channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1); - channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); - channel_rudder = RC_Channel::rc_channel(rcmap.yaw()-1); + channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1); + channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1); + channel_rudder = RC_Channels::rc_channel(rcmap.yaw()-1); // set rc channel ranges channel_roll->set_angle(SERVO_MAX); @@ -25,19 +25,19 @@ void Plane::set_control_channels(void) channel_rudder->set_angle(SERVO_MAX); if (aparm.throttle_min >= 0) { // normal operation - channel_throttle->set_range(0, 100); + channel_throttle->set_range(100); } else { // reverse thrust channel_throttle->set_angle(100); } if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); + 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); } // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed - g2.servo_channels.set_esc_scaling(channel_throttle->get_ch_out()); + g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle); } /* @@ -58,29 +58,24 @@ void Plane::init_rc_in() */ void Plane::init_rc_out_main() { - // setup failsafe for bottom 4 channels. We don't do all channels - // yet as some may be for VTOL motors in a quadplane - RC_Channel::setup_failsafe_trim_mask(0x000F); - /* change throttle trim to minimum throttle. This prevents a configuration error where the user sets CH3_TRIM incorrectly and the motor may start on power up */ - channel_throttle->set_radio_trim(throttle_min()); - - channel_roll->enable_out(); - channel_pitch->enable_out(); - - if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) { - channel_throttle->enable_out(); + if (aparm.throttle_min >= 0) { + SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttle); } - channel_rudder->enable_out(); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + // setup PX4 to output the min throttle when safety off if arming // is setup for min on disarm if (arming.arming_required() == AP_Arming::YES_MIN_PWM) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); + 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); } } @@ -90,17 +85,17 @@ void Plane::init_rc_out_main() void Plane::init_rc_out_aux() { update_aux(); - RC_Channel_aux::enable_aux_servos(); + SRV_Channels::enable_aux_servos(); hal.rcout->cork(); // Initialization of servo outputs - RC_Channel::output_trim_all(); + SRV_Channels::output_trim_all(); servos_output(); // setup PWM values to send if the FMU firmware dies - RC_Channel::setup_failsafe_trim_all(); + SRV_Channels::setup_failsafe_trim_all(); } /* @@ -199,7 +194,7 @@ void Plane::read_radio() + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; } - RC_Channel::set_pwm_all(); + RC_Channels::set_pwm_all(); if (control_mode == TRAINING) { // in training mode we don't want to use a deadzone, as we @@ -215,10 +210,10 @@ void Plane::read_radio() control_failsafe(channel_throttle->get_radio_in()); - channel_throttle->set_servo_out(channel_throttle->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in()); - if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) { - float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f; + if (g.throttle_nudge && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 50 && geofence_stickmixing()) { + float nudge = (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) - 50) * 0.02f; if (ahrs.airspeed_sensor_enabled()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge; } else { @@ -326,8 +321,8 @@ void Plane::trim_control_surfaces() // the secondary aileron/elevator is trimmed only if it has a // corresponding transmitter input channel, which k_aileron // doesn't have - RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_aileron_with_input); - RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_elevator_with_input); + SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::k_aileron_with_input); + SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::k_elevator_with_input); } else{ if (elevon.ch1_temp != 0) { elevon.trim1 = elevon.ch1_temp; @@ -349,8 +344,6 @@ void Plane::trim_control_surfaces() channel_roll->save_eeprom(); channel_pitch->save_eeprom(); channel_rudder->save_eeprom(); - - g2.servo_channels.set_trim(); } void Plane::trim_radio() diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index c730dac814..eb7dded3ba 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -351,7 +351,7 @@ void Plane::update_sensor_status_flags(void) } } - if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) { + if (aparm.throttle_min < 0 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) { control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR; control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 2058adf7d8..00e81fea8d 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -22,7 +22,7 @@ /***************************************** * Throttle slew limit *****************************************/ -void Plane::throttle_slew_limit(int16_t last_throttle) +void Plane::throttle_slew_limit(void) { uint8_t slewrate = aparm.throttle_slewrate; if (control_mode==AUTO) { @@ -34,34 +34,8 @@ void Plane::throttle_slew_limit(int16_t last_throttle) } // if slew limit rate is set to zero then do not slew limit if (slewrate) { - // limit throttle change by the given percentage per second - float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min()); - // allow a minimum change of 1 PWM per cycle - if (temp < 1) { - temp = 1; - } - channel_throttle->set_radio_out(constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp)); - } -} - -/***************************************** -Flap slew limit -*****************************************/ -void Plane::flap_slew_limit(int8_t &last_value, int8_t &new_value) -{ - uint8_t slewrate = g.flap_slewrate; - // if slew limit rate is set to zero then do not slew limit - if (slewrate) { - // limit flap change by the given percentage per second - float temp = slewrate * G_Dt; - // allow a minimum change of 1% per cycle. This means the - // slowest flaps we can do is full change over 2 seconds - if (temp < 1) { - temp = 1; - } - new_value = constrain_int16(new_value, last_value - temp, last_value + temp); + SRV_Channels::limit_slew_rate(SRV_Channel::k_throttle, slewrate); } - last_value = new_value; } /* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. @@ -156,7 +130,7 @@ bool Plane::suppress_throttle(void) /* implement a software VTail or elevon mixer. There are 4 different mixing modes */ -void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16_t & chan2_out)const +void Plane::channel_output_mixer_pwm(uint8_t mixing_type, uint16_t & chan1_out, uint16_t & chan2_out) const { int16_t c1, c2; int16_t v1, v2; @@ -227,27 +201,38 @@ void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16 chan2_out = 1500 + v2; } -void Plane::channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const +/* + output mixer based on two channel output types + */ +void Plane::channel_output_mixer(uint8_t mixing_type, SRV_Channel::Aux_servo_function_t func1, SRV_Channel::Aux_servo_function_t func2) { - int16_t ch1 = chan1->get_radio_out(); - int16_t ch2 = chan2->get_radio_out(); + SRV_Channel *chan1, *chan2; + if (!(chan1 = SRV_Channels::get_channel_for(func1)) || + !(chan2 = SRV_Channels::get_channel_for(func2))) { + return; + } - channel_output_mixer(mixing_type,ch1,ch2); + uint16_t chan1_out, chan2_out; + chan1_out = chan1->get_output_pwm(); + chan2_out = chan2->get_output_pwm(); + + channel_output_mixer_pwm(mixing_type, chan1_out, chan2_out); - chan1->set_radio_out(ch1); - chan2->set_radio_out(ch2); + chan1->set_output_pwm(chan1_out); + chan2->set_output_pwm(chan2_out); } + /* setup flaperon output channels */ void Plane::flaperon_update(int8_t flap_percent) { - if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon1) || - !RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon2)) { + if (!SRV_Channels::function_assigned(SRV_Channel::k_flaperon1) || + !SRV_Channels::function_assigned(SRV_Channel::k_flaperon2)) { return; } - int16_t ch1, ch2; + uint16_t ch1, ch2; /* flaperons are implemented as a mixer between aileron and a percentage of flaps. Flap input can come from a manual channel @@ -258,12 +243,14 @@ void Plane::flaperon_update(int8_t flap_percent) by mixing gain). flapin_channel's trim is not used. */ - ch1 = channel_roll->get_radio_out(); + if (!SRV_Channels::get_output_pwm(SRV_Channel::k_aileron, ch1)) { + return; + } // The *5 is to take a percentage to a value from -500 to 500 for the mixer ch2 = 1500 - flap_percent * 5; - channel_output_mixer(g.flaperon_output, ch1, ch2); - RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::k_flaperon1, ch1); - RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::k_flaperon2, ch2); + channel_output_mixer_pwm(g.flaperon_output, ch1, ch2); + SRV_Channels::set_output_pwm_trimmed(SRV_Channel::k_flaperon1, ch1); + SRV_Channels::set_output_pwm_trimmed(SRV_Channel::k_flaperon2, ch2); } /* @@ -273,9 +260,8 @@ void Plane::flaperon_update(int8_t flap_percent) */ void Plane::set_servos_idle(void) { - RC_Channel_aux::output_ch_all(); if (auto_state.idle_wiggle_stage == 0) { - RC_Channel::output_trim_all(); + SRV_Channels::output_trim_all(); return; } int16_t servo_value = 0; @@ -292,57 +278,29 @@ void Plane::set_servos_idle(void) } else { auto_state.idle_wiggle_stage = 0; } - channel_roll->set_servo_out(servo_value); - channel_pitch->set_servo_out(servo_value); - channel_rudder->set_servo_out(servo_value); - channel_roll->calc_pwm(); - channel_pitch->calc_pwm(); - channel_rudder->calc_pwm(); - channel_throttle->output_trim(); -} - -/* - return minimum throttle PWM value, taking account of throttle reversal. For reverse thrust you get the throttle off position - */ -uint16_t Plane::throttle_min(void) const -{ - if (aparm.throttle_min < 0) { - return channel_throttle->get_radio_trim(); - } - return channel_throttle->get_reverse() ? channel_throttle->get_radio_max() : channel_throttle->get_radio_min(); -}; + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_value); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_value); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value); + SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle); + SRV_Channels::output_ch_all(); +} /* pass through channels in manual mode */ void Plane::set_servos_manual_passthrough(void) { - // 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)); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, channel_rudder->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); // 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); + SRV_Channels::copy_radio_in_out(SRV_Channel::k_aileron_with_input); + SRV_Channels::copy_radio_in_out(SRV_Channel::k_elevator_with_input); } /* @@ -353,8 +311,10 @@ void Plane::set_servos_old_elevons(void) /*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()); + int16_t roll = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); + int16_t pitch = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); + ch1 = pitch - (BOOL_TO_SIGN(g.reverse_elevons) * roll); + ch2 = pitch + (BOOL_TO_SIGN(g.reverse_elevons) * roll); /* Differential Spoilers If differential spoilers are setup, then we translate @@ -362,23 +322,24 @@ void Plane::set_servos_old_elevons(void) 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)) { + if (SRV_Channels::function_assigned(SRV_Channel::k_dspoiler1) && SRV_Channels::function_assigned(SRV_Channel::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()); + int16_t rudder = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder); + if (BOOL_TO_SIGN(g.reverse_elevons) * rudder < 0) { + ch1 += abs(rudder); + ch3 -= abs(rudder); } else { - ch2 += abs(channel_rudder->get_servo_out()); - ch4 -= abs(channel_rudder->get_servo_out()); + ch2 += abs(rudder); + ch4 -= abs(rudder); } - 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); + SRV_Channels::set_output_scaled(SRV_Channel::k_dspoiler1, ch3); + SRV_Channels::set_output_scaled(SRV_Channel::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))); + SRV_Channels::set_output_pwm_first(SRV_Channel::k_aileron, elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX))); + SRV_Channels::set_output_pwm_first(SRV_Channel::k_elevator, elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX))); } @@ -392,14 +353,14 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) // 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 + if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 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 && + } else if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0 && min_throttle < 0 && // reverse thrust is available throttle_watt_limit_min < -(min_throttle) - 25 && now - throttle_watt_limit_timer_ms >= 1) { @@ -412,12 +373,12 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) // 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 + if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 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 + } else if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 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--; @@ -441,20 +402,12 @@ void Plane::set_servos_controlled(void) set_servos_old_elevons(); } else { // 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()); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)); // 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()); - - // push out the PWM values - channel_roll->calc_pwm(); - channel_pitch->calc_pwm(); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)); } - channel_rudder->calc_pwm(); - // 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(); @@ -477,25 +430,25 @@ void Plane::set_servos_controlled(void) } } } - + // apply watt limiter throttle_watt_limiter(min_throttle, max_throttle); - - channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), - min_throttle, - max_throttle)); + + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, + constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); if (!hal.util->get_soft_armed()) { - channel_throttle->set_servo_out(0); - channel_throttle->calc_pwm(); + if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) { + SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + } else { + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); + } } else if (suppress_throttle()) { // throttle is suppressed in auto mode - channel_throttle->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 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(); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); } } else if (g.throttle_passthru_stabilize && (control_mode == STABILIZE || @@ -506,18 +459,14 @@ void Plane::set_servos_controlled(void) !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()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); } else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED - channel_throttle->set_radio_out(channel_throttle->get_radio_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); } 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(); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, quadplane.forward_throttle_pct()); } } @@ -529,11 +478,9 @@ 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); + RC_Channel *flapin = RC_Channels::rc_channel(g.flapin_channel-1); if (flapin != nullptr && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) { flapin->input(); manual_flap_percent = flapin->percent_input(); @@ -589,11 +536,13 @@ void Plane::set_servos_flaps(void) auto_flap_percent = manual_flap_percent; } - flap_slew_limit(last_auto_flap, auto_flap_percent); - flap_slew_limit(last_manual_flap, 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); - 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.flap_slewrate) { + SRV_Channels::limit_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate); + SRV_Channels::limit_slew_rate(SRV_Channel::k_flap, g.flap_slewrate); + } if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) { flaperon_update(auto_flap_percent); @@ -608,20 +557,22 @@ void Plane::set_servos_flaps(void) void Plane::servo_output_mixers(void) { if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder); + channel_output_mixer(g.vtail_output, SRV_Channel::k_elevator, SRV_Channel::k_rudder); } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, channel_pitch, channel_roll); + channel_output_mixer(g.elevon_output, SRV_Channel::k_elevator, SRV_Channel::k_aileron); // if (both) differential spoilers setup then apply rudder // control into splitting the two elevons 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)) { - int16_t ch3 = channel_roll->get_radio_out(); //diff spoiler 1 - int16_t ch4 = channel_pitch->get_radio_out(); //diff spoiler 2 + uint16_t ch3, ch4; + + if (SRV_Channels::function_assigned(SRV_Channel::k_dspoiler1) && + SRV_Channels::function_assigned(SRV_Channel::k_dspoiler2) && + SRV_Channels::get_output_pwm(SRV_Channel::k_aileron, ch3) && + SRV_Channels::get_output_pwm(SRV_Channel::k_elevator, ch4)) { // convert rudder-servo output (-4500 to 4500) to PWM offset // value (-500 to 500) and multiply by DSPOILR_RUD_RATE/100 // (rudder->servo_out * 500 / SERVO_MAX * dspoiler_rud_rate/100): - int16_t ruddVal = (int16_t)((int32_t)(channel_rudder->get_servo_out()) * + int16_t ruddVal = (int16_t)(int32_t(SRV_Channels::get_output_scaled(SRV_Channel::k_rudder)) * g.dspoiler_rud_rate / (SERVO_MAX/5)); if (ruddVal != 0) { //if nonzero rudder then apply to spoilers int16_t ch1 = ch3; //elevon 1 @@ -634,8 +585,8 @@ void Plane::servo_output_mixers(void) ch4 -= ruddVal; } // change elevon 1 & 2 positions; constrain min/max: - channel_roll->set_radio_out(constrain_int16(ch1, 900, 2100)); - channel_pitch->set_radio_out(constrain_int16(ch2, 900, 2100)); + SRV_Channels::set_output_pwm_first(SRV_Channel::k_aileron, constrain_int16(ch1, 900, 2100)); + SRV_Channels::set_output_pwm_first(SRV_Channel::k_elevator, constrain_int16(ch2, 900, 2100)); // constrain min/max for intermediate dspoiler positions: ch3 = constrain_int16(ch3, 900, 2100); ch4 = constrain_int16(ch4, 900, 2100); @@ -643,10 +594,10 @@ void Plane::servo_output_mixers(void) // set positions of differential spoilers (convert PWM // 900-2100 range to servo output (-4500 to 4500) // and use function that supports rev/min/max/trim): - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, - (ch3-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, - (ch4-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); + SRV_Channels::set_output_scaled(SRV_Channel::k_dspoiler1, + (int16_t(ch3)-1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); + SRV_Channels::set_output_scaled(SRV_Channel::k_dspoiler2, + (int16_t(ch4)-1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); } } } @@ -679,8 +630,6 @@ void Plane::set_servos(void) return; } - int16_t last_throttle = channel_throttle->get_radio_out(); - // do any transition updates for quadplane quadplane.update(); @@ -699,19 +648,23 @@ void Plane::set_servos(void) // 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)) { + } else if (!SRV_Channels::function_assigned(SRV_Channel::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); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 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 == TRAINING) { + steering_control.rudder = channel_rudder->get_control_in(); + } + + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering); if (control_mode == MANUAL) { set_servos_manual_passthrough(); @@ -727,12 +680,7 @@ void Plane::set_servos(void) 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()); + throttle_slew_limit(); } if (!arming.is_armed()) { @@ -745,14 +693,12 @@ void Plane::set_servos(void) break; case AP_Arming::YES_ZERO_PWM: - channel_throttle->set_servo_out(0); - channel_throttle->set_radio_out(0); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0); break; case AP_Arming::YES_MIN_PWM: default: - channel_throttle->set_servo_out(0); - channel_throttle->set_radio_out(throttle_min()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); break; } } @@ -778,26 +724,22 @@ void Plane::set_servos(void) // 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 (landing.get_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()); + SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); } else if (landing.get_then_servos_neutral() == 2) { - channel_roll->disable_out(); - channel_pitch->disable_out(); - channel_rudder->disable_out(); + SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); + SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); } } uint8_t override_pct; if (g2.ice_control.throttle_override(override_pct)) { // the ICE controller wants to override the throttle for starting - channel_throttle->set_servo_out(override_pct); - channel_throttle->calc_pwm(); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, override_pct); } - // allow for secondary throttle - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_throttle, channel_throttle->get_servo_out()); - // run output mixer and send values to the hal for output servos_output(); } @@ -812,30 +754,15 @@ void Plane::servos_output(void) { hal.rcout->cork(); - // to enable the throttle slew rate to work we need to remember - // and restore the throttle radio_out - uint16_t thr_radio_out_saved = channel_throttle->get_radio_out(); - - // remap servo output to SERVO* ranges if enabled - g2.servo_channels.remap_servo_output(); - // run vtail and elevon mixers servo_output_mixers(); - channel_roll->output(); - channel_pitch->output(); - channel_throttle->output(); - channel_rudder->output(); - - if (!afs.should_crash_vehicle()) { - RC_Channel_aux::output_ch_all(); - } + SRV_Channels::calc_pwm(); + + SRV_Channels::output_ch_all(); hal.rcout->push(); - // restore throttle radio out - channel_throttle->set_radio_out(thr_radio_out_saved); - if (g2.servo_channels.auto_trim_enabled()) { servos_auto_trim(); } @@ -848,10 +775,6 @@ void Plane::servos_output(void) */ void Plane::servos_auto_trim(void) { - if (!g2.servo_channels.enabled()) { - // only possible with SERVO_RNG_ENABLE=1 - return; - } // only in auto modes and FBWA if (!auto_throttle_mode && control_mode != FLY_BY_WIRE_A) { return; @@ -881,8 +804,8 @@ void Plane::servos_auto_trim(void) } // adjust trim on channels by a small amount according to I value - g2.servo_channels.adjust_trim(channel_roll->get_ch_out(), rollController.get_pid_info().I); - g2.servo_channels.adjust_trim(channel_pitch->get_ch_out(), pitchController.get_pid_info().I); + g2.servo_channels.adjust_trim(SRV_Channel::k_aileron, rollController.get_pid_info().I); + g2.servo_channels.adjust_trim(SRV_Channel::k_elevator, pitchController.get_pid_info().I); auto_trim.last_trim_check = now; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 401758210c..d47a327096 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -741,23 +741,6 @@ void Plane::print_comma(void) } #endif -/* - write to a servo - */ -void Plane::servo_write(uint8_t ch, uint16_t pwm) -{ -#if HIL_SUPPORT - if (g.hil_mode==1 && !g.hil_servos) { - if (ch < 8) { - RC_Channel::rc_channel(ch)->set_radio_out(pwm); - } - return; - } -#endif - hal.rcout->enable_ch(ch); - hal.rcout->write(ch, pwm); -} - /* should we log a message type now? */ @@ -787,11 +770,12 @@ int8_t Plane::throttle_percentage(void) } // to get the real throttle we need to use norm_output() which // returns a number from -1 to 1. + float throttle = SRV_Channels::get_output_norm(SRV_Channel::k_throttle); if (aparm.throttle_min >= 0) { - return constrain_int16(50*(channel_throttle->norm_output()+1), 0, 100); + return constrain_int16(100*throttle, 0, 100); } else { // reverse thrust - return constrain_int16(100*channel_throttle->norm_output(), -100, 100); + return constrain_int16(100*throttle, -100, 100); } } @@ -814,9 +798,6 @@ bool Plane::arm_motors(AP_Arming::ArmingMethod method) return false; } - // only log if arming was successful - channel_throttle->enable_out(); - change_arm_state(); return true; } @@ -829,9 +810,6 @@ bool Plane::disarm_motors(void) if (!arming.disarm()) { return false; } - if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) { - channel_throttle->disable_out(); - } if (control_mode != AUTO) { // reset the mission on disarm if we are not in auto mission.reset(); diff --git a/ArduPlane/test.cpp b/ArduPlane/test.cpp index 6234855dd2..ef70ffc9f2 100644 --- a/ArduPlane/test.cpp +++ b/ArduPlane/test.cpp @@ -7,15 +7,6 @@ // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details static const struct Menu::command test_menu_commands[] = { - {"pwm", MENU_FUNC(test_radio_pwm)}, - {"radio", MENU_FUNC(test_radio)}, - {"passthru", MENU_FUNC(test_passthru)}, - {"failsafe", MENU_FUNC(test_failsafe)}, - {"relay", MENU_FUNC(test_relay)}, - {"waypoints", MENU_FUNC(test_wp)}, - {"xbee", MENU_FUNC(test_xbee)}, - {"modeswitch", MENU_FUNC(test_modeswitch)}, - // Tests below here are for hardware sensors only present // when real sensors are attached or they are emulated {"gps", MENU_FUNC(test_gps)}, @@ -45,251 +36,6 @@ void Plane::print_hit_enter() cliSerial->printf("Hit Enter to exit.\n\n"); } -int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - while(1) { - hal.scheduler->delay(20); - - // Filters radio input - adjust filters in the radio.cpp file - // ---------------------------------------------------------- - read_radio(); - - cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - (int)channel_roll->get_radio_in(), - (int)channel_pitch->get_radio_in(), - (int)channel_throttle->get_radio_in(), - (int)channel_rudder->get_radio_in(), - (int)g.rc_5.get_radio_in(), - (int)g.rc_6.get_radio_in(), - (int)g.rc_7.get_radio_in(), - (int)g.rc_8.get_radio_in()); - - if(cliSerial->available() > 0) { - return (0); - } - } -} - - -int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - while(1) { - hal.scheduler->delay(20); - - // New radio frame? (we could use also if((millis()- timer) > 20) - if (hal.rcin->new_input()) { - cliSerial->print("CH:"); - for(int16_t i = 0; i < 8; i++) { - cliSerial->print(hal.rcin->read(i)); // Print channel values - print_comma(); - servo_write(i, hal.rcin->read(i)); // Copy input to Servos - } - cliSerial->println(); - } - if (cliSerial->available() > 0) { - return (0); - } - } - return 0; -} - -int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - // read the radio to set trims - // --------------------------- - trim_radio(); - - while(1) { - hal.scheduler->delay(20); - read_radio(); - - channel_roll->calc_pwm(); - channel_pitch->calc_pwm(); - channel_throttle->calc_pwm(); - channel_rudder->calc_pwm(); - - // write out the servo PWM values - // ------------------------------ - set_servos(); - - cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - (int)channel_roll->get_control_in(), - (int)channel_pitch->get_control_in(), - (int)channel_throttle->get_control_in(), - (int)channel_rudder->get_control_in(), - (int)g.rc_5.get_control_in(), - (int)g.rc_6.get_control_in(), - (int)g.rc_7.get_control_in(), - (int)g.rc_8.get_control_in() ); - - if(cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv) -{ - uint8_t fail_test = 0; - print_hit_enter(); - for(int16_t i = 0; i < 50; i++) { - hal.scheduler->delay(20); - read_radio(); - } - - // read the radio to set trims - // --------------------------- - trim_radio(); - - oldSwitchPosition = readSwitch(); - - cliSerial->println("Unplug battery, throttle in neutral, turn off radio."); - while(channel_throttle->get_control_in() > 0) { - hal.scheduler->delay(20); - read_radio(); - } - - while(1) { - hal.scheduler->delay(20); - read_radio(); - - if(channel_throttle->get_control_in() > 0) { - cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->get_control_in()); - fail_test++; - } - - if(oldSwitchPosition != readSwitch()) { - cliSerial->print("CONTROL MODE CHANGED: "); - print_flight_mode(cliSerial, readSwitch()); - cliSerial->println(); - fail_test++; - } - - if(rc_failsafe_active()) { - cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->get_radio_in()); - print_flight_mode(cliSerial, readSwitch()); - cliSerial->println(); - fail_test++; - } - - if(fail_test > 0) { - return (0); - } - if(cliSerial->available() > 0) { - cliSerial->println("LOS caused no change in APM."); - return (0); - } - } -} - -int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - while(1) { - cliSerial->println("Relay on"); - relay.on(0); - hal.scheduler->delay(3000); - if(cliSerial->available() > 0) { - return (0); - } - - cliSerial->println("Relay off"); - relay.off(0); - hal.scheduler->delay(3000); - if(cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv) -{ - hal.scheduler->delay(1000); - - // save the alitude above home option - if (g.RTL_altitude_cm < 0) { - cliSerial->println("Hold current altitude"); - }else{ - cliSerial->printf("Hold altitude of %dm\n", (int)g.RTL_altitude_cm/100); - } - - cliSerial->printf("%d waypoints\n", (int)mission.num_commands()); - cliSerial->printf("Hit radius: %d\n", (int)g.waypoint_radius); - cliSerial->printf("Loiter radius: %d\n\n", (int)g.loiter_radius); - - for(uint8_t i = 0; i <= mission.num_commands(); i++) { - AP_Mission::Mission_Command temp_cmd; - if (mission.read_cmd_from_storage(i,temp_cmd)) { - test_wp_print(temp_cmd); - } - } - - return (0); -} - -void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd) -{ - cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n", - (int)cmd.index, - (int)cmd.id, - (int)cmd.content.location.options, - (int)cmd.p1, - (long)cmd.content.location.alt, - (long)cmd.content.location.lat, - (long)cmd.content.location.lng); -} - -int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - cliSerial->println("Begin XBee X-CTU Range and RSSI Test:"); - - while(1) { - - if (hal.uartC->available()) - hal.uartC->write(hal.uartC->read()); - - if(cliSerial->available() > 0) { - return (0); - } - } -} - - -int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - cliSerial->print("Control CH "); - - cliSerial->println(FLIGHT_MODE_CHANNEL, BASE_DEC); - - while(1) { - hal.scheduler->delay(20); - uint8_t switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition) { - cliSerial->printf("Position %d\n", (int)switchPosition); - oldSwitchPosition = switchPosition; - } - if(cliSerial->available() > 0) { - return (0); - } - } -} - /* * test the dataflash is working */ diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 316c914d75..d5309c3643 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -15,7 +15,7 @@ void QuadPlane::tiltrotor_slew(float newtilt) tilt.current_tilt = constrain_float(newtilt, tilt.current_tilt-max_change, tilt.current_tilt+max_change); // translate to 0..1000 range and output - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_motor_tilt, 1000 * tilt.current_tilt); + SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * tilt.current_tilt); // setup tilt compensation motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&QuadPlane::tilt_compensate, void, float *, uint8_t)); @@ -42,7 +42,7 @@ void QuadPlane::tiltrotor_update(void) // a forward motor tiltrotor_slew(1); - float new_throttle = plane.channel_throttle->get_servo_out()*0.01f; + float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1); if (tilt.current_tilt < 1) { tilt.current_throttle = constrain_float(new_throttle, tilt.current_throttle-max_change, @@ -96,7 +96,7 @@ void QuadPlane::tiltrotor_update(void) // Q_TILT_MAX. Anything above 50% throttle gets // Q_TILT_MAX. Below 50% throttle we decrease linearly. This // relies heavily on Q_VFWD_GAIN being set appropriately. - float settilt = constrain_float(plane.channel_throttle->get_servo_out() / 50.0f, 0, 1); + float settilt = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 50.0f, 0, 1); tiltrotor_slew(settilt * tilt.max_angle_deg / 90.0f); } }