diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 | ||||
-rw-r--r-- | src/modules/mavlink/waypoints.c | 4 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 308 | ||||
-rw-r--r-- | src/modules/navigator/navigator_params.c | 6 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 4 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 4 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission.h | 2 |
7 files changed, 166 insertions, 164 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ee0aca69e..4f5e6d1a5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1230,7 +1230,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.radius); + float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 2ff11e813..1adfeafde 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -143,7 +143,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->pitch_min = mavlink_mission_item->param2; break; default: - mission_item->radius = mavlink_mission_item->param2; + mission_item->acceptance_radius = mavlink_mission_item->param2; break; } @@ -173,7 +173,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->param2 = mission_item->pitch_min; break; default: - mavlink_mission_item->param2 = mission_item->radius; + mavlink_mission_item->param2 = mission_item->acceptance_radius; break; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 757eb4690..a6f33b5e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -170,7 +170,8 @@ private: bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _force_takeoff; + bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ + bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ MissionFeasibilityChecker missionFeasiblityChecker; @@ -178,16 +179,20 @@ private: struct { float min_altitude; + float acceptance_radius; float loiter_radius; int onboard_mission_enabled; + float takeoff_alt; float land_alt; float rtl_alt; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; + param_t acceptance_radius; param_t loiter_radius; param_t onboard_mission_enabled; + param_t takeoff_alt; param_t land_alt; param_t rtl_alt; } _parameter_handles; /**< handles for parameters */ @@ -291,7 +296,12 @@ private: /** * Check if current mission item has been reached. */ - bool mission_item_reached(); + bool check_mission_item_reached(); + + /** + * Perform actions when current mission item reached. + */ + void on_mission_item_reached(); /** * Move to next waypoint @@ -379,13 +389,16 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), - _force_takeoff(false) + _need_takeoff(true), + _do_takeoff(false) { memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); + _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); + _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); @@ -436,8 +449,10 @@ Navigator::parameters_update() orb_copy(ORB_ID(parameter_update), _params_sub, &update); param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); + param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); @@ -726,27 +741,8 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (mission_item_reached()) { - if (myState == NAV_STATE_MISSION) { - if (_force_takeoff) { - /* forced takeoff completed */ - _force_takeoff = false; - mavlink_log_info(_mavlink_fd, "[navigator] forced takeoff completed"); - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - /* if no more mission items available send this to state machine and start loiter at the last WP */ - if (_mission.current_mission_available()) { - advance_mission(); - } else { - dispatch(EVENT_MISSION_FINISHED); - } - } else { - /* RTL finished */ - dispatch(EVENT_MISSION_FINISHED); - } + if (check_mission_item_reached()) { + on_mission_item_reached(); } } } @@ -976,7 +972,7 @@ Navigator::start_none() _reset_loiter_pos = true; _rtl_done = false; - _force_takeoff = false; + _do_takeoff = false; publish_mission_item_triplet(); } @@ -984,7 +980,7 @@ Navigator::start_none() void Navigator::start_loiter() { - _force_takeoff = false; + _do_takeoff = false; /* set loiter position if needed */ if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { @@ -997,7 +993,7 @@ Navigator::start_loiter() _mission_item_triplet.current.altitude_is_relative = false; float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; - /* Use current altitude if above min altitude set by parameter */ + /* use current altitude if above min altitude set by parameter */ 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)); @@ -1019,72 +1015,10 @@ Navigator::start_loiter() void Navigator::start_mission() { - int ret; - bool onboard; - unsigned index; - - // TODO set prev item to current position? - _reset_loiter_pos = true; _rtl_done = false; - _force_takeoff = false; - - _mission_item_triplet.previous_valid = false; - - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); - - if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); - - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF && _vstatus.condition_landed) { - /* if landed, takeoff first, if this not defined in mission */ - _force_takeoff = true; - - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; + _need_takeoff = true; - /* fill takeoff item */ - get_takeoff_item(&_mission_item_triplet.current); - if (_vstatus.is_rotary_wing) { - /* for VTOL use current position */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - } else { - /* for fixed wing use first waypoint position */ - // TODO what if first mission item has no lat/lon? - _mission_item_triplet.current.lat = _mission_item_triplet.next.lat; - _mission_item_triplet.current.lon = _mission_item_triplet.next.lon; - } - _mission_item_triplet.current.altitude = _global_pos.alt + _parameters.land_alt; - _mission_item_triplet.current.altitude_is_relative = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission started, force takeoff to %.1fm", _mission_item_triplet.current.altitude); - } else { - /* normal mission start: mission starts with takeoff item or vehicle is already in air */ - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - - 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; - } - - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); - } else { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); - } - } - } else { - /* mission is started without knowledge if there are more mission, indicates bug in navigator */ - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - mavlink_log_critical(_mavlink_fd, "[navigator] error: MISSION mode without mission defined"); - } - - publish_mission_item_triplet(); + advance_mission(); } void @@ -1095,6 +1029,7 @@ Navigator::advance_mission() _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; _reset_loiter_pos = true; + _do_takeoff = false; int ret; bool onboard; @@ -1103,14 +1038,54 @@ Navigator::advance_mission() ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + add_home_pos_to_rtl(&_mission_item_triplet.current); + + 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 + )) { + /* do special TAKEOFF handling for VTOL */ + _need_takeoff = false; + + /* check if we really need takeoff */ + float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) + wp_alt_amsl += _home_pos.altitude; + if (_vstatus.condition_landed || _global_pos.alt - _home_pos.altitude < _parameters.takeoff_alt || + (_mission_item_triplet.next.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && _global_pos.alt > wp_alt_amsl)) { + /* force TAKEOFF if (landed or near ground) and waypoint altitude is more than current */ + _do_takeoff = true; + + /* move current mission item to next */ + memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.next_valid = true; + + /* set current item to TAKEOFF */ + get_takeoff_item(&_mission_item_triplet.current); + + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + } + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* will need takeoff after landing */ + _need_takeoff = true; + } + } - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + if (_do_takeoff) { + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + 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 */ @@ -1118,16 +1093,16 @@ Navigator::advance_mission() warnx("ERROR: current WP can't be set"); } - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - - if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.next); + if (!_do_takeoff) { + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; - } else { - /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + 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; + } } publish_mission_item_triplet(); @@ -1149,6 +1124,7 @@ Navigator::start_rtl() { _reset_loiter_pos = true; _rtl_done = false; + _do_takeoff = false; /* discard all mission item and insert RTL item */ _mission_item_triplet.previous_valid = false; @@ -1157,12 +1133,13 @@ Navigator::start_rtl() _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.rtl_alt; + _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; @@ -1184,7 +1161,7 @@ Navigator::finish_rtl() } bool -Navigator::mission_item_reached() +Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ if (!_mission_item_triplet.current_valid) { @@ -1206,22 +1183,15 @@ Navigator::mission_item_reached() } uint64_t now = hrt_absolute_time(); - float orbit; + float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) { - - orbit = _mission_item_triplet.current.radius; + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item_triplet.current.acceptance_radius; } else { - - // XXX set default orbit via param - orbit = 15.0f; + acceptance_radius = _parameters.acceptance_radius; } - /* keep vertical orbit */ - float vertical_switch_distance = orbit; - - // TODO add frame // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; @@ -1232,10 +1202,12 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt; + 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, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt, + 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); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); @@ -1254,25 +1226,29 @@ Navigator::mission_item_reached() // // XXX TODO // } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - /* require only altitude for takeoff */ - if (current_alt >= _mission_item_triplet.current.altitude) + 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_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } } - /* check if required yaw reached */ - 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 */ + if (_vstatus.is_rotary_wing && !_do_takeoff) { + /* check yow 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; } /* check if the current waypoint was reached */ - if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { /* XXX announcement? */ @@ -1294,19 +1270,44 @@ Navigator::mission_item_reached() } void +Navigator::on_mission_item_reached() +{ + if (myState == NAV_STATE_MISSION) { + if (_do_takeoff) { + /* takeoff completed */ + _do_takeoff = false; + mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { + /* advance by one mission item */ + _mission.move_to_next(); + } + + if (_mission.current_mission_available()) { + advance_mission(); + } else { + /* if no more mission items available then finish mission */ + dispatch(EVENT_MISSION_FINISHED); + } + } else { + /* RTL finished */ + dispatch(EVENT_MISSION_FINISHED); + } +} + +void Navigator::get_loiter_item(struct mission_item_s *item) { //item->altitude_is_relative //item->lat //item->lon //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + //item->yaw + item->loiter_radius = _parameters.loiter_radius; item->loiter_direction = 1; item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->radius = 50.0f; // TODO: get rid of magic number - //item->time_inside - //item->pitch_min + item->acceptance_radius = _parameters.acceptance_radius; + item->time_inside = 0.0f; + item->pitch_min = 0.0f; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; @@ -1320,17 +1321,31 @@ Navigator::get_takeoff_item(mission_item_s *item) //item->lon //item->altitude item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_radius = _parameters.loiter_radius; item->loiter_direction = 1; item->nav_cmd = NAV_CMD_TAKEOFF; - item->radius = 50.0f; // TODO: get rid of magic number + item->acceptance_radius = _parameters.acceptance_radius; item->time_inside = 0.0f; - item->pitch_min = 0.3; // TODO: get rid of magic number + item->pitch_min = 0.0; item->autocontinue = true; item->origin = ORIGIN_ONBOARD; } void +Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) +{ + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* append the home position to RTL item */ + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; + new_mission_item->altitude_is_relative = false; + new_mission_item->loiter_radius = _parameters.loiter_radius; + new_mission_item->acceptance_radius = _parameters.acceptance_radius; + } +} + +void Navigator::publish_mission_item_triplet() { /* lazily publish the mission triplet only once available */ @@ -1424,7 +1439,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && a.loiter_direction == b.loiter_direction && a.nav_cmd == b.nav_cmd && - fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && a.autocontinue == b.autocontinue) { return true; @@ -1485,16 +1500,3 @@ int navigator_main(int argc, char *argv[]) return 0; } - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } -} diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 63601ce6d..8df47fc3b 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,7 +52,9 @@ */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); +PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 10.0f); // default TAKEOFF altitude, slow descend from this altitude when landing in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 10.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 78322aff3..6833ec43f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1161,9 +1161,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.radius = buf.triplet.current.radius; + log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index cb1393f1f..3afaaa2ad 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -217,7 +217,7 @@ struct log_GPSP_s { uint8_t nav_cmd; float loiter_radius; int8_t loiter_direction; - float radius; + float acceptance_radius; float time_inside; float pitch_min; }; @@ -287,7 +287,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"), + LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 95bceb8bb..194e2ed0c 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -87,7 +87,7 @@ struct mission_item_s float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< navigation command */ - float radius; /**< radius in which the mission is accepted as reached in meters */ + float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ |