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.

Sign up for GitHub

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

Jump to bottom

create a HAL_QUADPLANE_ENABLED define #17941

Merged
merged 2 commits into from Sep 14, 2021
Merged

create a HAL_QUADPLANE_ENABLED define #17941

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5616b5f
AP_Scripting: add and use HAL_QUADPLANE_ENABLED
peterbarker Sep 10, 2021
cfb40ab
ArduPlane: add and use HAL_QUADPLANE_ENABLED
peterbarker Sep 10, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
23 changes: 21 additions & 2 deletions ArduPlane/AP_Arming.cpp
View file
Open in desktop
Expand Up @@ -4,6 +4,8 @@
#include "AP_Arming.h"
#include "Plane.h"

#include "qautotune.h"

constexpr uint32_t AP_ARMING_DELAY_MS = 2000; // delay from arming to start of motor spoolup

const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {
Expand Down Expand Up @@ -72,6 +74,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}

#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.enabled() && !plane.quadplane.available()) {
check_failed(display_failure, "Quadplane enabled but not running");
ret = false;
Expand Down Expand Up @@ -99,6 +102,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
return false;
}
}
#endif

if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {
check_failed(display_failure, "No mission loaded");
Expand All @@ -116,11 +120,13 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}

#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.enabled() && ((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) &&
!plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto)) {
check_failed(display_failure,"not in Q mode");
ret = false;
}
#endif

if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){
int16_t trim = plane.channel_throttle->get_radio_trim();
Expand Down Expand Up @@ -203,7 +209,9 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
void AP_Arming_Plane::change_arm_state(void)
{
update_soft_armed();
#if HAL_QUADPLANE_ENABLED
plane.quadplane.set_armed(hal.util->get_soft_armed());
#endif
}

bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_checks)
Expand All @@ -212,12 +220,14 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
return false;
}

#if HAL_QUADPLANE_ENABLED
if ((method == Method::AUXSWITCH) && (plane.quadplane.options & QuadPlane::OPTION_AIRMODE)) {
// if no airmode switch assigned, honour the QuadPlane option bit:
if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr) {
plane.quadplane.air_mode = AirMode::ON;
}
}
#endif

change_arm_state();

Expand Down Expand Up @@ -260,9 +270,11 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
plane.throttle_suppressed = plane.control_mode->does_auto_throttle();

// if no airmode switch assigned, ensure airmode is off:
#if HAL_QUADPLANE_ENABLED
if ((plane.quadplane.air_mode == AirMode::ON) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) {
plane.quadplane.air_mode = AirMode::OFF;
}
#endif

//only log if disarming was successful
change_arm_state();
Expand All @@ -283,8 +295,15 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec

void AP_Arming_Plane::update_soft_armed()
{
hal.util->set_soft_armed((plane.quadplane.motor_test.running || is_armed()) &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
bool _armed = is_armed();
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.motor_test.running){
_armed = true;
}
#endif
_armed = _armed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;

hal.util->set_soft_armed(_armed);
AP::logger().set_vehicle_armed(hal.util->get_soft_armed());

// update delay_arming oneshot
Expand Down
106 changes: 82 additions & 24 deletions ArduPlane/ArduPlane.cpp
View file
Open in desktop
Expand Up @@ -128,7 +128,11 @@ void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
log_bit = MASK_LOG_PM;
}

#if HAL_QUADPLANE_ENABLED
constexpr int8_t Plane::_failsafe_priorities[7];
#else
constexpr int8_t Plane::_failsafe_priorities[6];
#endif

// update AHRS system
void Plane::ahrs_update()
Expand All @@ -145,7 +149,13 @@ void Plane::ahrs_update()
roll_limit_cd = aparm.roll_limit_cd;
pitch_limit_min_cd = aparm.pitch_limit_min_cd;

if (!quadplane.tailsitter.active()) {
bool rotate_limits = true;
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.active()) {
rotate_limits = false;
}
#endif
if (rotate_limits) {
roll_limit_cd *= ahrs.cos_pitch();
pitch_limit_min_cd *= fabsf(ahrs.cos_roll());
}
Expand All @@ -156,11 +166,13 @@ void Plane::ahrs_update()
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);

#if HAL_QUADPLANE_ENABLED
// check if we have had a yaw reset from the EKF
quadplane.check_yaw_reset();

// update inertial_nav for quadplane
quadplane.inertial_nav.update();
#endif
}

/*
Expand All @@ -175,10 +187,12 @@ void Plane::update_speed_height(void)
SpdHgt_Controller->update_50hz();
}

#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
quadplane.update_throttle_mix();
}
#endif
}


Expand Down Expand Up @@ -424,22 +438,37 @@ void Plane::update_control_mode(void)
steer_state.hold_course_cd = -1;
}

update_fly_forward();

control_mode->update();
}


void Plane::update_fly_forward(void)
{
// ensure we are fly-forward when we are flying as a pure fixed
// wing aircraft. This helps the EKF produce better state
// estimates as it can make stronger assumptions
#if HAL_QUADPLANE_ENABLED
if (quadplane.available() &&
quadplane.tailsitter.is_in_fw_flight()) {
ahrs.set_fly_forward(true);
} else if (quadplane.in_vtol_mode() ||
return;
}

if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
ahrs.set_fly_forward(false);
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
return;
}
#endif

if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
ahrs.set_fly_forward(landing.is_flying_forward());
} else {
ahrs.set_fly_forward(true);
return;
}

control_mode->update();
ahrs.set_fly_forward(true);
}

/*
Expand All @@ -466,9 +495,11 @@ void Plane::update_alt()
{
barometer.update();

#if HAL_QUADPLANE_ENABLED
if (quadplane.available()) {
quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio());
}
#endif

// calculate the sink rate.
float sink_rate;
Expand Down Expand Up @@ -524,9 +555,13 @@ void Plane::update_flight_stage(void)
// Update the speed & height controller states
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
if (control_mode == &mode_auto) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else if (auto_state.takeoff_complete == false) {
return;
}
#endif
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
Expand All @@ -539,23 +574,31 @@ void Plane::update_flight_stage(void)
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
} else if (quadplane.in_assisted_flight()) {
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
} else if (control_mode != &mode_takeoff) {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
} else if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}


Expand Down Expand Up @@ -613,11 +656,13 @@ bool Plane::get_wp_distance_m(float &distance) const
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() : 0;
} else {
distance = auto_state.wp_distance;
return true;
}
#endif
distance = auto_state.wp_distance;
return true;
}

Expand All @@ -627,11 +672,13 @@ bool Plane::get_wp_bearing_deg(float &bearing) const
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
} else {
bearing = nav_controller->target_bearing_cd() * 0.01;
return true;
}
#endif
bearing = nav_controller->target_bearing_cd() * 0.01;
return true;
}

Expand All @@ -641,11 +688,13 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
} else {
xtrack_error = nav_controller->crosstrack_error();
return true;
}
#endif
xtrack_error = nav_controller->crosstrack_error();
return true;
}

Expand Down Expand Up @@ -676,9 +725,11 @@ bool Plane::get_target_location(Location& target_loc)
case Mode::Number::GUIDED:
case Mode::Number::AUTO:
case Mode::Number::LOITER:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
#endif
target_loc = next_WP_loc;
return true;
break;
Expand All @@ -695,12 +746,19 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{
pitch = ahrs.pitch;
roll = ahrs.roll;
if (!quadplane.show_vtol_view() && !(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD
#if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) {
return;
}
#endif
if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
} else if (!quadplane.show_vtol_view()) {
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
return;
}
#if HAL_QUADPLANE_ENABLED
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
#endif
}
#endif

Expand Down