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

Plane: make precsion QLOITER possible from lua

This commit is contained in:
Andrew Tridgell 2024-03-07 16:05:41 +11:00 committed by Randy Mackay
parent 45a212e311
commit 7e7f689603
5 changed files with 45 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/ArduPlane.cpp
View File

@ -875,8 +875,8 @@ bool Plane::update_target_location(const Location &old_loc, const Location &new_
next_WP_loc = new_loc;
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland) {
mode_qland.last_target_loc_set_ms = AP_HAL::millis();
if (control_mode == &mode_qland || control_mode == &mode_qloiter) {
mode_qloiter.last_target_loc_set_ms = AP_HAL::millis();
}
#endif
@ -972,8 +972,7 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const
void Plane::precland_update(void)
{
// alt will be unused if we pass false through as the second parameter:
return g2.precland.update(rangefinder_state.height_estimate*100,
rangefinder_state.in_range && rangefinder_state.in_use);
return g2.precland.update(rangefinder_state.height_estimate*100, rangefinder_state.in_range);
}
#endif

5
ArduPlane/RC_Channel.cpp
View File

@ -177,6 +177,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
case AUX_FUNC::EMERGENCY_LANDING_EN:
case AUX_FUNC::FW_AUTOTUNE:
case AUX_FUNC::VFWD_THR_OVERRIDE:
case AUX_FUNC::PRECISION_LOITER:
break;
case AUX_FUNC::SOARING:
@ -443,6 +444,10 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
}
break;
case AUX_FUNC::PRECISION_LOITER:
// handled by lua scripting, just ignore here
break;
default:
return RC_Channel::do_aux_function(ch_option, ch_flag);
}

7
ArduPlane/mode.h
View File

@ -632,6 +632,8 @@ class ModeQLoiter : public Mode
{
friend class QuadPlane;
friend class ModeQLand;
friend class Plane;
public:
Number mode_number() const override { return Number::QLOITER; }
@ -649,13 +651,12 @@ public:
protected:
bool _enter() override;
uint32_t last_target_loc_set_ms;
};
class ModeQLand : public Mode
{
public:
friend class Plane;
Number mode_number() const override { return Number::QLAND; }
const char *name() const override { return "QLAND"; }
const char *name4() const override { return "QLND"; }
@ -671,8 +672,6 @@ protected:
bool _enter() override;
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
uint32_t last_target_loc_set_ms;
};
class ModeQRTL : public Mode

27
ArduPlane/mode_qland.cpp
View File

@ -20,9 +20,6 @@ bool ModeQLand::_enter()
plane.fence.auto_disable_fence_for_landing();
#endif
// clear precland timestamp
last_target_loc_set_ms = 0;
return true;
}
@ -33,30 +30,6 @@ void ModeQLand::update()
void ModeQLand::run()
{
/*
see if precision landing is active with an override of the
target location
*/
const uint32_t last_pos_set_ms = last_target_loc_set_ms;
const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms;
const uint32_t now_ms = AP_HAL::millis();
if (last_pos_set_ms != 0 && now_ms - last_pos_set_ms < 500) {
// we have an active landing target override
Vector2f rel_origin;
if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) {
quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y);
}
}
// allow for velocity override as well
if (last_vel_set_ms != 0 && now_ms - last_vel_set_ms < 500) {
// we have an active landing velocity override
Vector2f target_accel;
Vector2f target_speed_xy_cms{quadplane.poscontrol.velocity_match.x*100, quadplane.poscontrol.velocity_match.y*100};
quadplane.pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel);
}
/*
use QLOITER to do the main control
*/

34
ArduPlane/mode_qloiter.cpp
View File

@ -17,6 +17,10 @@ bool ModeQLoiter::_enter()
// prevent re-init of target position
quadplane.last_loiter_ms = AP_HAL::millis();
// clear precland timestamp
last_target_loc_set_ms = 0;
return true;
}
@ -29,6 +33,36 @@ void ModeQLoiter::update()
void ModeQLoiter::run()
{
const uint32_t now = AP_HAL::millis();
#if AC_PRECLAND_ENABLED
const uint32_t precland_timeout_ms = 250;
/*
see if precision landing or precision loiter is active with
an override of the target location.
*/
const uint32_t last_pos_set_ms = last_target_loc_set_ms;
const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms;
if (last_pos_set_ms != 0 && now - last_pos_set_ms < precland_timeout_ms) {
// we have an active landing target override
Vector2f rel_origin;
if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) {
quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y);
last_target_loc_set_ms = 0;
}
}
// allow for velocity override as well
if (last_vel_set_ms != 0 && now - last_vel_set_ms < precland_timeout_ms) {
// we have an active landing velocity override
Vector2f target_accel;
Vector2f target_speed_xy_cms{quadplane.poscontrol.velocity_match.x*100, quadplane.poscontrol.velocity_match.y*100};
quadplane.pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel);
quadplane.poscontrol.last_velocity_match_ms = 0;
}
#endif // AC_PRECLAND_ENABLED
if (quadplane.tailsitter.in_vtol_transition(now)) {
// Tailsitters in FW pull up phase of VTOL transition run FW controllers
Mode::run();