Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: follow provides distance and bearing to target

Browse Source
This commit is contained in:
Randy Mackay 2018-12-12 10:07:47 +09:00
parent b405286262
commit 6b5ff939f2
2 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
APMrover2/mode.h
View File

@ -79,10 +79,10 @@ public:
virtual bool requires_position() const { return true; } virtual bool requires_position() const { return true; }
virtual bool requires_velocity() const { return true; } virtual bool requires_velocity() const { return true; }
// return heading (in degrees) and cross track error (in meteres) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing() const; virtual float wp_bearing() const;
float nav_bearing() const; virtual float nav_bearing() const;
float crosstrack_error() const; virtual float crosstrack_error() const;
// //
// navigation methods // navigation methods
@ -493,6 +493,17 @@ public:
// methods that affect movement of the vehicle in this mode // methods that affect movement of the vehicle in this mode
void update() override; void update() override;
// attributes of the mode
bool is_autopilot_mode() const override { return true; }
// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing() const override;
float nav_bearing() const override { return wp_bearing(); }
float crosstrack_error() const override { return 0.0f; }
// return distance (in meters) to destination
float get_distance_to_destination() const override;
protected: protected:
bool _enter() override; bool _enter() override;

12
APMrover2/mode_follow.cpp
View File

@ -71,3 +71,15 @@ void ModeFollow::update()
calc_steering_to_heading(_desired_yaw_cd); calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(calc_reduced_speed_for_turn_or_distance(desired_speed), false, true); calc_throttle(calc_reduced_speed_for_turn_or_distance(desired_speed), false, true);
} }
// return desired heading (in degrees) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float ModeFollow::wp_bearing() const
{
return g2.follow.get_bearing_to_target();
}
// return distance (in meters) to destination
float ModeFollow::get_distance_to_destination() const
{
return g2.follow.get_distance_to_target();
}