From 6b97361c926762e6fb6844433a277b91e3e55925 Mon Sep 17 00:00:00 2001 From: daniele Date: Wed, 25 Mar 2020 16:34:48 +0100 Subject: [PATCH] add dummy if statement to avoid messing up the pr review --- src/modules/navigator/navigator_main.cpp | 104 ++++++++++++----------- 1 file changed, 53 insertions(+), 51 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ea7333174..a049a8f998 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -228,77 +228,79 @@ Navigator::run() } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { - position_setpoint_triplet_s *rep = get_reposition_triplet(); - position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); + if (true) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + position_setpoint_triplet_s *curr = get_position_setpoint_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; + // 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_LOITER; + rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_direction = 1; + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - // If no argument for ground speed, use default value. - if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { - rep->current.cruising_speed = get_cruising_speed(); + // If no argument for ground speed, use default value. + if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { + rep->current.cruising_speed = get_cruising_speed(); - } else { - rep->current.cruising_speed = cmd.param1; - } + } else { + rep->current.cruising_speed = cmd.param1; + } - rep->current.cruising_throttle = get_cruising_throttle(); - rep->current.acceptance_radius = get_acceptance_radius(); + rep->current.cruising_throttle = get_cruising_throttle(); + rep->current.acceptance_radius = get_acceptance_radius(); - // Go on and check which changes had been requested - if (PX4_ISFINITE(cmd.param4)) { - rep->current.yaw = cmd.param4; - rep->current.yaw_valid = true; + // Go on and check which changes had been requested + if (PX4_ISFINITE(cmd.param4)) { + rep->current.yaw = cmd.param4; + rep->current.yaw_valid = true; - } else { - rep->current.yaw = NAN; - rep->current.yaw_valid = false; - } + } else { + rep->current.yaw = NAN; + rep->current.yaw_valid = false; + } - if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { - // Position change with optional altitude change - rep->current.lat = cmd.param5; - rep->current.lon = cmd.param6; + // Position change with optional altitude change + rep->current.lat = cmd.param5; + rep->current.lon = cmd.param6; - if (PX4_ISFINITE(cmd.param7)) { + if (PX4_ISFINITE(cmd.param7)) { + rep->current.alt = cmd.param7; + + } else { + rep->current.alt = get_global_position()->alt; + } + + } else if (PX4_ISFINITE(cmd.param7) && curr->current.valid + && PX4_ISFINITE(curr->current.lat) + && PX4_ISFINITE(curr->current.lon)) { + + // Altitude without position change + rep->current.lat = curr->current.lat; + rep->current.lon = curr->current.lon; rep->current.alt = cmd.param7; } else { + // All three set to NaN - hold in current position + rep->current.lat = get_global_position()->lat; + rep->current.lon = get_global_position()->lon; rep->current.alt = get_global_position()->alt; } - } else if (PX4_ISFINITE(cmd.param7) && curr->current.valid - && PX4_ISFINITE(curr->current.lat) - && PX4_ISFINITE(curr->current.lon)) { + rep->previous.valid = true; + rep->previous.timestamp = hrt_absolute_time(); - // Altitude without position change - rep->current.lat = curr->current.lat; - rep->current.lon = curr->current.lon; - rep->current.alt = cmd.param7; + rep->current.valid = true; + rep->current.timestamp = hrt_absolute_time(); - } else { - // All three set to NaN - hold in current position - rep->current.lat = get_global_position()->lat; - rep->current.lon = get_global_position()->lon; - rep->current.alt = get_global_position()->alt; + rep->next.valid = false; } - rep->previous.valid = true; - rep->previous.timestamp = hrt_absolute_time(); - - rep->current.valid = true; - rep->current.timestamp = hrt_absolute_time(); - - rep->next.valid = false; - // CMD_DO_REPOSITION is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {