dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add a Secondary Geofence as an offset from the Primary Geofence. 

Breakout geofence code from Commander::run().
Refactor/re-write Navigator::geofence_breach_check().
This commit is contained in:
Mark Sauder 2022-01-28 22:13:13 +06:00 committed by mcsauder
parent 70178b66d8
commit 08e9d884d2
16 changed files with 1111 additions and 800 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
msg/GeofenceResult.msg
View File

@ -1,14 +1,17 @@
uint64 timestamp # time since system start (microseconds)
uint8 GF_ACTION_NONE = 0 # no action on geofence violation
uint8 GF_ACTION_WARN = 1 # critical mavlink message
uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER
uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
uint8 GF_ACTION_TERMINATE = 4 # flight termination
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND
uint64 timestamp # time since system start (microseconds)
uint8 GF_ACTION_NONE = 0 # no action on geofence violation
uint8 GF_ACTION_WARN = 1 # critical mavlink message
uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER
uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
uint8 GF_ACTION_TERMINATE = 4 # flight termination
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND
uint8 geofence_violation_reason # one of geofence_violation_reason_t::*
bool max_altitude_exceeded # true if the maximum altitude allowed is exceeded
bool max_distance_exceeded # true if the maximum distance from home allowed is exceeded
bool primary_geofence_breached # true if the primary geofence is breached
bool secondary_geofence_breached # true if the secondary geofence is breached
bool primary_geofence_breached # true if the primary geofence is breached
uint8 primary_geofence_action # action to take when the primary geofence is breached
uint8 primary_geofence_action # action to take when the primary geofence is breached
uint8 secondary_geofence_action # action to take when the secondary geofence is breached
bool home_required # true if the geofence requires a valid home position
bool home_required # true if the geofence requires a valid home position

66
src/lib/geo/geo.cpp
View File

@ -138,19 +138,18 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
}
}
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target)
void waypoint_from_heading_and_distance(const double lat_start, const double lon_start, const float bearing,
const float dist, double *lat_target, double *lon_target)
{
bearing = wrap_2pi(bearing);
float heading = wrap_2pi(bearing);
double radius_ratio = static_cast<double>(dist) / CONSTANTS_RADIUS_OF_EARTH;
double lat_start_rad = math::radians(lat_start);
double lon_start_rad = math::radians(lon_start);
*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
double)bearing));
*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
double)heading));
*lon_target = lon_start_rad + atan2(sin((double)heading) * sin(radius_ratio) * cos(lat_start_rad),
cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
*lat_target = math::degrees(*lat_target);
@ -218,45 +217,54 @@ void add_vector_to_global_position(double lat_now, double lon_now, float v_n, fl
int get_distance_to_line(struct crosstrack_error_s &crosstrack_error, double lat_now, double lon_now,
double lat_start, double lon_start, double lat_end, double lon_end)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line
// headed towards the end point.
int return_value = -1; // Set error flag, cleared when valid result calculated.
crosstrack_error.past_end = false;
crosstrack_error.distance = 0.0f;
crosstrack_error.bearing = 0.0f;
// This function returns the distance to the nearest point on the track line segment.
// Distance is positive if current position is right of the track and negative if left of the track
// as seen from a point on the track line headed towards the end point.
// If the point lies before the start or after the end we return the distance to the nearest endpoint.
// This is useful in calcluating distances to the nearest point on a polygon when past the end points of the line.
crosstrack_error.before_start = false;
crosstrack_error.past_end = false;
crosstrack_error.distance_to_start = 0.0f;
crosstrack_error.distance_to_end = 0.0f;
crosstrack_error.distance = 0.0f;
crosstrack_error.bearing = 0.0f;
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
// Return error if arguments are bad
if (dist_to_end < 0.1f) {
if (dist_to_start < 0.1f || dist_to_end < 0.1f) {
return -1;
}
float bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
float bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
float bearing_diff = wrap_pi(bearing_track - bearing_end);
float bearing_to_start = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
float bearing_to_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
float bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
// Return past_end = true if past end point of line
if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
crosstrack_error.past_end = true;
return_value = 0;
return return_value;
float bearing_diff_to_start = wrap_pi(bearing_track - bearing_to_start);
float bearing_diff_to_end = wrap_pi(bearing_track - bearing_to_end);
// before_start is true if before start point of line, past_end is true if past line segment end.
if (bearing_diff_to_start < M_PI_2_F || bearing_diff_to_start > -M_PI_2_F) {
crosstrack_error->before_start = true;
}
crosstrack_error.distance = (dist_to_end) * sinf(bearing_diff);
if (bearing_diff_to_end > M_PI_2_F || bearing_diff_to_end < -M_PI_2_F) {
crosstrack_error->past_end = true;
}
if (sinf(bearing_diff) >= 0) {
crosstrack_error.bearing = wrap_pi(bearing_track - M_PI_2_F);
crosstrack_error->distance_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
crosstrack_error->distance_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
crosstrack_error->distance = dist_to_end * sinf(bearing_diff_to_end);
if (sinf(bearing_diff_to_end) >= 0) {
crosstrack_error->bearing = wrap_pi(bearing_track - M_PI_2_F);
} else {
crosstrack_error.bearing = wrap_pi(bearing_track + M_PI_2_F);
}
return_value = 0;
return return_value;
return 0;
}
int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,

13
src/lib/geo/geo.h
View File

@ -72,12 +72,14 @@ static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f; // radians/
// XXX remove
struct crosstrack_error_s {
bool past_end; // Flag indicating we are past the end of the line/arc segment
float distance; // Distance in meters to closest point on line/arc
float bearing; // Bearing in radians to closest point on line/arc
bool before_start; // Flag indicating the vehicle is before the start of the line/arc segment
bool past_end; // Flag indicating the vehicle is past the end of the line/arc segment
float distance_to_start; // Distance in meters to closest endpoint on line/arc segment
float distance_to_end; // Distance in meters to closest endpoint on line/arc segment
float distance; // Distance in meters to closest point on line/arc
float bearing; // Bearing in radians to closest point on line/arc
} ;
/**
* Returns the distance to the next waypoint in meters.
*
@ -114,7 +116,8 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
* @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
*/
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
void waypoint_from_heading_and_distance(const double lat_start, const double lon_start, const float bearing,
const float dist,
double *lat_target, double *lon_target);
/**

437
src/modules/commander/Commander.cpp
View File

@ -72,17 +72,6 @@
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/tune_control.h>
typedef enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
} VEHICLE_MODE_FLAG;
// TODO: generate
static constexpr bool operator ==(const actuator_armed_s &a, const actuator_armed_s &b)
@ -95,6 +84,7 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme
a.force_failsafe == b.force_failsafe &&
a.in_esc_calibration_mode == b.in_esc_calibration_mode);
}
static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review");
#if defined(BOARD_HAS_POWER_CONTROL)
@ -115,6 +105,7 @@ static void stop_tune()
}
static orb_advert_t power_button_state_pub = nullptr;
static int power_button_state_notification_cb(board_power_button_state_notification_e request)
{
// Note: this can be called from IRQ handlers, so we publish a message that will be handled
@ -228,6 +219,7 @@ static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = N
}
#endif
int Commander::custom_command(int argc, char *argv[])
{
if (!is_running()) {
@ -471,29 +463,6 @@ int Commander::custom_command(int argc, char *argv[])
return print_usage("unknown command");
}
int Commander::print_status()
{
PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
perf_print_counter(_loop_perf);
perf_print_counter(_preflight_check_perf);
return 0;
}
extern "C" __EXPORT int commander_main(int argc, char *argv[])
{
return Commander::main(argc, argv);
}
bool Commander::shutdownIfAllowed()
{
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown);
}
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
{
switch (calling_reason) {
@ -531,6 +500,43 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
return "";
};
Commander::Commander() :
ModuleParams(nullptr)
{
_vehicle_land_detected.landed = true;
_vehicle_status.system_id = 1;
_vehicle_status.component_id = 1;
_vehicle_status.system_type = 0;
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
_vehicle_status.nav_state = _user_mode_intention.get();
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
/* mark all signals lost as long as they haven't been found */
_vehicle_status.gcs_connection_lost = true;
_vehicle_status.power_input_valid = true;
// default for vtol is rotary wing
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_param_mav_comp_id = param_find("MAV_COMP_ID");
_param_mav_sys_id = param_find("MAV_SYS_ID");
_param_mav_type = param_find("MAV_TYPE");
_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
updateParameters();
}
Commander::~Commander()
{
perf_free(_loop_perf);
perf_free(_preflight_check_perf);
}
transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
{
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
@ -592,7 +598,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
return arming_res;
}
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
transition_result_t Commander::disarm(const arm_disarm_reason_t calling_reason, const bool forced)
{
if (!forced) {
const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
@ -638,45 +644,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
return arming_res;
}
Commander::Commander() :
ModuleParams(nullptr)
{
_vehicle_land_detected.landed = true;
_vehicle_status.system_id = 1;
_vehicle_status.component_id = 1;
_vehicle_status.system_type = 0;
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
_vehicle_status.nav_state = _user_mode_intention.get();
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
/* mark all signals lost as long as they haven't been found */
_vehicle_status.gcs_connection_lost = true;
_vehicle_status.power_input_valid = true;
// default for vtol is rotary wing
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_param_mav_comp_id = param_find("MAV_COMP_ID");
_param_mav_sys_id = param_find("MAV_SYS_ID");
_param_mav_type = param_find("MAV_TYPE");
_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
updateParameters();
}
Commander::~Commander()
{
perf_free(_loop_perf);
perf_free(_preflight_check_perf);
}
bool
Commander::handle_command(const vehicle_command_s &cmd)
bool Commander::handleCommand(const vehicle_command_s &cmd)
{
/* only handle commands that are meant to be handled by this system and component, or broadcast */
if (((cmd.target_system != _vehicle_status.system_id) && (cmd.target_system != 0))
@ -913,7 +881,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
_actuator_armed.force_failsafe = true;
_flight_termination_triggered = true;
PX4_WARN("forcing failsafe (termination)");
send_parachute_command();
sendParachuteCommand();
}
} else {
@ -1126,13 +1094,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (param1 == 0) {
// 0: Do nothing for autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
#if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) {
// 1: Reboot autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); }
@ -1142,7 +1110,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) {
// 2: Shutdown autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); }
@ -1152,14 +1120,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); }
#endif // CONFIG_BOARDCTL_RESET
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
}
}
@ -1170,7 +1138,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else {
@ -1181,15 +1149,15 @@ Commander::handle_command(const vehicle_command_s &cmd)
(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
break;
}
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
@ -1200,19 +1168,19 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::MagCalibration);
} else if ((int)(cmd.param3) == 1) {
/* baro calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
/* disable RC control input completely */
_vehicle_status.rc_calibration_in_progress = true;
mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
@ -1221,39 +1189,39 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
} else if ((int)(cmd.param5) == 4) {
// accelerometer quick calibration
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
/* airspeed calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
if (check_battery_disconnected(&_mavlink_log_pub)) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) {
mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t");
@ -1267,7 +1235,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
}
} else if ((int)(cmd.param4) == 0) {
@ -1280,10 +1248,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
"Calibration: Restoring RC input");
}
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
}
}
@ -1295,10 +1263,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
// parameter 1: Heading (degrees)
// parameter 3: Latitude (degrees)
// parameter 4: Longitude (degrees)
@ -1330,28 +1298,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else {
if (((int)(cmd.param1)) == 0) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
} else if (((int)(cmd.param1)) == 1) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
} else if (((int)(cmd.param1)) == 2) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamResetAllConfig);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory);
} else if (((int)(cmd.param1)) == 4) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
}
}
@ -1361,7 +1329,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
_health_and_arming_checks.update(true);
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
break;
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
@ -1410,13 +1378,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
default:
/* Warn about unsupported commands, this makes sense because only commands
* to this component ID (or all) are passed by mavlink. */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
if (cmd_result != vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(cmd, cmd_result);
answerCommand(cmd, cmd_result);
}
return true;
@ -1540,7 +1508,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
_status_changed = true;
_actuator_armed.manual_lockdown = true;
send_parachute_command();
sendParachuteCommand();
}
break;
@ -1548,6 +1516,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
case action_request_s::ACTION_SWITCH_MODE:
if (!_user_mode_intention.change(action_request.mode, true)) {
printRejectMode(action_request.mode);
}
@ -1666,6 +1635,8 @@ void Commander::run()
handlePowerButtonState();
offboardControlUpdate();
systemPowerUpdate();
landDetectorUpdate();
@ -1690,6 +1661,8 @@ void Commander::run()
checkForMissionUpdate();
checkGeofenceStatus();
manualControlCheck();
offboardControlCheck();
@ -1735,7 +1708,7 @@ void Commander::run()
PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
}
if (handle_command(cmd)) {
if (handleCommand(cmd)) {
_status_changed = true;
}
}
@ -1786,9 +1759,9 @@ void Commander::run()
|| !(_actuator_armed == actuator_armed_prev)) {
// publish actuator_armed first (used by output modules)
_actuator_armed.armed = _arm_state_machine.isArmed();
_actuator_armed.armed = _arm_state_machine.isArmed();
_actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby();
_actuator_armed.timestamp = hrt_absolute_time();
_actuator_armed.timestamp = hrt_absolute_time();
_actuator_armed_pub.publish(_actuator_armed);
// update and publish vehicle_control_mode
@ -1796,7 +1769,7 @@ void Commander::run()
// vehicle_status publish (after prearm/preflight updates above)
_vehicle_status.arming_state = _arm_state_machine.getArmState();
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status_pub.publish(_vehicle_status);
// failure_detector_status publish
@ -1818,7 +1791,8 @@ void Commander::run()
checkWorkerThread();
updateTunes();
control_status_leds(_status_changed, _battery_warning);
controlStatusLeds(_status_changed, _battery_warning);
_status_changed = false;
@ -1840,6 +1814,7 @@ void Commander::run()
/* close fds */
led_deinit();
buzzer_deinit();
}
@ -1899,6 +1874,113 @@ void Commander::checkForMissionUpdate()
}
}
void Commander::checkGeofenceStatus()
{
/* start geofence result check */
if (_geofence_result_sub.update(&_geofence_result)) {
_vehicle_status.primary_geofence_breached = _geofence_result.primary_geofence_breached;
_vehicle_status.secondary_geofence_breached = _geofence_result.secondary_geofence_breached;
}
const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
// Geofence actions. Mavlink critical messages are sent by navigator
if (_arm_state_machine.isArmed()
&& _geofence_result.primary_geofence_action != geofence_result_s::GF_ACTION_NONE
&& _geofence_result.primary_geofence_action != geofence_result_s::GF_ACTION_WARN
&& _geofence_result.secondary_geofence_action != geofence_result_s::GF_ACTION_NONE
&& _geofence_result.secondary_geofence_action != geofence_result_s::GF_ACTION_WARN
&& !in_low_battery_failsafe_delay) {
// check for geofence violation transition
if ((_geofence_result.primary_geofence_breached && !_primary_geofence_breached_prev) ||
(_geofence_result.secondary_geofence_breached && !_secondary_geofence_breached_prev)) {
if (_geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_LOITER ||
_geofence_result.secondary_geofence_action == geofence_result_s::GF_ACTION_LOITER) {
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status,
commander_state_s::MAIN_STATE_AUTO_LOITER,
_vehicle_status_flags,
_commander_state)) {
_geofence_loiter_on = true;
}
}
if (_geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_RTL ||
_geofence_result.secondary_geofence_action == geofence_result_s::GF_ACTION_RTL) {
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status,
commander_state_s::MAIN_STATE_AUTO_RTL,
_vehicle_status_flags,
_commander_state)) {
_geofence_rtl_on = true;
}
}
if (_geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_LAND ||
_geofence_result.secondary_geofence_action == geofence_result_s::GF_ACTION_LAND) {
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status,
commander_state_s::MAIN_STATE_AUTO_LAND,
_vehicle_status_flags,
_commander_state)) {
_geofence_land_on = true;
}
}
if (_geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_TERMINATE ||
_geofence_result.secondary_geofence_action == geofence_result_s::GF_ACTION_TERMINATE) {
if (!_flight_termination_triggered && !_lockdown_triggered) {
_flight_termination_triggered = true;
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated\t");
events::send(events::ID("commander_geofence_termination"), {events::Log::Alert, events::LogInternal::Warning},
"Geofence violation! Flight terminated");
_actuator_armed.force_failsafe = true;
_status_changed = true;
sendParachuteCommand();
}
}
}
// reset if no longer in LOITER or if manually switched to LOITER
if (_commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER) {
_geofence_loiter_on = false;
}
// reset if no longer in RTL or if manually switched to RTL
if (_commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL) {
_geofence_rtl_on = false;
}
// reset if no longer in LAND or if manually switched to LAND
if (_commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
_geofence_land_on = false;
}
_primary_geofence_warning_action_on = _primary_geofence_warning_action_on || _geofence_loiter_on ||
_geofence_rtl_on || _geofence_land_on;
_secondary_geofence_warning_action_on = _secondary_geofence_warning_action_on || _geofence_loiter_on ||
_geofence_rtl_on || _geofence_land_on;
_primary_geofence_breached_prev = _geofence_result.primary_geofence_breached;
_secondary_geofence_breached_prev = _geofence_result.secondary_geofence_breached;
} else {
// Reset flags
_geofence_loiter_on = false;
_geofence_rtl_on = false;
_geofence_land_on = false;
_primary_geofence_warning_action_on = false;
_primary_geofence_breached_prev = false;
_secondary_geofence_warning_action_on = false;
_secondary_geofence_breached_prev = false;
}
}
bool Commander::getPrearmState() const
{
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
@ -1966,6 +2048,19 @@ void Commander::systemPowerUpdate()
}
}
Commander *Commander::instantiate(int argc, char *argv[])
{
Commander *instance = new Commander();
if (instance) {
if (argc >= 2 && !strcmp(argv[1], "-h")) {
instance->enable_hil();
}
}
return instance;
}
void Commander::landDetectorUpdate()
{
if (_vehicle_land_detected_sub.updated()) {
@ -2250,7 +2345,7 @@ void Commander::checkAndInformReadyForTakeoff()
#endif // CONFIG_ARCH_BOARD_PX4_SITL
}
void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
void Commander::controlStatusLeds(bool changed, const uint8_t battery_warning)
{
switch (blink_msg_state()) {
case 1:
@ -2350,7 +2445,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
_last_overload = overload;
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_controlStatusLeds)
if (_arm_state_machine.isArmed()) {
if (_vehicle_status.failsafe) {
@ -2410,7 +2505,7 @@ void Commander::updateControlMode()
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
}
void Commander::printRejectMode(uint8_t nav_state)
void Commander::printRejectMode(const uint8_t main_state)
{
if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) {
@ -2432,7 +2527,7 @@ void Commander::printRejectMode(uint8_t nav_state)
}
}
void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
void Commander::answerCommand(const vehicle_command_s &cmd, const uint8_t result)
{
switch (result) {
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED:
@ -2468,47 +2563,6 @@ void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
_vehicle_command_ack_pub.publish(command_ack);
}
int Commander::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
3250,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
// wait until task is up & running
if (wait_until_running() < 0) {
_task_id = -1;
return -1;
}
return 0;
}
Commander *Commander::instantiate(int argc, char *argv[])
{
Commander *instance = new Commander();
if (instance) {
if (argc >= 2 && !strcmp(argv[1], "-h")) {
instance->enable_hil();
}
}
return instance;
}
void Commander::enable_hil()
{
_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
}
void Commander::dataLinkCheck()
{
for (auto &telemetry_status : _telemetry_status_subs) {
@ -2788,7 +2842,7 @@ void Commander::offboardControlCheck()
}
}
void Commander::send_parachute_command()
void Commander::sendParachuteCommand()
{
vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
@ -2807,6 +2861,52 @@ void Commander::send_parachute_command()
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
}
bool Commander::shutdownIfAllowed()
{
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown);
}
void Commander::enable_hil()
{
_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
}
int Commander::print_status()
{
PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
perf_print_counter(_loop_perf);
perf_print_counter(_preflight_check_perf);
return 0;
}
int Commander::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
3250,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
// wait until task is up & running
if (wait_until_running() < 0) {
_task_id = -1;
return -1;
}
return 0;
}
int Commander::print_usage(const char *reason)
{
if (reason) {
@ -2849,3 +2949,8 @@ The commander module contains the state machine for mode switching and failsafe
return 1;
}
extern "C" __EXPORT int commander_main(int argc, char *argv[])
{
return Commander::main(argc, argv);
}

244
src/modules/commander/Commander.hpp
View File

@ -113,27 +113,69 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
void enable_hil();
void enableHIL();
private:
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
enum class ActuatorFailureActions {
DISABLED = 0,
AUTO_LOITER = 1,
AUTO_LAND = 2,
AUTO_RTL = 3,
TERMINATE = 4
};
transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
enum class PrearmedMode {
DISABLED = 0,
SAFETY_BUTTON = 1,
ALWAYS = 2
};
void battery_status_check();
enum class RcOverrideBits : int32_t {
AUTO_MODE_BIT = (1 << 0),
OFFBOARD_MODE_BIT = (1 << 1)
};
void control_status_leds(bool changed, const uint8_t battery_warning);
typedef enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
};
void answerCommand(const vehicle_command_s &cmd, const uint8_t result);
transition_result_t arm(const arm_disarm_reason_t calling_reason, const bool run_preflight_checks = true);
transition_result_t disarm(const arm_disarm_reason_t calling_reason, const bool forced = false);
void batteryStatusCheck();
void checkAndInformReadyForTakeoff();
void checkForMissionUpdate();
void checkGeofenceStatus();
void checkWindSpeedThresholds();
void checkWorkerThread();
void controlStatusLeds(bool changed, const uint8_t battery_warning);
/**
* Checks the status of all available data links and handles switching between different system telemetry states.
*/
void dataLinkCheck();
void manualControlCheck();
void executeActionRequest(const action_request_s &action_request);
void offboardControlCheck();
void handleAutoDisarm();
/**
* @brief Handle incoming vehicle command relavant to Commander
@ -141,168 +183,178 @@ private:
* It ignores irrelevant vehicle commands defined inside the switch case statement
* in the function.
*
* @param cmd Vehicle command to handle
* @param cmd Vehicle command to handle
*/
bool handle_command(const vehicle_command_s &cmd);
bool handleCommand(const vehicle_command_s &cmd);
unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
void executeActionRequest(const action_request_s &action_request);
void printRejectMode(uint8_t nav_state);
void updateControlMode();
bool shutdownIfAllowed();
void send_parachute_command();
void checkForMissionUpdate();
bool handleModeIntentionAndFailsafe();
void handlePowerButtonState();
void systemPowerUpdate();
void landDetectorUpdate();
void manualControlUpdate();
void offboardControlUpdate();
void printRejectMode(uint8_t nav_state);
void safetyButtonUpdate();
void vtolStatusUpdate();
void updateTunes();
void checkWorkerThread();
bool getPrearmState() const;
void handleAutoDisarm();
void sendParachuteCommand();
bool handleModeIntentionAndFailsafe();
bool shutdownIfAllowed();
void systemPowerUpdate();
void updateControlMode();
void updateParameters();
void checkAndInformReadyForTakeoff();
void updateTunes();
enum class PrearmedMode {
DISABLED = 0,
SAFETY_BUTTON = 1,
ALWAYS = 2
};
void vtolStatusUpdate();
enum class RcOverrideBits : int32_t {
AUTO_MODE_BIT = (1 << 0),
OFFBOARD_MODE_BIT = (1 << 1),
};
/* Decouple update interval and hysteresis counters, all depends on intervals */
// Decouple update interval and hysteresis counters, all depends on intervals
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
vehicle_status_s _vehicle_status{};
orb_advert_t _mavlink_log_pub{nullptr};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
ArmStateMachine _arm_state_machine{};
Failsafe _failsafe_instance{this};
FailsafeBase &_failsafe{_failsafe_instance};
FailureDetector _failure_detector{this};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
HomePosition _home_position{_vehicle_status_flags};
Safety _safety{};
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
WorkerThread _worker_thread{};
const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
HomePosition _home_position{_failsafe_flags};
actuator_armed_s _actuator_armed{};
commander_state_s _commander_state{};
geofence_result_s _geofence_result{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_land_detected_s _vehicle_land_detected{};
vehicle_status_s _vehicle_status{};
vehicle_status_flags_s _vehicle_status_flags{};
vtol_vehicle_status_s _vtol_vehicle_status{};
Hysteresis _auto_disarm_landed{false};
Hysteresis _auto_disarm_killed{false};
Hysteresis _offboard_available{false};
hrt_abstime _battery_failsafe_timestamp{0};
hrt_abstime _boot_timestamp{0};
hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
hrt_abstime _datalink_last_heartbeat_gcs{0};
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
hrt_abstime _datalink_last_heartbeat_parachute_system{0};
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};
hrt_abstime _overload_start{0}; ///< time when CPU overload started
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
hrt_abstime _led_armed_state_toggle {0};
#endif
hrt_abstime _led_overload_toggle {0};
hrt_abstime _overload_start{0}; ///< time when CPU overload started
hrt_abstime _last_health_and_arming_check{0};
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
hrt_abstime _last_termination_message_sent{0};
hrt_abstime _last_valid_manual_control_setpoint{0};
hrt_abstime _last_wind_warning{0};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _led_overload_toggle{0};
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
hrt_abstime _led_armed_state_toggle{0};
#endif
bool _flight_termination_triggered{false};
bool _lockdown_triggered{false};
hrt_abstime _overload_start{0}; ///< time when CPU overload started
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
bool _open_drone_id_system_lost{true};
bool _avoidance_system_lost{false};
bool _onboard_controller_lost{false};
bool _parachute_system_lost{true};
bool _circuit_breaker_flight_termination_disabled{false};
bool _last_overload{false};
bool _mode_switch_mapped{false};
bool _failsafe_old{false};
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
bool _flight_termination_triggered{false};
bool _imbalanced_propeller_check_triggered{false};
bool _is_throttle_above_center{false};
bool _is_throttle_low{false};
bool _arm_tune_played{false};
bool _was_armed{false};
bool _have_taken_off_since_arming{false};
bool _last_overload{false};
bool _lockdown_triggered{false};
bool _mode_switch_mapped{false};
bool _open_drone_id_system_lost{true};
bool _onboard_controller_lost{false};
bool _parachute_system_lost{true};
bool _rtl_time_actions_done{false};
bool _status_changed{true};
vehicle_land_detected_s _vehicle_land_detected{};
// Arming flags
bool _arm_tune_played{false};
bool _have_taken_off_since_arming{false};
bool _was_armed{false};
// commander publications
actuator_armed_s _actuator_armed{};
vehicle_control_mode_s _vehicle_control_mode{};
vtol_vehicle_status_s _vtol_vehicle_status{};
// Geofence flags
bool _geofence_loiter_on{false};
bool _geofence_rtl_on{false};
bool _geofence_land_on{false};
bool _primary_geofence_warning_action_on{false};
bool _primary_geofence_breached_prev{false};
bool _secondary_geofence_warning_action_on{false};
bool _secondary_geofence_breached_prev{false};
// Subscriptions
uORB::Subscription _action_request_sub{ORB_ID(action_request)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription _action_request_sub{ORB_ID(action_request)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription _wind_sub{ORB_ID(wind)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
#if defined(BOARD_HAS_POWER_CONTROL)
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
#endif // BOARD_HAS_POWER_CONTROL
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
// Publications
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
orb_advert_t _mavlink_log_pub{nullptr};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
// optional parameters
// Optional parameters
param_t _param_mav_comp_id{PARAM_INVALID};
param_t _param_mav_sys_id{PARAM_INVALID};
param_t _param_mav_type{PARAM_INVALID};

13
src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp
View File

@ -50,10 +50,11 @@ private:
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
(ParamInt<px4::params::GF_ACTION>) _param_gf_action
)
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
(ParamInt<px4::params::GF2_ACTION>) _param_gf2_action
)
};

2
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -339,7 +339,7 @@ private:
geofence_result_s geofence;
if (_geofence_sub.update(&geofence)) {
if (geofence.primary_geofence_breached) {
if (geofence.primary_geofence_breached || geofence.secondary_geofence_breached) {
msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
}

77
src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
View File

@ -50,14 +50,15 @@ public:
TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
Vector2d home_global(42.1, 8.2);
MapProjection ref{home_global(0), home_global(1)};
Vector2f waypoint_north_east_local(1.0, 1.0);
waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5;
Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5);
Vector2f waypoint_north_east_reprojected = ref.project(waypoint_north_east_global(0), waypoint_north_east_global(1));
@ -65,7 +66,6 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0));
EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1));
Vector2f waypoint_south_west_local = -waypoint_north_east_local;
Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5);
@ -75,7 +75,6 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0));
EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1));
Vector2d same_as_home_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 0.0);
EXPECT_LT(Vector2d(home_global - same_as_home_global).norm(), 1e-4);
@ -84,19 +83,19 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
FakeGeofence geo{};
Vector2d home_global(42.1, 8.2);
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.fence_violation = true;
geofence_result_s geofence_result;
geofence_result.primary_geofence_breached = true;
geofence_result.secondary_geofence_breached = true;
gf_avoidance.setHorizontalTestPointDistance(20.0f);
gf_avoidance.setTestPointBearing(0.0f);
gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(geofence_result, &geo);
// the expected loiter point is located test_point_distance behind
Vector2d loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, -20.0f);
@ -104,25 +103,26 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0));
EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE);
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(geofence_result, &geo);
loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, -M_PI_F * 0.5f, 20.0f);
EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0));
EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::RIGHT_INSIDE_LEFT_OUTSIDE);
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(geofence_result, &geo);
loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.5f, 20.0f);
EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0));
EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1));
gf_violation.flags.fence_violation = false;
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
geofence_result.primary_geofence_breached = false;
geofence_result.secondary_geofence_breached = false;
loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(geofence_result, &geo);
EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), home_global(0));
EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), home_global(1));
@ -148,8 +148,9 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
value = 8;
param_set(param, &value);
geofence_violation_type_u gf_violation;
gf_violation.flags.fence_violation = true;
geofence_result_s geofence_result;
geofence_result.primary_geofence_breached = true;
geofence_result.secondary_geofence_breached = true;
gf_avoidance.setHorizontalTestPointDistance(30.0f);
gf_avoidance.setTestPointBearing(0.0f);
@ -160,7 +161,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::GF_BOUNDARY_20M_AHEAD);
Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(geofence_result, &geo);
Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f,
20.0f - gf_avoidance.getMinHorDistToFenceMulticopter());
@ -174,7 +175,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
gf_avoidance.setHorizontalVelocity(0.1f);
gf_avoidance.computeBrakingDistanceMultirotor();
loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
loiter_point = gf_avoidance.generateLoiterPointForMultirotor(geofence_result, &geo);
loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f,
gf_avoidance.computeBrakingDistanceMultirotor());
@ -184,8 +185,10 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
EXPECT_LE(error, 0.0f);
gf_violation.flags.fence_violation = false;
loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
geofence_result.primary_geofence_breached = false;
geofence_result.secondary_geofence_breached = false;
loiter_point = gf_avoidance.generateLoiterPointForMultirotor(geofence_result, &geo);
EXPECT_LT(Vector2d(loiter_point - home_global).norm(), 1e-4);
}
@ -198,16 +201,17 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing)
gf_avoidance.setVerticalTestPointDistance(vertical_test_point_dist);
gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); // just care about altitude
geofence_violation_type_u gf_violation;
gf_violation.flags.max_altitude_exceeded = true;
float loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation);
geofence_result_s geofence_result;
geofence_result.max_altitude_exceeded = true;
float loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(geofence_result);
EXPECT_EQ(loiter_alt, current_alt_amsl - 2 * vertical_test_point_dist);
gf_violation.flags.max_altitude_exceeded = false;
geofence_result.max_altitude_exceeded = false;
loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation);
loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(geofence_result);
EXPECT_EQ(loiter_alt, current_alt_amsl);
}
@ -217,21 +221,22 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter)
GeofenceBreachAvoidance gf_avoidance(nullptr);
const float climbrate = 10.0f;
const float current_alt_amsl = 100.0f;
geofence_violation_type_u gf_violation;
gf_violation.flags.max_altitude_exceeded = true;
geofence_result_s geofence_result;
geofence_result.max_altitude_exceeded = true;
gf_avoidance.setClimbRate(climbrate);
gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl);
gf_avoidance.computeVerticalBrakingDistanceMultirotor();
float loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation);
float loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(geofence_result);
EXPECT_EQ(loiter_alt_amsl, current_alt_amsl + gf_avoidance.computeVerticalBrakingDistanceMultirotor() -
gf_avoidance.getMinVertDistToFenceMultirotor());
gf_violation.flags.max_altitude_exceeded = false;
geofence_result.max_altitude_exceeded = false;
loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation);
loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(geofence_result);
EXPECT_EQ(loiter_alt_amsl, current_alt_amsl);
}
@ -242,8 +247,9 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
FakeGeofence geo;
Vector2d home_global(42.1, 8.2);
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;
geofence_result_s geofence_result;
geofence_result.max_distance_exceeded = true;
const float hor_vel = 8.0f;
const float test_point_distance = 30.0f;
@ -260,7 +266,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
gf_avoidance.setMaxHorDistHome(max_dist_to_home);
gf_avoidance.setHomePosition(home_global(0), home_global(1), 0);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForMultirotor(geofence_result, &geo);
Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing,
max_dist_to_home - gf_avoidance.getMinHorDistToFenceMulticopter());
@ -274,8 +280,9 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing)
FakeGeofence geo;
Vector2d home_global(42.1, 8.2);
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;
geofence_result_s geofence_result;
geofence_result.max_distance_exceeded = true;
const float test_point_distance = 30.0f;
const float max_dist_to_home = 100.0f;
@ -289,7 +296,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing)
gf_avoidance.setMaxHorDistHome(max_dist_to_home);
gf_avoidance.setHomePosition(home_global(0), home_global(1), 0);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo);
Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(geofence_result, &geo);
Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing,
max_dist_to_home - 2.0f * test_point_distance);

2
src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp
View File

@ -51,7 +51,7 @@ public:
virtual ~FakeGeofence() {};
bool isInsidePolygonOrCircle(double lat, double lon, float altitude) override
bool isFakePrimaryGeofenceBreached(double lat, double lon, float altitude)
{
switch (_probe_function_behavior) {
case ProbeFunction::ALL_POINTS_OUTSIDE: {

53
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp
View File

@ -87,42 +87,45 @@ matrix::Vector2<double> GeofenceBreachAvoidance::waypointFromBearingAndDistance(
test_point_bearing = matrix::wrap_2pi(test_point_bearing + M_PI_F);
}
double fence_violation_test_point_lat, fence_violation_test_point_lon;
double fence_violation_test_point_lat{};
double fence_violation_test_point_lon{};
waypoint_from_heading_and_distance(current_pos_lat_lon(0), current_pos_lat_lon(1), test_point_bearing,
test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon);
return Vector2d(fence_violation_test_point_lat, fence_violation_test_point_lon);
}
Vector2d
GeofenceBreachAvoidance::getFenceViolationTestPoint()
Vector2d GeofenceBreachAvoidance::getFenceViolationTestPoint()
{
return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _test_point_distance);
}
Vector2d
GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence)
Vector2d GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_result_s geofence_result, Geofence *geofence)
{
if (violation_type.flags.fence_violation) {
if (geofence_result.primary_geofence_breached) {
const float bearing_90_left = matrix::wrap_2pi(_test_point_bearing - M_PI_F * 0.5f);
const float bearing_90_right = matrix::wrap_2pi(_test_point_bearing + M_PI_F * 0.5f);
double loiter_center_lat, loiter_center_lon;
double fence_violation_test_point_lat, fence_violation_test_point_lon;
double loiter_center_lat{};
double loiter_center_lon{};
double fence_violation_test_point_lat{};
double fence_violation_test_point_lon{};
waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_left,
_test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon);
const bool left_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat,
const bool left_side_is_inside_fence = geofence->isPrimaryGeofenceBreached(fence_violation_test_point_lat,
fence_violation_test_point_lon, _current_alt_amsl);
waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_right,
_test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon);
const bool right_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat,
const bool right_side_is_inside_fence = geofence->isPrimaryGeofenceBreached(fence_violation_test_point_lat,
fence_violation_test_point_lon, _current_alt_amsl);
float bearing_to_loiter_point;
float bearing_to_loiter_point{};
if (right_side_is_inside_fence && !left_side_is_inside_fence) {
bearing_to_loiter_point = bearing_90_right;
@ -139,7 +142,7 @@ GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_violation_type
return Vector2d(loiter_center_lat, loiter_center_lon);
} else if (violation_type.flags.dist_to_home_exceeded) {
} else if (geofence_result.max_distance_exceeded) {
return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - 2 * _test_point_distance, 0.0f));
@ -148,21 +151,21 @@ GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_violation_type
}
}
Vector2d
GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence)
Vector2d GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_result_s geofence_result,
Geofence *geofence)
{
if (violation_type.flags.fence_violation) {
if (geofence_result.primary_geofence_breached) {
float current_distance = _test_point_distance * 0.5f;
float current_min = 0.0f;
float current_max = _test_point_distance;
Vector2d test_point;
float current_min{};
float current_max{};
Vector2d test_point{};
// binary search for the distance from the drone to the geofence in the given direction
while (abs(current_max - current_min) > 0.5f) {
test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance);
if (!geofence->isInsidePolygonOrCircle(test_point(0), test_point(1), _current_alt_amsl)) {
if (!geofence->isPrimaryGeofenceBreached(test_point(0), test_point(1), _current_alt_amsl)) {
current_max = current_distance;
} else {
@ -181,7 +184,7 @@ GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_typ
return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance);
}
} else if (violation_type.flags.dist_to_home_exceeded) {
} else if (geofence_result.max_distance_exceeded) {
return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - _min_hor_dist_to_fence_mc, 0.0f));
@ -195,9 +198,9 @@ GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_typ
}
}
float GeofenceBreachAvoidance::generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type)
float GeofenceBreachAvoidance::generateLoiterAltitudeForFixedWing(geofence_result_s geofence_result)
{
if (violation_type.flags.max_altitude_exceeded) {
if (geofence_result.max_altitude_exceeded) {
return _current_alt_amsl - 2.0f * _vertical_test_point_distance;
} else {
@ -205,9 +208,9 @@ float GeofenceBreachAvoidance::generateLoiterAltitudeForFixedWing(geofence_viola
}
}
float GeofenceBreachAvoidance::generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type)
float GeofenceBreachAvoidance::generateLoiterAltitudeForMulticopter(geofence_result_s geofence_result)
{
if (violation_type.flags.max_altitude_exceeded) {
if (geofence_result.max_altitude_exceeded) {
return _current_alt_amsl + _multirotor_vertical_braking_distance - _min_vert_dist_to_fence_mc;
} else {

78
src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h
View File

@ -33,23 +33,15 @@
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/module_params.h>
#include "../geofence.h"
#include <px4_platform_common/defines.h>
#include <uORB/topics/geofence_result.h>
#include "../geofence.h"
class Geofence;
#define GEOFENCE_CHECK_INTERVAL_US 200000
union geofence_violation_type_u {
struct {
bool dist_to_home_exceeded: 1; ///< 0 - distance to home exceeded
bool max_altitude_exceeded: 1; ///< 1 - maximum altitude exceeded
bool fence_violation: 1; ///< 2- violation of user defined fence
} flags;
uint8_t value;
};
class GeofenceBreachAvoidance : public ModuleParams
{
public:
@ -59,47 +51,52 @@ public:
matrix::Vector2<double> getFenceViolationTestPoint();
matrix::Vector2<double> waypointFromBearingAndDistance(matrix::Vector2<double> current_pos_lat_lon,
float test_point_bearing, float test_point_distance);
matrix::Vector2<double> waypointFromBearingAndDistance(matrix::Vector2<double> const current_pos_lat_lon,
const float test_point_bearing, const float test_point_distance);
matrix::Vector2<double>
generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence);
matrix::Vector2<double> generateLoiterPointForFixedWing(geofence_result_s geofence_result, Geofence *geofence);
float computeBrakingDistanceMultirotor();
float computeVerticalBrakingDistanceMultirotor();
matrix::Vector2<double> generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence);
matrix::Vector2<double> generateLoiterPointForMultirotor(geofence_result_s geofence_result, Geofence *geofence);
float generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type);
float generateLoiterAltitudeForFixedWing(geofence_result_s geofence_result);
float generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type);
float generateLoiterAltitudeForMulticopter(geofence_result_s geofence_result);
float getMinHorDistToFenceMulticopter() {return _min_hor_dist_to_fence_mc;}
float getMinVertDistToFenceMultirotor() {return _min_vert_dist_to_fence_mc;}
void setTestPointBearing(float test_point_bearing) { _test_point_bearing = test_point_bearing; }
void setTestPointBearing(const float test_point_bearing) { _test_point_bearing = test_point_bearing; }
void setHorizontalTestPointDistance(float test_point_distance) { _test_point_distance = test_point_distance; }
void setHorizontalTestPointDistance(const float test_point_distance) { _test_point_distance = test_point_distance; }
void setVerticalTestPointDistance(float distance) { _vertical_test_point_distance = distance; }
void setVerticalTestPointDistance(const float distance) { _vertical_test_point_distance = distance; }
void setHorizontalVelocity(float velocity_hor_abs) { _velocity_hor_abs = velocity_hor_abs; }
void setHorizontalVelocity(const float velocity_hor_abs) { _velocity_hor_abs = velocity_hor_abs; }
void setClimbRate(float climbrate) { _climbrate = climbrate; }
void setClimbRate(const float climbrate) { _climbrate = climbrate; }
void setCurrentPosition(double lat, double lon, float alt);
void setCurrentPosition(const double lat, const double lon, const float alt);
void setHomePosition(double lat, double lon, float alt);
void setHomePosition(const double lat, const double lon, const float alt);
void setMaxHorDistHome(float dist) { _max_hor_dist_home = dist; }
void setMaxHorDistHome(const float dist) { _max_hor_dist_home = dist; }
void setMaxVerDistHome(float dist) { _max_ver_dist_home = dist; }
void setMaxVerDistHome(const float dist) { _max_ver_dist_home = dist; }
void updateParameters();
private:
void updateMinHorDistToFenceMultirotor();
void updateMinVertDistToFenceMultirotor();
matrix::Vector2<double> waypointFromHomeToTestPointAtDist(float distance);
struct {
param_t param_mpc_jerk_max;
param_t param_mpc_acc_hor;
@ -120,29 +117,26 @@ private:
} _params;
float _test_point_bearing{0.0f};
float _test_point_distance{0.0f};
float _vertical_test_point_distance{0.0f};
float _velocity_hor_abs{0.0f};
matrix::Vector2<double> _current_pos_lat_lon{};
matrix::Vector2<double> _home_lat_lon {};
float _climbrate{0.0f};
float _current_alt_amsl{0.0f};
float _home_alt_amsl{0.0f};
float _max_hor_dist_home{0.0f};
float _max_ver_dist_home{0.0f};
float _min_hor_dist_to_fence_mc{0.0f};
float _min_vert_dist_to_fence_mc{0.0f};
float _multirotor_braking_distance{0.0f};
float _multirotor_vertical_braking_distance{0.0f};
matrix::Vector2<double> _current_pos_lat_lon{};
matrix::Vector2<double> _home_lat_lon {};
float _home_alt_amsl{0.0f};
float _max_hor_dist_home{0.0f};
float _max_ver_dist_home{0.0f};
void updateMinHorDistToFenceMultirotor();
void updateMinVertDistToFenceMultirotor();
matrix::Vector2<double> waypointFromHomeToTestPointAtDist(float distance);
float _test_point_bearing{0.0f};
float _test_point_distance{0.0f};
float _vertical_test_point_distance{0.0f};
float _velocity_hor_abs{0.0f};
};

159
src/modules/navigator/geofence.cpp
View File

@ -189,13 +189,13 @@ bool Geofence::checkAll(const struct vehicle_global_position_s &global_position,
bool Geofence::checkAll(double lat, double lon, float altitude)
{
bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude);
bool inside_fence = isMaxDistanceBreached(lat, lon, altitude);
inside_fence = inside_fence && isBelowMaxAltitude(altitude);
inside_fence = inside_fence && isMaxAltitudeBreached(altitude);
// to be inside the geofence both fences have to report being inside
// as they both report being inside when not enabled
inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude);
inside_fence = inside_fence && isPrimaryGeofenceBreached(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
@ -242,9 +242,9 @@ bool Geofence::check(const struct mission_item_s &mission_item)
return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude);
}
bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude)
bool Geofence::isMaxDistanceBreached(const double lat, const double lon, const float altitude)
{
bool inside_fence = true;
bool exceeds_max_dist = false;
if (isHomeRequired() && _navigator->home_global_position_valid()) {
@ -269,16 +269,17 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude)
_last_horizontal_range_warning = hrt_absolute_time();
}
inside_fence = false;
exceeds_max_dist = true;
PX4_INFO("Max distance geofence breached");
}
}
return inside_fence;
return exceeds_max_dist;
}
bool Geofence::isBelowMaxAltitude(float altitude)
bool Geofence::isMaxAltitudeBreached(const float altitude)
{
bool inside_fence = true;
bool exceeds_max_alt = false;
if (isHomeRequired() && _navigator->home_alt_valid()) {
@ -297,22 +298,24 @@ bool Geofence::isBelowMaxAltitude(float altitude)
_last_vertical_range_warning = hrt_absolute_time();
}
inside_fence = false;
exceeds_max_alt = true;
PX4_INFO("Max altitude geofence breached");
}
}
return inside_fence;
return exceeds_max_alt;
}
bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
bool Geofence::isPrimaryGeofenceBreached(const double lat, const double lon, const float altitude)
{
// the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means
// the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now
if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) {
return true;
return false;
}
// we got the lock, now check if the fence data got updated
// dm items locked, check if the fence data was updated
mission_stats_entry_s stats;
int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
@ -322,27 +325,33 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
if (isEmpty()) {
dm_unlock(DM_KEY_FENCE_POINTS);
/* Empty fence -> accept all points */
// Empty fence -> accept all points
return true;
}
/* Vertical check */
if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly
// Vertical check. Only perform check if configured properly.
if (_altitude_max > _altitude_min) {
if (altitude > _altitude_max || altitude < _altitude_min) {
dm_unlock(DM_KEY_FENCE_POINTS);
return false;
return true;
}
// Secondary geofence check
if ((altitude > _altitude_max + _param_gf2_offset.get()) ||
(altitude < _altitude_min - _param_gf2_offset.get())) {
_secondary_geofence_breach = true;
}
}
/* Horizontal check: iterate all polygons & circles */
// Horizontal check: iterate all polygons & circles
bool outside_exclusion = true;
bool inside_inclusion = false;
bool had_inclusion_areas = false;
bool inside = false;
for (int polygon_index = 0; polygon_index < _num_polygons; ++polygon_index) {
if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) {
bool inside = insideCircle(_polygons[polygon_index], lat, lon, altitude);
inside = insideCircle(_polygons[polygon_index], lat, lon, altitude);
if (inside) {
inside_inclusion = true;
@ -351,36 +360,48 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
had_inclusion_areas = true;
} else if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
bool inside = insideCircle(_polygons[polygon_index], lat, lon, altitude);
inside = insideCircle(_polygons[polygon_index], lat, lon, altitude, false);
if (inside) {
outside_exclusion = false;
}
} else { // it's a polygon
bool inside = insidePolygon(_polygons[polygon_index], lat, lon, altitude);
} else if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
inside = insidePolygon(_polygons[polygon_index], lat, lon, altitude);
if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
if (inside) {
inside_inclusion = true;
}
if (inside) {
inside_inclusion = true;
}
had_inclusion_areas = true;
had_inclusion_areas = true;
} else { // exclusion
if (inside) {
outside_exclusion = false;
}
} else if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
inside = insidePolygon(_polygons[polygon_index], lat, lon, altitude, false);
if (inside) {
outside_exclusion = false;
}
}
}
dm_unlock(DM_KEY_FENCE_POINTS);
return (!had_inclusion_areas || inside_inclusion) && outside_exclusion;
if ((inside_inclusion && had_inclusion_areas) ||
(outside_exclusion && !had_inclusion_areas)) {
return false;
} else {
return true;
}
}
bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude)
bool Geofence::isSecondaryGeofenceBreached()
{
return _secondary_geofence_breach;
}
bool Geofence::insidePolygon(const PolygonInfo &polygon, const double lat, const double lon, const float altitude,
const bool inclusion_fence)
{
/**
* Adaptation of algorithm originally presented as
@ -391,7 +412,10 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
mission_fence_point_s temp_vertex_i{};
mission_fence_point_s temp_vertex_j{};
bool c = false;
crosstrack_error_s crosstrack_error{};
float distance_to_geofence{1e6f};
bool inside_polygon{false};
for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) {
if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i,
@ -415,16 +439,30 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
(double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
c = !c;
inside_polygon = !inside_polygon;
}
get_distance_to_line(&crosstrack_error, lat, lon,
temp_vertex_i.lat, temp_vertex_i.lon,
temp_vertex_j.lat, temp_vertex_j.lon);
distance_to_geofence = math::min(fabs(crosstrack_error.distance), fabs(distance_to_geofence));
}
if ((inside_polygon && !inclusion_fence) ||
(!inside_polygon && inclusion_fence)) {
if (distance_to_geofence > _param_gf2_offset.get()) {
_secondary_geofence_breach = true;
}
}
return c;
return inside_polygon;
}
bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude)
bool Geofence::insideCircle(const PolygonInfo &polygon, const double lat, const double lon, const float altitude,
const bool inclusion_fence)
{
mission_fence_point_s circle_point{};
if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point,
@ -445,21 +483,29 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
_projection_reference.initReference(lat, lon);
}
float x1, y1, x2, y2;
_projection_reference.project(lat, lon, x1, y1);
_projection_reference.project(circle_point.lat, circle_point.lon, x2, y2);
float dx = x1 - x2, dy = y1 - y2;
return dx * dx + dy * dy < circle_point.circle_radius * circle_point.circle_radius;
float distance_to_center = get_distance_to_next_waypoint(lat, lon, circle_point.lat, circle_point.lon);
bool inside_circle = false;
if (distance_to_center < circle_point.circle_radius) {
inside_circle = true;
} else {
if ((inclusion_fence == true && !inside_circle) ||
(!inclusion_fence == true && inside_circle)) {
float distance_to_geofence = distance_to_center - circle_point.circle_radius;
if (distance_to_geofence > _param_gf2_offset.get()) {
_secondary_geofence_breach = true;
}
}
}
return inside_circle;
}
bool
Geofence::valid()
{
return true; // always valid
}
int
Geofence::loadFromFile(const char *filename)
int Geofence::loadFromFile(const char *filename)
{
FILE *fp;
char line[120];
@ -471,7 +517,7 @@ Geofence::loadFromFile(const char *filename)
/* Make sure no data is left in the datamanager */
clearDm();
/* open the mixer definition file */
/* open the geofence file */
fp = fopen(GEOFENCE_FILENAME, "r");
if (fp == nullptr) {
@ -595,9 +641,10 @@ bool Geofence::isHomeRequired()
{
bool max_horizontal_enabled = (_param_gf_max_hor_dist.get() > FLT_EPSILON);
bool max_vertical_enabled = (_param_gf_max_ver_dist.get() > FLT_EPSILON);
bool geofence_action_rtl = (getGeofenceAction() == geofence_result_s::GF_ACTION_RTL);
bool primary_geofence_action_rtl = (getPrimaryGeofenceAction() == geofence_result_s::GF_ACTION_RTL);
bool secondary_geofence_action_rtl = (getPrimaryGeofenceAction() == geofence_result_s::GF_ACTION_RTL);
return max_horizontal_enabled || max_vertical_enabled || geofence_action_rtl;
return max_horizontal_enabled || max_vertical_enabled || primary_geofence_action_rtl || secondary_geofence_action_rtl;
}
void Geofence::printStatus()

131
src/modules/navigator/geofence.h
View File

@ -77,12 +77,6 @@ public:
GF_SOURCE_GPS = 1
};
/**
* update the geofence from dataman.
* It's generally not necessary to call this as it will automatically update when the data is changed.
*/
void updateFence();
/**
* Return whether the system obeys the geofence.
*
@ -103,17 +97,32 @@ public:
*
* @return false for a geofence violation
*/
bool checkAll(double lat, double lon, float altitude);
bool isCloserThanMaxDistToHome(double lat, double lon, float altitude);
bool isBelowMaxAltitude(float altitude);
virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude);
bool checkAll(const double lat, const double lon, const float altitude);
int clearDm();
int getSource() { return _param_gf_source.get(); }
bool valid();
int getPrimaryGeofenceAction() { return _param_gf_action.get(); }
int getSecondaryGeofenceAction() { return _param_gf2_action.get(); }
float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); }
float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); }
bool getPredict() { return _param_gf_predict.get(); }
bool isEmpty() { return _num_polygons == 0; }
bool isHomeRequired();
bool isMaxAltitudeBreached(const float altitude);
bool isMaxDistanceBreached(const double lat, const double lon, const float altitude);
bool isPrimaryGeofenceBreached(const double lat, const double lon, const float altitude);
bool isSecondaryGeofenceBreached();
/**
* Load a single inclusion polygon, replacing any already existing polygons.
@ -136,22 +145,19 @@ public:
*/
int loadFromFile(const char *filename);
bool isEmpty() { return _num_polygons == 0; }
int getSource() { return _param_gf_source.get(); }
int getGeofenceAction() { return _param_gf_action.get(); }
float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); }
float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); }
bool getPredict() { return _param_gf_predict.get(); }
bool isHomeRequired();
/**
* print Geofence status to the console
*/
void printStatus();
/**
* update the geofence from dataman.
* It's generally not necessary to call this as it will automatically update when the data is changed.
*/
void updateFence();
bool valid() { return true; } // always valid
private:
struct PolygonInfo {
@ -163,28 +169,9 @@ private:
};
};
Navigator *_navigator{nullptr};
PolygonInfo *_polygons{nullptr};
bool checkAll(const vehicle_global_position_s &global_position);
hrt_abstime _last_horizontal_range_warning{0};
hrt_abstime _last_vertical_range_warning{0};
float _altitude_min{0.0f};
float _altitude_max{0.0f};
int _num_polygons{0};
MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
uORB::SubscriptionData<vehicle_air_data_s> _sub_airdata;
int _outside_counter{0};
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated
/**
* implementation of updateFence(), but without locking
*/
void _updateFence();
bool checkAll(const vehicle_global_position_s &global_position, const float baro_altitude_amsl);
/**
* Check if a point passes the Geofence test.
@ -196,25 +183,47 @@ private:
* or: no polygon configured
* @return result of the check above (false for a geofence violation)
*/
bool checkPolygons(double lat, double lon, float altitude);
bool checkAll(const vehicle_global_position_s &global_position);
bool checkAll(const vehicle_global_position_s &global_position, float baro_altitude_amsl);
/**
* Check if a single point is within a polygon
* @return true if within polygon
*/
bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude);
bool checkPolygons(const double lat, const double lon, const float altitude);
/**
* Check if a single point is within a circle
* @param polygon must be a circle!
* @return true if within polygon the circle
*/
bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude);
bool insideCircle(const PolygonInfo &polygon, const double lat, const double lon, const float altitude,
const bool inclusion_fence = true);
/**
* Check if a single point is within a polygon
* @return true if within polygon
*/
bool insidePolygon(const PolygonInfo &polygon, const double lat, const double lon, const float altitude,
const bool inclusion_fence = true);
/**
* update the geofence from dataman.
*/
void _updateFence();
Navigator *_navigator{nullptr};
PolygonInfo *_polygons{nullptr};
MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
uORB::SubscriptionData<vehicle_air_data_s> _sub_airdata;
hrt_abstime _last_horizontal_range_warning{0};
hrt_abstime _last_vertical_range_warning{0};
float _altitude_min{0.0f};
float _altitude_max{0.0f};
int _num_polygons{0};
int _outside_counter{0};
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated
bool _secondary_geofence_breach{false};
DEFINE_PARAMETERS(
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
@ -223,6 +232,8 @@ private:
(ParamInt<px4::params::GF_COUNT>) _param_gf_count,
(ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_gf_max_hor_dist,
(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist,
(ParamBool<px4::params::GF_PREDICT>) _param_gf_predict
(ParamBool<px4::params::GF_PREDICT>) _param_gf_predict,
(ParamInt<px4::params::GF2_ACTION>) _param_gf2_action,
(ParamFloat<px4::params::GF2_OFFSET>) _param_gf2_offset
)
};

31
src/modules/navigator/geofence_params.c
View File

@ -61,6 +61,35 @@
*/
PARAM_DEFINE_INT32(GF_ACTION, 2);
/**
* Secondary Geofence violation action.
*
* Note: Setting this value to 4 enables flight termination,
* which will kill the vehicle on violation of the fence.
*
* @min 0
* @max 5
* @value 0 None
* @value 1 Warning
* @value 2 Hold mode
* @value 3 Return mode
* @value 4 Terminate
* @value 5 Land mode
* @group Geofence
*/
PARAM_DEFINE_INT32(GF2_ACTION, 0);
/**
* Secondary Geofence distance offset distance from the Primary Geofence, (m).
*
* Distance offset outside of primary Geofence to apply the GF2_ACTION.
* *
* @min 0
* @max 100
* @group Geofence
*/
PARAM_DEFINE_FLOAT(GF2_OFFSET, 0.f);
/**
* Geofence altitude mode
*
@ -125,7 +154,7 @@ PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0);
* @increment 1
* @group Geofence
*/
PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0);
PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0.f);
/**
* Use Pre-emptive geofence triggering

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

@ -66,19 +66,19 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/mode_completed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mode_completed.h>
#include <uORB/uORB.h>
using namespace time_literals;
@ -115,9 +115,7 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
/**
* Load fence from file
*/
/** Load fence from file */
void load_fence_from_file(const char *filename);
/**
@ -130,25 +128,36 @@ public:
*/
void publish_vehicle_cmd(vehicle_command_s *vcmd);
/**
* Check nearby traffic for potential collisions
*/
void check_traffic();
void take_traffic_conflict_action();
void run_fake_traffic();
/**
* Setters
/*
* Generate an artificial traffic indication
*
* @param distance Horizontal distance to this vehicle
* @param direction Direction in earth frame from this vehicle in radians
* @param traffic_heading Travel direction of the traffic in earth frame in radians
* @param altitude_diff Altitude difference, positive is up
* @param hor_velocity Horizontal velocity of traffic, in m/s
* @param ver_velocity Vertical velocity of traffic, in m/s
* @param emitter_type, Type of vehicle, as a number
*/
void fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff,
float hor_velocity, float ver_velocity, int emitter_type);
/** Check nearby traffic for potential collisions */
void check_traffic();
/** Buffer for air traffic to control the amount of messages sent to a user. */
bool buffer_air_traffic(uint32_t icao_address);
/** Setters */
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
void set_mission_result_updated() { _mission_result_updated = true; }
/**
* Getters
*/
/** Getters */
home_position_s *get_home_position() { return &_home_pos; }
mission_result_s *get_mission_result() { return &_mission_result; }
position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
@ -182,21 +191,18 @@ public:
/**
* Get the acceptance radius
*
* @return the distance at which the next waypoint should be used
*/
float get_acceptance_radius();
/**
* Get the altitude acceptance radius
*
* @return the distance from the target altitude before considering the waypoint reached
*/
float get_altitude_acceptance_radius();
/**
* Get the cruising speed
*
* @return the desired cruising speed for this mission
*/
float get_cruising_speed();
@ -214,7 +220,6 @@ public:
/**
* Reset cruising speed to default values
*
* For VTOL: resets both cruising speeds.
*/
void reset_cruising_speed();
@ -231,7 +236,6 @@ public:
/**
* Get the target throttle
*
* @return the desired throttle for this mission
*/
float get_cruising_throttle();
@ -276,127 +280,47 @@ public:
double get_mission_landing_lat() { return _mission.get_landing_lat(); }
double get_mission_landing_lon() { return _mission.get_landing_lon(); }
float get_mission_landing_alt() { return _mission.get_landing_alt(); }
float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
// RTL
bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
bool abort_landing();
void geofence_breach_check(bool &have_geofence_position_data);
// Param access
int get_loiter_min_alt() const { return _param_min_ltr_alt.get(); }
int get_landing_abort_min_alt() const { return _param_mis_lnd_abrt_alt.get(); }
float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); }
float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
int get_landing_abort_min_alt() const { return _param_mis_lnd_abrt_alt.get(); }
float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); }
int get_loiter_min_alt() const { return _param_min_ltr_alt.get(); }
float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); }
float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; }
float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
bool force_vtol();
void acquire_gimbal_control();
void release_gimbal_control();
void stop_capturing_images();
void calculate_breaking_stop(double &lat, double &lon, float &yaw);
void stop_capturing_images();
void calculate_breaking_stop(double &lat, double &lon, float &yaw);
void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS);
private:
int _local_pos_sub{-1};
int _mission_sub{-1};
int _vehicle_status_sub{-1};
struct traffic_buffer_s {
uint32_t icao_address;
hrt_abstime timestamp;
};
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
uORB::Publication<geofence_result_s> _geofence_result_pub{ORB_ID(geofence_result)};
uORB::Publication<mission_result_s> _mission_result_pub{ORB_ID(mission_result)};
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<vehicle_command_ack_s> _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_roi_s> _vehicle_roi_pub{ORB_ID(vehicle_roi)};
uORB::Publication<mode_completed_s> _mode_completed_pub{ORB_ID(mode_completed)};
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
// Subscriptions
home_position_s _home_pos{}; /**< home position for RTL */
mission_result_s _mission_result{};
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
sensor_gps_s _gps_pos{}; /**< gps position */
vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_status_s _vstatus{}; /**< vehicle status */
bool _rtl_activated{false};
// Publications
geofence_result_s _geofence_result{};
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */
position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */
position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */
vehicle_roi_s _vroi{}; /**< vehicle ROI */
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence; /**< class that handles the geofence */
GeofenceBreachAvoidance _gf_breach_avoidance;
hrt_abstime _last_geofence_check = 0;
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */
bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */
bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */
bool _mission_result_updated{false}; /**< flags if mission result has seen an update */
bool _shouldEngageMissionForLanding{false};
Mission _mission; /**< class that handles the missions */
Loiter _loiter; /**< class that handles loiter */
Takeoff _takeoff; /**< class for handling takeoff commands */
VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */
Land _land; /**< class for handling land commands */
PrecLand _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */
AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */
param_t _handle_back_trans_dec_mss{PARAM_INVALID};
param_t _handle_mpc_jerk_auto{PARAM_INVALID};
param_t _handle_mpc_acc_hor{PARAM_INVALID};
float _param_back_trans_dec_mss{0.f};
float _param_mpc_jerk_auto{4.f}; /**< initialized with the default jerk auto value to prevent division by 0 if the parameter is accidentally set to 0 */
float _param_mpc_acc_hor{3.f}; /**< initialized with the default horizontal acc value to prevent division by 0 if the parameter is accidentally set to 0 */
float _mission_cruising_speed_mc{-1.0f};
float _mission_cruising_speed_fw{-1.0f};
float _mission_throttle{NAN};
traffic_buffer_s _traffic_buffer{};
bool _is_capturing_images{false}; // keep track if we need to stop capturing images
// update subscriptions
void params_update();
/**
@ -409,30 +333,129 @@ private:
*/
void publish_mission_result();
void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
void publish_vehicle_command_ack(const vehicle_command_s &cmd, const uint8_t result);
bool geofence_allows_position(const vehicle_global_position_s &pos);
void geofence_breach_notification();
void geofence_breach_check(bool &have_geofence_position_data);
void geofence_breach_avoidance_check();
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Subscriptions
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
// Publications
uORB::Publication<geofence_result_s> _geofence_result_pub{ORB_ID(geofence_result)};
uORB::Publication<mission_result_s> _mission_result_pub{ORB_ID(mission_result)};
uORB::Publication<mode_completed_s> _mode_completed_pub{ORB_ID(mode_completed)};
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<vehicle_command_ack_s> _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_roi_s> _vehicle_roi_pub{ORB_ID(vehicle_roi)};
perf_counter_t _loop_perf{}; /**< loop performance counter */
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
geofence_result_s _geofence_result{};
home_position_s _home_pos{}; /**< home position for RTL */
mission_result_s _mission_result{};
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */
position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */
position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */
sensor_gps_s _gps_pos{}; /**< gps position */
traffic_buffer_s _traffic_buffer{};
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_roi_s _vroi{}; /**< vehicle ROI */
vehicle_status_s _vstatus{}; /**< vehicle status */
AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */
Geofence _geofence; /**< class that handles the geofence */
GeofenceBreachAvoidance _geofence_breach_avoidance;
Land _land; /**< class for handling land commands */
Loiter _loiter; /**< class that handles loiter */
Mission _mission; /**< class that handles the missions */
PrecLand _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */
Takeoff _takeoff; /**< class for handling takeoff commands */
VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */
hrt_abstime _last_geofence_check_timestamp{};
param_t _handle_back_trans_dec_mss{PARAM_INVALID};
param_t _handle_mpc_jerk_auto{PARAM_INVALID};
param_t _handle_mpc_acc_hor{PARAM_INVALID};
param_t _handle_reverse_delay{PARAM_INVALID};
float _param_back_trans_dec_mss{0.f};
float _param_mpc_jerk_auto{4.f}; /**< initialized with the default jerk auto value to prevent division by 0 if the parameter is accidentally set to 0 */
float _param_mpc_acc_hor{3.f}; /**< initialized with the default horizontal acc value to prevent division by 0 if the parameter is accidentally set to 0 */
float _param_reverse_delay{0.f};
float _mission_cruising_speed_mc{-1.0f};
float _mission_cruising_speed_fw{-1.0f};
float _mission_throttle{NAN};
int _local_pos_sub{-1};
int _mission_sub{-1};
int _vehicle_status_sub{-1};
uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/
bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
bool _is_capturing_images{false}; /**< keep track if we need to stop capturing images */
bool _mission_result_updated{false}; /**< flags if mission result has seen an update */
bool _mission_landing_in_progress{false}; /**< this flag gets set if the mission is currently executing on a landing pattern
* if mission mode is inactive, this flag will be cleared after 2 seconds */
bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */
bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */
bool _rtl_activated{false};
char _geofence_violation_warning[50];
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad, /**< loiter radius for fixedwing */
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad, /**< acceptance for takeoff */
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad, /**< acceptance rad for fixedwing alt */
(ParamFloat<px4::params::NAV_FW_ALTL_RAD>)
_param_nav_fw_altl_rad, /**< acceptance rad for fixedwing alt before landing*/
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad, /**< acceptance rad for multicopter alt */
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter alt */
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad, /**< acceptance for takeoff */
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad, /**< acceptance rad for fixedwing alt */
(ParamFloat<px4::params::NAV_FW_ALTL_RAD>) _param_nav_fw_altl_rad, /**< acceptance rad for fw alt before landing*/
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad, /**< loiter radius for fixedwing */
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad, /**< acceptance rad for multicopter alt */
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter alt */
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */
(ParamFloat<px4::params::NAV_TRAFF_A_HOR>) _param_nav_traff_a_hor_ct, /**< avoidance Distance Crosstrack*/
(ParamFloat<px4::params::NAV_TRAFF_A_VER>) _param_nav_traff_a_ver, /**< avoidance Distance Vertical*/
(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/
(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/
(ParamInt<px4::params::NAV_TRAFF_COLL_T>) _param_nav_traff_collision_time,
(ParamFloat<px4::params::NAV_MIN_LTR_ALT>) _param_min_ltr_alt, /**< minimum altitude in Loiter mode*/
(ParamFloat<px4::params::NAV_MIN_LTR_ALT>) _param_min_ltr_alt, /**< minimum altitude in Loiter mode*/
// non-navigator parameters: Mission (MIS_*)
(ParamFloat<px4::params::MIS_PD_TO>) _param_mis_payload_delivery_timeout,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
(ParamInt<px4::params::MIS_TKO_LAND_REQ>) _para_mis_takeoff_land_req,
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
(ParamFloat<px4::params::MIS_PD_TO>) _param_mis_payload_delivery_timeout,
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
(ParamInt<px4::params::MIS_LND_ABRT_ALT>) _param_mis_lnd_abrt_alt
)

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

@ -71,14 +71,14 @@ Navigator::Navigator() :
ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence(this),
_gf_breach_avoidance(this),
_mission(this),
_loiter(this),
_takeoff(this),
_vtol_takeoff(this),
_geofence_breach_avoidance(this),
_land(this),
_loiter(this),
_mission(this),
_precland(this),
_rtl(this)
_rtl(this),
_takeoff(this),
_vtol_takeoff(this)
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@ -913,148 +913,171 @@ void Navigator::run()
}
}
void Navigator::geofence_breach_notification()
{
if (_geofence_result.primary_geofence_breached || _geofence_result.secondary_geofence_breached) {
// Issue a warning about the geofence violation once and only if we are armed.
if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_critical(&_mavlink_log_pub, "%s", _geofence_violation_warning);
events::send(events::ID("navigator_geofence_violation"), {events::Log::Warning, events::LogInternal::Info},
_geofence_violation_warning);
_geofence_violation_warning_sent = true;
}
} else {
// Reset the _geofence_violation_warning_sent field.
_geofence_violation_warning_sent = false;
}
}
void Navigator::geofence_breach_check(bool &have_geofence_position_data)
{
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) {
(_geofence.getPrimaryGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(_geofence.getSecondaryGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&_last_geofence_check_timestamp) > GEOFENCE_CHECK_INTERVAL_US)) {
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
matrix::Vector2<double> fence_violation_test_point;
geofence_violation_type_u gf_violation_type{};
float test_point_bearing;
float test_point_distance;
float vertical_test_point_distance;
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
_gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs);
_gf_breach_avoidance.setClimbRate(-_local_pos.vz);
test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor();
vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor();
} else {
test_point_distance = 2.0f * get_loiter_radius();
vertical_test_point_distance = 5.0f;
if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) {
test_point_bearing = pos_ctrl_status.nav_bearing;
} else {
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
}
}
_gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance);
_gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance);
_gf_breach_avoidance.setTestPointBearing(test_point_bearing);
_gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt);
_gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome());
_gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome());
if (home_global_position_valid()) {
_gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
}
if (_geofence.getPredict()) {
fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
} else {
fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon);
vertical_test_point_distance = 0;
}
gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),
fence_violation_test_point(1),
_global_pos.alt);
gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt +
vertical_test_point_distance);
gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0),
fence_violation_test_point(1),
_global_pos.alt);
_last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.timestamp = hrt_absolute_time();
_geofence_result.primary_geofence_action = _geofence.getGeofenceAction();
_last_geofence_check_timestamp = hrt_absolute_time();
_geofence_result.timestamp = _last_geofence_check_timestamp;
_geofence_result.home_required = _geofence.isHomeRequired();
if (gf_violation_type.value) {
/* inform other apps via the mission result */
_geofence_result.primary_geofence_breached = true;
using geofence_violation_reason_t = events::px4::enums::geofence_violation_reason_t;
if (gf_violation_type.flags.fence_violation) {
_geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::fence_violation;
} else if (gf_violation_type.flags.max_altitude_exceeded) {
_geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::max_altitude_exceeded;
} else if (gf_violation_type.flags.dist_to_home_exceeded) {
_geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::dist_to_home_exceeded;
}
/* Issue a warning about the geofence violation once and only if we are armed */
if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// we have predicted a geofence violation and if the action is to loiter then
// demand a reposition to a location which is inside the geofence
if (_geofence.getGeofenceAction() == geofence_result_s::GF_ACTION_LOITER) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
matrix::Vector2<double> loiter_center_lat_lon;
matrix::Vector2<double> current_pos_lat_lon(_global_pos.lat, _global_pos.lon);
float loiter_altitude_amsl = _global_pos.alt;
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// the computation of the braking distance does not match the actual braking distance. Until we have a better model
// we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence
loiter_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForMultirotor(gf_violation_type,
&_geofence);
loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForMulticopter(gf_violation_type);
} else {
loiter_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForFixedWing(gf_violation_type, &_geofence);
loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForFixedWing(gf_violation_type);
}
rep->current.timestamp = hrt_absolute_time();
rep->current.yaw = get_local_position()->heading;
rep->current.yaw_valid = true;
rep->current.lat = loiter_center_lat_lon(0);
rep->current.lon = loiter_center_lat_lon(1);
rep->current.alt = loiter_altitude_amsl;
rep->current.valid = true;
rep->current.loiter_radius = get_loiter_radius();
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
rep->current.cruising_throttle = get_cruising_throttle();
rep->current.acceptance_radius = get_acceptance_radius();
rep->current.cruising_speed = get_cruising_speed();
}
_geofence_violation_warning_sent = true;
}
if (_geofence.getPredict()) {
// Geofence breach avoidance checks
geofence_breach_avoidance_check();
} else {
/* inform other apps via the mission result */
_geofence_result.primary_geofence_breached = false;
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
// Geofence breach checks
_geofence_result.max_altitude_exceeded = _geofence.isMaxAltitudeBreached(_global_pos.alt);
_geofence_result.max_distance_exceeded = _geofence.isMaxDistanceBreached(_home_pos.lat, _home_pos.lon,
_global_pos.alt);
_geofence_result.primary_geofence_action = _geofence.getPrimaryGeofenceAction();
_geofence_result.secondary_geofence_action = _geofence.getSecondaryGeofenceAction();
_geofence_result.primary_geofence_breached = _geofence.isPrimaryGeofenceBreached(_global_pos.lat, _global_pos.lon,
_global_pos.alt);
_geofence_result.secondary_geofence_breached = _geofence.isSecondaryGeofenceBreached();
if (_geofence_result.max_altitude_exceeded) {
snprintf(_geofence_violation_warning, sizeof(_geofence_violation_warning), "Geofence Max Altitude breached");
}
if (_geofence_result.max_distance_exceeded) {
snprintf(_geofence_violation_warning, sizeof(_geofence_violation_warning), "Geofence Max Distance breached");
}
if (_geofence_result.primary_geofence_breached) {
snprintf(_geofence_violation_warning, sizeof(_geofence_violation_warning), "Primary Geofence breached");
}
if (_geofence_result.secondary_geofence_breached) {
snprintf(_geofence_violation_warning, sizeof(_geofence_violation_warning), "Secondary Geofence breached");
}
}
// Inform other modules via the geofence result.
_geofence_result_pub.publish(_geofence_result);
geofence_breach_notification();
}
}
void Navigator::geofence_breach_avoidance_check()
{
float test_point_bearing{0};
float test_point_distance{0};
float test_point_altitude{0};
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
_geofence_breach_avoidance.setHorizontalVelocity(velocity_hor_abs);
_geofence_breach_avoidance.setClimbRate(-_local_pos.vz);
test_point_altitude = _geofence_breach_avoidance.computeVerticalBrakingDistanceMultirotor() + _global_pos.alt;
test_point_distance = _geofence_breach_avoidance.computeBrakingDistanceMultirotor();
} else {
test_point_altitude = _global_pos.alt + 5.0f;
test_point_distance = 2.0f * get_loiter_radius();
if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 &&
PX4_ISFINITE(pos_ctrl_status.nav_bearing)) {
test_point_bearing = pos_ctrl_status.nav_bearing;
} else {
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
}
}
_geofence_breach_avoidance.setHorizontalTestPointDistance(test_point_distance);
_geofence_breach_avoidance.setVerticalTestPointDistance(test_point_altitude);
_geofence_breach_avoidance.setTestPointBearing(test_point_bearing);
_geofence_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt);
_geofence_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome());
_geofence_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome());
if (home_global_position_valid()) {
_geofence_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
}
matrix::Vector2d fence_violation_test_point = _geofence_breach_avoidance.getFenceViolationTestPoint();
_geofence_result.max_altitude_exceeded = _geofence.isMaxAltitudeBreached(test_point_altitude);
_geofence_result.max_distance_exceeded = _geofence.isMaxDistanceBreached(_home_pos.lat, _home_pos.lon,
_global_pos.alt);
_geofence_result.primary_geofence_action = _geofence.getPrimaryGeofenceAction();
_geofence_result.primary_geofence_breached = _geofence.isPrimaryGeofenceBreached(fence_violation_test_point(0),
fence_violation_test_point(1), test_point_altitude);
if (_geofence_result.primary_geofence_breached) {
snprintf(_geofence_violation_warning, sizeof(_geofence_violation_warning), "Geofence avoidance enabled");
// We have predicted a geofence violation and if the action is to loiter then
// demand a reposition to a location which is inside the geofence
if (_geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_LOITER) {
matrix::Vector2<double> loiter_center_lat_lon;
float loiter_altitude_amsl = _global_pos.alt;
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// the computation of the braking distance does not match the actual braking distance. Until we have a better model
// we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence
loiter_center_lat_lon = _geofence_breach_avoidance.generateLoiterPointForMultirotor(_geofence_result, &_geofence);
loiter_altitude_amsl = _geofence_breach_avoidance.generateLoiterAltitudeForMulticopter(_geofence_result);
} else {
loiter_center_lat_lon = _geofence_breach_avoidance.generateLoiterPointForFixedWing(_geofence_result, &_geofence);
loiter_altitude_amsl = _geofence_breach_avoidance.generateLoiterAltitudeForFixedWing(_geofence_result);
}
position_setpoint_triplet_s *reposition_triplet = get_reposition_triplet();
reposition_triplet->current.timestamp = hrt_absolute_time();
reposition_triplet->current.yaw = get_local_position()->heading;
reposition_triplet->current.yaw_valid = true;
reposition_triplet->current.lat = loiter_center_lat_lon(0);
reposition_triplet->current.lon = loiter_center_lat_lon(1);
reposition_triplet->current.alt = loiter_altitude_amsl;
reposition_triplet->current.valid = true;
reposition_triplet->current.loiter_radius = get_loiter_radius();
reposition_triplet->current.alt_valid = true;
reposition_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
reposition_triplet->current.loiter_direction = 1;
reposition_triplet->current.cruising_throttle = get_cruising_throttle();
reposition_triplet->current.acceptance_radius = get_acceptance_radius();
reposition_triplet->current.cruising_speed = get_cruising_speed();
}
}
}
@ -1490,8 +1513,10 @@ Navigator::stop_capturing_images()
bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
{
if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN)) {
if ((_geofence.getPrimaryGeofenceAction() != geofence_result_s::GF_ACTION_NONE &&
_geofence.getPrimaryGeofenceAction() != geofence_result_s::GF_ACTION_WARN) ||
(_geofence.getSecondaryGeofenceAction() != geofence_result_s::GF_ACTION_NONE &&
_geofence.getSecondaryGeofenceAction() != geofence_result_s::GF_ACTION_WARN)) {
if (PX4_ISFINITE(pos.lat) && PX4_ISFINITE(pos.lon)) {
return _geofence.check(pos, _gps_pos);