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

Plane: added EKF yaw reset handling to quadplane 

this follows the implementation from copter. Thanks to Leonard for the
suggestion
This commit is contained in:
Andrew Tridgell 2017-04-18 22:40:06 +10:00
parent 516bf26719
commit 2d9c3e3d93
2 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/quadplane.cpp
View File

@ -762,6 +762,22 @@ void QuadPlane::init_hover(void)
init_throttle_wait();
}
/*
check for an EKF yaw reset
*/
void QuadPlane::check_yaw_reset(void)
{
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) {
// we only reset if the EKF yaw reset happened since the last
// mode change. This prevents a past reset from before
attitude_control->shift_ef_yaw_target(degrees(yaw_angle_change_rad) * 100);
ekfYawReset_ms = new_ekfYawReset_ms;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF yaw reset %.2f", degrees(yaw_angle_change_rad));
}
}
/*
hold hover with target climb rate
*/
@ -1299,7 +1315,7 @@ void QuadPlane::update(void)
if (!setup()) {
return;
}
if (plane.afs.should_crash_vehicle()) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->output();
@ -1311,6 +1327,8 @@ void QuadPlane::update(void)
return;
}
check_yaw_reset();
if (!in_vtol_mode()) {
update_transition();
} else {

8
ArduPlane/quadplane.h
View File

@ -139,6 +139,9 @@ private:
// update transition handling
void update_transition(void);
// check for an EKF yaw reset
void check_yaw_reset(void);
// hold hover (for transition)
void hold_hover(float target_climb_rate);
@ -242,7 +245,10 @@ private:
// ICEngine control on landing
AP_Int8 land_icengine_cut;
// time we last got an EKF yaw reset
uint32_t ekfYawReset_ms;
struct {
AP_Float gain;
float integrator;