diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 50107cb30f..506eae90da 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -154,7 +154,7 @@ static float get_pilot_desired_climb_rate(float throttle_control) } float desired_rate = 0.0f; - float mid_stick = g.rc_3.get_control_mid(); + float mid_stick = channel_throttle->get_control_mid(); float deadband_top = mid_stick + g.throttle_deadzone; float deadband_bottom = mid_stick - g.throttle_deadzone; @@ -190,7 +190,7 @@ static float get_non_takeoff_throttle() static float get_takeoff_trigger_throttle() { - return g.rc_3.get_control_mid() + g.takeoff_trigger_dz; + return channel_throttle->get_control_mid() + g.takeoff_trigger_dz; } // get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off @@ -213,7 +213,7 @@ static float get_throttle_pre_takeoff(float input_thr) float out_max = get_non_takeoff_throttle(); if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) { - in_min = g.rc_3.get_control_mid(); + in_min = channel_throttle->get_control_mid(); } float input_range = in_max-in_min; diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index fb67827074..593dcf1d06 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -25,10 +25,10 @@ static void acro_run() } // convert the input to the desired body frame rate - get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw); + get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw); // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); + pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); // run attitude controller attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index d8da51a81b..6ad11a8712 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -38,20 +38,20 @@ static void althold_run() // get pilot desired lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch); + get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off - if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { + if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))) { if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 061b132f02..c36250d85a 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -170,7 +170,7 @@ static void auto_wp_run() float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -225,8 +225,8 @@ static void auto_spline_run() // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + // get pilot's desired yaw rat + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index 385c531aff..612ad08c32 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -43,13 +43,13 @@ static void circle_run() // process pilot inputs if (!failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { circle_pilot_yaw_override = true; } // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); // check for pilot requested take-off if (ap.land_complete && target_climb_rate > 0) { diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde index 4fe25c87ed..5e5b9e85f9 100644 --- a/ArduCopter/control_flip.pde +++ b/ArduCopter/control_flip.pde @@ -50,7 +50,7 @@ static bool flip_init(bool ignore_checks) } // ensure roll input is less than 40deg - if (abs(g.rc_1.control_in) >= 4000) { + if (abs(channel_roll->control_in) >= 4000) { return false; } diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index ca856b48dc..ccc1c7b67c 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -231,7 +231,7 @@ static void guided_pos_control_run() float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -261,7 +261,7 @@ static void guided_vel_control_run() float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -300,7 +300,7 @@ static void guided_posvel_control_run() if (!failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 3192cc2247..d8d18230aa 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -47,20 +47,20 @@ static void loiter_run() update_simple_mode(); // process pilot's roll and pitch input - wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in); + wp_nav.set_pilot_desired_acceleration(channel_roll->control_in, channel_pitch->control_in); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off - if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { + if (ap.land_complete && (takeoff_state.running || channel_throttle->control_in > get_takeoff_trigger_throttle())) { if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index 1bcd0a7442..aef4e02ba8 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -156,17 +156,17 @@ static void poshold_run() update_simple_mode(); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot desired climb rate (for alt-hold mode and take-off) - target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off - if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { + if (ap.land_complete && (takeoff_state.running || channel_throttle->control_in > get_takeoff_trigger_throttle())) { if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 4fb2a6f4f1..cdff50461b 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -143,7 +143,7 @@ static void rtl_climb_return_run() float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -199,7 +199,7 @@ static void rtl_loiterathome_run() float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index 0e0d924b99..ee333c3dbc 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -28,7 +28,7 @@ static void sport_run() // if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || ap.throttle_zero) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average); + pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); return; } @@ -61,17 +61,17 @@ static void sport_run() } // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off - if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { + if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))) { if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde index 6f7d0ba5ea..78238d4eec 100644 --- a/ArduCopter/land_detector.pde +++ b/ArduCopter/land_detector.pde @@ -62,7 +62,7 @@ static void update_throttle_thr_mix() { if (mode_has_manual_throttle(control_mode)) { // manual throttle - if(!motors.armed() || g.rc_3.control_in <= 0) { + if(!motors.armed() || channel_throttle->control_in <= 0) { motors.set_throttle_mix_min(); } else { motors.set_throttle_mix_mid(); diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 3a90355920..a69db5b41c 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -719,14 +719,14 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs) // check throttle is not too high - skips checks if arming from GCS in Guided if (!(arming_from_gcs && control_mode == GUIDED)) { // above top of deadband is too always high - if (g.rc_3.control_in > get_takeoff_trigger_throttle()) { + if (channel_throttle->control_in > get_takeoff_trigger_throttle()) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Throttle too high")); } return false; } // in manual modes throttle must be at zero - if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && g.rc_3.control_in > 0) { + if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Throttle too high")); } @@ -822,7 +822,7 @@ static void lost_vehicle_check() } // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch - if (ap.throttle_zero && !motors.armed() && (g.rc_1.control_in > 4000) && (g.rc_2.control_in > 4000)) { + if (ap.throttle_zero && !motors.armed() && (channel_roll->control_in > 4000) && (channel_pitch->control_in > 4000)) { if (soundalarm_counter >= LOST_VEHICLE_DELAY) { if (AP_Notify::flags.vehicle_lost == false) { AP_Notify::flags.vehicle_lost = true; diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde index c162aaaa13..8d29942170 100644 --- a/ArduCopter/switches.pde +++ b/ArduCopter/switches.pde @@ -303,7 +303,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } // do not allow saving the first waypoint with zero throttle - if((mission.num_commands() == 0) && (g.rc_3.control_in == 0)){ + if((mission.num_commands() == 0) && (channel_throttle->control_in == 0)){ return; }