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

Plane: use new SRV_Channels API

Browse Source
This commit is contained in:
Andrew Tridgell 2016-10-22 21:27:57 +11:00
parent 4cb9f772f2
commit b83f50be0f
18 changed files with 310 additions and 803 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/ArduPlane.cpp
View File

@ -311,7 +311,7 @@ void Plane::afs_fs_check(void)
*/
void Plane::update_aux(void)
{
RC_Channel_aux::enable_aux_servos();
SRV_Channels::enable_aux_servos();
}
void Plane::one_second_loop()
@ -558,7 +558,7 @@ void Plane::handle_auto_mode(void)
// we are in the final stage of a landing - force
// zero throttle
channel_throttle->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else {
calc_throttle();
}
@ -685,7 +685,7 @@ void Plane::update_flight_mode(void)
// FBWA failsafe glide
nav_roll_cd = 0;
nav_pitch_cd = 0;
channel_throttle->set_servo_out(0);
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
if (g.fbwa_tdrag_chan > 0) {
// check for the user enabling FBWA taildrag takeoff mode
@ -748,10 +748,8 @@ void Plane::update_flight_mode(void)
break;
case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
channel_roll->set_servo_out(channel_roll->pwm_to_angle());
channel_pitch->set_servo_out(channel_pitch->pwm_to_angle());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->pwm_to_angle());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->pwm_to_angle());
steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle();
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000

78
ArduPlane/Attitude.cpp
View File

@ -19,8 +19,8 @@ float Plane::get_speed_scaler(void)
}
speed_scaler = constrain_float(speed_scaler, 0.5f, 2.0f);
} else {
if (channel_throttle->get_servo_out() > 0) {
speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->get_servo_out() / 2.0f); // First order taylor expansion of square root
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) {
speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 2.0f); // First order taylor expansion of square root
// Should maybe be to the 2/7 power, but we aren't going to implement that...
}else{
speed_scaler = 1.67f;
@ -80,9 +80,9 @@ void Plane::stabilize_roll(float speed_scaler)
if (control_mode == STABILIZE && channel_roll->get_control_in() != 0) {
disable_integrator = true;
}
channel_roll->set_servo_out(rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler,
disable_integrator));
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler,
disable_integrator));
}
/*
@ -96,17 +96,17 @@ void Plane::stabilize_pitch(float speed_scaler)
if (force_elevator != 0) {
// we are holding the tail down during takeoff. Just convert
// from a percentage to a -4500..4500 centidegree angle
channel_pitch->set_servo_out(45*force_elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator);
return;
}
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->get_servo_out() * g.kff_throttle_to_pitch;
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch;
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_pitch->get_control_in() != 0) {
disable_integrator = true;
}
channel_pitch->set_servo_out(pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler,
disable_integrator));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler,
disable_integrator));
}
/*
@ -127,17 +127,6 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
servo_out += channel->pwm_to_angle();
}
/*
One argument version for when the servo out in the rc channel
is the target
*/
void Plane::stick_mix_channel(RC_Channel * channel)
{
int16_t servo_out = channel->get_servo_out();
stick_mix_channel(channel,servo_out);
channel->set_servo_out(servo_out);
}
/*
this gives the user control of the aircraft in stabilization modes
*/
@ -157,8 +146,13 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == TRAINING) {
return;
}
stick_mix_channel(channel_roll);
stick_mix_channel(channel_pitch);
int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
stick_mix_channel(channel_roll, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
stick_mix_channel(channel_pitch, elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
}
/*
@ -268,25 +262,25 @@ void Plane::stabilize_yaw(float speed_scaler)
void Plane::stabilize_training(float speed_scaler)
{
if (training_manual_roll) {
channel_roll->set_servo_out(channel_roll->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && channel_roll->get_control_in() < channel_roll->get_servo_out()) ||
(nav_roll_cd < 0 && channel_roll->get_control_in() > channel_roll->get_servo_out())) {
if ((nav_roll_cd > 0 && channel_roll->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && channel_roll->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
// allow user to get out of the roll
channel_roll->set_servo_out(channel_roll->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
}
}
if (training_manual_pitch) {
channel_pitch->set_servo_out(channel_pitch->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < channel_pitch->get_servo_out()) ||
(nav_pitch_cd < 0 && channel_pitch->get_control_in() > channel_pitch->get_servo_out())) {
if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && channel_pitch->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
// allow user to get back to level
channel_pitch->set_servo_out(channel_pitch->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
}
}
@ -321,16 +315,16 @@ void Plane::stabilize_acro(float speed_scaler)
nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
// try to reduce the integrated angular error to zero. We set
// 'stabilze' to true, which disables the roll integrator
channel_roll->set_servo_out(rollController.get_servo_out(roll_error_cd,
speed_scaler,
true));
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd,
speed_scaler,
true));
} else {
/*
aileron stick is non-zero, use pure rate control until the
user releases the stick
*/
acro_state.locked_roll = false;
channel_roll->set_servo_out(rollController.get_rate_out(roll_rate, speed_scaler));
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_rate_out(roll_rate, speed_scaler));
}
if (g.acro_locking && is_zero(pitch_rate)) {
@ -345,15 +339,15 @@ void Plane::stabilize_acro(float speed_scaler)
// try to hold the locked pitch. Note that we have the pitch
// integrator enabled, which helps with inverted flight
nav_pitch_cd = acro_state.locked_pitch_cd;
channel_pitch->set_servo_out(pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false));
} else {
/*
user has non-zero pitch input, use a pure rate controller
*/
acro_state.locked_pitch = false;
channel_pitch->set_servo_out( pitchController.get_rate_out(pitch_rate, speed_scaler));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_rate_out(pitch_rate, speed_scaler));
}
/*
@ -423,7 +417,7 @@ void Plane::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
channel_throttle->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
return;
}
@ -436,7 +430,7 @@ void Plane::calc_throttle()
commanded_throttle = plane.guided_state.forced_throttle;
}
channel_throttle->set_servo_out(commanded_throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
}
/*****************************************
@ -464,7 +458,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
// add in rudder mixing from roll
commanded_rudder += channel_roll->get_servo_out() * g.kff_rudder_mix;
commanded_rudder += SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * g.kff_rudder_mix;
commanded_rudder += rudder_input;
}

8
ArduPlane/GCS_Mavlink.cpp
View File

@ -216,10 +216,10 @@ void Plane::send_servo_out(mavlink_channel_t chan)
chan,
millis(),
0, // port 0
10000 * channel_roll->norm_output() * (channel_roll->get_reverse()?-1:1),
10000 * channel_pitch->norm_output() * (channel_pitch->get_reverse()?-1:1),
10000 * channel_throttle->norm_output() * (channel_throttle->get_reverse()?-1:1),
10000 * channel_rudder->norm_output() * (channel_rudder->get_reverse()?-1:1),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) / 4500.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / 4500.0f),
0,
0,
0,

4
ArduPlane/Log.cpp
View File

@ -263,8 +263,8 @@ void Plane::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)channel_throttle->get_servo_out(),
rudder_out : (int16_t)channel_rudder->get_servo_out(),
throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
rudder_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_rudder),
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));

86
ArduPlane/Parameters.cpp
View File

@ -1047,72 +1047,6 @@ const AP_Param::Info Plane::var_info[] = {
(const void *)&plane.quadplane.attitude_control,
{group_info : AC_AttitudeControl_Multi::var_info}, AP_PARAM_FLAG_POINTER },
// RC channel
//-----------
// @Group: RC1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_1, "RC1_", RC_Channel),
// @Group: RC2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_2, "RC2_", RC_Channel),
// @Group: RC3_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_3, "RC3_", RC_Channel),
// @Group: RC4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_4, "RC4_", RC_Channel),
// @Group: RC5_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_5, "RC5_", RC_Channel_aux),
// @Group: RC6_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_6, "RC6_", RC_Channel_aux),
// @Group: RC7_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_7, "RC7_", RC_Channel_aux),
// @Group: RC8_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux),
// @Group: RC9_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_9, "RC9_", RC_Channel_aux),
// @Group: RC10_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_10, "RC10_", RC_Channel_aux),
// @Group: RC11_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_11, "RC11_", RC_Channel_aux),
// @Group: RC12_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_12, "RC12_", RC_Channel_aux),
// @Group: RC13_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_13, "RC13_", RC_Channel_aux),
// @Group: RC14_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_14, "RC14_", RC_Channel_aux),
// @Group: RC15_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_15, "RC15_", RC_Channel_aux),
// @Group: RC16_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_16, "RC16_", RC_Channel_aux),
// @Group: RLL2SRV_
// @Path: ../libraries/APM_Control/AP_RollController.cpp
GOBJECT(rollController, "RLL2SRV_", AP_RollController),
@ -1267,10 +1201,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp
AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine),
// @Group: SERVO_
// @Path: ../libraries/RC_Channel/SRV_Channel.cpp
AP_SUBGROUPINFO(servo_channels, "SERVO", 3, ParametersG2, SRV_Channels),
// 3 was used by prototype for servo_channels
// @Group: SYSID_ENFORCE
// @DisplayName: GCS sysid enforcement
// @Description: This controls whether packets from other than the expected GCS system ID will be accepted
@ -1282,6 +1214,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_Stats/AP_Stats.cpp
AP_SUBGROUPINFO(stats, "STAT", 5, ParametersG2, AP_Stats),
// @Group: SERVO
// @Path: ../libraries/SRV_Channel/SRV_Channel.cpp
AP_SUBGROUPINFO(servo_channels, "SERVO", 6, ParametersG2, SRV_Channels),
// @Group: RC
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(rc_channels, "RC", 7, ParametersG2, RC_Channels),
AP_GROUPEND
};
@ -1387,6 +1327,12 @@ void Plane::load_parameters(void)
// quadplanes needs a higher loop rate
AP_Param::set_default_by_name("SCHED_LOOP_RATE", 300);
}
// setup defaults in SRV_Channels
g2.servo_channels.set_default_function(CH_1, SRV_Channel::k_aileron);
g2.servo_channels.set_default_function(CH_2, SRV_Channel::k_elevator);
g2.servo_channels.set_default_function(CH_3, SRV_Channel::k_throttle);
g2.servo_channels.set_default_function(CH_4, SRV_Channel::k_rudder);
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
}

78
ArduPlane/Parameters.h
View File

@ -244,19 +244,19 @@ public:
k_param_battery_curr_pin, // unused - 169
//
// 170: Radio settings
// 170: Radio settings - all unused now
//
k_param_rc_1 = 170,
k_param_rc_2,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_rc_11,
k_param_rc_1_old = 170,
k_param_rc_2_old,
k_param_rc_3_old,
k_param_rc_4_old,
k_param_rc_5_old,
k_param_rc_6_old,
k_param_rc_7_old,
k_param_rc_8_old,
k_param_rc_9_old,
k_param_rc_10_old,
k_param_rc_11_old,
k_param_throttle_min,
k_param_throttle_max,
@ -270,13 +270,13 @@ public:
k_param_throttle_slewrate,
k_param_throttle_suppress_manual,
k_param_throttle_passthru_stabilize,
k_param_rc_12,
k_param_rc_12_old,
k_param_fs_batt_voltage,
k_param_fs_batt_mah,
k_param_short_fs_timeout,
k_param_long_fs_timeout,
k_param_rc_13,
k_param_rc_14,
k_param_rc_13_old,
k_param_rc_14_old,
k_param_tuning,
//
@ -290,8 +290,8 @@ public:
k_param_quadplane,
k_param_rtl_radius,
k_param_land_then_servos_neutral, // unused - moved to AP_Landing
k_param_rc_15,
k_param_rc_16,
k_param_rc_15_old,
k_param_rc_16_old,
//
// 210: flight modes
@ -515,47 +515,6 @@ public:
#endif
AP_Int16 gcs_pid_mask;
AP_Int8 parachute_channel;
// RC channels
RC_Channel rc_1;
RC_Channel rc_2;
RC_Channel rc_3;
RC_Channel rc_4;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
RC_Channel_aux rc_9;
RC_Channel_aux rc_10;
RC_Channel_aux rc_11;
RC_Channel_aux rc_12;
RC_Channel_aux rc_13;
RC_Channel_aux rc_14;
RC_Channel_aux rc_15;
RC_Channel_aux rc_16;
uint8_t _dummy;
Parameters() :
// variable default
//----------------------------------------
rc_1 (CH_1),
rc_2 (CH_2),
rc_3 (CH_3),
rc_4 (CH_4),
rc_5 (CH_5),
rc_6 (CH_6),
rc_7 (CH_7),
rc_8 (CH_8),
rc_9 (CH_9),
rc_10 (CH_10),
rc_11 (CH_11),
rc_12 (CH_12),
rc_13 (CH_13),
rc_14 (CH_14),
rc_15 (CH_15),
rc_16 (CH_16),
_dummy(0)
{}
};
/*
@ -577,6 +536,9 @@ public:
// internal combustion engine control
AP_ICEngine ice_control;
// RC input channels
RC_Channels rc_channels;
// control over servo output ranges
SRV_Channels servo_channels;

15
ArduPlane/Plane.h
View File

@ -43,7 +43,7 @@
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
#include <AP_AHRS/AP_AHRS.h> // ArduPilot Mega DCM Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include <RC_Channel/SRV_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
#include <Filter/Filter.h> // Filter library
#include <AP_Buffer/AP_Buffer.h> // APM FIFO Buffer
@ -189,7 +189,7 @@ private:
// board specific config
AP_BoardConfig BoardConfig;
// primary control channels
// primary input channels
RC_Channel *channel_roll;
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
@ -983,7 +983,6 @@ private:
void resetPerfData(void);
void check_usb_mux(void);
void print_comma(void);
void servo_write(uint8_t ch, uint16_t pwm);
bool should_log(uint32_t mask);
int8_t throttle_percentage(void);
void change_arm_state(void);
@ -1047,7 +1046,6 @@ private:
bool stick_mixing_enabled(void);
void stabilize_roll(float speed_scaler);
void stabilize_pitch(float speed_scaler);
static void stick_mix_channel(RC_Channel *channel);
static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
void stabilize_stick_mixing_direct();
void stabilize_stick_mixing_fbw();
@ -1057,11 +1055,10 @@ private:
void calc_nav_yaw_coordinated(float speed_scaler);
void calc_nav_yaw_course(void);
void calc_nav_yaw_ground(void);
void throttle_slew_limit(int16_t last_throttle);
void flap_slew_limit(int8_t &last_value, int8_t &new_value);
void throttle_slew_limit(void);
bool suppress_throttle(void);
void channel_output_mixer(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
void channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const;
void channel_output_mixer_pwm(uint8_t mixing_type, uint16_t & chan1, uint16_t & chan2)const;
void channel_output_mixer(uint8_t mixing_type, SRV_Channel::Aux_servo_function_t servo1, SRV_Channel::Aux_servo_function_t servo2);
void flaperon_update(int8_t flap_percent);
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
@ -1091,7 +1088,6 @@ private:
void log_init();
void init_capabilities(void);
void dataflash_periodic(void);
uint16_t throttle_min(void) const;
void parachute_check();
#if PARACHUTE == ENABLED
void do_parachute(const AP_Mission::Mission_Command& cmd);
@ -1120,7 +1116,6 @@ public:
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
int8_t main_menu_help(uint8_t argc, const Menu::arg *argv);
int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
int8_t test_radio(uint8_t argc, const Menu::arg *argv);
int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
int8_t test_relay(uint8_t argc, const Menu::arg *argv);

58
ArduPlane/afs_plane.cpp
View File

@ -15,30 +15,19 @@ AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Ba
*/
void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
{
// we are terminating. Setup primary output channels radio_out values
RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1);
RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
ch_roll->set_radio_out(ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
ch_pitch->set_radio_out(ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
ch_yaw->set_radio_out(ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
ch_throttle->set_radio_out(ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
RC_Channel_aux::disable_passthrough(true);
plane.g2.servo_channels.disable_passthrough(true);
plane.servos_output();
// and all aux channels
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_flap_auto, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_output_limit(SRV_Channel::k_flap, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator_with_input, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
plane.quadplane.afs_terminate();
@ -48,26 +37,15 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
{
const RC_Channel *ch_roll = RC_Channel::rc_channel(rcmap.roll()-1);
const RC_Channel *ch_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
const RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
const RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
// setup primary channel output values
hal.rcout->set_failsafe_pwm(1U<<(rcmap.roll()-1), ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.pitch()-1), ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.yaw()-1), ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.throttle()-1), ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
// and all aux channels
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
// all aux channels
SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap_auto, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator_with_input, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
if (plane.quadplane.available()) {
// setup AP_Motors outputs for failsafe

2
ArduPlane/control_modes.cpp
View File

@ -83,7 +83,7 @@ void Plane::read_control_switch()
}
} else if (!override_requested && px4io_override_enabled) {
px4io_override_enabled = false;
RC_Channel_aux::enable_aux_servos();
SRV_Channels::enable_aux_servos();
gcs_send_text(MAV_SEVERITY_WARNING, "PX4IO override disabled");
}
if (px4io_override_enabled &&

41
ArduPlane/failsafe.cpp
View File

@ -53,26 +53,23 @@ void Plane::failsafe_check(void)
// pass RC inputs to outputs every 20ms
hal.rcin->clear_overrides();
channel_roll->set_radio_out(channel_roll->read());
channel_pitch->set_radio_out(channel_pitch->read());
if (hal.util->get_soft_armed()) {
channel_throttle->set_radio_out(channel_throttle->read());
} else {
channel_throttle->set_servo_out(0);
channel_throttle->calc_pwm();
int16_t roll = channel_roll->get_control_in_zero_dz();
int16_t pitch = channel_pitch->get_control_in_zero_dz();
int16_t throttle = channel_throttle->get_control_in_zero_dz();
int16_t rudder = channel_rudder->get_control_in_zero_dz();
if (!hal.util->get_soft_armed()) {
throttle = 0;
}
channel_rudder->set_radio_out(channel_rudder->read());
int16_t roll = channel_roll->pwm_to_angle_dz(0);
int16_t pitch = channel_pitch->pwm_to_angle_dz(0);
int16_t rudder = channel_rudder->pwm_to_angle_dz(0);
// setup secondary output channels that don't have
// corresponding input channels
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, roll);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, pitch);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, rudder);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
@ -84,11 +81,11 @@ void Plane::failsafe_check(void)
// setup secondary output channels that do have
// corresponding input channels
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_elevator_with_input, true);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, 0);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, 0);
SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true);
SRV_Channels::copy_radio_in_out(SRV_Channel::k_aileron_with_input, true);
SRV_Channels::copy_radio_in_out(SRV_Channel::k_elevator_with_input, true);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0);
servos_output();

44
ArduPlane/px4_mixer.cpp
View File

@ -71,7 +71,7 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
for (uint8_t i=0; i<8; i++) {
int32_t c1, c2, mix=0;
bool rev = false;
RC_Channel_aux::Aux_servo_function_t function = RC_Channel_aux::channel_function(i);
SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(i);
if (i == rcmap.pitch()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) {
// first channel of VTAIL mix
c1 = rcmap.yaw()-1;
@ -98,27 +98,27 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
c2 = rcmap.roll()-1;
rev = false;
mix = mix_max*mixmul[g.elevon_output][0];
} else if (function == RC_Channel_aux::k_aileron ||
function == RC_Channel_aux::k_flaperon1 ||
function == RC_Channel_aux::k_flaperon2) {
} else if (function == SRV_Channel::k_aileron ||
function == SRV_Channel::k_flaperon1 ||
function == SRV_Channel::k_flaperon2) {
// a secondary aileron. We don't mix flap input in yet for flaperons
c1 = rcmap.roll()-1;
} else if (function == RC_Channel_aux::k_elevator) {
} else if (function == SRV_Channel::k_elevator) {
// a secondary elevator
c1 = rcmap.pitch()-1;
} else if (function == RC_Channel_aux::k_rudder ||
function == RC_Channel_aux::k_steering) {
} else if (function == SRV_Channel::k_rudder ||
function == SRV_Channel::k_steering) {
// a secondary rudder or wheel
c1 = rcmap.yaw()-1;
} else if (g.flapin_channel > 0 &&
(function == RC_Channel_aux::k_flap ||
function == RC_Channel_aux::k_flap_auto)) {
(function == SRV_Channel::k_flap ||
function == SRV_Channel::k_flap_auto)) {
// a flap output channel, and we have a manual flap input channel
c1 = g.flapin_channel-1;
} else if (i < 4 ||
function == RC_Channel_aux::k_elevator_with_input ||
function == RC_Channel_aux::k_aileron_with_input ||
function == RC_Channel_aux::k_manual) {
function == SRV_Channel::k_elevator_with_input ||
function == SRV_Channel::k_aileron_with_input ||
function == SRV_Channel::k_manual) {
// a pass-thru channel
c1 = i;
} else {
@ -132,8 +132,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
// pass through channel, possibly with reversal. We also
// adjust the gain based on the range of input and output
// channels and adjust for trims
const RC_Channel *chan1 = RC_Channel::rc_channel(i);
const RC_Channel *chan2 = RC_Channel::rc_channel(c1);
const RC_Channel *chan1 = RC_Channels::rc_channel(i);
const RC_Channel *chan2 = RC_Channels::rc_channel(c1);
int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim());
int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim());
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
@ -171,8 +171,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
return 0;
}
} else {
const RC_Channel *chan1 = RC_Channel::rc_channel(c1);
const RC_Channel *chan2 = RC_Channel::rc_channel(c2);
const RC_Channel *chan1 = RC_Channels::rc_channel(c1);
const RC_Channel *chan2 = RC_Channels::rc_channel(c2);
int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim());
int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim());
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
@ -268,7 +268,7 @@ bool Plane::setup_failsafe_mixing(void)
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
// make sure the throttle has a non-zero failsafe value before we
// disable safety. This prevents sending zero PWM during switch over
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, aparm.throttle_min<0?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
// we need to force safety on to allow us to load a mixer. We call
@ -294,7 +294,7 @@ bool Plane::setup_failsafe_mixing(void)
// mix/max/trim. We only do the first 8 channels due to
// a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS
for (uint8_t i=0; i<8; i++) {
RC_Channel *ch = RC_Channel::rc_channel(i);
RC_Channel *ch = RC_Channels::rc_channel(i);
if (ch == nullptr) {
continue;
}
@ -354,8 +354,8 @@ bool Plane::setup_failsafe_mixing(void)
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
if (RC_Channel_aux::channel_function(i) >= RC_Channel_aux::k_motor1 &&
RC_Channel_aux::channel_function(i) <= RC_Channel_aux::k_motor8) {
if (SRV_Channels::channel_function(i) >= SRV_Channel::k_motor1 &&
SRV_Channels::channel_function(i) <= SRV_Channel::k_motor8) {
pwm_values.values[i] = quadplane.thr_min_pwm;
} else {
pwm_values.values[i] = 900;
@ -367,8 +367,8 @@ bool Plane::setup_failsafe_mixing(void)
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
if (RC_Channel_aux::channel_function(i) >= RC_Channel_aux::k_motor1 &&
RC_Channel_aux::channel_function(i) <= RC_Channel_aux::k_motor8) {
if (SRV_Channels::channel_function(i) >= SRV_Channel::k_motor1 &&
SRV_Channels::channel_function(i) <= SRV_Channel::k_motor8) {
hal.rcout->write(i, quadplane.thr_min_pwm);
pwm_values.values[i] = quadplane.thr_min_pwm;
} else {

19
ArduPlane/quadplane.cpp
View File

@ -354,7 +354,7 @@ QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) :
void QuadPlane::setup_default_channels(uint8_t num_motors)
{
for (uint8_t i=0; i<num_motors; i++) {
RC_Channel_aux::set_aux_channel_default((RC_Channel_aux::Aux_servo_function_t)(RC_Channel_aux::k_motor1+i), CH_5+i);
SRV_Channels::set_aux_channel_default((SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+i), CH_5+i);
}
}
@ -406,10 +406,10 @@ bool QuadPlane::setup(void)
}
#if FRAME_CONFIG == TRI_FRAME
RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::k_motor1, CH_5);
RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::k_motor2, CH_6);
RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::k_motor4, CH_8);
RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::k_motor7, CH_11);
SRV_Channels::set_default_function(CH_5, SRV_Channel::k_motor1);
SRV_Channels::set_default_function(CH_6, SRV_Channel::k_motor2);
SRV_Channels::set_default_function(CH_8, SRV_Channel::k_motor4);
SRV_Channels::set_default_function(CH_11, SRV_Channel::k_motor7);
frame_class.set(AP_Motors::MOTOR_FRAME_TRI);
motors = new AP_MOTORS_CLASS(plane.scheduler.get_loop_rate_hz());
#else
@ -477,12 +477,9 @@ bool QuadPlane::setup(void)
// setup the trim of any motors used by AP_Motors so px4io
// failsafe will disable motors
for (uint8_t i=0; i<8; i++) {
RC_Channel_aux::Aux_servo_function_t func = (RC_Channel_aux::Aux_servo_function_t)(RC_Channel_aux::k_motor1+i);
RC_Channel_aux::set_servo_failsafe_pwm(func, thr_min_pwm);
uint8_t chan;
if (RC_Channel_aux::find_channel(func, chan)) {
RC_Channel::rc_channel(chan)->set_radio_trim(thr_min_pwm);
}
SRV_Channel::Aux_servo_function_t func = (SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor1+i);
SRV_Channels::set_failsafe_pwm(func, thr_min_pwm);
SRV_Channels::set_trim_to_pwm_for(func, thr_min_pwm);
}
#if HAVE_PX4_MIXER

57
ArduPlane/radio.cpp
View File

@ -11,13 +11,13 @@ void Plane::set_control_channels(void)
if (g.rudder_only) {
// in rudder only mode the roll and rudder channels are the
// same.
channel_roll = RC_Channel::rc_channel(rcmap.yaw()-1);
channel_roll = RC_Channels::rc_channel(rcmap.yaw()-1);
} else {
channel_roll = RC_Channel::rc_channel(rcmap.roll()-1);
channel_roll = RC_Channels::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);
channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1);
channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
channel_rudder = RC_Channels::rc_channel(rcmap.yaw()-1);
// set rc channel ranges
channel_roll->set_angle(SERVO_MAX);
@ -25,19 +25,19 @@ void Plane::set_control_channels(void)
channel_rudder->set_angle(SERVO_MAX);
if (aparm.throttle_min >= 0) {
// normal operation
channel_throttle->set_range(0, 100);
channel_throttle->set_range(100);
} else {
// reverse thrust
channel_throttle->set_angle(100);
}
if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, aparm.throttle_min<0?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
// take a proportion of speed
g2.servo_channels.set_esc_scaling(channel_throttle->get_ch_out());
g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
}
/*
@ -58,29 +58,24 @@ void Plane::init_rc_in()
*/
void Plane::init_rc_out_main()
{
// setup failsafe for bottom 4 channels. We don't do all channels
// yet as some may be for VTOL motors in a quadplane
RC_Channel::setup_failsafe_trim_mask(0x000F);
/*
change throttle trim to minimum throttle. This prevents a
configuration error where the user sets CH3_TRIM incorrectly and
the motor may start on power up
*/
channel_throttle->set_radio_trim(throttle_min());
channel_roll->enable_out();
channel_pitch->enable_out();
if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) {
channel_throttle->enable_out();
if (aparm.throttle_min >= 0) {
SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttle);
}
channel_rudder->enable_out();
SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
// setup PX4 to output the min throttle when safety off if arming
// is setup for min on disarm
if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, aparm.throttle_min<0?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
}
@ -90,17 +85,17 @@ void Plane::init_rc_out_main()
void Plane::init_rc_out_aux()
{
update_aux();
RC_Channel_aux::enable_aux_servos();
SRV_Channels::enable_aux_servos();
hal.rcout->cork();
// Initialization of servo outputs
RC_Channel::output_trim_all();
SRV_Channels::output_trim_all();
servos_output();
// setup PWM values to send if the FMU firmware dies
RC_Channel::setup_failsafe_trim_all();
SRV_Channels::setup_failsafe_trim_all();
}
/*
@ -199,7 +194,7 @@ void Plane::read_radio()
+ BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
}
RC_Channel::set_pwm_all();
RC_Channels::set_pwm_all();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
@ -215,10 +210,10 @@ void Plane::read_radio()
control_failsafe(channel_throttle->get_radio_in());
channel_throttle->set_servo_out(channel_throttle->get_control_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f;
if (g.throttle_nudge && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 50 && geofence_stickmixing()) {
float nudge = (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
} else {
@ -326,8 +321,8 @@ void Plane::trim_control_surfaces()
// the secondary aileron/elevator is trimmed only if it has a
// corresponding transmitter input channel, which k_aileron
// doesn't have
RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_aileron_with_input);
RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_elevator_with_input);
SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::k_aileron_with_input);
SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::k_elevator_with_input);
} else{
if (elevon.ch1_temp != 0) {
elevon.trim1 = elevon.ch1_temp;
@ -349,8 +344,6 @@ void Plane::trim_control_surfaces()
channel_roll->save_eeprom();
channel_pitch->save_eeprom();
channel_rudder->save_eeprom();
g2.servo_channels.set_trim();
}
void Plane::trim_radio()

2
ArduPlane/sensors.cpp
View File

@ -351,7 +351,7 @@ void Plane::update_sensor_status_flags(void)
}
}
if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) {
if (aparm.throttle_min < 0 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}

321
ArduPlane/servos.cpp
View File

@ -22,7 +22,7 @@
/*****************************************
* Throttle slew limit
*****************************************/
void Plane::throttle_slew_limit(int16_t last_throttle)
void Plane::throttle_slew_limit(void)
{
uint8_t slewrate = aparm.throttle_slewrate;
if (control_mode==AUTO) {
@ -34,36 +34,10 @@ void Plane::throttle_slew_limit(int16_t last_throttle)
}
// if slew limit rate is set to zero then do not slew limit
if (slewrate) {
// limit throttle change by the given percentage per second
float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
// allow a minimum change of 1 PWM per cycle
if (temp < 1) {
temp = 1;
}
channel_throttle->set_radio_out(constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp));
SRV_Channels::limit_slew_rate(SRV_Channel::k_throttle, slewrate);
}
}
/*****************************************
Flap slew limit
*****************************************/
void Plane::flap_slew_limit(int8_t &last_value, int8_t &new_value)
{
uint8_t slewrate = g.flap_slewrate;
// if slew limit rate is set to zero then do not slew limit
if (slewrate) {
// limit flap change by the given percentage per second
float temp = slewrate * G_Dt;
// allow a minimum change of 1% per cycle. This means the
// slowest flaps we can do is full change over 2 seconds
if (temp < 1) {
temp = 1;
}
new_value = constrain_int16(new_value, last_value - temp, last_value + temp);
}
last_value = new_value;
}
/* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
Disable throttle if following conditions are met:
@ -156,7 +130,7 @@ bool Plane::suppress_throttle(void)
/*
implement a software VTail or elevon mixer. There are 4 different mixing modes
*/
void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16_t & chan2_out)const
void Plane::channel_output_mixer_pwm(uint8_t mixing_type, uint16_t & chan1_out, uint16_t & chan2_out) const
{
int16_t c1, c2;
int16_t v1, v2;
@ -227,27 +201,38 @@ void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16
chan2_out = 1500 + v2;
}
void Plane::channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const
/*
output mixer based on two channel output types
*/
void Plane::channel_output_mixer(uint8_t mixing_type, SRV_Channel::Aux_servo_function_t func1, SRV_Channel::Aux_servo_function_t func2)
{
int16_t ch1 = chan1->get_radio_out();
int16_t ch2 = chan2->get_radio_out();
SRV_Channel *chan1, *chan2;
if (!(chan1 = SRV_Channels::get_channel_for(func1)) ||
!(chan2 = SRV_Channels::get_channel_for(func2))) {
return;
}
channel_output_mixer(mixing_type,ch1,ch2);
uint16_t chan1_out, chan2_out;
chan1_out = chan1->get_output_pwm();
chan2_out = chan2->get_output_pwm();
channel_output_mixer_pwm(mixing_type, chan1_out, chan2_out);
chan1->set_radio_out(ch1);
chan2->set_radio_out(ch2);
chan1->set_output_pwm(chan1_out);
chan2->set_output_pwm(chan2_out);
}
/*
setup flaperon output channels
*/
void Plane::flaperon_update(int8_t flap_percent)
{
if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon1) ||
!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon2)) {
if (!SRV_Channels::function_assigned(SRV_Channel::k_flaperon1) ||
!SRV_Channels::function_assigned(SRV_Channel::k_flaperon2)) {
return;
}
int16_t ch1, ch2;
uint16_t ch1, ch2;
/*
flaperons are implemented as a mixer between aileron and a
percentage of flaps. Flap input can come from a manual channel
@ -258,12 +243,14 @@ void Plane::flaperon_update(int8_t flap_percent)
by mixing gain). flapin_channel's trim is not used.
*/
ch1 = channel_roll->get_radio_out();
if (!SRV_Channels::get_output_pwm(SRV_Channel::k_aileron, ch1)) {
return;
}
// The *5 is to take a percentage to a value from -500 to 500 for the mixer
ch2 = 1500 - flap_percent * 5;
channel_output_mixer(g.flaperon_output, ch1, ch2);
RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::k_flaperon1, ch1);
RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::k_flaperon2, ch2);
channel_output_mixer_pwm(g.flaperon_output, ch1, ch2);
SRV_Channels::set_output_pwm_trimmed(SRV_Channel::k_flaperon1, ch1);
SRV_Channels::set_output_pwm_trimmed(SRV_Channel::k_flaperon2, ch2);
}
/*
@ -273,9 +260,8 @@ void Plane::flaperon_update(int8_t flap_percent)
*/
void Plane::set_servos_idle(void)
{
RC_Channel_aux::output_ch_all();
if (auto_state.idle_wiggle_stage == 0) {
RC_Channel::output_trim_all();
SRV_Channels::output_trim_all();
return;
}
int16_t servo_value = 0;
@ -292,57 +278,29 @@ void Plane::set_servos_idle(void)
} else {
auto_state.idle_wiggle_stage = 0;
}
channel_roll->set_servo_out(servo_value);
channel_pitch->set_servo_out(servo_value);
channel_rudder->set_servo_out(servo_value);
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
channel_rudder->calc_pwm();
channel_throttle->output_trim();
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_value);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_value);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value);
SRV_Channels::set_output_to_trim(SRV_Channel::k_throttle);
SRV_Channels::output_ch_all();
}
/*
return minimum throttle PWM value, taking account of throttle reversal. For reverse thrust you get the throttle off position
*/
uint16_t Plane::throttle_min(void) const
{
if (aparm.throttle_min < 0) {
return channel_throttle->get_radio_trim();
}
return channel_throttle->get_reverse() ? channel_throttle->get_radio_max() : channel_throttle->get_radio_min();
};
/*
pass through channels in manual mode
*/
void Plane::set_servos_manual_passthrough(void)
{
// do a direct pass through of radio values
if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
channel_roll->set_radio_out(channel_roll->get_radio_in());
channel_pitch->set_radio_out(channel_pitch->get_radio_in());
} else {
channel_roll->set_radio_out(channel_roll->read());
channel_pitch->set_radio_out(channel_pitch->read());
}
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
channel_rudder->set_radio_out(channel_rudder->get_radio_in());
// setup extra channels. We want this to come from the
// main input channel, but using the 2nd channels dead
// zone, reverse and min/max settings. We need to use
// 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
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0));
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, channel_rudder->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
// this variant assumes you have the corresponding
// input channel setup in your transmitter for manual control
// of the 2nd aileron
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input);
SRV_Channels::copy_radio_in_out(SRV_Channel::k_aileron_with_input);
SRV_Channels::copy_radio_in_out(SRV_Channel::k_elevator_with_input);
}
/*
@ -353,8 +311,10 @@ void Plane::set_servos_old_elevons(void)
/*Elevon mode*/
float ch1;
float ch2;
ch1 = channel_pitch->get_servo_out() - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());
ch2 = channel_pitch->get_servo_out() + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());
int16_t roll = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
int16_t pitch = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
ch1 = pitch - (BOOL_TO_SIGN(g.reverse_elevons) * roll);
ch2 = pitch + (BOOL_TO_SIGN(g.reverse_elevons) * roll);
/* Differential Spoilers
If differential spoilers are setup, then we translate
@ -362,23 +322,24 @@ void Plane::set_servos_old_elevons(void)
the side of the aircraft where we want to induce
additional drag.
*/
if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) {
if (SRV_Channels::function_assigned(SRV_Channel::k_dspoiler1) && SRV_Channels::function_assigned(SRV_Channel::k_dspoiler2)) {
float ch3 = ch1;
float ch4 = ch2;
if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->get_servo_out() < 0) {
ch1 += abs(channel_rudder->get_servo_out());
ch3 -= abs(channel_rudder->get_servo_out());
int16_t rudder = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder);
if (BOOL_TO_SIGN(g.reverse_elevons) * rudder < 0) {
ch1 += abs(rudder);
ch3 -= abs(rudder);
} else {
ch2 += abs(channel_rudder->get_servo_out());
ch4 -= abs(channel_rudder->get_servo_out());
ch2 += abs(rudder);
ch4 -= abs(rudder);
}
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, ch3);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, ch4);
SRV_Channels::set_output_scaled(SRV_Channel::k_dspoiler1, ch3);
SRV_Channels::set_output_scaled(SRV_Channel::k_dspoiler2, ch4);
}
// directly set the radio_out values for elevon mode
channel_roll->set_radio_out(elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)));
channel_pitch->set_radio_out(elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)));
SRV_Channels::set_output_pwm_first(SRV_Channel::k_aileron, elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)));
SRV_Channels::set_output_pwm_first(SRV_Channel::k_elevator, elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)));
}
@ -392,14 +353,14 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
// overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value
// throttle limit will attack by 10% per second
if (channel_throttle->get_servo_out() > 0 && // demanding too much positive thrust
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0 && // demanding too much positive thrust
throttle_watt_limit_max < max_throttle - 25 &&
now - throttle_watt_limit_timer_ms >= 1) {
// always allow for 25% throttle available regardless of battery status
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_max++;
} else if (channel_throttle->get_servo_out() < 0 &&
} else if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0 &&
min_throttle < 0 && // reverse thrust is available
throttle_watt_limit_min < -(min_throttle) - 25 &&
now - throttle_watt_limit_timer_ms >= 1) {
@ -412,12 +373,12 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
// it has been 1 second since last over-current, check if we can resume higher throttle.
// this throttle release is needed to allow raising the max_throttle as the battery voltage drains down
// throttle limit will release by 1% per second
if (channel_throttle->get_servo_out() > throttle_watt_limit_max && // demanding max forward thrust
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > throttle_watt_limit_max && // demanding max forward thrust
throttle_watt_limit_max > 0) { // and we're currently limiting it
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_max--;
} else if (channel_throttle->get_servo_out() < throttle_watt_limit_min && // demanding max negative thrust
} else if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < throttle_watt_limit_min && // demanding max negative thrust
throttle_watt_limit_min > 0) { // and we're limiting it
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_min--;
@ -441,20 +402,12 @@ void Plane::set_servos_controlled(void)
set_servos_old_elevons();
} else {
// both types of secondary aileron are slaved to the roll servo out
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_aileron));
// both types of secondary elevator are slaved to the pitch servo out
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());
// push out the PWM values
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_elevator));
}
channel_rudder->calc_pwm();
// convert 0 to 100% (or -100 to +100) into PWM
int8_t min_throttle = aparm.throttle_min.get();
int8_t max_throttle = aparm.throttle_max.get();
@ -477,25 +430,25 @@ void Plane::set_servos_controlled(void)
}
}
}
// apply watt limiter
throttle_watt_limiter(min_throttle, max_throttle);
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
min_throttle,
max_throttle));
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));
if (!hal.util->get_soft_armed()) {
channel_throttle->set_servo_out(0);
channel_throttle->calc_pwm();
if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
channel_throttle->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else {
channel_throttle->calc_pwm();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
@ -506,18 +459,14 @@ void Plane::set_servos_controlled(void)
!failsafe.ch3_counter) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
} else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
} else if (quadplane.in_vtol_mode()) {
// ask quadplane code for forward throttle
channel_throttle->set_servo_out(quadplane.forward_throttle_pct());
channel_throttle->calc_pwm();
} else {
// normal throttle calculation based on servo_out
channel_throttle->calc_pwm();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, quadplane.forward_throttle_pct());
}
}
@ -529,11 +478,9 @@ void Plane::set_servos_flaps(void)
// Auto flap deployment
int8_t auto_flap_percent = 0;
int8_t manual_flap_percent = 0;
static int8_t last_auto_flap;
static int8_t last_manual_flap;
// work out any manual flap input
RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1);
RC_Channel *flapin = RC_Channels::rc_channel(g.flapin_channel-1);
if (flapin != nullptr && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) {
flapin->input();
manual_flap_percent = flapin->percent_input();
@ -589,11 +536,13 @@ void Plane::set_servos_flaps(void)
auto_flap_percent = manual_flap_percent;
}
flap_slew_limit(last_auto_flap, auto_flap_percent);
flap_slew_limit(last_manual_flap, manual_flap_percent);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, auto_flap_percent);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap, manual_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent);
if (g.flap_slewrate) {
SRV_Channels::limit_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate);
SRV_Channels::limit_slew_rate(SRV_Channel::k_flap, g.flap_slewrate);
}
if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) {
flaperon_update(auto_flap_percent);
@ -608,20 +557,22 @@ void Plane::set_servos_flaps(void)
void Plane::servo_output_mixers(void)
{
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder);
channel_output_mixer(g.vtail_output, SRV_Channel::k_elevator, SRV_Channel::k_rudder);
} else if (g.elevon_output != MIXING_DISABLED) {
channel_output_mixer(g.elevon_output, channel_pitch, channel_roll);
channel_output_mixer(g.elevon_output, SRV_Channel::k_elevator, SRV_Channel::k_aileron);
// if (both) differential spoilers setup then apply rudder
// control into splitting the two elevons on the side of
// the aircraft where we want to induce additional drag:
if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) &&
RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) {
int16_t ch3 = channel_roll->get_radio_out(); //diff spoiler 1
int16_t ch4 = channel_pitch->get_radio_out(); //diff spoiler 2
uint16_t ch3, ch4;
if (SRV_Channels::function_assigned(SRV_Channel::k_dspoiler1) &&
SRV_Channels::function_assigned(SRV_Channel::k_dspoiler2) &&
SRV_Channels::get_output_pwm(SRV_Channel::k_aileron, ch3) &&
SRV_Channels::get_output_pwm(SRV_Channel::k_elevator, ch4)) {
// convert rudder-servo output (-4500 to 4500) to PWM offset
// value (-500 to 500) and multiply by DSPOILR_RUD_RATE/100
// (rudder->servo_out * 500 / SERVO_MAX * dspoiler_rud_rate/100):
int16_t ruddVal = (int16_t)((int32_t)(channel_rudder->get_servo_out()) *
int16_t ruddVal = (int16_t)(int32_t(SRV_Channels::get_output_scaled(SRV_Channel::k_rudder)) *
g.dspoiler_rud_rate / (SERVO_MAX/5));
if (ruddVal != 0) { //if nonzero rudder then apply to spoilers
int16_t ch1 = ch3; //elevon 1
@ -634,8 +585,8 @@ void Plane::servo_output_mixers(void)
ch4 -= ruddVal;
}
// change elevon 1 & 2 positions; constrain min/max:
channel_roll->set_radio_out(constrain_int16(ch1, 900, 2100));
channel_pitch->set_radio_out(constrain_int16(ch2, 900, 2100));
SRV_Channels::set_output_pwm_first(SRV_Channel::k_aileron, constrain_int16(ch1, 900, 2100));
SRV_Channels::set_output_pwm_first(SRV_Channel::k_elevator, constrain_int16(ch2, 900, 2100));
// constrain min/max for intermediate dspoiler positions:
ch3 = constrain_int16(ch3, 900, 2100);
ch4 = constrain_int16(ch4, 900, 2100);
@ -643,10 +594,10 @@ void Plane::servo_output_mixers(void)
// set positions of differential spoilers (convert PWM
// 900-2100 range to servo output (-4500 to 4500)
// and use function that supports rev/min/max/trim):
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1,
(ch3-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2,
(ch4-(int16_t)1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2);
SRV_Channels::set_output_scaled(SRV_Channel::k_dspoiler1,
(int16_t(ch3)-1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2);
SRV_Channels::set_output_scaled(SRV_Channel::k_dspoiler2,
(int16_t(ch4)-1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2);
}
}
}
@ -679,8 +630,6 @@ void Plane::set_servos(void)
return;
}
int16_t last_throttle = channel_throttle->get_radio_out();
// do any transition updates for quadplane
quadplane.update();
@ -699,19 +648,23 @@ void Plane::set_servos(void)
// wheel to the rudder just in case the barometer has drifted
// a lot
steering_control.steering = steering_control.rudder;
} else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) {
} else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
// we are within the ground steering altitude but don't have a
// dedicated steering channel. Set the rudder to the ground
// steering output
steering_control.rudder = steering_control.steering;
}
channel_rudder->set_servo_out(steering_control.rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, steering_control.steering);
if (control_mode == TRAINING) {
steering_control.rudder = channel_rudder->get_control_in();
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
set_servos_manual_passthrough();
@ -727,12 +680,7 @@ void Plane::set_servos(void)
quadplane.in_vtol_mode()) {
/* only do throttle slew limiting in modes where throttle
* control is automatic */
throttle_slew_limit(last_throttle);
}
if (control_mode == TRAINING) {
// copy rudder in training mode
channel_rudder->set_radio_out(channel_rudder->get_radio_in());
throttle_slew_limit();
}
if (!arming.is_armed()) {
@ -745,14 +693,12 @@ void Plane::set_servos(void)
break;
case AP_Arming::YES_ZERO_PWM:
channel_throttle->set_servo_out(0);
channel_throttle->set_radio_out(0);
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0);
break;
case AP_Arming::YES_MIN_PWM:
default:
channel_throttle->set_servo_out(0);
channel_throttle->set_radio_out(throttle_min());
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
break;
}
}
@ -778,26 +724,22 @@ void Plane::set_servos(void)
// after an auto land and auto disarm, set the servos to be neutral just
// in case we're upside down or some crazy angle and straining the servos.
if (landing.get_then_servos_neutral() == 1) {
channel_roll->set_radio_out(channel_roll->get_radio_trim());
channel_pitch->set_radio_out(channel_pitch->get_radio_trim());
channel_rudder->set_radio_out(channel_rudder->get_radio_trim());
SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
} else if (landing.get_then_servos_neutral() == 2) {
channel_roll->disable_out();
channel_pitch->disable_out();
channel_rudder->disable_out();
SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
}
uint8_t override_pct;
if (g2.ice_control.throttle_override(override_pct)) {
// the ICE controller wants to override the throttle for starting
channel_throttle->set_servo_out(override_pct);
channel_throttle->calc_pwm();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, override_pct);
}
// allow for secondary throttle
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_throttle, channel_throttle->get_servo_out());
// run output mixer and send values to the hal for output
servos_output();
}
@ -812,30 +754,15 @@ void Plane::servos_output(void)
{
hal.rcout->cork();
// to enable the throttle slew rate to work we need to remember
// and restore the throttle radio_out
uint16_t thr_radio_out_saved = channel_throttle->get_radio_out();
// remap servo output to SERVO* ranges if enabled
g2.servo_channels.remap_servo_output();
// run vtail and elevon mixers
servo_output_mixers();
channel_roll->output();
channel_pitch->output();
channel_throttle->output();
channel_rudder->output();
if (!afs.should_crash_vehicle()) {
RC_Channel_aux::output_ch_all();
}
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
hal.rcout->push();
// restore throttle radio out
channel_throttle->set_radio_out(thr_radio_out_saved);
if (g2.servo_channels.auto_trim_enabled()) {
servos_auto_trim();
}
@ -848,10 +775,6 @@ void Plane::servos_output(void)
*/
void Plane::servos_auto_trim(void)
{
if (!g2.servo_channels.enabled()) {
// only possible with SERVO_RNG_ENABLE=1
return;
}
// only in auto modes and FBWA
if (!auto_throttle_mode && control_mode != FLY_BY_WIRE_A) {
return;
@ -881,8 +804,8 @@ void Plane::servos_auto_trim(void)
}
// adjust trim on channels by a small amount according to I value
g2.servo_channels.adjust_trim(channel_roll->get_ch_out(), rollController.get_pid_info().I);
g2.servo_channels.adjust_trim(channel_pitch->get_ch_out(), pitchController.get_pid_info().I);
g2.servo_channels.adjust_trim(SRV_Channel::k_aileron, rollController.get_pid_info().I);
g2.servo_channels.adjust_trim(SRV_Channel::k_elevator, pitchController.get_pid_info().I);
auto_trim.last_trim_check = now;

28
ArduPlane/system.cpp
View File

@ -741,23 +741,6 @@ void Plane::print_comma(void)
}
#endif
/*
write to a servo
*/
void Plane::servo_write(uint8_t ch, uint16_t pwm)
{
#if HIL_SUPPORT
if (g.hil_mode==1 && !g.hil_servos) {
if (ch < 8) {
RC_Channel::rc_channel(ch)->set_radio_out(pwm);
}
return;
}
#endif
hal.rcout->enable_ch(ch);
hal.rcout->write(ch, pwm);
}
/*
should we log a message type now?
*/
@ -787,11 +770,12 @@ int8_t Plane::throttle_percentage(void)
}
// to get the real throttle we need to use norm_output() which
// returns a number from -1 to 1.
float throttle = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
if (aparm.throttle_min >= 0) {
return constrain_int16(50*(channel_throttle->norm_output()+1), 0, 100);
return constrain_int16(100*throttle, 0, 100);
} else {
// reverse thrust
return constrain_int16(100*channel_throttle->norm_output(), -100, 100);
return constrain_int16(100*throttle, -100, 100);
}
}
@ -814,9 +798,6 @@ bool Plane::arm_motors(AP_Arming::ArmingMethod method)
return false;
}
// only log if arming was successful
channel_throttle->enable_out();
change_arm_state();
return true;
}
@ -829,9 +810,6 @@ bool Plane::disarm_motors(void)
if (!arming.disarm()) {
return false;
}
if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) {
channel_throttle->disable_out();
}
if (control_mode != AUTO) {
// reset the mission on disarm if we are not in auto
mission.reset();

254
ArduPlane/test.cpp
View File

@ -7,15 +7,6 @@
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] = {
{"pwm", MENU_FUNC(test_radio_pwm)},
{"radio", MENU_FUNC(test_radio)},
{"passthru", MENU_FUNC(test_passthru)},
{"failsafe", MENU_FUNC(test_failsafe)},
{"relay", MENU_FUNC(test_relay)},
{"waypoints", MENU_FUNC(test_wp)},
{"xbee", MENU_FUNC(test_xbee)},
{"modeswitch", MENU_FUNC(test_modeswitch)},
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
{"gps", MENU_FUNC(test_gps)},
@ -45,251 +36,6 @@ void Plane::print_hit_enter()
cliSerial->printf("Hit Enter to exit.\n\n");
}
int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
while(1) {
hal.scheduler->delay(20);
// Filters radio input - adjust filters in the radio.cpp file
// ----------------------------------------------------------
read_radio();
cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
(int)channel_roll->get_radio_in(),
(int)channel_pitch->get_radio_in(),
(int)channel_throttle->get_radio_in(),
(int)channel_rudder->get_radio_in(),
(int)g.rc_5.get_radio_in(),
(int)g.rc_6.get_radio_in(),
(int)g.rc_7.get_radio_in(),
(int)g.rc_8.get_radio_in());
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
while(1) {
hal.scheduler->delay(20);
// New radio frame? (we could use also if((millis()- timer) > 20)
if (hal.rcin->new_input()) {
cliSerial->print("CH:");
for(int16_t i = 0; i < 8; i++) {
cliSerial->print(hal.rcin->read(i)); // Print channel values
print_comma();
servo_write(i, hal.rcin->read(i)); // Copy input to Servos
}
cliSerial->println();
}
if (cliSerial->available() > 0) {
return (0);
}
}
return 0;
}
int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1) {
hal.scheduler->delay(20);
read_radio();
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("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
(int)channel_roll->get_control_in(),
(int)channel_pitch->get_control_in(),
(int)channel_throttle->get_control_in(),
(int)channel_rudder->get_control_in(),
(int)g.rc_5.get_control_in(),
(int)g.rc_6.get_control_in(),
(int)g.rc_7.get_control_in(),
(int)g.rc_8.get_control_in() );
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test = 0;
print_hit_enter();
for(int16_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->println("Unplug battery, throttle in neutral, turn off radio.");
while(channel_throttle->get_control_in() > 0) {
hal.scheduler->delay(20);
read_radio();
}
while(1) {
hal.scheduler->delay(20);
read_radio();
if(channel_throttle->get_control_in() > 0) {
cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->get_control_in());
fail_test++;
}
if(oldSwitchPosition != readSwitch()) {
cliSerial->print("CONTROL MODE CHANGED: ");
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(rc_failsafe_active()) {
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->get_radio_in());
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(fail_test > 0) {
return (0);
}
if(cliSerial->available() > 0) {
cliSerial->println("LOS caused no change in APM.");
return (0);
}
}
}
int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
while(1) {
cliSerial->println("Relay on");
relay.on(0);
hal.scheduler->delay(3000);
if(cliSerial->available() > 0) {
return (0);
}
cliSerial->println("Relay off");
relay.off(0);
hal.scheduler->delay(3000);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv)
{
hal.scheduler->delay(1000);
// save the alitude above home option
if (g.RTL_altitude_cm < 0) {
cliSerial->println("Hold current altitude");
}else{
cliSerial->printf("Hold altitude of %dm\n", (int)g.RTL_altitude_cm/100);
}
cliSerial->printf("%d waypoints\n", (int)mission.num_commands());
cliSerial->printf("Hit radius: %d\n", (int)g.waypoint_radius);
cliSerial->printf("Loiter radius: %d\n\n", (int)g.loiter_radius);
for(uint8_t i = 0; i <= mission.num_commands(); i++) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(i,temp_cmd)) {
test_wp_print(temp_cmd);
}
}
return (0);
}
void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd)
{
cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n",
(int)cmd.index,
(int)cmd.id,
(int)cmd.content.location.options,
(int)cmd.p1,
(long)cmd.content.location.alt,
(long)cmd.content.location.lat,
(long)cmd.content.location.lng);
}
int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
cliSerial->println("Begin XBee X-CTU Range and RSSI Test:");
while(1) {
if (hal.uartC->available())
hal.uartC->write(hal.uartC->read());
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
cliSerial->print("Control CH ");
cliSerial->println(FLIGHT_MODE_CHANNEL, BASE_DEC);
while(1) {
hal.scheduler->delay(20);
uint8_t switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition) {
cliSerial->printf("Position %d\n", (int)switchPosition);
oldSwitchPosition = switchPosition;
}
if(cliSerial->available() > 0) {
return (0);
}
}
}
/*
* test the dataflash is working
*/

6
ArduPlane/tiltrotor.cpp
View File

@ -15,7 +15,7 @@ void QuadPlane::tiltrotor_slew(float newtilt)
tilt.current_tilt = constrain_float(newtilt, tilt.current_tilt-max_change, tilt.current_tilt+max_change);
// translate to 0..1000 range and output
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_motor_tilt, 1000 * tilt.current_tilt);
SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * tilt.current_tilt);
// setup tilt compensation
motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&QuadPlane::tilt_compensate, void, float *, uint8_t));
@ -42,7 +42,7 @@ void QuadPlane::tiltrotor_update(void)
// a forward motor
tiltrotor_slew(1);
float new_throttle = plane.channel_throttle->get_servo_out()*0.01f;
float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1);
if (tilt.current_tilt < 1) {
tilt.current_throttle = constrain_float(new_throttle,
tilt.current_throttle-max_change,
@ -96,7 +96,7 @@ void QuadPlane::tiltrotor_update(void)
// Q_TILT_MAX. Anything above 50% throttle gets
// Q_TILT_MAX. Below 50% throttle we decrease linearly. This
// relies heavily on Q_VFWD_GAIN being set appropriately.
float settilt = constrain_float(plane.channel_throttle->get_servo_out() / 50.0f, 0, 1);
float settilt = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 50.0f, 0, 1);
tiltrotor_slew(settilt * tilt.max_angle_deg / 90.0f);
}
}