mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: report crosstrack_error
This commit is contained in:
parent
bffe3f6c21
commit
2d5cbd2cf8
|
@ -168,7 +168,7 @@ void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
|
||||||
MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX),
|
MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX),
|
||||||
pos_control->get_alt_error() * 1.0e-2f,
|
pos_control->get_alt_error() * 1.0e-2f,
|
||||||
0,
|
0,
|
||||||
0);
|
flightmode->crosstrack_error() * 1.0e-2f);
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const
|
int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const
|
||||||
|
|
|
@ -93,6 +93,7 @@ protected:
|
||||||
virtual void run_autopilot() {}
|
virtual void run_autopilot() {}
|
||||||
virtual uint32_t wp_distance() const { return 0; }
|
virtual uint32_t wp_distance() const { return 0; }
|
||||||
virtual int32_t wp_bearing() const { return 0; }
|
virtual int32_t wp_bearing() const { return 0; }
|
||||||
|
virtual float crosstrack_error() const { return 0.0f;}
|
||||||
virtual bool get_wp(Location_Class &loc) { return false; };
|
virtual bool get_wp(Location_Class &loc) { return false; };
|
||||||
virtual bool in_guided_mode() const { return false; }
|
virtual bool in_guided_mode() const { return false; }
|
||||||
|
|
||||||
|
@ -280,6 +281,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 wp_nav->crosstrack_error();}
|
||||||
bool get_wp(Location_Class &loc) override;
|
bool get_wp(Location_Class &loc) override;
|
||||||
void run_autopilot() override;
|
void run_autopilot() override;
|
||||||
|
|
||||||
|
@ -780,6 +782,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;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
@ -957,6 +960,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 wp_nav->crosstrack_error();}
|
||||||
|
|
||||||
void descent_start();
|
void descent_start();
|
||||||
void descent_run();
|
void descent_run();
|
||||||
|
@ -1018,6 +1022,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 wp_nav->crosstrack_error();}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
|
@ -769,3 +769,12 @@ int32_t Copter::ModeGuided::wp_bearing() const
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
float Copter::ModeGuided::crosstrack_error() const
|
||||||
|
{
|
||||||
|
if (mode() == Guided_WP) {
|
||||||
|
return wp_nav->crosstrack_error();
|
||||||
|
} else {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
Loading…
Reference in New Issue