mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-02 03:49:12 +08:00
chore(guided_course): align speed handling with other auto modes
This commit is contained in:
@@ -52,7 +52,6 @@ Course::on_activation()
|
|||||||
const vehicle_local_position_s *lpos = _navigator->get_local_position();
|
const vehicle_local_position_s *lpos = _navigator->get_local_position();
|
||||||
|
|
||||||
_altitude = _navigator->get_global_position()->alt;
|
_altitude = _navigator->get_global_position()->alt;
|
||||||
_airspeed = -1.f; // default airspeed
|
|
||||||
|
|
||||||
if (lpos->v_xy_valid) {
|
if (lpos->v_xy_valid) {
|
||||||
_course = matrix::wrap_2pi(atan2f(lpos->vy, lpos->vx));
|
_course = matrix::wrap_2pi(atan2f(lpos->vy, lpos->vx));
|
||||||
@@ -107,7 +106,7 @@ Course::update_setpoint_triplet()
|
|||||||
pos_sp_triplet->current.yaw = NAN;
|
pos_sp_triplet->current.yaw = NAN;
|
||||||
pos_sp_triplet->current.course = _course;
|
pos_sp_triplet->current.course = _course;
|
||||||
|
|
||||||
pos_sp_triplet->current.cruising_speed = _airspeed;
|
pos_sp_triplet->current.cruising_speed = _navigator->get_cruising_speed();
|
||||||
pos_sp_triplet->current.cruising_throttle = NAN;
|
pos_sp_triplet->current.cruising_throttle = NAN;
|
||||||
pos_sp_triplet->current.loiter_radius = NAN;
|
pos_sp_triplet->current.loiter_radius = NAN;
|
||||||
pos_sp_triplet->current.acceptance_radius = _navigator->get_acceptance_radius();
|
pos_sp_triplet->current.acceptance_radius = _navigator->get_acceptance_radius();
|
||||||
|
|||||||
@@ -61,7 +61,6 @@ public:
|
|||||||
bool set_course(float course_rad);
|
bool set_course(float course_rad);
|
||||||
|
|
||||||
void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_setpoint_triplet(); }
|
void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_setpoint_triplet(); }
|
||||||
void set_airspeed(float airspeed) { _airspeed = airspeed; update_setpoint_triplet(); }
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/**
|
/**
|
||||||
@@ -71,5 +70,4 @@ private:
|
|||||||
|
|
||||||
float _course{0.f}; ///< [rad] current course bearing (ground track)
|
float _course{0.f}; ///< [rad] current course bearing (ground track)
|
||||||
float _altitude{0.f}; ///< [m] AMSL altitude setpoint
|
float _altitude{0.f}; ///< [m] AMSL altitude setpoint
|
||||||
float _airspeed{-1.f}; ///< [m/s] cruising speed setpoint (-1 = default)
|
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -732,17 +732,9 @@ void Navigator::run()
|
|||||||
// XXX not differentiating ground and airspeed yet
|
// XXX not differentiating ground and airspeed yet
|
||||||
set_cruising_speed(cmd.param2);
|
set_cruising_speed(cmd.param2);
|
||||||
|
|
||||||
if (_navigation_mode == &_course) {
|
|
||||||
_course.set_airspeed(cmd.param2);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
reset_cruising_speed();
|
reset_cruising_speed();
|
||||||
|
|
||||||
if (_navigation_mode == &_course) {
|
|
||||||
_course.set_airspeed(-1.f);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* if no speed target was given try to set throttle */
|
/* if no speed target was given try to set throttle */
|
||||||
if (cmd.param3 > FLT_EPSILON) {
|
if (cmd.param3 > FLT_EPSILON) {
|
||||||
set_cruising_throttle(cmd.param3 / 100);
|
set_cruising_throttle(cmd.param3 / 100);
|
||||||
|
|||||||
Reference in New Issue
Block a user