Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow channel mapping of first 4 channels

Browse Source 
this makes it easier to support DSM and SBUS radios
This commit is contained in:
Andrew Tridgell 2013-06-03 15:32:08 +10:00
parent 725293b9c3
commit fb9bf21522
12 changed files with 224 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduPlane/ArduPlane.pde
View File

@ -56,6 +56,7 @@
#include <AP_Navigation.h>
#include <AP_L1_Control.h>
#include <AP_RCMapper.h> // 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

172
ArduPlane/Attitude.pde
View File

@ -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);

10
ArduPlane/GCS_Mavlink.pde
View File

@ -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(

6
ArduPlane/Log.pde
View File

@ -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(),

1
ArduPlane/Parameters.h
View File

@ -228,6 +228,7 @@ public:
k_param_pitchController,
k_param_yawController,
k_param_L1_controller,
k_param_rcmap,
//
// 240: PID Controllers

5
ArduPlane/Parameters.pde
View File

@ -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),

20
ArduPlane/failsafe.pde
View File

@ -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);

2
ArduPlane/navigation.pde
View File

@ -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);
}

90
ArduPlane/radio.pde
View File

@ -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()

52
ArduPlane/setup.pde
View File

@ -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) {

6
ArduPlane/system.pde
View File

@ -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));

34
ArduPlane/test.pde
View File

@ -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++;