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

move flight termination and geofence flags from setpoint triplet to mission result

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-24 17:45:15 +02:00
parent a432d0493c
commit 06317046f2
7 changed files with 41 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/datalinkloss.cpp
+17 -14
View File
@@ -154,7 +154,8 @@ DataLinkLoss::set_dll_item()
}
case DLL_STATE_TERMINATE: {
/* Request flight termination from the commander */
pos_sp_triplet->flight_termination = true;
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@@ -180,23 +181,25 @@ DataLinkLoss::advance_dll()
switch (_dll_state) {
case DLL_STATE_NONE:
/* Check the number of data link losses. If above home fly home directly */
if (!_param_skipcommshold.get()) {
/* if number of data link losses limit is not reached fly to comms hold wp */
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
warnx("%d data link losses, limit is %d, fly to airfield home",
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
/* if number of data link losses limit is not reached fly to comms hold wp */
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
warnx("%d data link losses, limit is %d, fly to airfield home",
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
if (!_param_skipcommshold.get()) {
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
} else {
/* comms hold wp not active, fly to airfield home directly */
warnx("Skipping comms hold wp. Flying directly to airfield home");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
}
} else {
/* comms hold wp not active, fly to airfield home directly */
warnx("Skipping comms hold wp. Flying directly to airfield home");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
}
break;
case DLL_STATE_FLYTOCOMMSHOLDWP:
src/modules/navigator/gpsfailure.cpp
+2 -1
View File
@@ -140,7 +140,8 @@ GpsFailure::set_gpsf_item()
switch (_gpsf_state) {
case GPSF_STATE_TERMINATE: {
/* Request flight termination from the commander */
pos_sp_triplet->flight_termination = true;
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
warnx("gps fail: request flight termination");
}
default:
src/modules/navigator/navigator_main.cpp
+6 -10
View File
@@ -395,11 +395,9 @@ Navigator::task_main()
if (have_geofence_position_data) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
if (!inside) {
/* inform other apps via the sp triplet */
_pos_sp_triplet.geofence_violated = true;
if (_pos_sp_triplet.geofence_violated != true) {
_pos_sp_triplet_updated = true;
}
/* inform other apps via the mission result */
_mission_result.geofence_violated = true;
publish_mission_result();
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
@@ -407,11 +405,9 @@ Navigator::task_main()
_geofence_violation_warning_sent = true;
}
} else {
/* inform other apps via the sp triplet */
_pos_sp_triplet.geofence_violated = false;
if (_pos_sp_triplet.geofence_violated != false) {
_pos_sp_triplet_updated = true;
}
/* inform other apps via the mission result */
_mission_result.geofence_violated = false;
publish_mission_result();
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
src/modules/navigator/rcloss.cpp
+2 -1
View File
@@ -127,7 +127,8 @@ RCLoss::set_rcl_item()
}
case RCL_STATE_TERMINATE: {
/* Request flight termination from the commander */
pos_sp_triplet->flight_termination = true;
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;