diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 6311f63e3e..ed3d1782b2 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -23,46 +23,26 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - if (g_rc_function[RC_Channel_aux::k_flap]) { - g_rc_function[RC_Channel_aux::k_flap]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_flap_auto]) { - g_rc_function[RC_Channel_aux::k_flap_auto]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_aileron]) { - g_rc_function[RC_Channel_aux::k_aileron]->set_angle(SERVO_MAX); - } - if (g_rc_function[RC_Channel_aux::k_flaperon]) { - g_rc_function[RC_Channel_aux::k_flaperon]->set_range(0,100); - } + G_RC_AUX(k_flap)->set_range(0,100); + G_RC_AUX(k_flap_auto)->set_range(0,100); + G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); + G_RC_AUX(k_flaperon)->set_range(0,100); #if CAMERA == ENABLED - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) { - g_rc_function[RC_Channel_aux::k_mount_yaw]->set_range( + G_RC_AUX(k_mount_yaw)->set_range( g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) { - g_rc_function[RC_Channel_aux::k_mount_pitch]->set_range( + G_RC_AUX(k_mount_pitch)->set_range( g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_mount_roll]) { - g_rc_function[RC_Channel_aux::k_mount_roll]->set_range( + G_RC_AUX(k_mount_roll)->set_range( g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) { - g_rc_function[RC_Channel_aux::k_cam_trigger]->set_range( + G_RC_AUX(k_cam_trigger)->set_range( g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_cam_open]) { - g_rc_function[RC_Channel_aux::k_cam_open]->set_range(0,100); - } + G_RC_AUX(k_cam_open)->set_range(0,100); #endif - if (g_rc_function[RC_Channel_aux::k_egg_drop]) { - g_rc_function[RC_Channel_aux::k_egg_drop]->set_range(0,100); - } + G_RC_AUX(k_egg_drop)->set_range(0,100); } static void init_rc_out() @@ -195,7 +175,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]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + G_RC_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 +192,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]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } static void trim_radio() @@ -228,7 +208,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]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -244,5 +224,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]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 6fed9b86cb..ddfbc2d8a3 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -98,10 +98,7 @@ AP_Camera::trigger_pic_cleanup() case 0: case 2: case 3: - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) - { - g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; - } + G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; break; case 1: // TODO for some strange reason the function call bellow gives a linker error diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index bd705da8f8..6537c507a0 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef AP_DCM_h #define AP_DCM_h -// teporarily include all other classes here +// temporarily include all other classes here // since this naming is a bit off from the // convention and the AP_DCM should be the top // header file diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6cf76c9351..268533ee3b 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -156,12 +156,9 @@ void AP_Mount::update_mount() // write the results to the servos // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - if (g_rc_function[RC_Channel_aux::k_mount_roll]) - g_rc_function[RC_Channel_aux::k_mount_roll]->closest_limit(roll_angle/10); - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) - g_rc_function[RC_Channel_aux::k_mount_pitch]->closest_limit(pitch_angle/10); - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) - g_rc_function[RC_Channel_aux::k_mount_yaw]->closest_limit(yaw_angle/10); + G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); + G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); + G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); } void AP_Mount::set_mode(MountMode mode) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 4ff5990bbf..132e9146f3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -102,6 +102,7 @@ class RC_Channel{ int16_t _low; }; +// This is ugly, but it fixes compilation on arduino #include "RC_Channel_aux.h" #endif diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 8fae791fdc..6d8c228472 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -9,6 +9,9 @@ #include "RC_Channel.h" +// Macro to simplify accessing the auxiliary servos +#define G_RC_AUX(_t) if (g_rc_function[RC_Channel_aux::_t]) g_rc_function[RC_Channel_aux::_t] + /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function /// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements