Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add AP_INVERTED_FLIGHT_ENABLED #24332

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
6 changes: 4 additions & 2 deletions ArduCopter/RC_Channel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,9 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi
case AUX_FUNC::ACRO_TRAINER:
case AUX_FUNC::ATTCON_ACCEL_LIM:
case AUX_FUNC::ATTCON_FEEDFWD:
#if AP_INVERTED_FLIGHT_ENABLED
case AUX_FUNC::INVERTED:
#endif
case AUX_FUNC::MOTOR_INTERLOCK:
case AUX_FUNC::PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
case AUX_FUNC::PARACHUTE_ENABLE:
Expand Down Expand Up @@ -412,7 +414,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
break;
#endif

#if FRAME_CONFIG == HELI_FRAME
#if AP_INVERTED_FLIGHT_ENABLED && FRAME_CONFIG == HELI_FRAME
case AUX_FUNC::INVERTED:
switch (ch_flag) {
case AuxSwitchPos::HIGH:
Expand All @@ -430,7 +432,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
break;
}
break;
#endif
#endif // AP_INVERTED_FLIGHT_ENABLED && FRAME_CONFIG == HELI_FRAME

#if AP_WINCH_ENABLED
case AUX_FUNC::WINCH_ENABLE:
Expand Down
2 changes: 2 additions & 0 deletions ArduCopter/mode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -458,10 +458,12 @@ void Copter::exit_mode(Mode *&old_flightmode,
}
}

#if AP_INVERTED_FLIGHT_ENABLED
// Make sure inverted flight is disabled if not supported in the new mode
if (!new_flightmode->allows_inverted()) {
attitude_control->set_inverted_flight(false);
}
#endif
#endif //HELI_FRAME
}

Expand Down
4 changes: 3 additions & 1 deletion ArduCopter/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ class Mode {
virtual bool allows_autotune() const { return false; }
virtual bool allows_flip() const { return false; }

#if FRAME_CONFIG == HELI_FRAME
#if FRAME_CONFIG == HELI_FRAME && AP_INVERTED_FLIGHT_ENABLED
virtual bool allows_inverted() const { return false; };
#endif

Expand Down Expand Up @@ -1583,7 +1583,9 @@ class ModeStabilize_Heli : public ModeStabilize {
bool init(bool ignore_checks) override;
void run() override;

#if AP_INVERTED_FLIGHT_ENABLED
bool allows_inverted() const override { return true; };
#endif

protected:

Expand Down
6 changes: 6 additions & 0 deletions ArduPlane/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,7 @@ bool Plane::stick_mixing_enabled(void)
*/
void Plane::stabilize_roll()
{
#if AP_INVERTED_FLIGHT_ENABLED
if (fly_inverted()) {
// we want to fly upside down. We need to cope with wrap of
// the roll_sensor interfering with wrap of nav_roll, which
Expand All @@ -119,6 +120,7 @@ void Plane::stabilize_roll()
nav_roll_cd += 18000;
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
}
#endif

const float roll_out = stabilize_roll_get_roll_out();
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
Expand Down Expand Up @@ -306,9 +308,11 @@ void Plane::stabilize_stick_mixing_fbw()
} else if (pitch_input < -0.5f) {
pitch_input = (3*pitch_input + 1);
}
#if AP_INVERTED_FLIGHT_ENABLED
if (fly_inverted()) {
pitch_input = -pitch_input;
}
#endif
if (pitch_input > 0) {
nav_pitch_cd += pitch_input * aparm.pitch_limit_max*100;
} else {
Expand Down Expand Up @@ -641,10 +645,12 @@ void Plane::update_load_factor(void)
// stall prevention is disabled
return;
}
#if AP_INVERTED_FLIGHT_ENABLED
if (fly_inverted()) {
// no roll limits when inverted
return;
}
#endif
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.active()) {
// no limits while hovering
Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -314,8 +314,10 @@ class Plane : public AP_Vehicle {
// This is set to 254 when we need to re-read the switch
uint8_t oldSwitchPosition = 254;

#if AP_INVERTED_FLIGHT_ENABLED
// This is used to enable the inverted flight feature
bool inverted_flight;
#endif

// last time we ran roll/pitch stabilization
uint32_t last_stabilize_ms;
Expand Down Expand Up @@ -489,8 +491,10 @@ class Plane : public AP_Vehicle {
// are we headed to the land approach waypoint? Works for any nav type
bool wp_is_land_approach;

#if AP_INVERTED_FLIGHT_ENABLED
// should we fly inverted?
bool inverted_flight;
#endif

// should we enable cross-tracking for the next waypoint?
bool next_wp_crosstrack;
Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/RC_Channel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
case AUX_FUNC::TRAINING:
case AUX_FUNC::FLAP:
case AUX_FUNC::GUIDED:
#if AP_INVERTED_FLIGHT_ENABLED
case AUX_FUNC::INVERTED:
#endif
case AUX_FUNC::LOITER:
case AUX_FUNC::MANUAL:
case AUX_FUNC::RTL:
Expand Down Expand Up @@ -216,9 +218,11 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag)
{
switch(ch_option) {
#if AP_INVERTED_FLIGHT_ENABLED
case AUX_FUNC::INVERTED:
plane.inverted_flight = (ch_flag == AuxSwitchPos::HIGH);
break;
#endif

case AUX_FUNC::REVERSE_THROTTLE:
plane.reversed_throttle = (ch_flag == AuxSwitchPos::HIGH);
Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/commands_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -134,12 +134,14 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
do_set_home(cmd);
break;

#if AP_INVERTED_FLIGHT_ENABLED
case MAV_CMD_DO_INVERTED_FLIGHT:
if (cmd.p1 == 0 || cmd.p1 == 1) {
auto_state.inverted_flight = (bool)cmd.p1;
gcs().send_text(MAV_SEVERITY_INFO, "Set inverted %u", cmd.p1);
}
break;
#endif

case MAV_CMD_DO_LAND_START:
break;
Expand Down Expand Up @@ -319,7 +321,9 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
#if AP_INVERTED_FLIGHT_ENABLED
case MAV_CMD_DO_INVERTED_FLIGHT:
#endif
case MAV_CMD_DO_LAND_START:
case MAV_CMD_DO_FENCE_ENABLE:
case MAV_CMD_DO_AUTOTUNE_ENABLE:
Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/control_modes.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,7 @@ void Plane::autotune_enable(bool enable)
}
}

#if AP_INVERTED_FLIGHT_ENABLED
/*
are we flying inverted?
*/
Expand All @@ -186,3 +187,4 @@ bool Plane::fly_inverted(void)
}
return false;
}
#endif
4 changes: 3 additions & 1 deletion ArduPlane/mode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,11 @@ bool Mode::enter()
plane.nav_scripting.enabled = false;
#endif

#if AP_INVERTED_FLIGHT_ENABLED
// cancel inverted flight
plane.auto_state.inverted_flight = false;

#endif

// cancel waiting for rudder neutral
plane.takeoff_state.waiting_for_rudder_neutral = false;

Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/mode_fbwa.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,11 @@ void ModeFBWA::update()
}
plane.adjust_nav_pitch_throttle();
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);
#if AP_INVERTED_FLIGHT_ENABLED
if (plane.fly_inverted()) {
plane.nav_pitch_cd = -plane.nav_pitch_cd;
}
#endif
if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) {
// FBWA failsafe glide
plane.nav_roll_cd = 0;
Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/mode_training.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,11 @@ void ModeTraining::update()
plane.training_manual_pitch = true;
plane.nav_pitch_cd = 0;
}
#if AP_INVERTED_FLIGHT_ENABLED
if (plane.fly_inverted()) {
plane.nav_pitch_cd = -plane.nav_pitch_cd;
}
#endif
}

/*
Expand Down
6 changes: 6 additions & 0 deletions ArduPlane/tailsitter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -557,9 +557,11 @@ bool Tailsitter::transition_vtol_complete(void) const
return true;
}
int32_t roll_cd = labs(plane.ahrs.roll_sensor);
#if AP_INVERTED_FLIGHT_ENABLED
if (plane.fly_inverted()) {
roll_cd = 18000 - roll_cd;
}
#endif
if (roll_cd > MAX(4500, plane.roll_limit_cd + 500)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, roll error");
return true;
Expand Down Expand Up @@ -845,7 +847,11 @@ void Tailsitter_Transition::update()
quadplane.assisted_flight = true;
uint32_t dt = now - fw_transition_start_ms;
// multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
#if AP_INVERTED_FLIGHT_ENABLED
plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500);
#else
plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f, -8500, 8500);
#endif
Comment on lines +850 to +854
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would prefer to pull out either the sign or the fly inverted bool and keep just one line of the constrain. I think the maths is easier to understand if there is only one copy.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe we could take the opportunity to break it up a little.

Suggested change
#if AP_INVERTED_FLIGHT_ENABLED
plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500);
#else
plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f, -8500, 8500);
#endif
float pitch_ramp = quadplane.tailsitter.transition_rate_fw * dt * 0.1;
#if AP_INVERTED_FLIGHT_ENABLED
if (plane.fly_inverted()) {
// Transitioning to inverted flight, pitch up rather than down
pitch_ramp *= -1.0;
}
#endif
plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - pitch_ramp, -8500, 8500);

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe we could take the opportunity to break it up a little.

Yep, I did that, and random other changes like it.

And reverted those changes, because I really like to keep these add-defines things as completely no compiler output change in the case that you have not turned the option on. It means that no existing user will ever run into problems with the code.

I've achieved that with this PR now, and updated the blocks in the opening comment appropriately.

It does uglify the code a bit, but I do plan on going back to the places and cleaning it up by factoring bits and pieces. Especially the bit in takeoff.cpp. The block that patch is in will make for a nice new method with earl returns etc, something like:

    if (do_takeoff_attitude_check &&
         !takeoff_attitude_is_good()) {
            gcs().send_text(MAV_SEVERITY_WARNING, "Bad launch AUTO");
            takeoff_state.accel_event_counter = 0;
            goto no_launch;
        }
    }
        // Check aircraft attitude for bad launch
        bool takeoff_attitude_is_good(void) const
        {
            if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500) {
                return false;
            }
#if AP_INVERTED_FLIGHT_ENABLED
            if (fly_inverted()) {
                // skip roll check if we're *supposed* to be upside-down
                return true;
            }
#if endif
            if (labs(ahrs.roll_sensor) > 3000) {
                    return false;
            }
            return true;
        }
    }

I think that's the same logic, but I'm not convinced it's good logic.... e.g. should those pitch limits be the other way around if you're inverted? Why aren't we checking roll when inverted?

Give I think we now have a partner who flies this way, we should probably give it more serious consideration now, and I think this would make for a good separate PR...

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've created a patch on top of this over here which shows things prettied up a bit:
https://github.com/ArduPilot/ardupilot/compare/master...peterbarker:ardupilot:pr/inverted-flight-tidy?expand=1

plane.nav_roll_cd = 0;
quadplane.disable_yaw_rate_time_constant();
quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
Expand Down
6 changes: 5 additions & 1 deletion ArduPlane/takeoff.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,11 @@ bool Plane::auto_takeoff_check(void)
if (do_takeoff_attitude_check) {
// Check aircraft attitude for bad launch
if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500 ||
(!fly_inverted() && labs(ahrs.roll_sensor) > 3000)) {
(
#if AP_INVERTED_FLIGHT_ENABLED
!fly_inverted() &&
#endif
labs(ahrs.roll_sensor) > 3000)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Bad launch AUTO");
takeoff_state.accel_event_counter = 0;
goto no_launch;
Expand Down
2 changes: 2 additions & 0 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.h
Original file line number Diff line number Diff line change
Expand Up @@ -376,8 +376,10 @@ class AC_AttitudeControl {
char *failure_msg,
const uint8_t failure_msg_len);

#if AP_INVERTED_FLIGHT_ENABLED
// enable inverted flight on backends that support it
virtual void set_inverted_flight(bool inverted) {}
#endif

// get the slew rate value for roll, pitch and yaw, for oscillation detection in lua scripts
void get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate);
Expand Down
6 changes: 6 additions & 0 deletions libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -567,18 +567,22 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
#if AP_INVERTED_FLIGHT_ENABLED
if (_inverted_flight) {
euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
}
#endif
AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds);
}

// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
#if AP_INVERTED_FLIGHT_ENABLED
if (_inverted_flight) {
euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
}
#endif
AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw);
}

Expand All @@ -591,6 +595,7 @@ void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate)
#endif
}

#if AP_INVERTED_FLIGHT_ENABLED
// enable/disable inverted flight
void AC_AttitudeControl_Heli::set_inverted_flight(bool inverted)
{
Expand All @@ -599,3 +604,4 @@ void AC_AttitudeControl_Heli::set_inverted_flight(bool inverted)
}
_inverted_flight = inverted;
}
#endif
4 changes: 3 additions & 1 deletion libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
Original file line number Diff line number Diff line change
Expand Up @@ -81,9 +81,11 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {

// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;


#if AP_INVERTED_FLIGHT_ENABLED
// enable/disable inverted flight
void set_inverted_flight(bool inverted) override;
#endif

// set the PID notch sample rates
void set_notch_sample_rate(float sample_rate) override;
Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_HAL/AP_HAL_Boards.h
Original file line number Diff line number Diff line change
Expand Up @@ -373,3 +373,7 @@
#endif

#define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON)

#ifndef AP_INVERTED_FLIGHT_ENABLED
#define AP_INVERTED_FLIGHT_ENABLED 1
#endif
6 changes: 6 additions & 0 deletions libraries/AP_Mission/AP_Mission.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1222,9 +1222,11 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=release. See PARACHUTE_ACTION enum
break;

#if AP_INVERTED_FLIGHT_ENABLED
case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210
cmd.p1 = packet.param1; // normal=0 inverted=1
break;
#endif

#if AP_GRIPPER_ENABLED
case MAV_CMD_DO_GRIPPER: // MAV ID: 211
Expand Down Expand Up @@ -1738,9 +1740,11 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
packet.param2 = cmd.content.auxfunction.switchpos;
break;

#if AP_INVERTED_FLIGHT_ENABLED
case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210
packet.param1 = cmd.p1; // normal=0 inverted=1
break;
#endif

#if AP_GRIPPER_ENABLED
case MAV_CMD_DO_GRIPPER: // MAV ID: 211
Expand Down Expand Up @@ -2621,8 +2625,10 @@ const char *AP_Mission::Mission_Command::type() const
return "VTOLTakeoff";
case MAV_CMD_NAV_VTOL_LAND:
return "VTOLLand";
#if AP_INVERTED_FLIGHT_ENABLED
case MAV_CMD_DO_INVERTED_FLIGHT:
return "InvertedFlight";
#endif
case MAV_CMD_DO_FENCE_ENABLE:
return "FenceEnable";
case MAV_CMD_DO_AUTOTUNE_ENABLE:
Expand Down
3 changes: 3 additions & 0 deletions libraries/RC_Channel/RC_Channel.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/Bitmask.h>
#include <AP_HAL/AP_HAL_Boards.h>

#define NUM_RC_CHANNELS 16

Expand Down Expand Up @@ -144,7 +145,9 @@ class RC_Channel {
AVOID_PROXIMITY = 40, // enable object avoidance using proximity sensors (ie. horizontal lidar)
ARMDISARM_UNUSED = 41, // UNUSED
SMART_RTL = 42, // change to SmartRTL flight mode
#if AP_INVERTED_FLIGHT_ENABLED
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is the first example of using a define to remove something from the enum. This would start a new pattern.

I do think it is a good idea. The "KILL_IMU" option would be a good candidate too.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

True.

However, we do it a lot in other enumerations, and I'd put down a little bit of money we'll find at least one thing that's Not Quite when we gate them.

INVERTED = 43, // enable inverted flight
#endif
WINCH_ENABLE = 44, // winch enable/disable
WINCH_CONTROL = 45, // winch control
RC_OVERRIDE_ENABLE = 46, // enable RC Override
Expand Down
Loading