Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18: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_Navigation.h>
#include <AP_L1_Control.h> #include <AP_L1_Control.h>
#include <AP_RCMapper.h> // RC input mapping library
// Pre-AP_HAL compatibility // Pre-AP_HAL compatibility
#include "compat.h" #include "compat.h"
@ -98,6 +99,15 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
// //
static Parameters g; 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 // prototypes
static void update_events(void); static void update_events(void);
@ -1036,7 +1046,7 @@ static void update_current_flight_mode(void)
} }
// max throttle for takeoff // max throttle for takeoff
g.channel_throttle.servo_out = g.throttle_max; channel_throttle->servo_out = g.throttle_max;
break; break;
@ -1063,7 +1073,7 @@ static void update_current_flight_mode(void)
if (land_complete) { if (land_complete) {
// we are in the final stage of a landing - force // we are in the final stage of a landing - force
// zero throttle // zero throttle
g.channel_throttle.servo_out = 0; channel_throttle->servo_out = 0;
} }
break; break;
@ -1123,9 +1133,9 @@ static void update_current_flight_mode(void)
case FLY_BY_WIRE_A: { case FLY_BY_WIRE_A: {
// set nav_roll and nav_pitch using sticks // 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); 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) { if (pitch_input > 0) {
nav_pitch_cd = pitch_input * g.pitch_limit_max_cd; nav_pitch_cd = pitch_input * g.pitch_limit_max_cd;
} else { } else {
@ -1146,10 +1156,10 @@ static void update_current_flight_mode(void)
// Thanks to Yury MonZon for the altitude limit code! // 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; float elevator_input;
elevator_input = g.channel_pitch.norm_input(); elevator_input = channel_pitch->norm_input();
if (g.flybywire_elev_reverse) { if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input; elevator_input = -elevator_input;
@ -1195,9 +1205,9 @@ static void update_current_flight_mode(void)
case MANUAL: case MANUAL:
// servo_out is for Sim control only // servo_out is for Sim control only
// --------------------------------- // ---------------------------------
g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); channel_roll->servo_out = channel_roll->pwm_to_angle();
g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); channel_pitch->servo_out = channel_pitch->pwm_to_angle();
g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); channel_rudder->servo_out = channel_rudder->pwm_to_angle();
break; break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 //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); speed_scaler = constrain_float(speed_scaler, 0.5, 2.0);
} else { } else {
if (g.channel_throttle.servo_out > 0) { if (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 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... // Should maybe be to the 2/7 power, but we aren't goint to implement that...
}else{ }else{
speed_scaler = 1.67; speed_scaler = 1.67;
@ -44,7 +44,7 @@ static bool stick_mixing_enabled(void)
geofence_stickmixing() && geofence_stickmixing() &&
failsafe == FAILSAFE_NONE && failsafe == FAILSAFE_NONE &&
(g.throttle_fs_enabled == 0 || (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 // we're in an auto mode, and haven't triggered failsafe
return true; return true;
} else { } else {
@ -73,7 +73,7 @@ static void stabilize_roll(float speed_scaler)
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; 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) 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; int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch;
g.channel_pitch.servo_out = g.pitchController.get_servo_out(demanded_pitch, channel_pitch->servo_out = g.pitchController.get_servo_out(demanded_pitch,
speed_scaler, speed_scaler,
control_mode == STABILIZE, control_mode == STABILIZE,
g.flybywire_airspeed_min, g.flybywire_airspeed_max); g.flybywire_airspeed_min, g.flybywire_airspeed_max);
@ -105,25 +105,25 @@ static void stabilize_stick_mixing_direct()
float ch1_inf; float ch1_inf;
float ch2_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 = fabsf(ch1_inf);
ch1_inf = min(ch1_inf, 400.0); ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - 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 = fabsf(ch2_inf);
ch2_inf = min(ch2_inf, 400.0); ch2_inf = min(ch2_inf, 400.0);
ch2_inf = ((400.0 - ch2_inf) /400.0); ch2_inf = ((400.0 - ch2_inf) /400.0);
// scale the sensor input based on the stick input // scale the sensor input based on the stick input
// ----------------------------------------------- // -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf; channel_roll->servo_out *= ch1_inf;
g.channel_pitch.servo_out *= ch2_inf; channel_pitch->servo_out *= ch2_inf;
// Mix in stick inputs // Mix in stick inputs
// ------------------- // -------------------
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); channel_roll->servo_out += channel_roll->pwm_to_angle();
g.channel_pitch.servo_out += g.channel_pitch.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 // non-linear and ends up as 2x the maximum, to ensure that
// the user can direct the plane in any direction with stick // the user can direct the plane in any direction with stick
// mixing. // mixing.
float roll_input = g.channel_roll.norm_input(); float roll_input = channel_roll->norm_input();
if (roll_input > 0.5f) { if (roll_input > 0.5f) {
roll_input = (3*roll_input - 1); roll_input = (3*roll_input - 1);
} else if (roll_input < -0.5f) { } 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 += roll_input * g.roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get()); 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) { if (fabsf(pitch_input) > 0.5f) {
pitch_input = (3*pitch_input - 1); 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 // stick mixing performed for rudder for all cases including FBW
// important for steering on the ground during landing // 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 = fabsf(ch4_inf);
ch4_inf = min(ch4_inf, 400.0); ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - 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 // Apply output to Rudder
calc_nav_yaw(speed_scaler, ch4_inf); calc_nav_yaw(speed_scaler, ch4_inf);
g.channel_rudder.servo_out *= ch4_inf; channel_rudder->servo_out *= ch4_inf;
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle(); 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) static void stabilize_training(float speed_scaler)
{ {
if (training_manual_roll) { if (training_manual_roll) {
g.channel_roll.servo_out = g.channel_roll.control_in; channel_roll->servo_out = channel_roll->control_in;
} else { } else {
// calculate what is needed to hold // calculate what is needed to hold
stabilize_roll(speed_scaler); stabilize_roll(speed_scaler);
if ((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 && g.channel_roll.control_in > g.channel_roll.servo_out)) { (nav_roll_cd < 0 && channel_roll->control_in > channel_roll->servo_out)) {
// allow user to get out of the roll // 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) { if (training_manual_pitch) {
g.channel_pitch.servo_out = g.channel_pitch.control_in; channel_pitch->servo_out = channel_pitch->control_in;
} else { } else {
stabilize_pitch(speed_scaler); stabilize_pitch(speed_scaler);
if ((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 && g.channel_pitch.control_in > g.channel_pitch.servo_out)) { (nav_pitch_cd < 0 && channel_pitch->control_in > channel_pitch->servo_out)) {
// allow user to get back to level // 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. 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 && abs(current_loc.alt - home.alt) < 500 &&
fabs(barometer.get_climb_rate()) < 0.5f && fabs(barometer.get_climb_rate()) < 0.5f &&
g_gps->ground_speed < 300) { 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 // user has asked for zero throttle - this may be done by a
// mission which wants to turn off the engine for a parachute // mission which wants to turn off the engine for a parachute
// landing // landing
g.channel_throttle.servo_out = 0; channel_throttle->servo_out = 0;
return; return;
} }
@ -284,22 +284,22 @@ static void calc_throttle()
// AUTO, RTL, etc // AUTO, RTL, etc
// --------------------------------------------------------------------------- // ---------------------------------------------------------------------------
if (nav_pitch_cd >= 0) { 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 { } 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 { } else {
// throttle control with airspeed compensation // throttle control with airspeed compensation
// ------------------------------------------- // -------------------------------------------
energy_error = airspeed_energy_error + altitude_error_cm * 0.098f; energy_error = airspeed_energy_error + altitude_error_cm * 0.098f;
// positive energy errors make the throttle go higher // positive energy errors make the throttle go higher
g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error); 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 += (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()); 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) { if (hold_course_cd != -1) {
// steering on or close to ground // steering on or close to ground
int32_t bearing_error_cd = nav_controller->bearing_error_cd(); 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) + channel_rudder->servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) +
g.kff_rudder_mix * g.channel_roll.servo_out; g.kff_rudder_mix * channel_roll->servo_out;
g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500); channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
return; 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, control_mode == STABILIZE,
g.flybywire_airspeed_min, g.flybywire_airspeed_max); g.flybywire_airspeed_min, g.flybywire_airspeed_max);
// add in rudder mixing from roll // add in rudder mixing from roll
g.channel_rudder.servo_out += g.channel_roll.servo_out * g.kff_rudder_mix; channel_rudder->servo_out += 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 = 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 slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate) { if (g.throttle_slewrate) {
// limit throttle change by the given percentage per second // 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 // allow a minimum change of 1 PWM per cycle
if (temp < 1) { if (temp < 1) {
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) 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) { if (control_mode == MANUAL) {
// do a direct pass through of radio values // do a direct pass through of radio values
if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) { if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
g.channel_roll.radio_out = g.channel_roll.radio_in; channel_roll->radio_out = channel_roll->radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in; channel_pitch->radio_out = channel_pitch->radio_in;
} else { } else {
g.channel_roll.radio_out = hal.rcin->read(CH_ROLL); channel_roll->radio_out = hal.rcin->read(CH_ROLL);
g.channel_pitch.radio_out = hal.rcin->read(CH_PITCH); channel_pitch->radio_out = hal.rcin->read(CH_PITCH);
} }
g.channel_throttle.radio_out = g.channel_throttle.radio_in; channel_throttle->radio_out = channel_throttle->radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in; channel_rudder->radio_out = channel_rudder->radio_in;
// setup extra aileron channel. We want this to come from the // setup extra aileron channel. We want this to come from the
// main aileron input channel, but using the 2nd channels dead // 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 // pwm_to_angle_dz() to ensure we don't trim the value for the
// deadzone of the main aileron channel, otherwise the 2nd // deadzone of the main aileron channel, otherwise the 2nd
// aileron won't quite follow the first one // 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); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, aileron_in);
// setup extra elevator channel following the same logic // 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); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, elevator_in);
// this aileron variant assumes you have the corresponding // 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) { if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) {
// set any differential spoilers to follow the elevons in // set any differential spoilers to follow the elevons in
// manual mode. // 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_dspoiler1, 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_dspoiler2, channel_pitch->radio_out);
} }
} else { } else {
if (g.mix_mode == 0) { if (g.mix_mode == 0) {
// both types of secondary aileron are slaved to the roll servo out // 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, 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_with_input, channel_roll->servo_out);
// both types of secondary elevator are slaved to the pitch 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, 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_with_input, channel_pitch->servo_out);
}else{ }else{
/*Elevon mode*/ /*Elevon mode*/
float ch1; float ch1;
float ch2; float ch2;
ch1 = 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 = g.channel_pitch.servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * g.channel_roll.servo_out); ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
/* Differential Spoilers /* Differential Spoilers
If differential spoilers are setup, then we translate 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)) { 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 ch3 = ch1;
float ch4 = ch2; float ch4 = ch2;
if ( BOOL_TO_SIGN(g.reverse_elevons) * g.channel_rudder.servo_out < 0) { if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) {
ch1 += abs(g.channel_rudder.servo_out); ch1 += abs(channel_rudder->servo_out);
ch3 -= abs(g.channel_rudder.servo_out); ch3 -= abs(channel_rudder->servo_out);
} else { } else {
ch2 += abs(g.channel_rudder.servo_out); ch2 += abs(channel_rudder->servo_out);
ch4 -= abs(g.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_dspoiler1, ch3);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4);
} }
// directly set the radio_out values for elevon mode // 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)); 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_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
} }
#if OBC_FAILSAFE == ENABLED #if OBC_FAILSAFE == ENABLED
@ -606,37 +606,37 @@ static void set_servos(void)
// the plane. Only used in extreme circumstances to meet the // the plane. Only used in extreme circumstances to meet the
// OBC rules // OBC rules
if (obc.crash_plane()) { if (obc.crash_plane()) {
g.channel_roll.servo_out = -4500; channel_roll->servo_out = -4500;
g.channel_pitch.servo_out = -4500; channel_pitch->servo_out = -4500;
g.channel_rudder.servo_out = -4500; channel_rudder->servo_out = -4500;
g.channel_throttle.servo_out = 0; channel_throttle->servo_out = 0;
} }
#endif #endif
// push out the PWM values // push out the PWM values
if (g.mix_mode == 0) { if (g.mix_mode == 0) {
g.channel_roll.calc_pwm(); channel_roll->calc_pwm();
g.channel_pitch.calc_pwm(); channel_pitch->calc_pwm();
} }
g.channel_rudder.calc_pwm(); channel_rudder->calc_pwm();
#if THROTTLE_OUT == 0 #if THROTTLE_OUT == 0
g.channel_throttle.servo_out = 0; channel_throttle->servo_out = 0;
#else #else
// convert 0 to 100% into PWM // 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_min.get(),
g.throttle_max.get()); g.throttle_max.get());
if (suppress_throttle()) { if (suppress_throttle()) {
// throttle is suppressed in auto mode // throttle is suppressed in auto mode
g.channel_throttle.servo_out = 0; channel_throttle->servo_out = 0;
if (g.throttle_suppress_manual) { if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed // 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 { } else {
g.channel_throttle.calc_pwm(); channel_throttle->calc_pwm();
} }
} else if (g.throttle_passthru_stabilize && } else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE || (control_mode == STABILIZE ||
@ -644,10 +644,10 @@ static void set_servos(void)
control_mode == FLY_BY_WIRE_A)) { control_mode == FLY_BY_WIRE_A)) {
// manual pass through of throttle while in FBWA or // manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set // 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 { } else {
// normal throttle calculation based on servo_out // normal throttle calculation based on servo_out
g.channel_throttle.calc_pwm(); channel_throttle->calc_pwm();
} }
#endif #endif
} }
@ -683,7 +683,7 @@ static void set_servos(void)
if (control_mode == TRAINING) { if (control_mode == TRAINING) {
// copy rudder in training mode // 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 #if HIL_MODE != HIL_MODE_DISABLED
@ -693,17 +693,17 @@ static void set_servos(void)
#endif #endif
if (g.vtail_output != MIXING_DISABLED) { 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) { } 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 // 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_1, channel_roll->radio_out); // send to Servos
hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos hal.rcout->write(CH_2, channel_pitch->radio_out); // send to Servos
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos hal.rcout->write(CH_3, channel_throttle->radio_out); // send to Servos
hal.rcout->write(CH_4, g.channel_rudder.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 // Route configurable aux. functions to their respective servos
g.rc_5.output_ch(CH_5); g.rc_5.output_ch(CH_5);
g.rc_6.output_ch(CH_6); 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, chan,
millis(), millis(),
0, // port 0 0, // port 0
10000 * g.channel_roll.norm_output(), 10000 * channel_roll->norm_output(),
10000 * g.channel_pitch.norm_output(), 10000 * channel_pitch->norm_output(),
10000 * g.channel_throttle.norm_output(), 10000 * channel_throttle->norm_output(),
10000 * g.channel_rudder.norm_output(), 10000 * channel_rudder->norm_output(),
0, 0,
0, 0,
0, 0,
@ -377,7 +377,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
} else if (!ahrs.airspeed_estimate(&aspeed)) { } else if (!ahrs.airspeed_estimate(&aspeed)) {
aspeed = 0; 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); throttle_norm = constrain_int16(throttle_norm, -100, 100);
uint16_t throttle = ((uint16_t)(throttle_norm + 100)) / 2; uint16_t throttle = ((uint16_t)(throttle_norm + 100)) / 2;
mavlink_msg_vfr_hud_send( 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, roll : (int16_t)ahrs.roll_sensor,
nav_pitch_cd : (int16_t)nav_pitch_cd, nav_pitch_cd : (int16_t)nav_pitch_cd,
pitch : (int16_t)ahrs.pitch_sensor, pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)g.channel_throttle.servo_out, throttle_out : (int16_t)channel_throttle->servo_out,
rudder_out : (int16_t)g.channel_rudder.servo_out, rudder_out : (int16_t)channel_rudder->servo_out,
accel_y : accel.y accel_y : accel.y
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -374,7 +374,7 @@ static void Log_Write_Current()
{ {
struct log_Current pkt = { struct log_Current pkt = {
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), 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), battery_voltage : (int16_t)(battery.voltage * 100.0),
current_amps : (int16_t)(battery.current_amps * 100.0), current_amps : (int16_t)(battery.current_amps * 100.0),
board_voltage : board_voltage(), board_voltage : board_voltage(),

1
ArduPlane/Parameters.h
View File

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

5
ArduPlane/Parameters.pde
View File

@ -774,6 +774,11 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: COMPASS_ // @Group: COMPASS_
// @Path: ../libraries/AP_Compass/Compass.cpp // @Path: ../libraries/AP_Compass/Compass.cpp
GOBJECT(compass, "COMPASS_", Compass), GOBJECT(compass, "COMPASS_", Compass),
// @Group: RCMAP_
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
GOBJECT(rcmap, "RCMAP_", RCMapper),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", 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 // pass RC inputs to outputs every 20ms
last_timestamp = tnow; last_timestamp = tnow;
hal.rcin->clear_overrides(); hal.rcin->clear_overrides();
g.channel_roll.radio_out = hal.rcin->read(CH_1); channel_roll->radio_out = hal.rcin->read(CH_1);
g.channel_pitch.radio_out = hal.rcin->read(CH_2); channel_pitch->radio_out = hal.rcin->read(CH_2);
g.channel_throttle.radio_out = hal.rcin->read(CH_3); channel_throttle->radio_out = hal.rcin->read(CH_3);
g.channel_rudder.radio_out = hal.rcin->read(CH_4); channel_rudder->radio_out = hal.rcin->read(CH_4);
if (g.vtail_output != MIXING_DISABLED) { 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) { } 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) { 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_2, channel_pitch->radio_out);
servo_write(CH_3, g.channel_throttle.radio_out); servo_write(CH_3, channel_throttle->radio_out);
servo_write(CH_4, g.channel_rudder.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_manual, true);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, 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) { if (control_mode == FLY_BY_WIRE_B) {
target_airspeed_cm = ((int)(g.flybywire_airspeed_max - target_airspeed_cm = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) * g.flybywire_airspeed_min) *
g.channel_throttle.servo_out) + channel_throttle->servo_out) +
((int)g.flybywire_airspeed_min * 100); ((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() static void init_rc_in()
{ {
// set rc channel ranges // set rc channel ranges
g.channel_roll.set_angle(SERVO_MAX); channel_roll->set_angle(SERVO_MAX);
g.channel_pitch.set_angle(SERVO_MAX); channel_pitch->set_angle(SERVO_MAX);
g.channel_rudder.set_angle(SERVO_MAX); channel_rudder->set_angle(SERVO_MAX);
g.channel_throttle.set_range(0, 100); channel_throttle->set_range(0, 100);
// set rc dead zones // set rc dead zones
g.channel_roll.set_dead_zone(60); channel_roll->set_dead_zone(60);
g.channel_pitch.set_dead_zone(60); channel_pitch->set_dead_zone(60);
g.channel_rudder.set_dead_zone(60); channel_rudder->set_dead_zone(60);
g.channel_throttle.set_dead_zone(6); channel_throttle->set_dead_zone(6);
//g.channel_roll.dead_zone = 60; //channel_roll->dead_zone = 60;
//g.channel_pitch.dead_zone = 60; //channel_pitch->dead_zone = 60;
//g.channel_rudder.dead_zone = 60; //channel_rudder->dead_zone = 60;
//g.channel_throttle.dead_zone = 6; //channel_throttle->dead_zone = 6;
//set auxiliary ranges //set auxiliary ranges
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@ -43,10 +43,10 @@ static void init_rc_out()
enable_aux_servos(); enable_aux_servos();
// Initialization of servo outputs // Initialization of servo outputs
servo_write(CH_1, g.channel_roll.radio_trim); servo_write(CH_1, channel_roll->radio_trim);
servo_write(CH_2, g.channel_pitch.radio_trim); servo_write(CH_2, channel_pitch->radio_trim);
servo_write(CH_3, g.channel_throttle.radio_min); servo_write(CH_3, channel_throttle->radio_min);
servo_write(CH_4, g.channel_rudder.radio_trim); servo_write(CH_4, channel_rudder->radio_trim);
servo_write(CH_5, g.rc_5.radio_trim); servo_write(CH_5, g.rc_5.radio_trim);
servo_write(CH_6, g.rc_6.radio_trim); servo_write(CH_6, g.rc_6.radio_trim);
@ -80,15 +80,15 @@ static void read_radio()
if (control_mode == TRAINING) { if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we // in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits // want manual pass through when not exceeding attitude limits
g.channel_roll.set_pwm_no_deadzone(pwm_roll); channel_roll->set_pwm_no_deadzone(pwm_roll);
g.channel_pitch.set_pwm_no_deadzone(pwm_pitch); channel_pitch->set_pwm_no_deadzone(pwm_pitch);
g.channel_throttle.set_pwm_no_deadzone(hal.rcin->read(CH_3)); channel_throttle->set_pwm_no_deadzone(hal.rcin->read(CH_3));
g.channel_rudder.set_pwm_no_deadzone(hal.rcin->read(CH_4)); channel_rudder->set_pwm_no_deadzone(hal.rcin->read(CH_4));
} else { } else {
g.channel_roll.set_pwm(pwm_roll); channel_roll->set_pwm(pwm_roll);
g.channel_pitch.set_pwm(pwm_pitch); channel_pitch->set_pwm(pwm_pitch);
g.channel_throttle.set_pwm(hal.rcin->read(CH_3)); channel_throttle->set_pwm(hal.rcin->read(CH_3));
g.channel_rudder.set_pwm(hal.rcin->read(CH_4)); channel_rudder->set_pwm(hal.rcin->read(CH_4));
} }
g.rc_5.set_pwm(hal.rcin->read(CH_5)); 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_7.set_pwm(hal.rcin->read(CH_7));
g.rc_8.set_pwm(hal.rcin->read(CH_8)); 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) { if (g.throttle_nudge && channel_throttle->servo_out > 50) {
float nudge = (g.channel_throttle.servo_out - 50) * 0.02; float nudge = (channel_throttle->servo_out - 50) * 0.02;
if (alt_control_airspeed()) { if (alt_control_airspeed()) {
airspeed_nudge_cm = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; airspeed_nudge_cm = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else { } else {
@ -167,12 +167,12 @@ static void control_failsafe(uint16_t pwm)
static void trim_control_surfaces() static void trim_control_surfaces()
{ {
read_radio(); read_radio();
int16_t trim_roll_range = (g.channel_roll.radio_max - g.channel_roll.radio_min)/5; int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5;
int16_t trim_pitch_range = (g.channel_pitch.radio_max - g.channel_pitch.radio_min)/5; int16_t trim_pitch_range = (channel_pitch->radio_max - channel_pitch->radio_min)/5;
if (g.channel_roll.radio_in < g.channel_roll.radio_min+trim_roll_range || if (channel_roll->radio_in < channel_roll->radio_min+trim_roll_range ||
g.channel_roll.radio_in > g.channel_roll.radio_max-trim_roll_range || channel_roll->radio_in > channel_roll->radio_max-trim_roll_range ||
g.channel_pitch.radio_in < g.channel_pitch.radio_min+trim_pitch_range || channel_pitch->radio_in < channel_pitch->radio_min+trim_pitch_range ||
g.channel_pitch.radio_in > g.channel_pitch.radio_max-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 // don't trim for extreme values - if we attempt to trim so
// there is less than 20 percent range left then assume the // there is less than 20 percent range left then assume the
// sticks are not properly centered. This also prevents // sticks are not properly centered. This also prevents
@ -183,11 +183,11 @@ static void trim_control_surfaces()
// Store control surface trim values // Store control surface trim values
// --------------------------------- // ---------------------------------
if(g.mix_mode == 0) { if(g.mix_mode == 0) {
if (g.channel_roll.radio_in != 0) { if (channel_roll->radio_in != 0) {
g.channel_roll.radio_trim = g.channel_roll.radio_in; channel_roll->radio_trim = channel_roll->radio_in;
} }
if (g.channel_pitch.radio_in != 0) { if (channel_pitch->radio_in != 0) {
g.channel_pitch.radio_trim = g.channel_pitch.radio_in; channel_pitch->radio_trim = channel_pitch->radio_in;
} }
// the secondary aileron/elevator is trimmed only if it has a // 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 //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 //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; uint16_t center = 1500;
g.channel_roll.radio_trim = center; channel_roll->radio_trim = center;
g.channel_pitch.radio_trim = center; channel_pitch->radio_trim = center;
} }
if (g.channel_rudder.radio_in != 0) { if (channel_rudder->radio_in != 0) {
g.channel_rudder.radio_trim = g.channel_rudder.radio_in; channel_rudder->radio_trim = channel_rudder->radio_in;
} }
// save to eeprom // save to eeprom
g.channel_roll.save_eeprom(); channel_roll->save_eeprom();
g.channel_pitch.save_eeprom(); channel_pitch->save_eeprom();
g.channel_rudder.save_eeprom(); channel_rudder->save_eeprom();
} }
static void trim_radio() 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) { while(1) {
cliSerial->printf_P(PSTR("\nNo radio; Check connectors.")); cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000); 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; channel_roll->radio_min = channel_roll->radio_in;
g.channel_pitch.radio_min = g.channel_pitch.radio_in; channel_pitch->radio_min = channel_pitch->radio_in;
g.channel_throttle.radio_min = g.channel_throttle.radio_in; channel_throttle->radio_min = channel_throttle->radio_in;
g.channel_rudder.radio_min = g.channel_rudder.radio_in; channel_rudder->radio_min = channel_rudder->radio_in;
g.rc_5.radio_min = g.rc_5.radio_in; g.rc_5.radio_min = g.rc_5.radio_in;
g.rc_6.radio_min = g.rc_6.radio_in; g.rc_6.radio_min = g.rc_6.radio_in;
g.rc_7.radio_min = g.rc_7.radio_in; g.rc_7.radio_min = g.rc_7.radio_in;
g.rc_8.radio_min = g.rc_8.radio_in; g.rc_8.radio_min = g.rc_8.radio_in;
g.channel_roll.radio_max = g.channel_roll.radio_in; channel_roll->radio_max = channel_roll->radio_in;
g.channel_pitch.radio_max = g.channel_pitch.radio_in; channel_pitch->radio_max = channel_pitch->radio_in;
g.channel_throttle.radio_max = g.channel_throttle.radio_in; channel_throttle->radio_max = channel_throttle->radio_in;
g.channel_rudder.radio_max = g.channel_rudder.radio_in; channel_rudder->radio_max = channel_rudder->radio_in;
g.rc_5.radio_max = g.rc_5.radio_in; g.rc_5.radio_max = g.rc_5.radio_in;
g.rc_6.radio_max = g.rc_6.radio_in; g.rc_6.radio_max = g.rc_6.radio_in;
g.rc_7.radio_max = g.rc_7.radio_in; g.rc_7.radio_max = g.rc_7.radio_in;
g.rc_8.radio_max = g.rc_8.radio_in; g.rc_8.radio_max = g.rc_8.radio_in;
g.channel_roll.radio_trim = g.channel_roll.radio_in; channel_roll->radio_trim = channel_roll->radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in; channel_pitch->radio_trim = channel_pitch->radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in; channel_rudder->radio_trim = channel_rudder->radio_in;
g.rc_5.radio_trim = 1500; g.rc_5.radio_trim = 1500;
g.rc_6.radio_trim = 1500; g.rc_6.radio_trim = 1500;
g.rc_7.radio_trim = 1500; g.rc_7.radio_trim = 1500;
@ -236,10 +236,10 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
// ---------------------------------------------------------- // ----------------------------------------------------------
read_radio(); read_radio();
g.channel_roll.update_min_max(); channel_roll->update_min_max();
g.channel_pitch.update_min_max(); channel_pitch->update_min_max();
g.channel_throttle.update_min_max(); channel_throttle->update_min_max();
g.channel_rudder.update_min_max(); channel_rudder->update_min_max();
g.rc_5.update_min_max(); g.rc_5.update_min_max();
g.rc_6.update_min_max(); g.rc_6.update_min_max();
g.rc_7.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) { while (cliSerial->available() > 0) {
cliSerial->read(); cliSerial->read();
} }
g.channel_roll.save_eeprom(); channel_roll->save_eeprom();
g.channel_pitch.save_eeprom(); channel_pitch->save_eeprom();
g.channel_throttle.save_eeprom(); channel_throttle->save_eeprom();
g.channel_rudder.save_eeprom(); channel_rudder->save_eeprom();
g.rc_5.save_eeprom(); g.rc_5.save_eeprom();
g.rc_6.save_eeprom(); g.rc_6.save_eeprom();
g.rc_7.save_eeprom(); g.rc_7.save_eeprom();
@ -542,10 +542,10 @@ static void report_flight_modes()
static void static void
print_radio_values() 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("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)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.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)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.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)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.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("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("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); 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; 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; 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; bouncer = -10;
} }
if (bouncer >0) { if (bouncer >0) {

6
ArduPlane/system.pde
View File

@ -110,6 +110,12 @@ static void init_ardupilot()
// //
load_parameters(); 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 // reset the uartA baud rate after parameter load
hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD)); 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(); read_radio();
cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), 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)channel_roll->radio_in,
(int)g.channel_pitch.radio_in, (int)channel_pitch->radio_in,
(int)g.channel_throttle.radio_in, (int)channel_throttle->radio_in,
(int)g.channel_rudder.radio_in, (int)channel_rudder->radio_in,
(int)g.rc_5.radio_in, (int)g.rc_5.radio_in,
(int)g.rc_6.radio_in, (int)g.rc_6.radio_in,
(int)g.rc_7.radio_in, (int)g.rc_7.radio_in,
@ -169,20 +169,20 @@ test_radio(uint8_t argc, const Menu::arg *argv)
delay(20); delay(20);
read_radio(); read_radio();
g.channel_roll.calc_pwm(); channel_roll->calc_pwm();
g.channel_pitch.calc_pwm(); channel_pitch->calc_pwm();
g.channel_throttle.calc_pwm(); channel_throttle->calc_pwm();
g.channel_rudder.calc_pwm(); channel_rudder->calc_pwm();
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
set_servos(); set_servos();
cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), 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)channel_roll->control_in,
(int)g.channel_pitch.control_in, (int)channel_pitch->control_in,
(int)g.channel_throttle.control_in, (int)channel_throttle->control_in,
(int)g.channel_rudder.control_in, (int)channel_rudder->control_in,
(int)g.rc_5.control_in, (int)g.rc_5.control_in,
(int)g.rc_6.control_in, (int)g.rc_6.control_in,
(int)g.rc_7.control_in, (int)g.rc_7.control_in,
@ -211,7 +211,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch(); oldSwitchPosition = readSwitch();
cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); 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); delay(20);
read_radio(); read_radio();
} }
@ -220,8 +220,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
delay(20); delay(20);
read_radio(); read_radio();
if(g.channel_throttle.control_in > 0) { if(channel_throttle->control_in > 0) {
cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)g.channel_throttle.control_in); cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)channel_throttle->control_in);
fail_test++; fail_test++;
} }
@ -232,8 +232,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
fail_test++; fail_test++;
} }
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()) { if(g.throttle_fs_enabled && channel_throttle->get_failsafe()) {
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)g.channel_throttle.radio_in); cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)channel_throttle->radio_in);
print_flight_mode(cliSerial, readSwitch()); print_flight_mode(cliSerial, readSwitch());
cliSerial->println(); cliSerial->println();
fail_test++; fail_test++;