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

cammander, navigator: code style fixed

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-23 17:56:38 +01:00
parent 3fe5e88fbe
commit 6ed6c5bb1f
3 changed files with 257 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+20 -6
View File
@@ -372,6 +372,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
if (hil_ret == OK)
ret = true;
@@ -406,6 +407,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
}
}
if (arming_res == TRANSITION_CHANGED)
ret = true;
@@ -454,6 +456,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
}
} else {
mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd");
}
@@ -497,8 +500,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
case VEHICLE_CMD_OVERRIDE_GOTO: {
// TODO listen vehicle_command topic directly from navigator (?)
// TODO listen vehicle_command topic directly from navigator (?)
unsigned int mav_goto = cmd->param1;
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->set_nav_state = NAV_STATE_LOITER;
status->set_nav_state_timestamp = hrt_absolute_time();
@@ -519,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
/* Flight termination */
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
@@ -891,6 +895,7 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
@@ -1066,9 +1071,9 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@@ -1086,7 +1091,7 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1143,14 +1148,18 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = true;
status_changed = true;
}
if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
status.set_nav_state = NAV_STATE_RTL;
status.set_nav_state_timestamp = hrt_absolute_time();
} else if (status.main_state != MAIN_STATE_SEATBELT) {
res = main_state_transition(&status, MAIN_STATE_SEATBELT);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
}
@@ -1178,13 +1187,16 @@ int commander_thread_main(int argc, char *argv[])
if (offboard_sp.armed && !armed.armed) {
if (!safety.safety_switch_available || safety.safety_off) {
transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
if (res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
}
}
} else if (!offboard_sp.armed && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed);
if (res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal");
}
@@ -1202,9 +1214,11 @@ int commander_thread_main(int argc, char *argv[])
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
if (flighttermination_res == TRANSITION_CHANGED) {
tune_positive();
}
} else {
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
}
src/modules/commander/state_machine_helper.cpp
+29 -27
View File
@@ -67,7 +67,7 @@ static bool flighttermination_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
arming_state_t new_arming_state, struct actuator_armed_s *armed)
{
/*
* Perform an atomic state update
@@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
@@ -238,8 +239,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
/* need at minimum altitude estimate */
if (!current_state->is_rotary_wing ||
(current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid)) {
(current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
@@ -249,7 +250,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
/* need at minimum local position estimate */
if (current_state->condition_local_position_valid ||
current_state->condition_global_position_valid) {
current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
@@ -373,35 +374,36 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_flighttermination_state == status->flighttermination_state) {
ret = TRANSITION_NOT_CHANGED;
/* only check transition if the new state is actually different from the current one */
if (new_flighttermination_state == status->flighttermination_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
} else {
switch (new_flighttermination_state) {
case FLIGHTTERMINATION_STATE_ON:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
break;
case FLIGHTTERMINATION_STATE_OFF:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
break;
switch (new_flighttermination_state) {
case FLIGHTTERMINATION_STATE_ON:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
break;
default:
break;
}
case FLIGHTTERMINATION_STATE_OFF:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
break;
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
// TODO
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
default:
break;
}
return ret;
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
// TODO
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
}
return ret;
}
src/modules/navigator/navigator_main.cpp
+208 -142
View File
@@ -167,12 +167,12 @@ private:
struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence;
bool _geofence_violation_warning_sent;
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
bool _inside_fence; /**< vehicle is inside fence */
struct navigation_capabilities_s _nav_caps;
@@ -369,7 +369,7 @@ static const int ERROR = -1;
Navigator *g_navigator;
}
Navigator::Navigator() :
Navigator::Navigator() :
/* state machine transition table */
StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
@@ -508,16 +508,20 @@ void
Navigator::offboard_mission_update(bool isrotaryWing)
{
struct mission_s offboard_mission;
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current;
if (offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
@@ -534,6 +538,7 @@ void
Navigator::onboard_mission_update()
{
struct mission_s onboard_mission;
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -576,11 +581,13 @@ Navigator::task_main()
* else clear geofence data in datamanager
*/
struct stat buffer;
if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) {
if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
warnx("Try to load geofence.txt");
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
if (_geofence.clearDm() > 0 )
if (_geofence.clearDm() > 0)
warnx("Geofence cleared");
else
warnx("Could not clear geofence");
@@ -597,7 +604,7 @@ Navigator::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
/* copy all topics first time */
vehicle_status_update();
parameters_update();
@@ -665,8 +672,9 @@ Navigator::task_main()
/* Evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.main_state == MAIN_STATE_AUTO &&
(_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) {
(_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) {
bool stick_mode = false;
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
@@ -674,21 +682,27 @@ Navigator::task_main()
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
stick_mode = true;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
@@ -714,15 +728,18 @@ Navigator::task_main()
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAV_STATE_RTL:
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
default:
@@ -735,6 +752,7 @@ Navigator::task_main()
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
@@ -762,7 +780,7 @@ Navigator::task_main()
/* offboard mission updated */
if (fds[4].revents & POLLIN) {
offboard_mission_update(_vstatus.is_rotary_wing);
// XXX check if mission really changed
// XXX check if mission really changed
dispatch(EVENT_MISSION_CHANGED);
}
@@ -783,6 +801,7 @@ Navigator::task_main()
/* global position updated */
if (fds[1].revents & POLLIN) {
global_position_update();
/* only check if waypoint has been reached in MISSION and RTL modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (check_mission_item_reached()) {
@@ -791,15 +810,15 @@ Navigator::task_main()
}
/* Check geofence violation */
if(!_geofence.inside(&_global_pos)) {
if (!_geofence.inside(&_global_pos)) {
//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent)
{
if (!_geofence_violation_warning_sent) {
mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
@@ -849,48 +868,55 @@ Navigator::start()
}
void
Navigator::status()
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
if (_global_pos.valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)_global_pos.relative_alt);
(double)_global_pos.alt, (double)_global_pos.relative_alt);
warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f",
(double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
(double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
}
if (_fence_valid) {
warnx("Geofence is valid");
// warnx("Vertex longitude latitude");
// for (unsigned i = 0; i < _fence.count; i++)
// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
} else {
warnx("Geofence not set");
}
switch (myState) {
case NAV_STATE_NONE:
warnx("State: None");
break;
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case NAV_STATE_RTL:
warnx("State: RTL");
break;
default:
warnx("State: Unknown");
break;
case NAV_STATE_NONE:
warnx("State: None");
break;
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case NAV_STATE_RTL:
warnx("State: RTL");
break;
default:
warnx("State: Unknown");
break;
}
}
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* STATE_NONE */
{
/* STATE_NONE */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -899,8 +925,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
},
{
/* STATE_READY */
{
/* STATE_READY */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
@@ -910,7 +936,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
},
{
/* STATE_LOITER */
/* STATE_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
@@ -919,8 +945,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
},
{
/* STATE_MISSION */
{
/* STATE_MISSION */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -929,8 +955,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
},
{
/* STATE_RTL */
{
/* STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -993,6 +1019,7 @@ Navigator::start_loiter()
if (_global_pos.alt < min_alt_amsl) {
_mission_item_triplet.current.altitude = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
_mission_item_triplet.current.altitude = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
@@ -1043,27 +1070,28 @@ Navigator::set_mission_item()
add_home_pos_to_rtl(&_mission_item_triplet.current);
if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
/* don't reset RTL state on RTL or LOITER items */
_rtl_state = RTL_STATE_NONE;
}
if (_vstatus.is_rotary_wing) {
if (_need_takeoff && (
_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED
)) {
_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED
)) {
/* do special TAKEOFF handling for VTOL */
_need_takeoff = false;
/* calculate desired takeoff altitude AMSL */
float takeoff_alt_amsl = _mission_item_triplet.current.altitude;
if (_mission_item_triplet.current.altitude_is_relative)
takeoff_alt_amsl += _home_pos.altitude;
@@ -1089,6 +1117,7 @@ Navigator::set_mission_item()
_mission_item_triplet.current.altitude = takeoff_alt_amsl;
_mission_item_triplet.current.altitude_is_relative = false;
}
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
/* will need takeoff after landing */
_need_takeoff = true;
@@ -1097,13 +1126,16 @@ Navigator::set_mission_item()
if (_do_takeoff) {
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude);
} else {
if (onboard) {
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
} else {
mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
}
}
} else {
/* since a mission is not advanced without WPs available, this is not supposed to happen */
_mission_item_triplet.current_valid = false;
@@ -1116,6 +1148,7 @@ Navigator::set_mission_item()
if (ret == OK) {
add_home_pos_to_rtl(&_mission_item_triplet.next);
_mission_item_triplet.next_valid = true;
} else {
/* this will fail for the last WP */
_mission_item_triplet.next_valid = false;
@@ -1129,17 +1162,21 @@ void
Navigator::start_rtl()
{
_do_takeoff = false;
if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
if (_reset_loiter_pos) {
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.altitude = _global_pos.alt;
}
}
}
_reset_loiter_pos = true;
mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
set_rtl_item();
@@ -1150,107 +1187,112 @@ Navigator::set_rtl_item()
{
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
float climb_alt = _home_pos.altitude + _parameters.rtl_alt;
if (_vstatus.condition_landed)
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
float climb_alt = _home_pos.altitude + _parameters.rtl_alt;
if (_vstatus.condition_landed)
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.altitude = climb_alt;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
break;
}
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.altitude = climb_alt;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
break;
}
case RTL_STATE_RETURN: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
// don't change altitude setpoint
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
break;
}
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
// don't change altitude setpoint
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
break;
}
case RTL_STATE_DESCEND: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
float descend_alt = _home_pos.altitude + _parameters.land_alt;
float descend_alt = _home_pos.altitude + _parameters.land_alt;
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = descend_alt;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
break;
}
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = descend_alt;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
break;
}
case RTL_STATE_LAND: {
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = _home_pos.altitude;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_LAND;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
break;
}
_mission_item_triplet.current.altitude_is_relative = false;
_mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = _home_pos.altitude;
_mission_item_triplet.current.yaw = NAN;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
_mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.nav_cmd = NAV_CMD_LAND;
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
_mission_item_triplet.current.time_inside = 0.0f;
_mission_item_triplet.current.pitch_min = 0.0f;
_mission_item_triplet.current.autocontinue = true;
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
break;
}
default: {
mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
break;
}
mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
break;
}
}
publish_mission_item_triplet();
@@ -1267,6 +1309,7 @@ Navigator::check_mission_item_reached()
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
if (_vstatus.is_rotary_wing) {
return _vstatus.condition_landed;
} else {
/* For fw there is currently no landing detector:
* make sure control is not stopped when overshooting the landing waypoint */
@@ -1281,7 +1324,7 @@ Navigator::check_mission_item_reached()
_mission_item_triplet.current.loiter_radius > 0.01f) {
return false;
}
}
uint64_t now = hrt_absolute_time();
@@ -1301,18 +1344,20 @@ Navigator::check_mission_item_reached()
/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
float wp_alt_amsl = _mission_item_triplet.current.altitude;
if (_mission_item_triplet.current.altitude_is_relative)
_mission_item_triplet.current.altitude += _home_pos.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl,
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
&dist_xy, &dist_z);
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
&dist_xy, &dist_z);
if (_do_takeoff) {
if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
/* require only altitude for takeoff */
_waypoint_position_reached = true;
}
} else {
if (dist >= 0.0f && dist <= acceptance_radius) {
_waypoint_position_reached = true;
@@ -1324,9 +1369,11 @@ Navigator::check_mission_item_reached()
if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
@@ -1336,20 +1383,22 @@ Navigator::check_mission_item_reached()
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
if (_mission_item_triplet.current.time_inside > 0.01f) {
mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside);
}
}
/* check if the MAV was long enough inside the waypoint orbit */
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6)
|| _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|| _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
_time_first_inside_orbit = 0;
_waypoint_yaw_reached = false;
_waypoint_position_reached = false;
return true;
}
}
return false;
}
@@ -1362,6 +1411,7 @@ Navigator::on_mission_item_reached()
/* takeoff completed */
_do_takeoff = false;
mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
} else {
/* advance by one mission item */
_mission.move_to_next();
@@ -1369,23 +1419,28 @@ Navigator::on_mission_item_reached()
if (_mission.current_mission_available()) {
set_mission_item();
} else {
/* if no more mission items available then finish mission */
/* loiter at last waypoint */
_reset_loiter_pos = false;
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
} else {
/* RTL finished */
if (_rtl_state == RTL_STATE_LAND) {
/* landed at home position */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed");
dispatch(EVENT_READY_REQUESTED);
} else {
/* next RTL step */
_rtl_state = (RTLState)(_rtl_state + 1);
@@ -1508,6 +1563,7 @@ Navigator::publish_control_mode()
case MAIN_STATE_AUTO:
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_manual_enabled = false;
if (myState == NAV_STATE_READY) {
/* disable all controllers, armed but idle */
_control_mode.flag_control_rates_enabled = false;
@@ -1516,6 +1572,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
} else {
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
@@ -1524,6 +1581,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
}
break;
case MAIN_STATE_OFFBOARD:
@@ -1539,6 +1597,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
@@ -1547,6 +1606,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
@@ -1555,6 +1615,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = true;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
@@ -1563,6 +1624,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
break;
default:
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
@@ -1572,6 +1634,7 @@ Navigator::publish_control_mode()
_control_mode.flag_control_velocity_enabled = false;
break;
}
break;
default:
@@ -1591,7 +1654,8 @@ Navigator::publish_control_mode()
}
}
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b)
{
if (a.altitude_is_relative == b.altitude_is_relative &&
fabs(a.lat - b.lat) < FLT_EPSILON &&
fabs(a.lon - b.lon) < FLT_EPSILON &&
@@ -1604,6 +1668,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const
fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
a.autocontinue == b.autocontinue) {
return true;
} else {
return false;
}
@@ -1664,8 +1729,9 @@ int navigator_main(int argc, char *argv[])
} 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);
navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
} else {
usage();