diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index f1c046b069133a..6a1841c3c30e42 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -265,7 +265,7 @@ void Plane::one_second_loop() gcs().send_message(MSG_HEARTBEAT); // make it possible to change control channel ordering at runtime - set_control_channels(); + g2.rc_channels.set_control_channels(); #if HAL_WITH_IO_MCU iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 8d333a1057d3e7..411f49645367f1 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1084,10 +1084,11 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) uint32_t tnow = AP_HAL::millis(); - manual_override(plane.channel_roll, packet.y, 1000, 2000, tnow); - manual_override(plane.channel_pitch, packet.x, 1000, 2000, tnow, true); - manual_override(plane.channel_throttle, packet.z, 0, 1000, tnow); - manual_override(plane.channel_rudder, packet.r, 1000, 2000, tnow); + RC_Channels_Plane &rc = plane.g2.rc_channels; + manual_override(rc.channel_roll, packet.y, 1000, 2000, tnow); + manual_override(rc.channel_pitch, packet.x, 1000, 2000, tnow, true); + manual_override(rc.channel_throttle, packet.z, 0, 1000, tnow); + manual_override(rc.channel_rudder, packet.r, 1000, 2000, tnow); // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes plane.failsafe.last_heartbeat_ms = tnow; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 5af7c81bf871aa..d22492e25c82dc 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -966,7 +966,7 @@ const AP_Param::Info Plane::var_info[] = { // @Group: RCMAP_ // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp - GOBJECT(rcmap, "RCMAP_", RCMapper), + GOBJECT(rcmap_old, "RCMAP_", RCMapper), // @Group: SR0_ // @Path: GCS_Mavlink.cpp @@ -1343,7 +1343,7 @@ void Plane::load_parameters(void) Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old, Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old }; const uint16_t old_aux_chan_mask = 0x3FF0; - SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap); + SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap_old); // possibly convert elevon and vtail mixers convert_mixers(); diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 50745f83f60708..73236bbc93d320 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -323,7 +323,7 @@ class Parameters { k_param_pitchController, k_param_yawController, k_param_L1_controller, - k_param_rcmap, + k_param_rcmap_old, k_param_TECS_controller, k_param_rally_total_old, //unused k_param_steerController, diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index bed8d246fb62b5..8d8ed6b95b0793 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -185,8 +185,8 @@ class Plane : public AP_HAL::HAL::Callbacks { // main loop scheduler AP_Scheduler scheduler; - // mapping between input channels - RCMapper rcmap; + // mapping between input channels; only used for parameter conversion + RCMapper rcmap_old; // board specific config AP_BoardConfig BoardConfig; @@ -197,10 +197,10 @@ class Plane : public AP_HAL::HAL::Callbacks { #endif // primary input channels - RC_Channel *channel_roll; - RC_Channel *channel_pitch; - RC_Channel *channel_throttle; - RC_Channel *channel_rudder; + RC_Channel *&channel_roll = g2.rc_channels.channel_roll; + RC_Channel *&channel_pitch = g2.rc_channels.channel_pitch; + RC_Channel *&channel_throttle = g2.rc_channels.channel_throttle; + RC_Channel *&channel_rudder = g2.rc_channels.channel_rudder; // notification object for LEDs, buzzers etc (parameter set to false disables external leds) AP_Notify notify; @@ -925,7 +925,6 @@ class Plane : public AP_HAL::HAL::Callbacks { void setup_turn_angle(void); bool reached_loiter_target(void); void set_control_channels(void); - void init_rc_in(); void init_rc_out_main(); void init_rc_out_aux(); void rudder_arm_disarm_check(); diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index 5b31e90b003b00..eff8751025bcd7 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -22,6 +22,8 @@ class RC_Channels_Plane : public RC_Channels { public: + void init() override; + RC_Channel_Plane obj_channels[NUM_RC_CHANNELS]; RC_Channel_Plane *channel(const uint8_t chan) override { if (chan > NUM_RC_CHANNELS) { @@ -32,6 +34,13 @@ class RC_Channels_Plane : public RC_Channels bool has_valid_input() const override; + void set_control_channels(void); + + // primary input control channels + RC_Channel *channel_roll; + RC_Channel *channel_pitch; + RC_Channel *channel_rudder; + protected: // note that these callbacks are not presently used on Plane: diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 659639e960ffa1..2738ed5a1865a2 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -65,6 +65,10 @@ class QuadPlane return initialised; } + bool enabled(void) const { + return enable; + } + // is quadplane assisting? bool in_assisted_flight(void) const { return available() && assisted_flight; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 21f89d1e4975f1..3409135b8e7520 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -6,24 +6,24 @@ /* allow for runtime change of control channel ordering */ -void Plane::set_control_channels(void) +void RC_Channels_Plane::set_control_channels(void) { - if (g.rudder_only) { + init_channel(channel_pitch, RC_Channel::AUX_FUNC::PITCH, "Pitch", 2); + init_channel(channel_throttle, RC_Channel::AUX_FUNC::THROTTLE, "Throttle", 3); + init_channel(channel_rudder, RC_Channel::AUX_FUNC::YAW, "Yaw", 4); + if (plane.g.rudder_only) { // in rudder only mode the roll and rudder channels are the // same. - channel_roll = RC_Channels::rc_channel(rcmap.yaw()-1); + channel_roll = channel_rudder; } else { - channel_roll = RC_Channels::rc_channel(rcmap.roll()-1); + init_channel(channel_roll, RC_Channel::AUX_FUNC::ROLL, "Roll", 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); channel_pitch->set_angle(SERVO_MAX); channel_rudder->set_angle(SERVO_MAX); - if (!have_reverse_thrust()) { + if (!plane.have_reverse_thrust()) { // normal operation channel_throttle->set_range(100); } else { @@ -40,23 +40,27 @@ void Plane::set_control_channels(void) SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 100); } - if (!arming.is_armed() && arming.arming_required() == AP_Arming::Required::YES_MIN_PWM) { - SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); + if (!plane.arming.is_armed() && plane.arming.arming_required() == AP_Arming::YES_MIN_PWM) { + SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, plane.have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } - if (!quadplane.enable) { + if (!plane.quadplane.enabled()) { // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. For quadplanes we use AP_Motors // scaling - g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle); + plane.g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle); } } /* initialise RC input channels */ -void Plane::init_rc_in() +void RC_Channels_Plane::init() { + RC_Channels::init(); + + set_control_channels(); + // set rc dead zones channel_roll->set_default_dead_zone(30); channel_pitch->set_default_dead_zone(30); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 97ece4d2eb90cc..41df6a3e06a7e4 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -48,7 +48,8 @@ void Plane::init_ardupilot() ins.set_log_raw_bit(MASK_LOG_IMU_RAW); - set_control_channels(); + // initialise rc channels including setting mode + g2.rc_channels.init(); mavlink_system.sysid = g.sysid_this_mav; @@ -67,9 +68,6 @@ void Plane::init_ardupilot() BoardConfig_CAN.init(); #endif - // initialise rc channels including setting mode - rc().init(); - relay.init(); // initialise notify system @@ -123,8 +121,6 @@ void Plane::init_ardupilot() gps.set_log_gps_bit(MASK_LOG_GPS); gps.init(serial_manager); - init_rc_in(); // sets up rc channels from radio - #if MOUNT == ENABLED // initialise camera mount camera_mount.init(serial_manager);