mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:55:34 +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();
|
||||
|
||||
_altitude = _navigator->get_global_position()->alt;
|
||||
_airspeed = -1.f; // default airspeed
|
||||
|
||||
if (lpos->v_xy_valid) {
|
||||
_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.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.loiter_radius = NAN;
|
||||
pos_sp_triplet->current.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
|
||||
@@ -61,7 +61,6 @@ public:
|
||||
bool set_course(float course_rad);
|
||||
|
||||
void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_setpoint_triplet(); }
|
||||
void set_airspeed(float airspeed) { _airspeed = airspeed; update_setpoint_triplet(); }
|
||||
|
||||
private:
|
||||
/**
|
||||
@@ -71,5 +70,4 @@ private:
|
||||
|
||||
float _course{0.f}; ///< [rad] current course bearing (ground track)
|
||||
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
|
||||
set_cruising_speed(cmd.param2);
|
||||
|
||||
if (_navigation_mode == &_course) {
|
||||
_course.set_airspeed(cmd.param2);
|
||||
}
|
||||
|
||||
} else {
|
||||
reset_cruising_speed();
|
||||
|
||||
if (_navigation_mode == &_course) {
|
||||
_course.set_airspeed(-1.f);
|
||||
}
|
||||
|
||||
/* if no speed target was given try to set throttle */
|
||||
if (cmd.param3 > FLT_EPSILON) {
|
||||
set_cruising_throttle(cmd.param3 / 100);
|
||||
|
||||
Reference in New Issue
Block a user