fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: add position setpoint reset helper with safe defaults

Browse Source 
- use in reset_triplets()
This commit is contained in:
RomanBapst 2020-01-17 10:48:38 +03:00 committed by Daniel Agar
parent 98042bf58f
commit 0e90448e52
3 changed files with 35 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/navigator/navigator.h
View File

@ -222,12 +222,16 @@ public:
*/
void reset_cruising_speed();
/**
* Set triplets to invalid
*/
void reset_triplets();
/**
* Set position setpoint to safe defaults
*/
void reset_position_setpoint(position_setpoint_s &sp);
/**
* Get the target throttle
*

36
src/modules/navigator/navigator_main.cpp
View File

@ -296,7 +296,11 @@ Navigator::run()
}
rep->previous.valid = true;
rep->previous.timestamp = hrt_absolute_time();
rep->current.valid = true;
rep->current.timestamp = hrt_absolute_time();
rep->next.valid = false;
// CMD_DO_REPOSITION is acknowledged by commander
@ -316,7 +320,9 @@ Navigator::run()
if (home_position_valid()) {
rep->current.yaw = cmd.param4;
rep->previous.valid = true;
rep->previous.timestamp = hrt_absolute_time();
} else {
rep->current.yaw = get_local_position()->yaw;
@ -336,6 +342,8 @@ Navigator::run()
rep->current.alt = cmd.param7;
rep->current.valid = true;
rep->current.timestamp = hrt_absolute_time();
rep->next.valid = false;
// CMD_NAV_TAKEOFF is acknowledged by commander
@ -669,6 +677,7 @@ Navigator::run()
|| (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) {
reset_triplets();
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.current.timestamp = hrt_absolute_time();
@ -830,14 +839,27 @@ Navigator::reset_cruising_speed()
void
Navigator::reset_triplets()
{
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet_updated = true;
_pos_sp_triplet.previous.acceptance_radius = get_default_acceptance_radius();
_pos_sp_triplet.current.acceptance_radius = get_default_acceptance_radius();
_pos_sp_triplet.next.acceptance_radius = get_default_acceptance_radius();
reset_position_setpoint(_pos_sp_triplet.previous);
reset_position_setpoint(_pos_sp_triplet.current);
reset_position_setpoint(_pos_sp_triplet.next);
_pos_sp_triplet_updated = true;
}
void
Navigator::reset_position_setpoint(position_setpoint_s &sp)
{
sp = position_setpoint_s{};
sp.timestamp = hrt_absolute_time();
sp.lat = static_cast<double>(NAN);
sp.lon = static_cast<double>(NAN);;
sp.loiter_radius = get_loiter_radius();
sp.acceptance_radius = get_default_acceptance_radius();
sp.cruising_speed = get_cruising_speed();
sp.cruising_throttle = get_cruising_throttle();
sp.valid = false;
sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
sp.disable_weather_vane = true;
}
float

1
src/modules/navigator/takeoff.cpp
View File

@ -128,6 +128,7 @@ Takeoff::set_takeoff_position()
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.yaw_valid = true;
pos_sp_triplet->next.valid = false;