Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Deadreckoning Failsafe Testing #29197

Closed
3 changes: 3 additions & 0 deletions ArduSub/ArduSub.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -251,6 +251,9 @@ void Sub::three_hz_loop()

failsafe_internal_temperature_check();

// check for deadreckoning failsafe
failsafe_deadreckon_check();

// check if we've lost contact with the ground station
failsafe_gcs_check();

Expand Down
21 changes: 21 additions & 0 deletions ArduSub/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -735,6 +735,27 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard
AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0),

// @Param: FS_DR_ENABLE
// @DisplayName: DeadReckon Failsafe Action
// @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates
// @Values: 0: Disabled/NoAction, 1: Warn only, 2: Surface
// @User: Standard
AP_GROUPINFO("FS_DR_ENABLE", 22, ParametersG2, failsafe_dr_enable, FS_DR_WARN_ONLY),

// @Param: FS_DR_TIMEOUT
// @DisplayName: DeadReckon Failsafe Timeout
// @Description: DeadReckoning is available for this many seconds after losing position and/or velocity source. After this timeout elapses the EKF failsafe will trigger in modes requiring a position estimate
// @Range: 0 120
// @User: Standard
AP_GROUPINFO("FS_DR_TIMEOUT", 23, ParametersG2, failsafe_dr_timeout, 120),

// @Param: FS_DR_FORCE
// @DisplayName: Force DeadReckon Failsafe
// @Description: Force a deadreckoning failsafe to be triggered after 20 seconds by having the EKF report deadreckoning is enabled in the three_hz_loop.
// @Values: 0: Disabled/Do not force, 1: Enable/Force
// @User: Advanced
AP_GROUPINFO("FS_DR_FORCE", 24, ParametersG2, failsafe_dr_force, FS_DR_NONE),

AP_GROUPEND
};

Expand Down
5 changes: 5 additions & 0 deletions ArduSub/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -371,6 +371,11 @@ class ParametersG2 {
AP_Float backup_origin_lat;
AP_Float backup_origin_lon;
AP_Float backup_origin_alt;

// AP_Int8 failsafe_dr_timeout;
AP_Int8 failsafe_dr_enable;
AP_Int32 failsafe_dr_timeout;
AP_Int8 failsafe_dr_force;
};

extern const AP_Param::Info var_info[];
Expand Down
23 changes: 22 additions & 1 deletion ArduSub/Sub.h
Original file line number Diff line number Diff line change
Expand Up @@ -229,6 +229,7 @@ class Sub : public AP_Vehicle {
uint8_t internal_temperature : 1; // true if temperature is over threshold
uint8_t crash : 1; // true if we are crashed
uint8_t sensor_health : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes)
uint8_t deadreckon : 1; // true if a dead reckoning failsafe has triggered
} failsafe;

bool any_failsafe_triggered() const {
Expand All @@ -243,9 +244,27 @@ class Sub : public AP_Vehicle {
|| failsafe.internal_temperature
|| failsafe.crash
|| failsafe.sensor_health
|| failsafe.deadreckon
);
}

// dead reckoning state
struct {
bool active; // true if dead reckoning (position estimate using estimated airspeed, no position or velocity source)
bool timeout; // true if dead reckoning has timedout and EKF's position and velocity estimate should no longer be trusted
uint32_t start_ms = 0; // system time that EKF began deadreckoning
} dead_reckoning;

struct {
enum {
READY,
ACTIVE,
DONE
} status = READY; // true if deadreckoning has been forced
uint32_t force_start_ms; // system time that began forcing deadreckoning
uint32_t init_ms = 0;
} dr_forced;

// sensor health for logging
struct {
uint8_t depth : 1; // true if depth sensor is healthy
Expand Down Expand Up @@ -305,7 +324,7 @@ class Sub : public AP_Vehicle {
// Navigation Yaw control
// auto flight mode's yaw mode
uint8_t auto_yaw_mode;

// Parameter to set yaw rate only
bool yaw_rate_only;

Expand Down Expand Up @@ -455,6 +474,8 @@ class Sub : public AP_Vehicle {
void failsafe_pilot_input_check(void);
void set_neutral_controls(void);
void failsafe_terrain_check();
void failsafe_deadreckon_check();
void maybe_force_deadreckoning(const char* prefix_str, uint32_t const& now);
void failsafe_terrain_set_status(bool data_ok);
void failsafe_terrain_on_event();
void mainloop_failsafe_enable();
Expand Down
5 changes: 5 additions & 0 deletions ArduSub/defines.h
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,11 @@ enum LoggingParameters {
#define FS_PILOT_INPUT_WARN_ONLY 1
#define FS_PILOT_INPUT_DISARM 2

// Dead reckoning failsafe actions
#define FS_DR_NONE 0 // Do nothing
#define FS_DR_WARN_ONLY 1 // Only send warning to gcs
#define FS_DR_SURFACE 2 // Attempt to surface

// Amount of time to attempt recovery of valid rangefinder data before
// initiating terrain failsafe action
#define FS_TERRAIN_RECOVER_TIMEOUT_MS 10000
Expand Down
74 changes: 74 additions & 0 deletions ArduSub/failsafe.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -442,6 +442,80 @@ void Sub::failsafe_terrain_check()
}
}

// forcing the deadreckoning failsafe
void Sub::maybe_force_deadreckoning(const char* prefix_str, uint32_t const& now)
{
static const auto TEN_SECONDS_MS = 10000;
gps.force_disable(dr_forced.status == dr_forced.ACTIVE);

if (g2.failsafe_dr_force > 0 && mission.state() == mission.MISSION_RUNNING) {
if (dr_forced.init_ms == 0) {
dr_forced.status = dr_forced.READY;
dr_forced.init_ms = now;
dead_reckoning.start_ms = 0;
} else if (dead_reckoning.start_ms > 0 && (now - dead_reckoning.start_ms) > TEN_SECONDS_MS) {
gcs().send_text(MAV_SEVERITY_INFO, "%s: forcing GPS ON after %dms", prefix_str, dead_reckoning.start_ms);
dr_forced.status = dr_forced.DONE;
gps.force_disable(false);
} else if (dr_forced.status == dr_forced.READY && (now - dr_forced.init_ms) > TEN_SECONDS_MS) {
gcs().send_text(MAV_SEVERITY_INFO, "%s: forcing GPS OFF at %dms", prefix_str, (now - dr_forced.init_ms));
dr_forced.status = dr_forced.ACTIVE;
gps.force_disable(true);
}
}
}

// dead reckoning alert and failsafe
void Sub::failsafe_deadreckon_check()
{
// update dead reckoning state
const char* dr_prefix_str = "Dead Reckoning";

// get EKF filter status
const uint32_t now_ms = AP_HAL::millis();
maybe_force_deadreckoning(dr_prefix_str, now_ms);
bool ekf_dead_reckoning = inertial_nav.get_filter_status().flags.dead_reckoning;

// alert user to start or stop of dead reckoning
if (mission.state() == mission.MISSION_RUNNING && dead_reckoning.active != ekf_dead_reckoning) {
dead_reckoning.active = ekf_dead_reckoning;
if (dead_reckoning.active) {
dead_reckoning.start_ms = now_ms;
gcs().send_text(MAV_SEVERITY_CRITICAL,"%s started", dr_prefix_str);
} else {
dead_reckoning.start_ms = 0;
dead_reckoning.timeout = false;
gcs().send_text(MAV_SEVERITY_CRITICAL,"%s stopped", dr_prefix_str);
}
}

// check for timeout
if (dead_reckoning.active && !dead_reckoning.timeout) {
const uint32_t dr_timeout_ms = uint32_t(constrain_float(g2.failsafe_dr_timeout * 1000.0f, 0.0f, UINT32_MAX));
if (now_ms - dead_reckoning.start_ms > dr_timeout_ms) {
dead_reckoning.timeout = true;
gcs().send_text(MAV_SEVERITY_CRITICAL,"%s timeout", dr_prefix_str);
set_mode(Mode::Number::SURFACE, ModeReason::DEADRECKON_FAILSAFE);
}
}

// exit immediately if deadreckon failsafe is disabled
if (g2.failsafe_dr_enable <= 0) {
failsafe.deadreckon = false;
return;
}

// check for failsafe action
if (failsafe.deadreckon != ekf_dead_reckoning) {
failsafe.deadreckon = ekf_dead_reckoning;

// take the failsafe action
if (failsafe.deadreckon) {

}
}
}

// This gets called if mission items are in ALT_ABOVE_TERRAIN frame
// Terrain failure occurs when terrain data is not found, or rangefinder is not enabled or healthy
// set terrain data status (found or not found)
Expand Down
3 changes: 2 additions & 1 deletion ArduSub/mode_auto.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ bool ModeAuto::init(bool ignore_checks) {
guided_limit_clear();

// start/resume the mission (based on MIS_RESTART parameter)
gcs().send_text(MAV_SEVERITY_INFO, "sub.mission.start_or_resume()...");
sub.mission.start_or_resume();
return true;
}
Expand Down Expand Up @@ -378,7 +379,7 @@ void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps

// sets the desired yaw rate
void ModeAuto::set_yaw_rate(float turn_rate_dps)
{
{
// set sub to desired yaw rate
sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE); // deg / sec

Expand Down
1 change: 1 addition & 0 deletions Tools/autotest/locations.txt
Original file line number Diff line number Diff line change
Expand Up @@ -104,3 +104,4 @@ Egge=60.215720,10.324071,198,303
Gundaroo=-35.02349196,149.26496411,576.8,0
Kaga=36.3268982,136.3316638,44,0
UCSB=34.413963,-119.848946,0,0
ras_test=40.4279,-72.7939,0,64.23
2 changes: 1 addition & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -794,7 +794,7 @@ void NavEKF3_core::updateFilterStatus(void)
(imuSampleTime_ms - lastTasFailTime_ms) < 1000 &&
(imuSampleTime_ms - lastTasPassTime_ms) > 3000;
status.flags.initalized = status.flags.initalized || healthy();
status.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav);
status.flags.dead_reckoning = !doingNormalGpsNav;

filterStatus.value = status.value;
}
Expand Down
89 changes: 89 additions & 0 deletions params.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
#!/usr/bin/env python3


from pathlib import Path
import re

PARAM_RE = re.compile(r"\s*\/\/\s*@Param:")
GROUP_RE = re.compile(r"\s*\/\/\s*@Group:")
GPATH_RE = re.compile(r"\s*\/\/\s*@Path:")
AT_STARTS = re.compile(r"\s*\/\/\s*@")

def add_param_info(line, param_name, param_dict):
"""
"""
elem = re.sub(AT_STARTS, "", line)
ess = param_dict.get(param_name, {})
[fullkey, fullvalue] = elem.split(":", maxsplit=1)
key = fullkey.strip()
value = fullvalue.strip()
if key == "Range":
[start, stop] = value.split(" ", maxsplit=1)
ess["Range Start"] = start
ess["Range End"] = stop
param_dict[param_name] = ess
else:
ess[key] = value
param_dict[param_name] = ess
return param_dict

def extract_param_name(line, group):
l0 = re.sub(AT_STARTS, "", line)
pname = l0.split(":", maxsplit=1)[1]
return group.strip() + pname.strip()

def get_params(file, group=""):
"""
"""
param_dict = {}
with open(file, "rt", buffering=1) as pfile:
param_name = None
for line in pfile:
if re.search(PARAM_RE, line):
param_name = extract_param_name(line, group)
param_dict[param_name] = {}
elif param_name is not None and re.search(AT_STARTS, line):
param_dict = add_param_info(line, param_name, param_dict)
elif param_name is not None:
param_name = None
return param_dict

def get_groups(file, cgrp=""):
"""
"""
groups = []
pmap = {}
with open(file, "rt", buffering=1) as gfile:
group_found = False
gname = None
for line in gfile:
if re.search(GPATH_RE, line) and group_found:
# Get the group path relative to this file.
relative_dir = file.parents[0]
[_, gpath] = line.split(":", maxsplit=1)
pgen0 = (relative_dir.joinpath(p.strip()) for p in gpath.split(","))
groups = groups + [(gname, f.resolve()) for f in pgen0]
elif re.search(GROUP_RE, line):
gname = cgrp + line.split(":", maxsplit=1)[1].strip()
group_found = True
else:
group_found = False
for (name, path) in groups:
if path != file:
pmap = {**pmap, **get_groups(path, name), **get_params(path, name)}
return pmap

#
# Main Entry Point of script
#
if __name__ == "__main__":
tags = ["Param", "DisplayName", "Description", "Units", "Range Start", "Range End", "Increment", "Additional"]
map = get_groups(Path("./ArduSub/Parameters.cpp"))
separator = "$$"
print(separator.join(tags))
for param, info in map.items():
print(separator.join(
[param] +
[info.pop(tag, "") for tag in tags[1:-1]] +
[separator.join(key + ":" + value for key, value in info.items() if key not in tags)]
))
6 changes: 6 additions & 0 deletions ras/missions/long_wp_test
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
QGC WPL 110
0 0 0 16 0.000000 0.000000 0.000000 0.000000 40.427900 -72.793900 -0.030000 1
1 0 3 16 0.000000 0.000000 0.000000 0.000000 40.430440 -72.788241 -30.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 40.425469 -72.783516 -30.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 40.423230 -72.790452 -30.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 40.427769 -72.793940 -0.300000 1
6 changes: 6 additions & 0 deletions ras/missions/short_wp_test.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
QGC WPL 110
0 0 0 16 0.000000 0.000000 0.000000 0.000000 40.427900 -72.793900 -0.040000 1
1 0 3 16 0.000000 0.000000 0.000000 0.000000 40.427904 -72.793886 -25.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 40.427894 -72.793882 -25.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 40.427890 -72.793895 -25.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 40.427899 -72.793899 -0.300000 1
Loading
Loading