diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index ee7ac3d36b..f1ea6c2091 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -958,6 +958,9 @@ static void one_second_loop() // send a heartbeat gcs_send_message(MSG_HEARTBEAT); + + // make it possible to change control channel ordering at runtime + set_control_channels(); } static void update_GPS(void) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 1d8bb4835f..48b2a17c31 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -4,7 +4,20 @@ // ---------------------------------------------------------------------------- static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +/* + allow for runtime change of control channel ordering + */ +static void set_control_channels(void) +{ + channel_roll = RC_Channel::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); +} +/* + initialise RC input channels + */ static void init_rc_in() { // set rc channel ranges @@ -34,6 +47,9 @@ static void init_rc_in() #endif } +/* + initialise RC output channels + */ static void init_rc_out() { channel_roll->enable_out(); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index b520adb538..f8146c4d4a 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -110,11 +110,7 @@ static void init_ardupilot() // load_parameters(); - // setup channel mappings. Changing these requires a reboot - channel_roll = RC_Channel::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); + set_control_channels(); // reset the uartA baud rate after parameter load hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD));