diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 02884aab63..d802efe651 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -735,7 +735,7 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); - update_aux_servo_function(); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); #if CAMERA == ENABLED camera_mount.update_mount_type(); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index fff75fda4f..5c5b01b10f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,19 +41,6 @@ #define GPS_PROTOCOL_MTK16 6 #define GPS_PROTOCOL_AUTO 7 -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 4f7292879d..6311f63e3e 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -195,7 +195,7 @@ static void trim_control_surfaces() g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel }else{ elevon1_trim = ch1_temp; @@ -212,7 +212,7 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } static void trim_radio() @@ -228,7 +228,7 @@ static void trim_radio() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -244,30 +244,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); -} - -// update the g_rc_function array from pointers to rc_x channels -// This should be done periodically because the user might change the configuration and -// expects the changes to take effect instantly -static void update_aux_servo_function() -{ - // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function - RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos - aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)g.rc_5.function.get(); - aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)g.rc_6.function.get(); - aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)g.rc_7.function.get(); - aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)g.rc_8.function.get(); - - // Assume that no auxiliary function is used - for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) - { - g_rc_function[i] = NULL; - } - - // assign the RC channel to each function - g_rc_function[aux_servo_function[CH_5]] = &g.rc_5; - g_rc_function[aux_servo_function[CH_6]] = &g.rc_6; - g_rc_function[aux_servo_function[CH_7]] = &g.rc_7; - g_rc_function[aux_servo_function[CH_8]] = &g.rc_8; + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 236f85593d..bcac692abe 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,6 +5,17 @@ #define MIN_PULSEWIDTH 900 #define MAX_PULSEWIDTH 2100 +// Radio channels +// Note channels are from 0! +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + #include class APM_RC_Class diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index f6ceb5fed5..caa7affb61 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -3,6 +3,8 @@ #include #include "RC_Channel_aux.h" +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function + int16_t RC_Channel_aux::closest_limit(int16_t angle) { @@ -67,3 +69,28 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) APM_RC.OutputCh(ch_nr, radio_out); } + +// update the g_rc_function array from pointers to rc_x channels +// This should be done periodically because the user might change the configuration and +// expects the changes to take effect instantly +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8) +{ + // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function + RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos + aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)rc_5->function.get(); + aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)rc_6->function.get(); + aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)rc_7->function.get(); + aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)rc_8->function.get(); + + // Assume that no auxiliary function is used + for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) + { + g_rc_function[i] = NULL; + } + + // assign the RC channel to each function + g_rc_function[aux_servo_function[CH_5]] = rc_5; + g_rc_function[aux_servo_function[CH_6]] = rc_6; + g_rc_function[aux_servo_function[CH_7]] = rc_7; + g_rc_function[aux_servo_function[CH_8]] = rc_8; +} diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 7e7fd4f167..8fae791fdc 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /// @file RC_Channel_aux.h -/// @brief RC_Channel manager, with EEPROM-backed storage of constants. +/// @brief RC_Channel manager for Channels 5..8, with EEPROM-backed storage of constants. /// @author Amilcar Lucas #ifndef RC_CHANNEL_AUX_H_ @@ -53,4 +53,6 @@ public: }; +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8); + #endif /* RC_CHANNEL_AUX_H_ */