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

navigator fix code style

This commit is contained in:
Daniel Agar 2017-01-04 17:27:13 -05:00
parent be14c11589
commit c280358e32
11 changed files with 410 additions and 312 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Tools/files_to_check_code_style.sh
View File

@ -17,13 +17,12 @@ exec find src \
-path src/lib/mathlib -prune -o \
-path src/lib/matrix -prune -o \
-path src/drivers/bootloaders -o \
-path src/modules/uavcanesc -o \
-path src/modules/uavcannode -o \
-path src/modules/commander -prune -o \
-path src/modules/mavlink -prune -o \
-path src/modules/navigator -prune -o \
-path src/modules/sdlog2 -prune -o \
-path src/modules/systemlib/uthash -prune -o \
-path src/modules/uavcan -prune -o \
-path src/modules/uavcan/libuavcan -prune -o \
-path src/modules/uavcanesc -o \
-path src/modules/uavcannode -o \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

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

@ -289,6 +289,7 @@ Mission::find_offboard_land_start()
for (size_t i = 0; i < _offboard_mission.count; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return -1;

30
src/modules/navigator/mission_block.cpp
View File

@ -66,7 +66,7 @@ orb_advert_t actuator_pub_fd;
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_mission_item({0}),
_mission_item( {0}),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
@ -119,6 +119,7 @@ MissionBlock::is_mission_item_reached()
return true;
case NAV_CMD_DO_VTOL_TRANSITION:
/*
* We wait half a second to give the transition command time to propagate.
* Then monitor the transition status for completion.
@ -128,6 +129,7 @@ MissionBlock::is_mission_item_reached()
_action_start = 0;
return true;
} else {
return false;
}
@ -164,6 +166,7 @@ MissionBlock::is_mission_item_reached()
(_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) {
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
/* close to waypoint, but altitude error greater than twice acceptance */
if ((dist >= 0.0f)
&& (dist_z > 2 * _navigator->get_altitude_acceptance_radius())
@ -176,6 +179,7 @@ MissionBlock::is_mission_item_reached()
curr_sp->loiter_direction = 1;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
/* restore SETPOINT_TYPE_POSITION */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
@ -214,12 +218,14 @@ MissionBlock::is_mission_item_reached()
altitude_amsl - altitude_acceptance_radius) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
@ -233,6 +239,7 @@ MissionBlock::is_mission_item_reached()
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
} else {
_time_first_inside_orbit = 0;
}
@ -281,12 +288,14 @@ MissionBlock::is_mission_item_reached()
next_sp.lat, next_sp.lon);
_waypoint_yaw_reached = false;
} else {
_waypoint_yaw_reached = true;
}
}
}
}
} else {
/* for normal mission items used their acceptance radius */
float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
@ -405,6 +414,7 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item,
case NAV_CMD_VIDEO_STOP_CAPTURE:
cmd->target_component = 100; // MAV_COMP_ID_CAMERA
break;
default:
cmd->target_component = _navigator->get_vstatus()->component_id;
break;
@ -517,30 +527,37 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
// fall through
case NAV_CMD_VTOL_TAKEOFF:
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()){
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()) {
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LAND:
case NAV_CMD_VTOL_LAND:
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) {
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LOITER_TO_ALT:
// initially use current altitude, and switch to mission item altitude once in loiter position
sp->alt = math::max(_navigator->get_global_position()->alt, _navigator->get_home_position()->alt + _param_loiter_min_alt.get());
sp->alt = math::max(_navigator->get_global_position()->alt,
_navigator->get_home_position()->alt + _param_loiter_min_alt.get());
// fall through
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
sp->disable_mc_yaw_control = true;
}
break;
default:
@ -601,7 +618,8 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
}
void
MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw)
MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
float yaw)
{
if (_navigator->get_land_detected()->landed) {
/* landed, don't takeoff, but switch to IDLE mode */
@ -619,6 +637,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
if (min_clearance > 8.0f) {
item->altitude += min_clearance;
} else {
item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
}
@ -662,8 +681,10 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
struct vehicle_command_s cmd = {};
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
@ -679,6 +700,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
item->yaw = _navigator->get_global_position()->yaw;
/* use home position */
} else {
item->lat = _navigator->get_home_position()->lat;
item->lon = _navigator->get_home_position()->lon;

2
src/modules/navigator/mission_block.h
View File

@ -129,7 +129,7 @@ protected:
/**
* Set follow_target item
*/
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw);
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
void issue_command(const struct mission_item_s *item);

62
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -81,8 +81,10 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p
failed = true;
warned = true;
mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock.");
} else {
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
failed = failed
|| !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
}
// check if all mission item commands are supported
@ -91,7 +93,9 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
if (isRotarywing) {
failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
failed = failed
|| !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
} else {
failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
}
@ -137,7 +141,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
return true;
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems,
Geofence &geofence, float home_alt, bool home_valid)
{
/* Update fixed wing navigation capabilites */
updateNavigationCapabilities();
@ -149,7 +154,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
return resLanding;
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt)
{
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
@ -199,10 +205,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i + 1);
return false;
} else {
mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i + 1);
return true;
}
}
@ -215,10 +222,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i + 1);
return false;
} else {
mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i + 1);
return true;
}
}
@ -227,7 +235,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
return true;
}
bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed) {
bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems,
bool condition_landed)
{
// do not allow mission if we find unsupported item
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
@ -265,7 +275,8 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i + 1),
(int)missionitem.nav_cmd);
return false;
}
@ -280,6 +291,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
}
return true;
}
@ -291,6 +303,7 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
@ -298,15 +311,19 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
if (missionitem.nav_cmd == NAV_CMD_LAND) {
struct mission_item_s missionitem_previous;
if (i != 0) {
if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
if (dm_read(dm_current, i - 1, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat,
missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
_fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude,
_fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
@ -320,6 +337,7 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
if (missionitem_previous.altitude <= slope_alt_req) {
/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
return true;
} else {
/* Landing waypoint is above altitude of slope at the given waypoint distance */
mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close");
@ -328,17 +346,20 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
(double)(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
return false;
}
} else {
/* Last wp is in flare region */
//xxx give recommendations
mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint");
return false;
}
} else {
mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint");
return false;
@ -351,7 +372,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
}
bool
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued)
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon,
float dist_first_wp, bool &warning_issued)
{
/* check if first waypoint is not too far from home */
@ -363,7 +385,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
if (dm_read(dm_current, i,
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* Check non navigation item */
if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){
if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
/* check actuator number */
if (mission_item.params[0] < 0 || mission_item.params[0] > 5) {
@ -371,6 +393,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
warning_issued = true;
return false;
}
/* check actuator value */
if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) {
mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
@ -378,6 +401,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
return false;
}
}
/* check only items with valid lat/lon */
else if (isPositionCommand(mission_item.nav_cmd)) {
@ -387,11 +411,13 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
if (dist_to_1wp < dist_first_wp) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
warning_issued = true;
}
return true;
} else {
@ -420,7 +446,8 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
}
bool
MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
MissionFeasibilityChecker::isPositionCommand(unsigned cmd)
{
if (cmd == NAV_CMD_WAYPOINT ||
cmd == NAV_CMD_LOITER_UNLIMITED ||
cmd == NAV_CMD_LOITER_TIME_LIMIT ||
@ -431,6 +458,7 @@ MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
cmd == NAV_CMD_VTOL_LAND) {
return true;
} else {
return false;

12
src/modules/navigator/mission_feasibility_checker.h
View File

@ -63,18 +63,22 @@ private:
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
bool &warning_issued, bool throw_error = false);
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);
bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp,
bool &warning_issued);
bool isPositionCommand(unsigned cmd);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
bool home_valid);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
void updateNavigationCapabilities();
/* Checks specific to rotarywing airframes */
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad);
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
bool home_valid, float default_acceptance_rad);
public:
MissionFeasibilityChecker();

26
src/modules/navigator/navigation.h
View File

@ -69,19 +69,19 @@ enum NAV_CMD {
NAV_CMD_VTOL_LAND = 85,
NAV_CMD_DO_JUMP = 177,
NAV_CMD_DO_CHANGE_SPEED = 178,
NAV_CMD_DO_SET_SERVO=183,
NAV_CMD_DO_LAND_START=189,
NAV_CMD_DO_SET_ROI=201,
NAV_CMD_DO_DIGICAM_CONTROL=203,
NAV_CMD_DO_MOUNT_CONFIGURE=204,
NAV_CMD_DO_MOUNT_CONTROL=205,
NAV_CMD_DO_SET_CAM_TRIGG_DIST=206,
NAV_CMD_IMAGE_START_CAPTURE=2000,
NAV_CMD_IMAGE_STOP_CAPTURE=2001,
NAV_CMD_VIDEO_START_CAPTURE=2500,
NAV_CMD_VIDEO_STOP_CAPTURE=2501,
NAV_CMD_DO_VTOL_TRANSITION=3000,
NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */
NAV_CMD_DO_SET_SERVO = 183,
NAV_CMD_DO_LAND_START = 189,
NAV_CMD_DO_SET_ROI = 201,
NAV_CMD_DO_DIGICAM_CONTROL = 203,
NAV_CMD_DO_MOUNT_CONFIGURE = 204,
NAV_CMD_DO_MOUNT_CONTROL = 205,
NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
NAV_CMD_IMAGE_START_CAPTURE = 2000,
NAV_CMD_IMAGE_STOP_CAPTURE = 2001,
NAV_CMD_VIDEO_START_CAPTURE = 2500,
NAV_CMD_VIDEO_STOP_CAPTURE = 2501,
NAV_CMD_DO_VTOL_TRANSITION = 3000,
NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
};
enum ORIGIN {

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

@ -133,24 +133,24 @@ public:
/**
* Getters
*/
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_land_detected_s* get_land_detected() { return &_land_detected; }
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
struct vehicle_status_s *get_vstatus() { return &_vstatus; }
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; }
struct home_position_s *get_home_position() { return &_home_pos; }
bool home_position_valid() { return (_home_pos.timestamp > 0); }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s* get_reposition_triplet() { return &_reposition_triplet; }
struct position_setpoint_triplet_s* get_takeoff_triplet() { return &_takeoff_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
struct mission_result_s *get_mission_result() { return &_mission_result; }
struct geofence_result_s *get_geofence_result() { return &_geofence_result; }
struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence& get_geofence() { return _geofence; }
Geofence &get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
@ -189,7 +189,7 @@ public:
* For VTOL: sets cuising speed for current mode only (multirotor or fixed-wing).
*
*/
void set_cruising_speed(float speed=-1.0f);
void set_cruising_speed(float speed = -1.0f);
/**
* Reset cruising speed to default values
@ -208,7 +208,7 @@ public:
/**
* Set the target throttle
*/
void set_cruising_throttle(float throttle=-1.0f) { _mission_throttle = throttle; }
void set_cruising_throttle(float throttle = -1.0f) { _mission_throttle = throttle; }
/**
* Get the acceptance radius given the mission item preset radius
@ -228,7 +228,7 @@ public:
bool abort_landing();
static float get_time_inside(struct mission_item_s& item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; }
static float get_time_inside(struct mission_item_s &item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; }
private:
@ -337,7 +337,7 @@ private:
/**
* Retrieve home position
*/
void home_position_update(bool force=false);
void home_position_update(bool force = false);
/**
* Retrieve fixed wing navigation capabilities
@ -393,7 +393,7 @@ private:
/* this class has ptr data members, so it should not be copied,
* consequently the copy constructors are private.
*/
Navigator(const Navigator&);
Navigator operator=(const Navigator&);
Navigator(const Navigator &);
Navigator operator=(const Navigator &);
};
#endif

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

@ -344,6 +344,7 @@ Navigator::task_main()
global_pos_available_once = false;
PX4_WARN("global position timeout");
}
/* Let the loop run anyway, don't do `continue` here. */
} else if (pret < 0) {
@ -351,14 +352,17 @@ Navigator::task_main()
PX4_ERR("nav: poll error %d, %d", pret, errno);
usleep(10000);
continue;
} else {
if (fds[0].revents & POLLIN) {
/* success, global pos is available */
global_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
global_pos_available_once = true;
}
}
@ -369,8 +373,10 @@ Navigator::task_main()
/* gps updated */
orb_check(_gps_pos_sub, &updated);
if (updated) {
gps_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
}
@ -378,12 +384,14 @@ Navigator::task_main()
/* sensors combined updated */
orb_check(_sensor_combined_sub, &updated);
if (updated) {
sensor_combined_update();
}
/* parameters updated */
orb_check(_param_update_sub, &updated);
if (updated) {
params_update();
updateParams();
@ -391,35 +399,41 @@ Navigator::task_main()
/* vehicle control mode updated */
orb_check(_control_mode_sub, &updated);
if (updated) {
vehicle_control_mode_update();
}
/* vehicle status updated */
orb_check(_vstatus_sub, &updated);
if (updated) {
vehicle_status_update();
}
/* vehicle land detected updated */
orb_check(_land_detected_sub, &updated);
if (updated) {
vehicle_land_detected_update();
}
/* navigation capabilities updated */
orb_check(_fw_pos_ctrl_status_sub, &updated);
if (updated) {
fw_pos_ctrl_status_update();
}
/* home position updated */
orb_check(_home_pos_sub, &updated);
if (updated) {
home_position_update();
}
orb_check(_vehicle_command_sub, &updated);
if (updated) {
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
@ -441,6 +455,7 @@ Navigator::task_main()
// Go on and check which changes had been requested
if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4;
} else {
rep->current.yaw = NAN;
}
@ -456,6 +471,7 @@ Navigator::task_main()
if (PX4_ISFINITE(cmd.param7)) {
rep->current.alt = cmd.param7;
} else {
rep->current.alt = get_global_position()->alt;
}
@ -463,6 +479,7 @@ Navigator::task_main()
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
@ -480,6 +497,7 @@ Navigator::task_main()
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else {
// If one of them is non-finite, reset both
rep->current.lat = NAN;
@ -498,6 +516,7 @@ Navigator::task_main()
* use MAV_CMD_MISSION_START to start the mission there
*/
unsigned land_start = _mission.find_offboard_land_start();
if (land_start != -1) {
vehicle_command_s vcmd = {};
vcmd.target_system = get_vstatus()->system_id;
@ -542,15 +561,18 @@ Navigator::task_main()
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.geofence_action = _geofence.getGeofenceAction();
if (!inside) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
@ -561,6 +583,7 @@ Navigator::task_main()
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = false;
@ -576,46 +599,57 @@ Navigator::task_main()
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rcLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rtl;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_dataLinkLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_follow_target;
break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
@ -657,6 +691,7 @@ Navigator::task_main()
perf_end(_loop_perf);
}
PX4_INFO("exiting");
_navigator_task = -1;
@ -748,6 +783,7 @@ Navigator::get_altitude_acceptance_radius()
{
if (!this->get_vstatus()->is_rotary_wing) {
return _param_fw_alt_acceptance_radius.get();
} else {
return _param_mc_alt_acceptance_radius.get();
}
@ -778,7 +814,8 @@ Navigator::get_cruising_speed()
}
void
Navigator::set_cruising_speed(float speed) {
Navigator::set_cruising_speed(float speed)
{
if (_vstatus.is_rotary_wing) {
_mission_cruising_speed_mc = speed;
@ -800,6 +837,7 @@ Navigator::get_cruising_throttle()
/* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */
if (_mission_throttle > FLT_EPSILON) {
return _mission_throttle;
} else {
return _param_cruising_throttle_plane.get();
}
@ -898,12 +936,16 @@ int navigator_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
delete navigator::g_navigator;
navigator::g_navigator = nullptr;
} else if (!strcmp(argv[1], "status")) {
navigator::g_navigator->status();
} else if (!strcmp(argv[1], "fence")) {
navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
} else if (!strcmp(argv[1], "fencefile")) {
navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
} else {
usage();
return 1;
@ -970,7 +1012,7 @@ Navigator::publish_att_sp()
}
void
Navigator::set_mission_failure(const char* reason)
Navigator::set_mission_failure(const char *reason)
{
if (!_mission_result.mission_failure) {
_mission_result.mission_failure = true;

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

@ -80,6 +80,7 @@ void
Takeoff::on_active()
{
struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
if (rep->current.valid) {
// reset the position
set_takeoff_position();
@ -115,6 +116,7 @@ Takeoff::set_takeoff_position()
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Using minimum takeoff altitude: %.2f m", (double)_param_min_alt.get());
}
} else {
// Use home + minimum clearance but only notify.
abs_altitude = min_abs_altitude;