From fb9bf215221adb67171c52cb80cde0e9e1ad61bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Jun 2013 15:32:08 +1000 Subject: [PATCH] Plane: allow channel mapping of first 4 channels this makes it easier to support DSM and SBUS radios --- ArduPlane/ArduPlane.pde | 28 +++++-- ArduPlane/Attitude.pde | 172 +++++++++++++++++++------------------- ArduPlane/GCS_Mavlink.pde | 10 +-- ArduPlane/Log.pde | 6 +- ArduPlane/Parameters.h | 1 + ArduPlane/Parameters.pde | 5 ++ ArduPlane/failsafe.pde | 20 ++--- ArduPlane/navigation.pde | 2 +- ArduPlane/radio.pde | 90 ++++++++++---------- ArduPlane/setup.pde | 52 ++++++------ ArduPlane/system.pde | 6 ++ ArduPlane/test.pde | 34 ++++---- 12 files changed, 224 insertions(+), 202 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 5365f629a0..ee7ac3d36b 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -56,6 +56,7 @@ #include #include +#include // RC input mapping library // Pre-AP_HAL compatibility #include "compat.h" @@ -98,6 +99,15 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor: // static Parameters g; +// mapping between input channels +static RCMapper rcmap; + +// primary control channels +static RC_Channel *channel_roll; +static RC_Channel *channel_pitch; +static RC_Channel *channel_throttle; +static RC_Channel *channel_rudder; + //////////////////////////////////////////////////////////////////////////////// // prototypes static void update_events(void); @@ -1036,7 +1046,7 @@ static void update_current_flight_mode(void) } // max throttle for takeoff - g.channel_throttle.servo_out = g.throttle_max; + channel_throttle->servo_out = g.throttle_max; break; @@ -1063,7 +1073,7 @@ static void update_current_flight_mode(void) if (land_complete) { // we are in the final stage of a landing - force // zero throttle - g.channel_throttle.servo_out = 0; + channel_throttle->servo_out = 0; } break; @@ -1123,9 +1133,9 @@ static void update_current_flight_mode(void) case FLY_BY_WIRE_A: { // set nav_roll and nav_pitch using sticks - nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd; + nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd, g.roll_limit_cd); - float pitch_input = g.channel_pitch.norm_input(); + float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { nav_pitch_cd = pitch_input * g.pitch_limit_max_cd; } else { @@ -1146,10 +1156,10 @@ static void update_current_flight_mode(void) // Thanks to Yury MonZon for the altitude limit code! - nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd; + nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd; float elevator_input; - elevator_input = g.channel_pitch.norm_input(); + elevator_input = channel_pitch->norm_input(); if (g.flybywire_elev_reverse) { elevator_input = -elevator_input; @@ -1195,9 +1205,9 @@ static void update_current_flight_mode(void) case MANUAL: // servo_out is for Sim control only // --------------------------------- - g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); - g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); - g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); + channel_roll->servo_out = channel_roll->pwm_to_angle(); + channel_pitch->servo_out = channel_pitch->pwm_to_angle(); + channel_rudder->servo_out = channel_rudder->pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 42016f768f..23f8d7c8d8 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -21,8 +21,8 @@ static float get_speed_scaler(void) } speed_scaler = constrain_float(speed_scaler, 0.5, 2.0); } else { - if (g.channel_throttle.servo_out > 0) { - speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root + if (channel_throttle->servo_out > 0) { + speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / channel_throttle->servo_out / 2.0); // First order taylor expansion of square root // Should maybe be to the 2/7 power, but we aren't goint to implement that... }else{ speed_scaler = 1.67; @@ -44,7 +44,7 @@ static bool stick_mixing_enabled(void) geofence_stickmixing() && failsafe == FAILSAFE_NONE && (g.throttle_fs_enabled == 0 || - g.channel_throttle.radio_in >= g.throttle_fs_value)) { + channel_throttle->radio_in >= g.throttle_fs_value)) { // we're in an auto mode, and haven't triggered failsafe return true; } else { @@ -73,7 +73,7 @@ static void stabilize_roll(float speed_scaler) if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; } - g.channel_roll.servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min); + channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min); } /* @@ -83,8 +83,8 @@ static void stabilize_roll(float speed_scaler) */ static void stabilize_pitch(float speed_scaler) { - int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + g.channel_throttle.servo_out * g.kff_throttle_to_pitch; - g.channel_pitch.servo_out = g.pitchController.get_servo_out(demanded_pitch, + int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch; + channel_pitch->servo_out = g.pitchController.get_servo_out(demanded_pitch, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min, g.flybywire_airspeed_max); @@ -105,25 +105,25 @@ static void stabilize_stick_mixing_direct() float ch1_inf; float ch2_inf; - ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; + ch1_inf = (float)channel_roll->radio_in - (float)channel_roll->radio_trim; ch1_inf = fabsf(ch1_inf); ch1_inf = min(ch1_inf, 400.0); ch1_inf = ((400.0 - ch1_inf) /400.0); - ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim; + ch2_inf = (float)channel_pitch->radio_in - channel_pitch->radio_trim; ch2_inf = fabsf(ch2_inf); ch2_inf = min(ch2_inf, 400.0); ch2_inf = ((400.0 - ch2_inf) /400.0); // scale the sensor input based on the stick input // ----------------------------------------------- - g.channel_roll.servo_out *= ch1_inf; - g.channel_pitch.servo_out *= ch2_inf; + channel_roll->servo_out *= ch1_inf; + channel_pitch->servo_out *= ch2_inf; // Mix in stick inputs // ------------------- - g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); - g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle(); + channel_roll->servo_out += channel_roll->pwm_to_angle(); + channel_pitch->servo_out += channel_pitch->pwm_to_angle(); } /* @@ -144,7 +144,7 @@ static void stabilize_stick_mixing_fbw() // non-linear and ends up as 2x the maximum, to ensure that // the user can direct the plane in any direction with stick // mixing. - float roll_input = g.channel_roll.norm_input(); + float roll_input = channel_roll->norm_input(); if (roll_input > 0.5f) { roll_input = (3*roll_input - 1); } else if (roll_input < -0.5f) { @@ -153,7 +153,7 @@ static void stabilize_stick_mixing_fbw() nav_roll_cd += roll_input * g.roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get()); - float pitch_input = g.channel_pitch.norm_input(); + float pitch_input = channel_pitch->norm_input(); if (fabsf(pitch_input) > 0.5f) { pitch_input = (3*pitch_input - 1); } @@ -180,7 +180,7 @@ static void stabilize_yaw(float speed_scaler) // stick mixing performed for rudder for all cases including FBW // important for steering on the ground during landing // ----------------------------------------------- - ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; + ch4_inf = (float)channel_rudder->radio_in - (float)channel_rudder->radio_trim; ch4_inf = fabsf(ch4_inf); ch4_inf = min(ch4_inf, 400.0); ch4_inf = ((400.0 - ch4_inf) /400.0); @@ -188,8 +188,8 @@ static void stabilize_yaw(float speed_scaler) // Apply output to Rudder calc_nav_yaw(speed_scaler, ch4_inf); - g.channel_rudder.servo_out *= ch4_inf; - g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle(); + channel_rudder->servo_out *= ch4_inf; + channel_rudder->servo_out += channel_rudder->pwm_to_angle(); } @@ -199,25 +199,25 @@ static void stabilize_yaw(float speed_scaler) static void stabilize_training(float speed_scaler) { if (training_manual_roll) { - g.channel_roll.servo_out = g.channel_roll.control_in; + channel_roll->servo_out = channel_roll->control_in; } else { // calculate what is needed to hold stabilize_roll(speed_scaler); - if ((nav_roll_cd > 0 && g.channel_roll.control_in < g.channel_roll.servo_out) || - (nav_roll_cd < 0 && g.channel_roll.control_in > g.channel_roll.servo_out)) { + if ((nav_roll_cd > 0 && channel_roll->control_in < channel_roll->servo_out) || + (nav_roll_cd < 0 && channel_roll->control_in > channel_roll->servo_out)) { // allow user to get out of the roll - g.channel_roll.servo_out = g.channel_roll.control_in; + channel_roll->servo_out = channel_roll->control_in; } } if (training_manual_pitch) { - g.channel_pitch.servo_out = g.channel_pitch.control_in; + channel_pitch->servo_out = channel_pitch->control_in; } else { stabilize_pitch(speed_scaler); - if ((nav_pitch_cd > 0 && g.channel_pitch.control_in < g.channel_pitch.servo_out) || - (nav_pitch_cd < 0 && g.channel_pitch.control_in > g.channel_pitch.servo_out)) { + if ((nav_pitch_cd > 0 && channel_pitch->control_in < channel_pitch->servo_out) || + (nav_pitch_cd < 0 && channel_pitch->control_in > channel_pitch->servo_out)) { // allow user to get back to level - g.channel_pitch.servo_out = g.channel_pitch.control_in; + channel_pitch->servo_out = channel_pitch->control_in; } } @@ -249,7 +249,7 @@ static void stabilize() /* see if we should zero the attitude controller integrators. */ - if (g.channel_throttle.control_in == 0 && + if (channel_throttle->control_in == 0 && abs(current_loc.alt - home.alt) < 500 && fabs(barometer.get_climb_rate()) < 0.5f && g_gps->ground_speed < 300) { @@ -269,7 +269,7 @@ static void calc_throttle() // user has asked for zero throttle - this may be done by a // mission which wants to turn off the engine for a parachute // landing - g.channel_throttle.servo_out = 0; + channel_throttle->servo_out = 0; return; } @@ -284,22 +284,22 @@ static void calc_throttle() // AUTO, RTL, etc // --------------------------------------------------------------------------- if (nav_pitch_cd >= 0) { - g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch_cd / g.pitch_limit_max_cd; + channel_throttle->servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch_cd / g.pitch_limit_max_cd; } else { - g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch_cd / g.pitch_limit_min_cd; + channel_throttle->servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch_cd / g.pitch_limit_min_cd; } - g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); + channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, g.throttle_min.get(), g.throttle_max.get()); } else { // throttle control with airspeed compensation // ------------------------------------------- energy_error = airspeed_energy_error + altitude_error_cm * 0.098f; // positive energy errors make the throttle go higher - g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error); - g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle); + channel_throttle->servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error); + channel_throttle->servo_out += (channel_pitch->servo_out * g.kff_pitch_to_throttle); - g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, + channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, g.throttle_min.get(), g.throttle_max.get()); } @@ -316,19 +316,19 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf) if (hold_course_cd != -1) { // steering on or close to ground int32_t bearing_error_cd = nav_controller->bearing_error_cd(); - g.channel_rudder.servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) + - g.kff_rudder_mix * g.channel_roll.servo_out; - g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500); + channel_rudder->servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) + + g.kff_rudder_mix * channel_roll->servo_out; + channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500); return; } - g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler, + channel_rudder->servo_out = g.yawController.get_servo_out(speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min, g.flybywire_airspeed_max); // add in rudder mixing from roll - g.channel_rudder.servo_out += g.channel_roll.servo_out * g.kff_rudder_mix; - g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500); + channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix; + channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500); } @@ -372,12 +372,12 @@ static void throttle_slew_limit(int16_t last_throttle) // if slew limit rate is set to zero then do not slew limit if (g.throttle_slewrate) { // limit throttle change by the given percentage per second - float temp = g.throttle_slewrate * G_Dt * 0.01 * fabsf(g.channel_throttle.radio_max - g.channel_throttle.radio_min); + float temp = g.throttle_slewrate * G_Dt * 0.01 * fabsf(channel_throttle->radio_max - channel_throttle->radio_min); // allow a minimum change of 1 PWM per cycle if (temp < 1) { temp = 1; } - g.channel_throttle.radio_out = constrain_int16(g.channel_throttle.radio_out, last_throttle - temp, last_throttle + temp); + channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp); } } @@ -516,19 +516,19 @@ static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_ *****************************************/ static void set_servos(void) { - int16_t last_throttle = g.channel_throttle.radio_out; + int16_t last_throttle = channel_throttle->radio_out; if (control_mode == MANUAL) { // do a direct pass through of radio values if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) { - g.channel_roll.radio_out = g.channel_roll.radio_in; - g.channel_pitch.radio_out = g.channel_pitch.radio_in; + channel_roll->radio_out = channel_roll->radio_in; + channel_pitch->radio_out = channel_pitch->radio_in; } else { - g.channel_roll.radio_out = hal.rcin->read(CH_ROLL); - g.channel_pitch.radio_out = hal.rcin->read(CH_PITCH); + channel_roll->radio_out = hal.rcin->read(CH_ROLL); + channel_pitch->radio_out = hal.rcin->read(CH_PITCH); } - g.channel_throttle.radio_out = g.channel_throttle.radio_in; - g.channel_rudder.radio_out = g.channel_rudder.radio_in; + channel_throttle->radio_out = channel_throttle->radio_in; + channel_rudder->radio_out = channel_rudder->radio_in; // setup extra aileron channel. We want this to come from the // main aileron input channel, but using the 2nd channels dead @@ -536,11 +536,11 @@ static void set_servos(void) // pwm_to_angle_dz() to ensure we don't trim the value for the // deadzone of the main aileron channel, otherwise the 2nd // aileron won't quite follow the first one - int16_t aileron_in = g.channel_roll.pwm_to_angle_dz(0); + int16_t aileron_in = channel_roll->pwm_to_angle_dz(0); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, aileron_in); // setup extra elevator channel following the same logic - int16_t elevator_in = g.channel_pitch.pwm_to_angle_dz(0); + int16_t elevator_in = channel_pitch->pwm_to_angle_dz(0); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, elevator_in); // this aileron variant assumes you have the corresponding @@ -557,24 +557,24 @@ static void set_servos(void) if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) { // set any differential spoilers to follow the elevons in // manual mode. - RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, g.channel_roll.radio_out); - RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, g.channel_pitch.radio_out); + RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->radio_out); + RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->radio_out); } } else { if (g.mix_mode == 0) { // both types of secondary aileron are slaved to the roll servo out - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, g.channel_roll.servo_out); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, g.channel_roll.servo_out); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->servo_out); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out); // both types of secondary elevator are slaved to the pitch servo out - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, g.channel_pitch.servo_out); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, g.channel_pitch.servo_out); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out); + RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out); }else{ /*Elevon mode*/ float ch1; float ch2; - ch1 = g.channel_pitch.servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * g.channel_roll.servo_out); - ch2 = g.channel_pitch.servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * g.channel_roll.servo_out); + ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); + ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); /* Differential Spoilers If differential spoilers are setup, then we translate @@ -585,20 +585,20 @@ static void set_servos(void) if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) { float ch3 = ch1; float ch4 = ch2; - if ( BOOL_TO_SIGN(g.reverse_elevons) * g.channel_rudder.servo_out < 0) { - ch1 += abs(g.channel_rudder.servo_out); - ch3 -= abs(g.channel_rudder.servo_out); + if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) { + ch1 += abs(channel_rudder->servo_out); + ch3 -= abs(channel_rudder->servo_out); } else { - ch2 += abs(g.channel_rudder.servo_out); - ch4 -= abs(g.channel_rudder.servo_out); + ch2 += abs(channel_rudder->servo_out); + ch4 -= abs(channel_rudder->servo_out); } RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4); } // directly set the radio_out values for elevon mode - g.channel_roll.radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); - g.channel_pitch.radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); + channel_roll->radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); + channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); } #if OBC_FAILSAFE == ENABLED @@ -606,37 +606,37 @@ static void set_servos(void) // the plane. Only used in extreme circumstances to meet the // OBC rules if (obc.crash_plane()) { - g.channel_roll.servo_out = -4500; - g.channel_pitch.servo_out = -4500; - g.channel_rudder.servo_out = -4500; - g.channel_throttle.servo_out = 0; + channel_roll->servo_out = -4500; + channel_pitch->servo_out = -4500; + channel_rudder->servo_out = -4500; + channel_throttle->servo_out = 0; } #endif // push out the PWM values if (g.mix_mode == 0) { - g.channel_roll.calc_pwm(); - g.channel_pitch.calc_pwm(); + channel_roll->calc_pwm(); + channel_pitch->calc_pwm(); } - g.channel_rudder.calc_pwm(); + channel_rudder->calc_pwm(); #if THROTTLE_OUT == 0 - g.channel_throttle.servo_out = 0; + channel_throttle->servo_out = 0; #else // convert 0 to 100% into PWM - g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, + channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, g.throttle_min.get(), g.throttle_max.get()); if (suppress_throttle()) { // throttle is suppressed in auto mode - g.channel_throttle.servo_out = 0; + channel_throttle->servo_out = 0; if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed - g.channel_throttle.radio_out = g.channel_throttle.radio_in; + channel_throttle->radio_out = channel_throttle->radio_in; } else { - g.channel_throttle.calc_pwm(); + channel_throttle->calc_pwm(); } } else if (g.throttle_passthru_stabilize && (control_mode == STABILIZE || @@ -644,10 +644,10 @@ static void set_servos(void) control_mode == FLY_BY_WIRE_A)) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set - g.channel_throttle.radio_out = g.channel_throttle.radio_in; + channel_throttle->radio_out = channel_throttle->radio_in; } else { // normal throttle calculation based on servo_out - g.channel_throttle.calc_pwm(); + channel_throttle->calc_pwm(); } #endif } @@ -683,7 +683,7 @@ static void set_servos(void) if (control_mode == TRAINING) { // copy rudder in training mode - g.channel_rudder.radio_out = g.channel_rudder.radio_in; + channel_rudder->radio_out = channel_rudder->radio_in; } #if HIL_MODE != HIL_MODE_DISABLED @@ -693,17 +693,17 @@ static void set_servos(void) #endif if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, g.channel_pitch.radio_out, g.channel_rudder.radio_out); + channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, g.channel_pitch.radio_out, g.channel_roll.radio_out); + channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); } // send values to the PWM timers for output // ---------------------------------------- - hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos - hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos - hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos - hal.rcout->write(CH_4, g.channel_rudder.radio_out); // send to Servos + hal.rcout->write(CH_1, channel_roll->radio_out); // send to Servos + hal.rcout->write(CH_2, channel_pitch->radio_out); // send to Servos + hal.rcout->write(CH_3, channel_throttle->radio_out); // send to Servos + hal.rcout->write(CH_4, channel_rudder->radio_out); // send to Servos // Route configurable aux. functions to their respective servos g.rc_5.output_ch(CH_5); g.rc_6.output_ch(CH_6); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 84e2398b9d..54ed153c7c 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -308,10 +308,10 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) chan, millis(), 0, // port 0 - 10000 * g.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.norm_output(), + 10000 * channel_roll->norm_output(), + 10000 * channel_pitch->norm_output(), + 10000 * channel_throttle->norm_output(), + 10000 * channel_rudder->norm_output(), 0, 0, 0, @@ -377,7 +377,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) } else if (!ahrs.airspeed_estimate(&aspeed)) { aspeed = 0; } - float throttle_norm = g.channel_throttle.norm_output() * 100; + float throttle_norm = channel_throttle->norm_output() * 100; throttle_norm = constrain_int16(throttle_norm, -100, 100); uint16_t throttle = ((uint16_t)(throttle_norm + 100)) / 2; mavlink_msg_vfr_hud_send( diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 2bd019d037..5bffca8bb3 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -312,8 +312,8 @@ static void Log_Write_Control_Tuning() roll : (int16_t)ahrs.roll_sensor, nav_pitch_cd : (int16_t)nav_pitch_cd, pitch : (int16_t)ahrs.pitch_sensor, - throttle_out : (int16_t)g.channel_throttle.servo_out, - rudder_out : (int16_t)g.channel_rudder.servo_out, + throttle_out : (int16_t)channel_throttle->servo_out, + rudder_out : (int16_t)channel_rudder->servo_out, accel_y : accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -374,7 +374,7 @@ static void Log_Write_Current() { struct log_Current pkt = { LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), - throttle_in : g.channel_throttle.control_in, + throttle_in : channel_throttle->control_in, battery_voltage : (int16_t)(battery.voltage * 100.0), current_amps : (int16_t)(battery.current_amps * 100.0), board_voltage : board_voltage(), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index fe3cc5d79e..177cc5ae4c 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -228,6 +228,7 @@ public: k_param_pitchController, k_param_yawController, k_param_L1_controller, + k_param_rcmap, // // 240: PID Controllers diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 45229063fa..b04e73db0d 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -774,6 +774,11 @@ const AP_Param::Info var_info[] PROGMEM = { // @Group: COMPASS_ // @Path: ../libraries/AP_Compass/Compass.cpp GOBJECT(compass, "COMPASS_", Compass), + + // @Group: RCMAP_ + // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp + GOBJECT(rcmap, "RCMAP_", RCMapper), + GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK), diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde index 05851abc6e..3f04c2c026 100644 --- a/ArduPlane/failsafe.pde +++ b/ArduPlane/failsafe.pde @@ -39,21 +39,21 @@ void failsafe_check(uint32_t tnow) // pass RC inputs to outputs every 20ms last_timestamp = tnow; hal.rcin->clear_overrides(); - g.channel_roll.radio_out = hal.rcin->read(CH_1); - g.channel_pitch.radio_out = hal.rcin->read(CH_2); - g.channel_throttle.radio_out = hal.rcin->read(CH_3); - g.channel_rudder.radio_out = hal.rcin->read(CH_4); + channel_roll->radio_out = hal.rcin->read(CH_1); + channel_pitch->radio_out = hal.rcin->read(CH_2); + channel_throttle->radio_out = hal.rcin->read(CH_3); + channel_rudder->radio_out = hal.rcin->read(CH_4); if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, g.channel_pitch.radio_out, g.channel_rudder.radio_out); + channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, g.channel_pitch.radio_out, g.channel_roll.radio_out); + channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); } if (!demoing_servos) { - servo_write(CH_1, g.channel_roll.radio_out); + servo_write(CH_1, channel_roll->radio_out); } - servo_write(CH_2, g.channel_pitch.radio_out); - servo_write(CH_3, g.channel_throttle.radio_out); - servo_write(CH_4, g.channel_rudder.radio_out); + servo_write(CH_2, channel_pitch->radio_out); + servo_write(CH_3, channel_throttle->radio_out); + servo_write(CH_4, channel_rudder->radio_out); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true); diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 064cff6fd7..9b8a9066c5 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -87,7 +87,7 @@ static void calc_airspeed_errors() if (control_mode == FLY_BY_WIRE_B) { target_airspeed_cm = ((int)(g.flybywire_airspeed_max - g.flybywire_airspeed_min) * - g.channel_throttle.servo_out) + + channel_throttle->servo_out) + ((int)g.flybywire_airspeed_min * 100); } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 12dcf10b1e..b93e74a75b 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -8,21 +8,21 @@ static uint8_t failsafeCounter = 0; // we wait a second to take o static void init_rc_in() { // set rc channel ranges - g.channel_roll.set_angle(SERVO_MAX); - g.channel_pitch.set_angle(SERVO_MAX); - g.channel_rudder.set_angle(SERVO_MAX); - g.channel_throttle.set_range(0, 100); + channel_roll->set_angle(SERVO_MAX); + channel_pitch->set_angle(SERVO_MAX); + channel_rudder->set_angle(SERVO_MAX); + channel_throttle->set_range(0, 100); // set rc dead zones - g.channel_roll.set_dead_zone(60); - g.channel_pitch.set_dead_zone(60); - g.channel_rudder.set_dead_zone(60); - g.channel_throttle.set_dead_zone(6); + channel_roll->set_dead_zone(60); + channel_pitch->set_dead_zone(60); + channel_rudder->set_dead_zone(60); + channel_throttle->set_dead_zone(6); - //g.channel_roll.dead_zone = 60; - //g.channel_pitch.dead_zone = 60; - //g.channel_rudder.dead_zone = 60; - //g.channel_throttle.dead_zone = 6; + //channel_roll->dead_zone = 60; + //channel_pitch->dead_zone = 60; + //channel_rudder->dead_zone = 60; + //channel_throttle->dead_zone = 6; //set auxiliary ranges #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 @@ -43,10 +43,10 @@ static void init_rc_out() enable_aux_servos(); // Initialization of servo outputs - servo_write(CH_1, g.channel_roll.radio_trim); - servo_write(CH_2, g.channel_pitch.radio_trim); - servo_write(CH_3, g.channel_throttle.radio_min); - servo_write(CH_4, g.channel_rudder.radio_trim); + servo_write(CH_1, channel_roll->radio_trim); + servo_write(CH_2, channel_pitch->radio_trim); + servo_write(CH_3, channel_throttle->radio_min); + servo_write(CH_4, channel_rudder->radio_trim); servo_write(CH_5, g.rc_5.radio_trim); servo_write(CH_6, g.rc_6.radio_trim); @@ -80,15 +80,15 @@ static void read_radio() if (control_mode == TRAINING) { // in training mode we don't want to use a deadzone, as we // want manual pass through when not exceeding attitude limits - g.channel_roll.set_pwm_no_deadzone(pwm_roll); - g.channel_pitch.set_pwm_no_deadzone(pwm_pitch); - g.channel_throttle.set_pwm_no_deadzone(hal.rcin->read(CH_3)); - g.channel_rudder.set_pwm_no_deadzone(hal.rcin->read(CH_4)); + channel_roll->set_pwm_no_deadzone(pwm_roll); + channel_pitch->set_pwm_no_deadzone(pwm_pitch); + channel_throttle->set_pwm_no_deadzone(hal.rcin->read(CH_3)); + channel_rudder->set_pwm_no_deadzone(hal.rcin->read(CH_4)); } else { - g.channel_roll.set_pwm(pwm_roll); - g.channel_pitch.set_pwm(pwm_pitch); - g.channel_throttle.set_pwm(hal.rcin->read(CH_3)); - g.channel_rudder.set_pwm(hal.rcin->read(CH_4)); + channel_roll->set_pwm(pwm_roll); + channel_pitch->set_pwm(pwm_pitch); + channel_throttle->set_pwm(hal.rcin->read(CH_3)); + channel_rudder->set_pwm(hal.rcin->read(CH_4)); } g.rc_5.set_pwm(hal.rcin->read(CH_5)); @@ -96,12 +96,12 @@ static void read_radio() g.rc_7.set_pwm(hal.rcin->read(CH_7)); g.rc_8.set_pwm(hal.rcin->read(CH_8)); - control_failsafe(g.channel_throttle.radio_in); + control_failsafe(channel_throttle->radio_in); - g.channel_throttle.servo_out = g.channel_throttle.control_in; + channel_throttle->servo_out = channel_throttle->control_in; - if (g.throttle_nudge && g.channel_throttle.servo_out > 50) { - float nudge = (g.channel_throttle.servo_out - 50) * 0.02; + if (g.throttle_nudge && channel_throttle->servo_out > 50) { + float nudge = (channel_throttle->servo_out - 50) * 0.02; if (alt_control_airspeed()) { airspeed_nudge_cm = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; } else { @@ -167,12 +167,12 @@ static void control_failsafe(uint16_t pwm) static void trim_control_surfaces() { read_radio(); - int16_t trim_roll_range = (g.channel_roll.radio_max - g.channel_roll.radio_min)/5; - int16_t trim_pitch_range = (g.channel_pitch.radio_max - g.channel_pitch.radio_min)/5; - if (g.channel_roll.radio_in < g.channel_roll.radio_min+trim_roll_range || - g.channel_roll.radio_in > g.channel_roll.radio_max-trim_roll_range || - g.channel_pitch.radio_in < g.channel_pitch.radio_min+trim_pitch_range || - g.channel_pitch.radio_in > g.channel_pitch.radio_max-trim_pitch_range) { + int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5; + int16_t trim_pitch_range = (channel_pitch->radio_max - channel_pitch->radio_min)/5; + if (channel_roll->radio_in < channel_roll->radio_min+trim_roll_range || + channel_roll->radio_in > channel_roll->radio_max-trim_roll_range || + channel_pitch->radio_in < channel_pitch->radio_min+trim_pitch_range || + channel_pitch->radio_in > channel_pitch->radio_max-trim_pitch_range) { // don't trim for extreme values - if we attempt to trim so // there is less than 20 percent range left then assume the // sticks are not properly centered. This also prevents @@ -183,11 +183,11 @@ static void trim_control_surfaces() // Store control surface trim values // --------------------------------- if(g.mix_mode == 0) { - if (g.channel_roll.radio_in != 0) { - g.channel_roll.radio_trim = g.channel_roll.radio_in; + if (channel_roll->radio_in != 0) { + channel_roll->radio_trim = channel_roll->radio_in; } - if (g.channel_pitch.radio_in != 0) { - g.channel_pitch.radio_trim = g.channel_pitch.radio_in; + if (channel_pitch->radio_in != 0) { + channel_pitch->radio_trim = channel_pitch->radio_in; } // the secondary aileron/elevator is trimmed only if it has a @@ -205,17 +205,17 @@ static void trim_control_surfaces() //Recompute values here using new values for elevon1_trim and elevon2_trim //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed uint16_t center = 1500; - g.channel_roll.radio_trim = center; - g.channel_pitch.radio_trim = center; + channel_roll->radio_trim = center; + channel_pitch->radio_trim = center; } - if (g.channel_rudder.radio_in != 0) { - g.channel_rudder.radio_trim = g.channel_rudder.radio_in; + if (channel_rudder->radio_in != 0) { + channel_rudder->radio_trim = channel_rudder->radio_in; } // save to eeprom - g.channel_roll.save_eeprom(); - g.channel_pitch.save_eeprom(); - g.channel_rudder.save_eeprom(); + channel_roll->save_eeprom(); + channel_pitch->save_eeprom(); + channel_rudder->save_eeprom(); } static void trim_radio() diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 37de1895bb..11d63f9ab4 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -194,7 +194,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) } - if(g.channel_roll.radio_in < 500) { + if(channel_roll->radio_in < 500) { while(1) { cliSerial->printf_P(PSTR("\nNo radio; Check connectors.")); delay(1000); @@ -202,27 +202,27 @@ setup_radio(uint8_t argc, const Menu::arg *argv) } } - g.channel_roll.radio_min = g.channel_roll.radio_in; - g.channel_pitch.radio_min = g.channel_pitch.radio_in; - g.channel_throttle.radio_min = g.channel_throttle.radio_in; - g.channel_rudder.radio_min = g.channel_rudder.radio_in; + channel_roll->radio_min = channel_roll->radio_in; + channel_pitch->radio_min = channel_pitch->radio_in; + channel_throttle->radio_min = channel_throttle->radio_in; + channel_rudder->radio_min = channel_rudder->radio_in; g.rc_5.radio_min = g.rc_5.radio_in; g.rc_6.radio_min = g.rc_6.radio_in; g.rc_7.radio_min = g.rc_7.radio_in; g.rc_8.radio_min = g.rc_8.radio_in; - g.channel_roll.radio_max = g.channel_roll.radio_in; - g.channel_pitch.radio_max = g.channel_pitch.radio_in; - g.channel_throttle.radio_max = g.channel_throttle.radio_in; - g.channel_rudder.radio_max = g.channel_rudder.radio_in; + channel_roll->radio_max = channel_roll->radio_in; + channel_pitch->radio_max = channel_pitch->radio_in; + channel_throttle->radio_max = channel_throttle->radio_in; + channel_rudder->radio_max = channel_rudder->radio_in; g.rc_5.radio_max = g.rc_5.radio_in; g.rc_6.radio_max = g.rc_6.radio_in; g.rc_7.radio_max = g.rc_7.radio_in; g.rc_8.radio_max = g.rc_8.radio_in; - 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; + channel_roll->radio_trim = channel_roll->radio_in; + channel_pitch->radio_trim = channel_pitch->radio_in; + channel_rudder->radio_trim = channel_rudder->radio_in; g.rc_5.radio_trim = 1500; g.rc_6.radio_trim = 1500; g.rc_7.radio_trim = 1500; @@ -236,10 +236,10 @@ setup_radio(uint8_t argc, const Menu::arg *argv) // ---------------------------------------------------------- read_radio(); - g.channel_roll.update_min_max(); - g.channel_pitch.update_min_max(); - g.channel_throttle.update_min_max(); - g.channel_rudder.update_min_max(); + channel_roll->update_min_max(); + channel_pitch->update_min_max(); + channel_throttle->update_min_max(); + channel_rudder->update_min_max(); g.rc_5.update_min_max(); g.rc_6.update_min_max(); g.rc_7.update_min_max(); @@ -249,10 +249,10 @@ setup_radio(uint8_t argc, const Menu::arg *argv) while (cliSerial->available() > 0) { cliSerial->read(); } - g.channel_roll.save_eeprom(); - g.channel_pitch.save_eeprom(); - g.channel_throttle.save_eeprom(); - g.channel_rudder.save_eeprom(); + channel_roll->save_eeprom(); + channel_pitch->save_eeprom(); + channel_throttle->save_eeprom(); + channel_rudder->save_eeprom(); g.rc_5.save_eeprom(); g.rc_6.save_eeprom(); g.rc_7.save_eeprom(); @@ -542,10 +542,10 @@ static void report_flight_modes() static void print_radio_values() { - cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); - cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max); - cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max); - cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max); + cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)channel_roll->radio_min, (int)channel_roll->radio_trim, (int)channel_roll->radio_max); + cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)channel_pitch->radio_min, (int)channel_pitch->radio_trim, (int)channel_pitch->radio_max); + cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)channel_throttle->radio_min, (int)channel_throttle->radio_trim, (int)channel_throttle->radio_max); + cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)channel_rudder->radio_min, (int)channel_rudder->radio_trim, (int)channel_rudder->radio_max); cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max); cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max); cliSerial->printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max); @@ -591,10 +591,10 @@ radio_input_switch(void) static int8_t bouncer = 0; - if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) { + if (int16_t(channel_roll->radio_in - channel_roll->radio_trim) > 100) { bouncer = 10; } - if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) { + if (int16_t(channel_roll->radio_in - channel_roll->radio_trim) < -100) { bouncer = -10; } if (bouncer >0) { diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index d822d93482..b520adb538 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -110,6 +110,12 @@ 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); + // reset the uartA baud rate after parameter load hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD)); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index ca110c7da1..54ce0a1271 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -113,10 +113,10 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) read_radio(); cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - (int)g.channel_roll.radio_in, - (int)g.channel_pitch.radio_in, - (int)g.channel_throttle.radio_in, - (int)g.channel_rudder.radio_in, + (int)channel_roll->radio_in, + (int)channel_pitch->radio_in, + (int)channel_throttle->radio_in, + (int)channel_rudder->radio_in, (int)g.rc_5.radio_in, (int)g.rc_6.radio_in, (int)g.rc_7.radio_in, @@ -169,20 +169,20 @@ test_radio(uint8_t argc, const Menu::arg *argv) delay(20); read_radio(); - g.channel_roll.calc_pwm(); - g.channel_pitch.calc_pwm(); - g.channel_throttle.calc_pwm(); - g.channel_rudder.calc_pwm(); + channel_roll->calc_pwm(); + channel_pitch->calc_pwm(); + channel_throttle->calc_pwm(); + channel_rudder->calc_pwm(); // write out the servo PWM values // ------------------------------ set_servos(); cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - (int)g.channel_roll.control_in, - (int)g.channel_pitch.control_in, - (int)g.channel_throttle.control_in, - (int)g.channel_rudder.control_in, + (int)channel_roll->control_in, + (int)channel_pitch->control_in, + (int)channel_throttle->control_in, + (int)channel_rudder->control_in, (int)g.rc_5.control_in, (int)g.rc_6.control_in, (int)g.rc_7.control_in, @@ -211,7 +211,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) oldSwitchPosition = readSwitch(); cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); - while(g.channel_throttle.control_in > 0) { + while(channel_throttle->control_in > 0) { delay(20); read_radio(); } @@ -220,8 +220,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) delay(20); read_radio(); - if(g.channel_throttle.control_in > 0) { - cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)g.channel_throttle.control_in); + if(channel_throttle->control_in > 0) { + cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)channel_throttle->control_in); fail_test++; } @@ -232,8 +232,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) fail_test++; } - if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()) { - cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.radio_in); + if(g.throttle_fs_enabled && channel_throttle->get_failsafe()) { + cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)channel_throttle->radio_in); print_flight_mode(cliSerial, readSwitch()); cliSerial->println(); fail_test++;