From 8960ab3402a87b182d17ab1b3cb165115e0f4bad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 May 2016 13:58:42 +0200 Subject: [PATCH] Navigator: differentiate between takeoff and reposition commands, perform calculations for repositions only when armed. --- src/modules/navigator/loiter.cpp | 58 ++++++++++++++++------ src/modules/navigator/loiter.h | 6 +++ src/modules/navigator/navigator.h | 2 + src/modules/navigator/navigator_main.cpp | 26 ++++++++-- src/modules/navigator/takeoff.cpp | 62 ++++++++++++++++-------- src/modules/navigator/takeoff.h | 2 + 6 files changed, 119 insertions(+), 37 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f1af2fbbdd..6ef7fd304d 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -58,7 +58,8 @@ Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _param_min_alt(this, "MIS_LTRMIN_ALT", false) + _param_min_alt(this, "MIS_LTRMIN_ALT", false), + _loiter_pos_set(false) { // load initial params updateParams(); @@ -71,6 +72,7 @@ Loiter::~Loiter() void Loiter::on_inactive() { + _loiter_pos_set = false; } void @@ -79,19 +81,7 @@ Loiter::on_activation() if (_navigator->get_reposition_triplet()->current.valid) { reposition(); } else { - // set current mission item to loiter - set_loiter_item(&_mission_item, _param_min_alt.get()); - - // convert mission item to current setpoint - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.velocity_valid = false; - pos_sp_triplet->previous.valid = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); - - _navigator->set_position_setpoint_triplet_updated(); + set_loiter_position(); } } @@ -101,11 +91,46 @@ Loiter::on_active() if (_navigator->get_reposition_triplet()->current.valid) { reposition(); } + + // reset the loiter position if we get disarmed + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + _loiter_pos_set = false; + } +} + +void +Loiter::set_loiter_position() +{ + // not setting loiter position until armed + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED || + _loiter_pos_set) { + return; + } else { + _loiter_pos_set = true; + } + + // set current mission item to loiter + set_loiter_item(&_mission_item, _param_min_alt.get()); + + // convert mission item to current setpoint + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.velocity_valid = false; + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); } void Loiter::reposition() { + // we can't reposition if we are not armed yet + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + return; + } struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet(); @@ -115,7 +140,10 @@ Loiter::reposition() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->current.velocity_valid = false; - memcpy(&pos_sp_triplet->previous, &rep->previous, sizeof(rep->previous)); + pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw; + pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt; memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current)); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index a401380b3f..802e1f6151 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -67,7 +67,13 @@ private: */ void reposition(); + /** + * Set the position to hold based on the current local position + */ + void set_loiter_position(); + control::BlockParamFloat _param_min_alt; + bool _loiter_pos_set; }; #endif diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8e1dd84adc..61cb005893 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -143,6 +143,7 @@ public: 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; } @@ -231,6 +232,7 @@ private: navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet; /**< triplet for non-mission direct position command */ + position_setpoint_triplet_s _takeoff_triplet; /**< triplet for non-mission direct takeoff command */ mission_result_s _mission_result; geofence_result_s _geofence_result; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 292476c0e0..aa3cc37fa2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -129,6 +129,7 @@ Navigator::Navigator() : _nav_caps{}, _pos_sp_triplet{}, _reposition_triplet{}, + _takeoff_triplet{}, _mission_result{}, _att_sp{}, _mission_item_valid(false), @@ -415,7 +416,7 @@ Navigator::task_main() struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next - rep->previous.yaw = NAN; + rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; @@ -448,9 +449,28 @@ 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(); + + // store current position as previous position and goal as next + rep->previous.yaw = get_global_position()->yaw; + rep->previous.lat = get_global_position()->lat; + rep->previous.lon = get_global_position()->lon; + rep->previous.alt = get_global_position()->alt; + + rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_direction = 1; + rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + rep->current.yaw = cmd.param4; + rep->current.lat = cmd.param5 / (double)1e7; + rep->current.lon = cmd.param6 / (double)1e7; + rep->current.alt = cmd.param7; + + rep->previous.valid = true; + rep->current.valid = true; + rep->next.valid = false; - if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { warnx("navigator: got pause/continue command"); } } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index de13ae6480..e075c248f1 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -57,7 +57,7 @@ Takeoff::Takeoff(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _param_min_alt(this, "MIS_TAKEOFF_ALT", false) { - /* load initial params */ + // load initial params updateParams(); } @@ -72,6 +72,32 @@ Takeoff::on_inactive() void Takeoff::on_activation() +{ + set_takeoff_position(); +} + +void +Takeoff::on_active() +{ + struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); + if (rep->current.valid) { + // reset the position + set_takeoff_position(); + + } else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + // set loiter item so position controllers stop doing takeoff logic + set_loiter_item(&_mission_item); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void +Takeoff::set_takeoff_position() { // set current mission item to takeoff set_takeoff_item(&_mission_item, _param_min_alt.get()); @@ -89,9 +115,22 @@ Takeoff::on_activation() pos_sp_triplet->next.valid = false; // check if a specific target altitude has been set - struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet(); - if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) { - pos_sp_triplet->current.alt = rep->current.alt; + struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); + if (rep->current.valid) { + if (PX4_ISFINITE(rep->current.alt)) { + pos_sp_triplet->current.alt = rep->current.alt; + } + + // Go on and check which changes had been requested + if (PX4_ISFINITE(rep->current.yaw)) { + pos_sp_triplet->current.yaw = rep->current.yaw; + } + + if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) { + pos_sp_triplet->current.lat = rep->current.lat; + pos_sp_triplet->current.lon = rep->current.lon; + } + // mark this as done memset(rep, 0, sizeof(*rep)); } @@ -100,18 +139,3 @@ Takeoff::on_activation() _navigator->set_position_setpoint_triplet_updated(); } - -void -Takeoff::on_active() -{ - if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { - _navigator->get_mission_result()->finished = true; - _navigator->set_mission_result_updated(); - - /* set loiter item so position controllers stop doing takeoff logic */ - set_loiter_item(&_mission_item); - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - _navigator->set_position_setpoint_triplet_updated(); - } -} diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h index 2ac50b8748..810f6efc19 100644 --- a/src/modules/navigator/takeoff.h +++ b/src/modules/navigator/takeoff.h @@ -62,6 +62,8 @@ public: private: control::BlockParamFloat _param_min_alt; + + void set_takeoff_position(); }; #endif