ArduCopter: mark get_wp() const

This commit is contained in:
Josh Henderson 2021-07-06 17:02:26 -04:00 committed by Randy Mackay
parent da906f6b0f
commit 2ba6ae6196
6 changed files with 11 additions and 11 deletions

View File

@ -79,7 +79,7 @@ public:
virtual bool requires_terrain_failsafe() const { return false; }
// functions for reporting to GCS
virtual bool get_wp(Location &loc) { return false; };
virtual bool get_wp(Location &loc) const { return false; };
virtual int32_t wp_bearing() const { return 0; }
virtual uint32_t wp_distance() const { return 0; }
virtual float crosstrack_error() const { return 0.0f;}
@ -419,7 +419,7 @@ protected:
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
float crosstrack_error() const override { return wp_nav->crosstrack_error();}
bool get_wp(Location &loc) override;
bool get_wp(Location &loc) const override;
private:
@ -850,7 +850,7 @@ public:
void set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust);
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
bool get_wp(Location &loc) override;
bool get_wp(Location &loc) const override;
void set_accel(const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
void set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
@ -1148,7 +1148,7 @@ public:
bool requires_terrain_failsafe() const override { return true; }
// for reporting to GCS
bool get_wp(Location &loc) override;
bool get_wp(Location &loc) const override;
// RTL states
enum class SubMode : uint8_t {
@ -1272,7 +1272,7 @@ protected:
const char *name4() const override { return "SRTL"; }
// for reporting to GCS
bool get_wp(Location &loc) override;
bool get_wp(Location &loc) const override;
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
float crosstrack_error() const override { return wp_nav->crosstrack_error();}
@ -1537,7 +1537,7 @@ protected:
const char *name4() const override { return "FOLL"; }
// for reporting to GCS
bool get_wp(Location &loc) override;
bool get_wp(Location &loc) const override;
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;

View File

@ -657,7 +657,7 @@ int32_t ModeAuto::wp_bearing() const
}
}
bool ModeAuto::get_wp(Location& destination)
bool ModeAuto::get_wp(Location& destination) const
{
switch (_mode) {
case SubMode::NAVGUIDED:

View File

@ -168,7 +168,7 @@ int32_t ModeFollow::wp_bearing() const
/*
get target position for mavlink reporting
*/
bool ModeFollow::get_wp(Location &loc)
bool ModeFollow::get_wp(Location &loc) const
{
float dist = g2.follow.get_distance_to_target();
float bearing = g2.follow.get_bearing_to_target();

View File

@ -284,7 +284,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
return true;
}
bool ModeGuided::get_wp(Location& destination)
bool ModeGuided::get_wp(Location& destination) const
{
if (guided_mode != SubMode::WP) {
return false;

View File

@ -544,7 +544,7 @@ void ModeRTL::compute_return_target()
rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
}
bool ModeRTL::get_wp(Location& destination)
bool ModeRTL::get_wp(Location& destination) const
{
// provide target in states which use wp_nav
switch (_state) {

View File

@ -180,7 +180,7 @@ void ModeSmartRTL::save_position()
copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
}
bool ModeSmartRTL::get_wp(Location& destination)
bool ModeSmartRTL::get_wp(Location& destination) const
{
// provide target in states which use wp_nav
switch (smart_rtl_state) {