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

navigator: publish navigator_state

Browse Source 
feedback to commander
This commit is contained in:
bresch
2024-08-14 11:08:02 +02:00
committed by Mathieu Bresciani
parent 1fa878ad88
commit 9f69e9ee6c
6 changed files with 68 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+37
View File
@@ -41,6 +41,7 @@
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* and many more...
*/
#include "navigator.h"
@@ -897,6 +898,8 @@ void Navigator::run()
publish_mission_result();
}
publish_navigator_status();
_geofence.run();
perf_end(_loop_perf);
@@ -1355,6 +1358,40 @@ void Navigator::set_mission_failure_heading_timeout()
}
}
void Navigator::trigger_failsafe(const uint8_t nav_state)
{
if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) {
_navigator_status.failure = true;
_navigator_status.nav_state = nav_state;
_navigator_status_updated = true;
}
}
void Navigator::publish_navigator_status()
{
uint8_t current_nav_state = _vstatus.nav_state;
if (_navigation_mode != nullptr) {
current_nav_state = _navigation_mode->getNavigatorStateId();
}
if (_navigator_status.nav_state != current_nav_state) {
_navigator_status.nav_state = current_nav_state;
_navigator_status.failure = false;
_navigator_status_updated = true;
}
if (_navigator_status_updated
|| (hrt_elapsed_time(&_last_navigator_status_publication) > 500_ms)) {
_navigator_status.timestamp = hrt_absolute_time();
_navigator_status_pub.publish(_navigator_status);
_navigator_status_updated = false;
_last_navigator_status_publication = hrt_absolute_time();
}
}
void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
{
vcmd->timestamp = hrt_absolute_time();