Copter: Guided and Loiter mode returns Crosstrack error

This commit is contained in:
Leonard Hall 2021-07-21 10:08:05 +09:30 committed by Andrew Tridgell
parent 5001f47313
commit 6606ab4996
2 changed files with 2 additions and 0 deletions

View File

@ -1028,6 +1028,7 @@ protected:
uint32_t wp_distance() const override; uint32_t wp_distance() const override;
int32_t wp_bearing() const override; int32_t wp_bearing() const override;
float crosstrack_error() const override { return pos_control->crosstrack_error();}
#if PRECISION_LANDING == ENABLED #if PRECISION_LANDING == ENABLED
bool do_precision_loiter(); bool do_precision_loiter();

View File

@ -1024,6 +1024,7 @@ float ModeGuided::crosstrack_error() const
case SubMode::Accel: case SubMode::Accel:
case SubMode::VelAccel: case SubMode::VelAccel:
case SubMode::PosVelAccel: case SubMode::PosVelAccel:
return pos_control->crosstrack_error();
case SubMode::Angle: case SubMode::Angle:
// no track to have a crosstrack to // no track to have a crosstrack to
return 0; return 0;