mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Plane: added new TRAINING mode
this mode gives manual control when the roll or pitch is within the set limits (the same limits as FBW mode), and prevents the pilot from flying beyond those limits, essentially a "attitude limited manual" mode
This commit is contained in:
Andrew Tridgell
parent
cd430cb6a8
commit
feca411943
@ -208,6 +208,10 @@ AP_AHRS_HIL ahrs(&ins, g_gps);
|
||||
#error Unrecognised HIL_MODE setting.
|
||||
#endif // HIL MODE
|
||||
|
||||
// Training mode
|
||||
static bool training_manual_roll; // user has manual roll control
|
||||
static bool training_manual_pitch; // user has manual pitch control
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// GCS selection
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -1095,6 +1099,37 @@ static void update_current_flight_mode(void)
|
||||
calc_throttle();
|
||||
break;
|
||||
|
||||
case TRAINING: {
|
||||
training_manual_roll = false;
|
||||
training_manual_pitch = false;
|
||||
|
||||
// if the roll is past the set roll limit, then
|
||||
// we set target roll to the limit
|
||||
if (ahrs.roll_sensor >= g.roll_limit_cd) {
|
||||
nav_roll_cd = g.roll_limit_cd;
|
||||
} else if (ahrs.roll_sensor <= -g.roll_limit_cd) {
|
||||
nav_roll_cd = -g.roll_limit_cd;
|
||||
} else {
|
||||
training_manual_roll = true;
|
||||
nav_roll_cd = 0;
|
||||
}
|
||||
|
||||
// if the pitch is past the set pitch limits, then
|
||||
// we set target pitch to the limit
|
||||
if (ahrs.pitch_sensor >= g.pitch_limit_max_cd) {
|
||||
nav_pitch_cd = g.pitch_limit_max_cd;
|
||||
} else if (ahrs.pitch_sensor <= g.pitch_limit_min_cd) {
|
||||
nav_pitch_cd = g.pitch_limit_min_cd;
|
||||
} else {
|
||||
training_manual_pitch = true;
|
||||
nav_pitch_cd = 0;
|
||||
}
|
||||
if (inverted_flight) {
|
||||
nav_pitch_cd = -nav_pitch_cd;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case FLY_BY_WIRE_A: {
|
||||
// set nav_roll and nav_pitch using sticks
|
||||
nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd;
|
||||
@ -1192,11 +1227,12 @@ static void update_navigation()
|
||||
break;
|
||||
|
||||
case MANUAL:
|
||||
case STABILIZE:
|
||||
case TRAINING:
|
||||
case INITIALISING:
|
||||
case FLY_BY_WIRE_A:
|
||||
case FLY_BY_WIRE_B:
|
||||
case CIRCLE:
|
||||
case STABILIZE:
|
||||
// nothing to do
|
||||
break;
|
||||
}
|
||||
|
||||
@ -54,14 +54,14 @@ static bool stick_mixing_enabled(void)
|
||||
}
|
||||
|
||||
|
||||
static void stabilize()
|
||||
/*
|
||||
this is the main roll stabilization function. It takes the
|
||||
previously set nav_roll calculates roll servo_out to try to
|
||||
stabilize the plane at the given roll
|
||||
*/
|
||||
static void stabilize_roll(float speed_scaler)
|
||||
{
|
||||
float ch1_inf = 1.0;
|
||||
float ch2_inf = 1.0;
|
||||
float ch4_inf = 1.0;
|
||||
float speed_scaler = get_speed_scaler();
|
||||
|
||||
if(crash_timer > 0) {
|
||||
if (crash_timer > 0) {
|
||||
nav_roll_cd = 0;
|
||||
}
|
||||
|
||||
@ -78,50 +78,80 @@ static void stabilize()
|
||||
#if APM_CONTROL == DISABLED
|
||||
// Calculate dersired servo output for the roll
|
||||
// ---------------------------------------------
|
||||
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
|
||||
int32_t tempcalc = nav_pitch_cd +
|
||||
fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
|
||||
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
|
||||
(ahrs.pitch_sensor - g.pitch_trim_cd);
|
||||
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
|
||||
#else // APM_CONTROL == ENABLED
|
||||
// calculate roll and pitch control using new APM_Control library
|
||||
g.channel_roll.servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
this is the main pitch stabilization function. It takes the
|
||||
previously set nav_pitch and calculates servo_out values to try to
|
||||
stabilize the plane at the given attitude.
|
||||
*/
|
||||
static void stabilize_pitch(float speed_scaler)
|
||||
{
|
||||
#if APM_CONTROL == DISABLED
|
||||
int32_t tempcalc = nav_pitch_cd +
|
||||
fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
|
||||
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
|
||||
(ahrs.pitch_sensor - g.pitch_trim_cd);
|
||||
if (inverted_flight) {
|
||||
// when flying upside down the elevator control is inverted
|
||||
tempcalc = -tempcalc;
|
||||
}
|
||||
g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, speed_scaler);
|
||||
g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, speed_scaler);
|
||||
#else // APM_CONTROL == ENABLED
|
||||
// calculate roll and pitch control using new APM_Control library
|
||||
g.channel_roll.servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE);
|
||||
g.channel_pitch.servo_out = g.pitchController.get_servo_out(nav_pitch_cd, speed_scaler, control_mode == STABILIZE);
|
||||
g.channel_pitch.servo_out = g.pitchController.get_servo_out(nav_pitch_cd, speed_scaler, control_mode == STABILIZE);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
this gives the user control of the aircraft in stabilization modes
|
||||
*/
|
||||
static void stabilize_stick_mixing()
|
||||
{
|
||||
if (!stick_mixing_enabled() ||
|
||||
control_mode == FLY_BY_WIRE_A ||
|
||||
control_mode == FLY_BY_WIRE_B ||
|
||||
control_mode == TRAINING) {
|
||||
return;
|
||||
}
|
||||
// do stick mixing on aileron/elevator
|
||||
float ch1_inf;
|
||||
float ch2_inf;
|
||||
|
||||
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
|
||||
ch1_inf = fabs(ch1_inf);
|
||||
ch1_inf = min(ch1_inf, 400.0);
|
||||
ch1_inf = ((400.0 - ch1_inf) /400.0);
|
||||
|
||||
ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
|
||||
ch2_inf = fabs(ch2_inf);
|
||||
ch2_inf = min(ch2_inf, 400.0);
|
||||
ch2_inf = ((400.0 - ch2_inf) /400.0);
|
||||
|
||||
// scale the sensor input based on the stick input
|
||||
// -----------------------------------------------
|
||||
g.channel_roll.servo_out *= ch1_inf;
|
||||
g.channel_pitch.servo_out *= ch2_inf;
|
||||
|
||||
// Mix in stick inputs
|
||||
// -------------------
|
||||
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
|
||||
g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
stabilize the yaw axis
|
||||
*/
|
||||
static void stabilize_yaw(float speed_scaler)
|
||||
{
|
||||
float ch4_inf = 1.0;
|
||||
|
||||
// Mix Stick input to allow users to override control surfaces
|
||||
// -----------------------------------------------------------
|
||||
if (stick_mixing_enabled()) {
|
||||
if (control_mode != FLY_BY_WIRE_A &&
|
||||
control_mode != FLY_BY_WIRE_B) {
|
||||
// do stick mixing on aileron/elevator if not in a fly by
|
||||
// wire mode
|
||||
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
|
||||
ch1_inf = fabs(ch1_inf);
|
||||
ch1_inf = min(ch1_inf, 400.0);
|
||||
ch1_inf = ((400.0 - ch1_inf) /400.0);
|
||||
|
||||
ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
|
||||
ch2_inf = fabs(ch2_inf);
|
||||
ch2_inf = min(ch2_inf, 400.0);
|
||||
ch2_inf = ((400.0 - ch2_inf) /400.0);
|
||||
|
||||
// scale the sensor input based on the stick input
|
||||
// -----------------------------------------------
|
||||
g.channel_roll.servo_out *= ch1_inf;
|
||||
g.channel_pitch.servo_out *= ch2_inf;
|
||||
|
||||
// Mix in stick inputs
|
||||
// -------------------
|
||||
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
|
||||
g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
|
||||
}
|
||||
|
||||
// stick mixing performed for rudder for all cases including FBW
|
||||
// important for steering on the ground during landing
|
||||
// -----------------------------------------------
|
||||
@ -131,19 +161,64 @@ static void stabilize()
|
||||
ch4_inf = ((400.0 - ch4_inf) /400.0);
|
||||
}
|
||||
|
||||
// Apply output to Rudder
|
||||
// ----------------------
|
||||
calc_nav_yaw(speed_scaler, ch4_inf);
|
||||
g.channel_rudder.servo_out *= ch4_inf;
|
||||
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
|
||||
|
||||
// Call slew rate limiter if used
|
||||
// ------------------------------
|
||||
//#if(ROLL_SLEW_LIMIT != 0)
|
||||
// g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out);
|
||||
//#endif
|
||||
// Apply output to Rudder
|
||||
calc_nav_yaw(speed_scaler, ch4_inf);
|
||||
g.channel_rudder.servo_out *= ch4_inf;
|
||||
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
a special stabilization function for training mode
|
||||
*/
|
||||
static void stabilize_training(float speed_scaler)
|
||||
{
|
||||
if (training_manual_roll) {
|
||||
g.channel_roll.servo_out = g.channel_roll.control_in;
|
||||
} else {
|
||||
// calculate what is needed to hold
|
||||
stabilize_roll(speed_scaler);
|
||||
if ((nav_roll_cd > 0 && g.channel_roll.control_in < g.channel_roll.servo_out) ||
|
||||
(nav_roll_cd < 0 && g.channel_roll.control_in > g.channel_roll.servo_out)) {
|
||||
// allow user to get out of the roll
|
||||
g.channel_roll.servo_out = g.channel_roll.control_in;
|
||||
}
|
||||
}
|
||||
|
||||
if (training_manual_pitch) {
|
||||
g.channel_pitch.servo_out = g.channel_pitch.control_in;
|
||||
} else {
|
||||
stabilize_pitch(speed_scaler);
|
||||
if ((nav_pitch_cd > 0 && g.channel_pitch.control_in < g.channel_pitch.servo_out) ||
|
||||
(nav_pitch_cd < 0 && g.channel_pitch.control_in > g.channel_pitch.servo_out)) {
|
||||
// allow user to get back to level
|
||||
g.channel_pitch.servo_out = g.channel_pitch.control_in;
|
||||
}
|
||||
}
|
||||
|
||||
stabilize_stick_mixing();
|
||||
stabilize_yaw(speed_scaler);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
main stabilization function for all 3 axes
|
||||
*/
|
||||
static void stabilize()
|
||||
{
|
||||
float speed_scaler = get_speed_scaler();
|
||||
|
||||
if (control_mode == TRAINING) {
|
||||
stabilize_training(speed_scaler);
|
||||
} else {
|
||||
stabilize_roll(speed_scaler);
|
||||
stabilize_pitch(speed_scaler);
|
||||
stabilize_stick_mixing();
|
||||
stabilize_yaw(speed_scaler);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void crash_checker()
|
||||
{
|
||||
if(ahrs.pitch_sensor < -4500) {
|
||||
@ -192,8 +267,8 @@ static void calc_throttle()
|
||||
* Calculate desired roll/pitch/yaw angles (in medium freq loop)
|
||||
*****************************************/
|
||||
|
||||
// Yaw is separated into a function for future implementation of heading hold on rolling take-off
|
||||
// ----------------------------------------------------------------------------------------
|
||||
// Yaw is separated into a function for heading hold on rolling take-off
|
||||
// ----------------------------------------------------------------------
|
||||
static void calc_nav_yaw(float speed_scaler, float ch4_inf)
|
||||
{
|
||||
if (hold_course != -1) {
|
||||
@ -349,10 +424,9 @@ static bool suppress_throttle(void)
|
||||
*****************************************/
|
||||
static void set_servos(void)
|
||||
{
|
||||
int16_t flapSpeedSource = 0;
|
||||
int16_t last_throttle = g.channel_throttle.radio_out;
|
||||
|
||||
if(control_mode == MANUAL) {
|
||||
if (control_mode == MANUAL) {
|
||||
// do a direct pass through of radio values
|
||||
if (g.mix_mode == 0) {
|
||||
g.channel_roll.radio_out = g.channel_roll.radio_in;
|
||||
@ -462,7 +536,9 @@ static void set_servos(void)
|
||||
g.channel_throttle.calc_pwm();
|
||||
}
|
||||
} else if (g.throttle_passthru_stabilize &&
|
||||
(control_mode == STABILIZE || control_mode == FLY_BY_WIRE_A)) {
|
||||
(control_mode == STABILIZE ||
|
||||
control_mode == TRAINING ||
|
||||
control_mode == FLY_BY_WIRE_A)) {
|
||||
// manual pass through of throttle while in FBWA or
|
||||
// STABILIZE mode with THR_PASS_STAB set
|
||||
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
|
||||
@ -477,6 +553,8 @@ static void set_servos(void)
|
||||
if(control_mode < FLY_BY_WIRE_B) {
|
||||
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_flap_auto);
|
||||
} else if (control_mode >= FLY_BY_WIRE_B) {
|
||||
int16_t flapSpeedSource = 0;
|
||||
|
||||
// FIXME: use target_airspeed in both FBW_B and g.airspeed_enabled cases - Doug?
|
||||
if (control_mode == FLY_BY_WIRE_B) {
|
||||
flapSpeedSource = target_airspeed_cm * 0.01;
|
||||
|
||||
@ -38,6 +38,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||
// ArduPlane documentation
|
||||
switch (control_mode) {
|
||||
case MANUAL:
|
||||
case TRAINING:
|
||||
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||
break;
|
||||
case STABILIZE:
|
||||
@ -61,6 +62,10 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||
break;
|
||||
}
|
||||
|
||||
if (!training_manual_pitch || !training_manual_roll) {
|
||||
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
}
|
||||
|
||||
if (control_mode != MANUAL && control_mode != INITIALISING) {
|
||||
// stabiliser of some form is enabled
|
||||
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
@ -162,6 +167,13 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
||||
control_sensors_enabled |= (1<<15); // motor control
|
||||
break;
|
||||
|
||||
case TRAINING:
|
||||
if (!training_manual_roll || !training_manual_pitch) {
|
||||
control_sensors_enabled |= (1<<10); // 3D angular rate control
|
||||
control_sensors_enabled |= (1<<11); // attitude stabilisation
|
||||
}
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
case RTL:
|
||||
case LOITER:
|
||||
@ -1156,6 +1168,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
case MANUAL:
|
||||
case CIRCLE:
|
||||
case STABILIZE:
|
||||
case TRAINING:
|
||||
case FLY_BY_WIRE_A:
|
||||
case FLY_BY_WIRE_B:
|
||||
case AUTO:
|
||||
|
||||
@ -355,7 +355,7 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
|
||||
// @Param: FLTMODE1
|
||||
// @DisplayName: FlightMode1
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @User: Standard
|
||||
// @Description: Flight mode for switch position 1 (910 to 1230 and above 2049)
|
||||
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
|
||||
@ -363,35 +363,35 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Param: FLTMODE2
|
||||
// @DisplayName: FlightMode2
|
||||
// @Description: Flight mode for switch position 2 (1231 to 1360)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
|
||||
|
||||
// @Param: FLTMODE3
|
||||
// @DisplayName: FlightMode3
|
||||
// @Description: Flight mode for switch position 3 (1361 to 1490)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
|
||||
|
||||
// @Param: FLTMODE4
|
||||
// @DisplayName: FlightMode4
|
||||
// @Description: Flight mode for switch position 4 (1491 to 1620)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
|
||||
|
||||
// @Param: FLTMODE5
|
||||
// @DisplayName: FlightMode5
|
||||
// @Description: Flight mode for switch position 5 (1621 to 1749)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
|
||||
|
||||
// @Param: FLTMODE6
|
||||
// @DisplayName: FlightMode6
|
||||
// @Description: Flight mode for switch position 6 (1750 to 2049)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
|
||||
|
||||
|
||||
@ -60,16 +60,17 @@
|
||||
#define HIL_MODE_SENSORS 2
|
||||
|
||||
enum FlightMode {
|
||||
MANUAL = 0,
|
||||
CIRCLE = 1,
|
||||
STABILIZE = 2,
|
||||
MANUAL = 0,
|
||||
CIRCLE = 1,
|
||||
STABILIZE = 2,
|
||||
TRAINING = 3,
|
||||
FLY_BY_WIRE_A = 5,
|
||||
FLY_BY_WIRE_B = 6,
|
||||
AUTO = 10,
|
||||
RTL = 11,
|
||||
LOITER = 12,
|
||||
GUIDED = 15,
|
||||
INITIALISING = 16
|
||||
AUTO = 10,
|
||||
RTL = 11,
|
||||
LOITER = 12,
|
||||
GUIDED = 15,
|
||||
INITIALISING = 16
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -11,8 +11,9 @@ static void failsafe_short_on_event(int16_t fstype)
|
||||
{
|
||||
case MANUAL:
|
||||
case STABILIZE:
|
||||
case FLY_BY_WIRE_A: // middle position
|
||||
case FLY_BY_WIRE_B: // middle position
|
||||
case FLY_BY_WIRE_A:
|
||||
case FLY_BY_WIRE_B:
|
||||
case TRAINING:
|
||||
set_mode(CIRCLE);
|
||||
break;
|
||||
|
||||
@ -43,8 +44,9 @@ static void failsafe_long_on_event(int16_t fstype)
|
||||
{
|
||||
case MANUAL:
|
||||
case STABILIZE:
|
||||
case FLY_BY_WIRE_A: // middle position
|
||||
case FLY_BY_WIRE_B: // middle position
|
||||
case FLY_BY_WIRE_A:
|
||||
case FLY_BY_WIRE_B:
|
||||
case TRAINING:
|
||||
case CIRCLE:
|
||||
set_mode(RTL);
|
||||
break;
|
||||
|
||||
@ -73,17 +73,30 @@ static void read_radio()
|
||||
{
|
||||
ch1_temp = hal.rcin->read(CH_ROLL);
|
||||
ch2_temp = hal.rcin->read(CH_PITCH);
|
||||
uint16_t pwm_roll, pwm_pitch;
|
||||
|
||||
if(g.mix_mode == 0) {
|
||||
g.channel_roll.set_pwm(ch1_temp);
|
||||
g.channel_pitch.set_pwm(ch2_temp);
|
||||
if (g.mix_mode == 0) {
|
||||
pwm_roll = ch1_temp;
|
||||
pwm_pitch = ch2_temp;
|
||||
}else{
|
||||
g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
|
||||
g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
|
||||
pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500;
|
||||
pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500;
|
||||
}
|
||||
|
||||
if (control_mode == TRAINING) {
|
||||
// in training mode we don't want to use a deadzone, as we
|
||||
// want manual pass through when not exceeding attitude limits
|
||||
g.channel_roll.set_pwm_no_deadzone(pwm_roll);
|
||||
g.channel_pitch.set_pwm_no_deadzone(pwm_pitch);
|
||||
g.channel_throttle.set_pwm_no_deadzone(hal.rcin->read(CH_3));
|
||||
g.channel_rudder.set_pwm_no_deadzone(hal.rcin->read(CH_4));
|
||||
} else {
|
||||
g.channel_roll.set_pwm(pwm_roll);
|
||||
g.channel_pitch.set_pwm(pwm_pitch);
|
||||
g.channel_throttle.set_pwm(hal.rcin->read(CH_3));
|
||||
g.channel_rudder.set_pwm(hal.rcin->read(CH_4));
|
||||
}
|
||||
|
||||
g.channel_throttle.set_pwm(hal.rcin->read(CH_3));
|
||||
g.channel_rudder.set_pwm(hal.rcin->read(CH_4));
|
||||
g.rc_5.set_pwm(hal.rcin->read(CH_5));
|
||||
g.rc_6.set_pwm(hal.rcin->read(CH_6));
|
||||
g.rc_7.set_pwm(hal.rcin->read(CH_7));
|
||||
|
||||
@ -233,6 +233,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
mode != MANUAL &&
|
||||
mode != CIRCLE &&
|
||||
mode != STABILIZE &&
|
||||
mode != TRAINING &&
|
||||
mode != FLY_BY_WIRE_A &&
|
||||
mode != FLY_BY_WIRE_B &&
|
||||
mode != AUTO &&
|
||||
|
||||
@ -330,6 +330,7 @@ static void set_mode(enum FlightMode mode)
|
||||
case MANUAL:
|
||||
case CIRCLE:
|
||||
case STABILIZE:
|
||||
case TRAINING:
|
||||
case FLY_BY_WIRE_A:
|
||||
case FLY_BY_WIRE_B:
|
||||
break;
|
||||
@ -571,6 +572,9 @@ print_flight_mode(uint8_t mode)
|
||||
case STABILIZE:
|
||||
cliSerial->println_P(PSTR("Stabilize"));
|
||||
break;
|
||||
case TRAINING:
|
||||
cliSerial->println_P(PSTR("Training"));
|
||||
break;
|
||||
case FLY_BY_WIRE_A:
|
||||
cliSerial->println_P(PSTR("FBW_A"));
|
||||
break;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user