Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: add mode ahrs convenience refence

This commit is contained in:
Iampete1 2023-02-12 13:33:27 +00:00 committed by Andrew Tridgell
parent ed5ca01e47
commit 549dd3875d
11 changed files with 23 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/mode.cpp
View File

@ -1,8 +1,9 @@
#include "Plane.h" #include "Plane.h"
Mode::Mode() Mode::Mode() :
ahrs(plane.ahrs)
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
: quadplane(plane.quadplane), , quadplane(plane.quadplane),
pos_control(plane.quadplane.pos_control), pos_control(plane.quadplane.pos_control),
attitude_control(plane.quadplane.attitude_control), attitude_control(plane.quadplane.attitude_control),
loiter_nav(plane.quadplane.loiter_nav), loiter_nav(plane.quadplane.loiter_nav),
@ -64,7 +65,7 @@ bool Mode::enter()
// zero initial pitch and highest airspeed on mode change // zero initial pitch and highest airspeed on mode change
plane.auto_state.highest_airspeed = 0; plane.auto_state.highest_airspeed = 0;
plane.auto_state.initial_pitch_cd = plane.ahrs.pitch_sensor; plane.auto_state.initial_pitch_cd = ahrs.pitch_sensor;
// disable taildrag takeoff on mode change // disable taildrag takeoff on mode change
plane.auto_state.fbwa_tdrag_takeoff_mode = false; plane.auto_state.fbwa_tdrag_takeoff_mode = false;

2
ArduPlane/mode.h
View File

@ -7,6 +7,7 @@
#include <AP_ADSB/AP_ADSB.h> #include <AP_ADSB/AP_ADSB.h>
#include <AP_Vehicle/ModeReason.h> #include <AP_Vehicle/ModeReason.h>
#include "quadplane.h" #include "quadplane.h"
#include <AP_AHRS/AP_AHRS.h>
class AC_PosControl; class AC_PosControl;
class AC_AttitudeControl_Multi; class AC_AttitudeControl_Multi;
@ -143,6 +144,7 @@ protected:
QuadPlane& quadplane; QuadPlane& quadplane;
QuadPlane::PosControlState &poscontrol; QuadPlane::PosControlState &poscontrol;
#endif #endif
AP_AHRS& ahrs;
}; };

6
ArduPlane/mode_acro.cpp
View File

@ -5,7 +5,7 @@ bool ModeAcro::_enter()
{ {
plane.acro_state.locked_roll = false; plane.acro_state.locked_roll = false;
plane.acro_state.locked_pitch = false; plane.acro_state.locked_pitch = false;
IGNORE_RETURN(plane.ahrs.get_quaternion(plane.acro_state.q)); IGNORE_RETURN(ahrs.get_quaternion(plane.acro_state.q));
return true; return true;
} }
@ -15,12 +15,12 @@ void ModeAcro::update()
if (plane.acro_state.locked_roll) { if (plane.acro_state.locked_roll) {
plane.nav_roll_cd = plane.acro_state.locked_roll_err; plane.nav_roll_cd = plane.acro_state.locked_roll_err;
} else { } else {
plane.nav_roll_cd = plane.ahrs.roll_sensor; plane.nav_roll_cd = ahrs.roll_sensor;
} }
if (plane.acro_state.locked_pitch) { if (plane.acro_state.locked_pitch) {
plane.nav_pitch_cd = plane.acro_state.locked_pitch_cd; plane.nav_pitch_cd = plane.acro_state.locked_pitch_cd;
} else { } else {
plane.nav_pitch_cd = plane.ahrs.pitch_sensor; plane.nav_pitch_cd = ahrs.pitch_sensor;
} }
} }

4
ArduPlane/mode_auto.cpp
View File

@ -100,8 +100,8 @@ void ModeAuto::update()
#if AP_SCRIPTING_ENABLED #if AP_SCRIPTING_ENABLED
} else if (nav_cmd_id == MAV_CMD_NAV_SCRIPT_TIME) { } else if (nav_cmd_id == MAV_CMD_NAV_SCRIPT_TIME) {
// NAV_SCRIPTING has a desired roll and pitch rate and desired throttle // NAV_SCRIPTING has a desired roll and pitch rate and desired throttle
plane.nav_roll_cd = plane.ahrs.roll_sensor; plane.nav_roll_cd = ahrs.roll_sensor;
plane.nav_pitch_cd = plane.ahrs.pitch_sensor; plane.nav_pitch_cd = ahrs.pitch_sensor;
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
#endif #endif
} else { } else {

2
ArduPlane/mode_guided.cpp
View File

@ -15,7 +15,7 @@ bool ModeGuided::_enter()
/* /*
if using Q_GUIDED_MODE then project forward by the stopping distance if using Q_GUIDED_MODE then project forward by the stopping distance
*/ */
loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), loc.offset_bearing(degrees(ahrs.groundspeed_vector().angle()),
plane.quadplane.stopping_distance()); plane.quadplane.stopping_distance());
} }
#endif #endif

2
ArduPlane/mode_loiter.cpp
View File

@ -63,7 +63,7 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd)
// Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed. // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed.
// get current heading. // get current heading.
const int32_t heading_cd = (wrap_360(degrees(plane.ahrs.groundspeed_vector().angle())))*100; const int32_t heading_cd = (wrap_360(degrees(ahrs.groundspeed_vector().angle())))*100;
const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);

4
ArduPlane/mode_manual.cpp
View File

@ -7,7 +7,7 @@ void ModeManual::update()
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false));
plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false); plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false);
plane.nav_roll_cd = plane.ahrs.roll_sensor; plane.nav_roll_cd = ahrs.roll_sensor;
plane.nav_pitch_cd = plane.ahrs.pitch_sensor; plane.nav_pitch_cd = ahrs.pitch_sensor;
} }

2
ArduPlane/mode_qacro.cpp
View File

@ -12,7 +12,7 @@ bool ModeQAcro::_enter()
// disable yaw rate time contant to mantain old behaviour // disable yaw rate time contant to mantain old behaviour
quadplane.disable_yaw_rate_time_constant(); quadplane.disable_yaw_rate_time_constant();
IGNORE_RETURN(plane.ahrs.get_quaternion(plane.acro_state.q)); IGNORE_RETURN(ahrs.get_quaternion(plane.acro_state.q));
return true; return true;
} }

2
ArduPlane/mode_qloiter.cpp
View File

@ -100,7 +100,7 @@ void ModeQLoiter::run()
float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground); float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground);
if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) { if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) {
quadplane.ahrs.set_touchdown_expected(true); ahrs.set_touchdown_expected(true);
} }
pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0); pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);

4
ArduPlane/mode_takeoff.cpp
View File

@ -61,7 +61,7 @@ void ModeTakeoff::update()
{ {
if (!takeoff_started) { if (!takeoff_started) {
// see if we will skip takeoff as already flying // see if we will skip takeoff as already flying
if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && plane.ahrs.groundspeed() > 3) { if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling"); gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling");
plane.prev_WP_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc;
@ -75,7 +75,7 @@ void ModeTakeoff::update()
// takeoff point, at a height of TKOFF_ALT // takeoff point, at a height of TKOFF_ALT
const float dist = target_dist; const float dist = target_dist;
const float alt = target_alt; const float alt = target_alt;
const float direction = degrees(plane.ahrs.yaw); const float direction = degrees(ahrs.yaw);
start_loc = plane.current_loc; start_loc = plane.current_loc;
plane.prev_WP_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc;

8
ArduPlane/mode_training.cpp
View File

@ -9,9 +9,9 @@ void ModeTraining::update()
// if the roll is past the set roll limit, then // if the roll is past the set roll limit, then
// we set target roll to the limit // we set target roll to the limit
if (plane.ahrs.roll_sensor >= plane.roll_limit_cd) { if (ahrs.roll_sensor >= plane.roll_limit_cd) {
plane.nav_roll_cd = plane.roll_limit_cd; plane.nav_roll_cd = plane.roll_limit_cd;
} else if (plane.ahrs.roll_sensor <= -plane.roll_limit_cd) { } else if (ahrs.roll_sensor <= -plane.roll_limit_cd) {
plane.nav_roll_cd = -plane.roll_limit_cd; plane.nav_roll_cd = -plane.roll_limit_cd;
} else { } else {
plane.training_manual_roll = true; plane.training_manual_roll = true;
@ -20,9 +20,9 @@ void ModeTraining::update()
// if the pitch is past the set pitch limits, then // if the pitch is past the set pitch limits, then
// we set target pitch to the limit // we set target pitch to the limit
if (plane.ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) { if (ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) {
plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd; plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd;
} else if (plane.ahrs.pitch_sensor <= plane.pitch_limit_min_cd) { } else if (ahrs.pitch_sensor <= plane.pitch_limit_min_cd) {
plane.nav_pitch_cd = plane.pitch_limit_min_cd; plane.nav_pitch_cd = plane.pitch_limit_min_cd;
} else { } else {
plane.training_manual_pitch = true; plane.training_manual_pitch = true;