diff --git a/msg/DatamanRequest.msg b/msg/DatamanRequest.msg index b65386cc15..04e2eaba25 100644 --- a/msg/DatamanRequest.msg +++ b/msg/DatamanRequest.msg @@ -4,5 +4,5 @@ uint8 client_id uint8 request_type # id/read/write/clear uint8 item # dm_item_t uint32 index -uint8[56] data +uint8[40] data uint32 data_length diff --git a/msg/DatamanResponse.msg b/msg/DatamanResponse.msg index ebf752db50..a670635c3b 100644 --- a/msg/DatamanResponse.msg +++ b/msg/DatamanResponse.msg @@ -4,7 +4,7 @@ uint8 client_id uint8 request_type # id/read/write/clear uint8 item # dm_item_t uint32 index -uint8[56] data +uint8[40] data uint8 STATUS_SUCCESS = 0 uint8 STATUS_FAILURE_ID_ERR = 1 diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 36d4453344..e56fd01296 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -329,8 +329,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t mission_item.nav_cmd = mission_fence_point.nav_cmd; mission_item.frame = mission_fence_point.frame; - mission_item.lat = mission_fence_point.lat; - mission_item.lon = mission_fence_point.lon; + mission_item.setLatEncoded(mission_fence_point.lat); + mission_item.setLonEncoded(mission_fence_point.lon); mission_item.altitude = mission_fence_point.alt; if (mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || @@ -1182,8 +1182,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) case MAV_MISSION_TYPE_FENCE: { // Write a geofence point mission_fence_point_s mission_fence_point; mission_fence_point.nav_cmd = mission_item.nav_cmd; - mission_fence_point.lat = mission_item.lat; - mission_fence_point.lon = mission_item.lon; + mission_fence_point.lat = mission_item.getLat(); + mission_fence_point.lon = mission_item.getLon(); mission_fence_point.alt = mission_item.altitude; if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || @@ -1408,12 +1408,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * * alignment, so we can just swap float for int32_t. */ const mavlink_mission_item_int_t *item_int = reinterpret_cast(mavlink_mission_item); - mission_item->lat = ((double)item_int->x) * 1e-7; - mission_item->lon = ((double)item_int->y) * 1e-7; + mission_item->___lat_int = item_int->x; + mission_item->___lon_int = item_int->y; + mission_item->int_encoded = true; } else { - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; + mission_item->___lat_float = mavlink_mission_item->x; + mission_item->___lon_float = mavlink_mission_item->y; + mission_item->int_encoded = false; } mission_item->altitude = mavlink_mission_item->z; @@ -1553,10 +1555,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * = reinterpret_cast(mavlink_mission_item); mission_item->params[4] = ((double)item_int->x); mission_item->params[5] = ((double)item_int->y); + mission_item->int_encoded = false; } else { mission_item->params[4] = (double)mavlink_mission_item->x; mission_item->params[5] = (double)mavlink_mission_item->y; + mission_item->int_encoded = false; } mission_item->params[6] = mavlink_mission_item->z; @@ -1727,12 +1731,12 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item_int_t *item_int = reinterpret_cast(mavlink_mission_item); - item_int->x = round(mission_item->lat * 1e7); - item_int->y = round(mission_item->lon * 1e7); + item_int->x = mission_item->___lat_int; + item_int->y = mission_item->___lon_int; } else { - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->x = mission_item->___lat_float; + mavlink_mission_item->y = mission_item->___lon_float; } mavlink_mission_item->z = mission_item->altitude; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index c35d2c1bde..eb0b5c56b7 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -402,8 +402,8 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item // assume this is a fixed-wing landing pattern with orbit to alt followed // by tangent exit to landing approach and touchdown at landing waypoint - const float distance_orbit_center_to_land = get_distance_to_next_waypoint(_mission_item_previous.lat, - _mission_item_previous.lon, mission_item.lat, mission_item.lon); + const float distance_orbit_center_to_land = get_distance_to_next_waypoint(_mission_item_previous.getLat(), + _mission_item_previous.getLon(), mission_item.getLat(), mission_item.getLon()); const float orbit_radius = fabsf(_mission_item_previous.loiter_radius); if (distance_orbit_center_to_land <= orbit_radius) { @@ -420,8 +420,9 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item } else if (_mission_item_previous.nav_cmd == NAV_CMD_WAYPOINT) { // approaching directly from waypoint position - const float waypoint_distance = get_distance_to_next_waypoint(_mission_item_previous.lat, _mission_item_previous.lon, - mission_item.lat, mission_item.lon); + const float waypoint_distance = get_distance_to_next_waypoint(_mission_item_previous.getLat(), + _mission_item_previous.getLon(), + mission_item.getLat(), mission_item.getLon()); landing_approach_distance = waypoint_distance; } else { @@ -627,7 +628,7 @@ bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s & _first_waypoint_found = true; const float dist_to_1wp_from_home_pos = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, + mission_item.getLat(), mission_item.getLon(), _home_lat_lon(0), _home_lat_lon(1)); if (dist_to_1wp_from_home_pos < _param_mis_dist_1wp) { @@ -666,7 +667,7 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi if (PX4_ISFINITE(_last_lat) && PX4_ISFINITE(_last_lon)) { /* check distance from current position to item */ const float dist_between_waypoints = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, + mission_item.getLat(), mission_item.getLon(), _last_lat, _last_lon); @@ -687,8 +688,8 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi } } - _last_lat = mission_item.lat; - _last_lon = mission_item.lon; + _last_lat = mission_item.getLat(); + _last_lon = mission_item.getLon(); _last_cmd = mission_item.nav_cmd; /* We ran through all waypoints and have not found any distances between waypoints that are too far. */ diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 4366244148..c2b2b63cd8 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -167,8 +167,8 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) checker.publishLanded(true); checker.publishCurrentPosition(0, 0); waypoint_from_heading_and_distance(0, 0, 0, 501, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; + mission_item.setLatEncoded(lat_new); + mission_item.setLonEncoded(lon_new); // THEN: pass checker.processNextItem(mission_item, 0, 1); @@ -179,8 +179,8 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) checker.publishLanded(true); checker.publishCurrentPosition(0, 0); waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; + mission_item.setLatEncoded(lat_new); + mission_item.setLonEncoded(lon_new); // THEN: pass checker.processNextItem(mission_item, 0, 1); @@ -289,9 +289,9 @@ TEST_F(FeasibilityCheckerTest, fixed_wing_land_approach) checker.processNextItem(mission_item, 0, 2); double lat_new, lon_new; - waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; + waypoint_from_heading_and_distance(mission_item.getLat(), mission_item.getLon(), 0, 99, &lat_new, &lon_new); + mission_item.setLatEncoded(lat_new); + mission_item.setLonEncoded(lon_new); mission_item.nav_cmd = NAV_CMD_LAND; mission_item.altitude = 40; @@ -310,9 +310,9 @@ TEST_F(FeasibilityCheckerTest, fixed_wing_land_approach) checker.processNextItem(mission_item, 0, 2); - waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; + waypoint_from_heading_and_distance(mission_item.getLat(), mission_item.getLon(), 0, 99, &lat_new, &lon_new); + mission_item.setLatEncoded(lat_new); + mission_item.setLonEncoded(lon_new); mission_item.nav_cmd = NAV_CMD_LAND; mission_item.altitude = 40; @@ -331,9 +331,9 @@ TEST_F(FeasibilityCheckerTest, fixed_wing_land_approach) checker.processNextItem(mission_item, 0, 2); - waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; + waypoint_from_heading_and_distance(mission_item.getLat(), mission_item.getLon(), 0, 1, &lat_new, &lon_new); + mission_item.setLatEncoded(lat_new); + mission_item.setLonEncoded(lon_new); mission_item.nav_cmd = NAV_CMD_LAND; mission_item.altitude = 40; @@ -352,9 +352,9 @@ TEST_F(FeasibilityCheckerTest, fixed_wing_land_approach) checker.processNextItem(mission_item, 0, 2); - waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; + waypoint_from_heading_and_distance(mission_item.getLat(), mission_item.getLon(), 0, 1, &lat_new, &lon_new); + mission_item.setLatEncoded(lat_new); + mission_item.setLonEncoded(lon_new); mission_item.nav_cmd = NAV_CMD_LAND; mission_item.altitude = 40; @@ -409,9 +409,9 @@ TEST_F(FeasibilityCheckerTest, fixed_wing_landing) mission_item.nav_cmd = NAV_CMD_LAND; mission_item.altitude = 0; double lat_new, lon_new; - waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 200, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; + waypoint_from_heading_and_distance(mission_item.getLat(), mission_item.getLon(), 0, 200, &lat_new, &lon_new); + mission_item.setLatEncoded(lat_new); + mission_item.setLonEncoded(lon_new); checker.processNextItem(mission_item, 1, 3); mission_item.nav_cmd = NAV_CMD_DO_LAND_START; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 2c351ffa6e..c31d469714 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -60,7 +60,11 @@ Land::on_activation() if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _navigator->get_local_position()->xy_global) { // only execute if global position is valid - _navigator->preproject_stop_point(_mission_item.lat, _mission_item.lon); + double lat = _mission_item.getLat(); + double lon = _mission_item.getLon(); + _navigator->preproject_stop_point(lat, lon); + _mission_item.setLatEncoded(lat); + _mission_item.setLonEncoded(lon); } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 08a200eda0..d6fc40f9ed 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -140,7 +140,7 @@ Mission::do_need_move_to_takeoff() if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + float d_current = get_distance_to_next_waypoint(_mission_item.getLat(), _mission_item.getLon(), _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); @@ -317,8 +317,8 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; } - _mission_item.lat = _global_pos_sub.get().lat; - _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.setLatEncoded(_global_pos_sub.get().lat); + _mission_item.setLonEncoded(_global_pos_sub.get().lon); _mission_item.yaw = NAN; // FlightTaskAuto handles yaw directly _mission_item.altitude = _mission_init_climb_altitude_amsl; _mission_item.altitude_is_relative = false; @@ -365,15 +365,15 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ _mission_item.yaw = get_bearing_to_next_waypoint( _global_pos_sub.get().lat, _global_pos_sub.get().lon, - _mission_item.lat, _mission_item.lon); + _mission_item.getLat(), _mission_item.getLon()); _mission_item.force_heading = true; new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; /* set position setpoint to current while aligning */ - _mission_item.lat = _global_pos_sub.get().lat; - _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.setLatEncoded(_global_pos_sub.get().lat); + _mission_item.setLonEncoded(_global_pos_sub.get().lon); } /* heading is aligned now, prepare transition */ diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 0d653a1882..c47cca2c5c 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -280,8 +280,8 @@ MissionBase::on_active() reinterpret_cast(&next_position_mission_item), sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT); if (success) { - _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, - next_position_mission_item.lat, next_position_mission_item.lon)); + _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.getLat(), _mission_item.getLon(), + next_position_mission_item.getLat(), next_position_mission_item.getLon())); _mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode } } @@ -384,7 +384,7 @@ MissionBase::isLanding() // distance to the WP is below the loiter radius + acceptance. if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U] && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { - const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + const float d_current = get_distance_to_next_waypoint(_mission_item.getLat(), _mission_item.getLon(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. @@ -581,7 +581,7 @@ MissionBase::set_mission_result() bool MissionBase::do_need_move_to_item() { - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + float d_current = get_distance_to_next_waypoint(_mission_item.getLat(), _mission_item.getLon(), _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); @@ -905,8 +905,8 @@ MissionBase::do_abort_landing() vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; vcmd.param1 = -1; vcmd.param2 = 1; - vcmd.param5 = _mission_item.lat; - vcmd.param6 = _mission_item.lon; + vcmd.param5 = _mission_item.getLat(); + vcmd.param6 = _mission_item.getLon(); vcmd.param7 = alt_sp; _navigator->publish_vehicle_cmd(&vcmd); @@ -918,8 +918,8 @@ void MissionBase::publish_navigator_mission_item() navigator_mission_item.sequence_current = _mission.current_seq; navigator_mission_item.nav_cmd = _mission_item.nav_cmd; - navigator_mission_item.latitude = _mission_item.lat; - navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.latitude = _mission_item.getLat(); + navigator_mission_item.longitude = _mission_item.getLon(); navigator_mission_item.altitude = _mission_item.altitude; navigator_mission_item.time_inside = get_time_inside(_mission_item); @@ -1183,7 +1183,7 @@ int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, floa if (!((mission.nav_cmd == NAV_CMD_LAND) && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && (!vehicle_status.is_vtol))) { - float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon, + float dist = get_distance_to_point_global_wgs84(mission.getLat(), mission.getLon(), MissionBlock::get_absolute_altitude_for_item(mission, home_alt), lat, lon, diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1b05628a6c..95e08f1a6a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -169,7 +169,7 @@ MissionBlock::is_mission_item_reached_or_completed() && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : _navigator->get_loiter_radius(); - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl, + dist = get_distance_to_point_global_wgs84(_mission_item.getLat(), _mission_item.getLon(), mission_item_altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, @@ -247,7 +247,7 @@ MissionBlock::is_mission_item_reached_or_completed() dist_xy = -1.0f; dist_z = -1.0f; - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, + dist = get_distance_to_point_global_wgs84(_mission_item.getLat(), _mission_item.getLon(), curr_sp->alt, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, @@ -280,7 +280,7 @@ MissionBlock::is_mission_item_reached_or_completed() if (curr_sp->valid) { // location of gate (mission item) - MapProjection ref_pos{_mission_item.lat, _mission_item.lon}; + MapProjection ref_pos{_mission_item.getLat(), _mission_item.getLon()}; // current setpoint matrix::Vector2f gate_to_curr_sp = ref_pos.project(curr_sp->lat, curr_sp->lon); @@ -541,8 +541,8 @@ MissionBlock::issue_command(const mission_item_s &item) if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision - vcmd.param5 = item.lat; - vcmd.param6 = item.lon; + vcmd.param5 = item.getLat(); + vcmd.param6 = item.getLon(); if (item.altitude_is_relative) { vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; @@ -618,8 +618,8 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi return false; } - sp->lat = item.lat; - sp->lon = item.lon; + sp->lat = item.getLat(); + sp->lon = item.getLon(); sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; sp->loiter_radius = (fabsf(item.loiter_radius) > FLT_EPSILON) ? fabsf(item.loiter_radius) : @@ -702,8 +702,8 @@ MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *it const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - item->lat = pos_sp_triplet->current.lat; - item->lon = pos_sp_triplet->current.lon; + item->setLatEncoded(pos_sp_triplet->current.lat); + item->setLonEncoded(pos_sp_triplet->current.lon); item->altitude = pos_sp_triplet->current.alt; item->loiter_radius = pos_sp_triplet->current.loiter_direction_counter_clockwise ? -pos_sp_triplet->current.loiter_radius : pos_sp_triplet->current.loiter_radius; @@ -715,8 +715,8 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item) { setLoiterItemCommonFields(item); - item->lat = _navigator->get_global_position()->lat; - item->lon = _navigator->get_global_position()->lon; + item->setLatEncoded(_navigator->get_global_position()->lat); + item->setLonEncoded(_navigator->get_global_position()->lon); // check if minimum loiter altitude is specified, and enforce it if so float loiter_altitude_amsl = _navigator->get_global_position()->alt; @@ -736,7 +736,11 @@ MissionBlock::setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s { setLoiterItemCommonFields(item); - _navigator->preproject_stop_point(item->lat, item->lon); + double lat = item->getLat(); + double lon = item->getLon(); + _navigator->preproject_stop_point(lat, lon); + item->setLatEncoded(lat); + item->setLonEncoded(lon); item->altitude = _navigator->get_global_position()->alt; item->loiter_radius = _navigator->get_loiter_radius(); @@ -762,8 +766,8 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) item->nav_cmd = NAV_CMD_TAKEOFF; /* use current position */ - item->lat = _navigator->get_global_position()->lat; - item->lon = _navigator->get_global_position()->lon; + item->setLatEncoded(_navigator->get_global_position()->lat); + item->setLonEncoded(_navigator->get_global_position()->lon); item->yaw = NAN; item->altitude = abs_altitude; @@ -793,12 +797,13 @@ MissionBlock::set_land_item(struct mission_item_s *item) // set land item to current position if (_navigator->get_local_position()->xy_global) { - item->lat = _navigator->get_global_position()->lat; - item->lon = _navigator->get_global_position()->lon; + item->setLatEncoded(_navigator->get_global_position()->lat); + item->setLonEncoded(_navigator->get_global_position()->lon); } else { - item->lat = (double)NAN; - item->lon = (double)NAN; + item->___lat_float = NAN; + item->___lon_float = NAN; + item->int_encoded = false; } item->yaw = NAN; @@ -816,8 +821,8 @@ void MissionBlock::set_idle_item(struct mission_item_s *item) { item->nav_cmd = NAV_CMD_IDLE; - item->lat = _navigator->get_home_position()->lat; - item->lon = _navigator->get_home_position()->lon; + item->setLatEncoded(_navigator->get_home_position()->lat); + item->setLonEncoded(_navigator->get_home_position()->lon); item->altitude_is_relative = false; item->altitude = _navigator->get_home_position()->alt; item->yaw = NAN; @@ -859,13 +864,13 @@ MissionBlock::copy_position_if_valid(struct mission_item_s *const mission_item, const struct position_setpoint_s *const setpoint) const { if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - mission_item->lat = setpoint->lat; - mission_item->lon = setpoint->lon; + mission_item->setLatEncoded(setpoint->lat); + mission_item->setLonEncoded(setpoint->lon); mission_item->altitude = setpoint->alt; } else { - mission_item->lat = _navigator->get_global_position()->lat; - mission_item->lon = _navigator->get_global_position()->lon; + mission_item->setLatEncoded(_navigator->get_global_position()->lat); + mission_item->setLonEncoded(_navigator->get_global_position()->lon); mission_item->altitude = _navigator->get_global_position()->alt; } @@ -883,15 +888,16 @@ MissionBlock::set_align_mission_item(struct mission_item_s *const mission_item, mission_item->time_inside = 0.0f; mission_item->yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - mission_item_next->lat, mission_item_next->lon); + mission_item_next->getLat(), mission_item_next->getLon()); mission_item->force_heading = true; } void MissionBlock::initialize() { - _mission_item.lat = (double)NAN; - _mission_item.lon = (double)NAN; + _mission_item.___lat_float = NAN; + _mission_item.___lon_float = NAN; + _mission_item.int_encoded = false; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); @@ -904,8 +910,8 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Positio float loiter_radius) const { item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - item.lat = pos_yaw_sp.lat; - item.lon = pos_yaw_sp.lon; + item.setLatEncoded(pos_yaw_sp.lat); + item.setLonEncoded(pos_yaw_sp.lon); item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; item.yaw = pos_yaw_sp.yaw; @@ -929,8 +935,8 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Position item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; } - item.lat = pos_yaw_sp.lat; - item.lon = pos_yaw_sp.lon; + item.setLatEncoded(pos_yaw_sp.lat); + item.setLonEncoded(pos_yaw_sp.lon); item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; @@ -946,8 +952,8 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Position void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_WAYPOINT; - item.lat = pos_yaw_sp.lat; - item.lon = pos_yaw_sp.lon; + item.setLatEncoded(pos_yaw_sp.lat); + item.setLonEncoded(pos_yaw_sp.lon); item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; @@ -962,8 +968,8 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Posi void MissionBlock::setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_LAND; - item.lat = pos_yaw_sp.lat; - item.lon = pos_yaw_sp.lon; + item.setLatEncoded(pos_yaw_sp.lat); + item.setLonEncoded(pos_yaw_sp.lon); item.altitude = pos_yaw_sp.alt; item.yaw = pos_yaw_sp.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 8049df1236..d5f7e88179 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -139,7 +139,7 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().checkPointAgainstAllGeofences( - missionitem.lat, missionitem.lon, missionitem.altitude)) { + missionitem.getLat(), missionitem.getLon(), missionitem.altitude)) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1); events::send(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info}, diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 520430c014..457fbcf9b3 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -43,9 +43,14 @@ #pragma once +#include #include #include +#if !defined(__STDC_IEC_559__) && !defined(__GCC_IEC_559) +# error "Non standard float implementation does not guarantee 32-bit floats" +#endif + #if defined(MEMORY_CONSTRAINED_SYSTEM) # define NUM_MISSIONS_SUPPORTED 50 #elif defined(__PX4_POSIX) @@ -144,9 +149,6 @@ enum NAV_FRAME { * possibility of unaligned memory accesses. */ struct mission_item_s { - double lat; /**< latitude in degrees */ - double lon; /**< longitude in degrees */ - // Union to support both Mission Item categories in MAVLink such as: // 1. With Global coordinate (param5 ~ 7 corresponds to lat, lon and altitude) // 2. Without global coordinate (when frame = MAV_FRAME_MISSION) @@ -159,12 +161,27 @@ struct mission_item_s { union { float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */ + int16_t do_jump_mission_index; /**< index where the do jump will go to */ + uint16_t vertex_count; /**< Polygon vertex count (geofence) */ + }; + union { + float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ + uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */ + uint16_t land_precision; /**< Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing, 2 = required precision landing (with search) */ + }; + union { + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */ + uint16_t do_jump_current_count; /**< count how many times the jump has been done */ }; - float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ - float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */ float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ - float ___lat_float; /**< padding */ - float ___lon_float; /**< padding */ + union { + int32_t ___lat_int; /**< Latitude stored as int32 */ + float ___lat_float; /**< Latitude stored as float */ + }; + union { + int32_t ___lon_int; /**< Longitude stored as int32 */ + float ___lon_float; /**< Longitude stored as float */ + }; float altitude; /**< altitude in meters (AMSL) */ }; @@ -172,16 +189,7 @@ struct mission_item_s { float params[7]; /**< array to store mission command values with no global coordinates (frame = MAV_FRAME_MISSION) */ }; - uint16_t nav_cmd; /**< navigation command */ - - int16_t do_jump_mission_index; /**< index where the do jump will go to */ - uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */ - - union { - uint16_t do_jump_current_count; /**< count how many times the jump has been done */ - uint16_t vertex_count; /**< Polygon vertex count (geofence) */ - uint16_t land_precision; /**< Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing, 2 = required precision landing (with search) */ - }; + uint16_t nav_cmd; /**< navigation command */ struct { uint16_t frame : 4, /**< mission frame */ @@ -191,10 +199,41 @@ struct mission_item_s { altitude_is_relative : 1, /**< true if altitude is relative from start point */ autocontinue : 1, /**< true if next waypoint should follow after this one */ vtol_back_transition : 1, /**< part of the vtol back transition sequence */ - _padding0 : 4; /**< padding remaining unused bits */ + int_encoded : 1, /**< specifies if latitude/longitude are encoded as int32 */ + _padding0 : 3; /**< padding remaining unused bits */ }; - uint8_t _padding1[2]; /**< padding struct size to alignment boundary */ + void setLatEncoded(const double lat) + { + ___lat_int = lat * 1e7; + int_encoded = true; + } + + void setLonEncoded(const double lon) + { + ___lon_int = lon * 1e7; + int_encoded = true; + } + + double getLat() const + { + if (int_encoded) { + return ((double)___lat_int) * 1e-7; + + } else { + return (double)___lat_float; + } + } + + double getLon() const + { + if (int_encoded) { + return ((double)___lon_int) * 1e-7; + + } else { + return (double)___lon_float; + } + } }; /** diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index f27c112aac..f7194d7bb8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -422,7 +422,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo "Mission land item could not be read"); } - float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.getLat(), land_mission_item.getLon())}; if ((dist + MIN_DIST_THRESHOLD) < min_dist) { if (_param_rtl_type.get() != 0) { @@ -456,10 +456,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo // Ignore safepoints which are too close to the homepoint const float dist_to_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon, - mission_safe_point.lat, mission_safe_point.lon); + mission_safe_point.getLat(), mission_safe_point.getLon()); if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { - float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.getLat(), mission_safe_point.getLon())}; PositionYawSetpoint safepoint_position; setSafepointAsDestination(safepoint_position, mission_safe_point); @@ -492,8 +492,8 @@ void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_ite { rtl_position.alt = land_mission_item.altitude_is_relative ? land_mission_item.altitude + _home_pos_sub.get().alt : land_mission_item.altitude; - rtl_position.lat = land_mission_item.lat; - rtl_position.lon = land_mission_item.lon; + rtl_position.lat = land_mission_item.getLat(); + rtl_position.lon = land_mission_item.getLon(); rtl_position.yaw = _home_pos_sub.get().yaw; } @@ -504,15 +504,15 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, // TODO: handle all possible mission_safe_point.frame cases switch (mission_safe_point.frame) { case 0: // MAV_FRAME_GLOBAL - rtl_position.lat = mission_safe_point.lat; - rtl_position.lon = mission_safe_point.lon; + rtl_position.lat = mission_safe_point.getLat(); + rtl_position.lon = mission_safe_point.getLon(); rtl_position.alt = mission_safe_point.altitude; rtl_position.yaw = _home_pos_sub.get().yaw;; break; case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT - rtl_position.lat = mission_safe_point.lat; - rtl_position.lon = mission_safe_point.lon; + rtl_position.lat = mission_safe_point.getLat(); + rtl_position.lon = mission_safe_point.getLon(); rtl_position.alt = mission_safe_point.altitude + _home_pos_sub.get().alt; // alt of safe point is rel to home rtl_position.yaw = _home_pos_sub.get().yaw;; break; @@ -705,20 +705,21 @@ land_approaches_s RTL::readVtolLandApproaches(PositionYawSetpoint rtl_position) break; } - const float dist_to_safepoint = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, rtl_position.lat, + const float dist_to_safepoint = get_distance_to_next_waypoint(mission_item.getLat(), mission_item.getLon(), + rtl_position.lat, rtl_position.lon); if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { foundHomeLandApproaches = true; - vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon); + vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.getLat(), mission_item.getLon()); } sector_counter = 0; } if (foundHomeLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { - vtol_land_approaches.approaches[sector_counter].lat = mission_item.lat; - vtol_land_approaches.approaches[sector_counter].lon = mission_item.lon; + vtol_land_approaches.approaches[sector_counter].lat = mission_item.getLat(); + vtol_land_approaches.approaches[sector_counter].lon = mission_item.getLon(); vtol_land_approaches.approaches[sector_counter].height_m = MissionBlock::get_absolute_altitude_for_item(mission_item, _home_pos_sub.get().alt); vtol_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 88f4da2249..b1ad63e227 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -596,8 +596,8 @@ void RtlDirect::publish_rtl_direct_navigator_mission_item() navigator_mission_item.sequence_current = static_cast(_rtl_state); navigator_mission_item.nav_cmd = _mission_item.nav_cmd; - navigator_mission_item.latitude = _mission_item.lat; - navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.latitude = _mission_item.getLat(); + navigator_mission_item.longitude = _mission_item.getLon(); navigator_mission_item.altitude = _mission_item.altitude; navigator_mission_item.time_inside = get_time_inside(_mission_item); diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 6b7f2f649a..20f28ebd63 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -133,8 +133,8 @@ void RtlDirectMissionLand::setActiveMissionItems() _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; } - _mission_item.lat = _global_pos_sub.get().lat; - _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.setLatEncoded(_global_pos_sub.get().lat); + _mission_item.setLonEncoded(_global_pos_sub.get().lon); _mission_item.altitude = _rtl_alt; _mission_item.altitude_is_relative = false; @@ -309,11 +309,11 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() // Go to loiter matrix::Vector2f direction{}; get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), - next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + next_position_mission_item.getLat(), next_position_mission_item.getLon(), &direction(0), &direction(1)); float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, - next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.getLat(), + next_position_mission_item.getLon()); if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { hor_dist = math::max(0.f, hor_dist - next_position_mission_item.loiter_radius); @@ -330,11 +330,11 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() // Go to point horizontally matrix::Vector2f direction{}; get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), - next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + next_position_mission_item.getLat(), next_position_mission_item.getLon(), &direction(0), &direction(1)); float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, - next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.getLat(), + next_position_mission_item.getLon()); if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { hor_dist = math::max(0.f, hor_dist - next_position_mission_item.loiter_radius); @@ -354,10 +354,10 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() matrix::Vector2f direction{}; get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), - next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + next_position_mission_item.getLat(), next_position_mission_item.getLon(), &direction(0), &direction(1)); const float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.getLat(), next_position_mission_item.getLon()); // For fixed wing, add diagonal line if ((_vehicle_status_sub.get().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING) @@ -385,10 +385,10 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() // Default assume can go to the location directly matrix::Vector2f direction{}; get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), - next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); + next_position_mission_item.getLat(), next_position_mission_item.getLon(), &direction(0), &direction(1)); const float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.getLat(), next_position_mission_item.getLon()); _rtl_time_estimator.addDistance(hor_dist, direction, get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); @@ -397,8 +397,8 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() } start_item_index = next_mission_item_index + 1; - hor_position_at_calculation_point(0) = next_position_mission_item.lat; - hor_position_at_calculation_point(1) = next_position_mission_item.lon; + hor_position_at_calculation_point(0) = next_position_mission_item.getLat(); + hor_position_at_calculation_point(1) = next_position_mission_item.getLon(); altitude_at_calculation_point = get_absolute_altitude_for_item(next_position_mission_item); diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index b50d49e50a..4af844bea4 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -216,8 +216,8 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) altitude = pos_sp_triplet->current.alt; } - _mission_item.lat = _home_pos_sub.get().lat; - _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.setLatEncoded(_home_pos_sub.get().lat); + _mission_item.setLonEncoded(_home_pos_sub.get().lon); _mission_item.altitude = altitude; _mission_item.altitude_is_relative = false; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -247,8 +247,8 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND || _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.lat = _home_pos_sub.get().lat; - _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.setLatEncoded(_home_pos_sub.get().lat); + _mission_item.setLonEncoded(_home_pos_sub.get().lon); _mission_item.yaw = NAN; if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 503a0279f9..616c052204 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -71,8 +71,8 @@ VtolTakeoff::on_active() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, - _mission_item.lon, _loiter_location(0), _loiter_location(1))); + _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.getLat(), + _mission_item.getLon(), _loiter_location(0), _loiter_location(1))); _mission_item.force_heading = true; mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.cruising_speed = -1.f; @@ -86,8 +86,8 @@ VtolTakeoff::on_active() case vtol_takeoff_state::ALIGN_HEADING: { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - _mission_item.lat = _loiter_location(0); - _mission_item.lon = _loiter_location(1); + _mission_item.setLatEncoded(_loiter_location(0)); + _mission_item.setLonEncoded(_loiter_location(1)); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous = pos_sp_triplet->current; @@ -127,8 +127,8 @@ VtolTakeoff::on_active() pos_sp_triplet->current.cruising_speed = -1.f; pos_sp_triplet->current.cruising_throttle = -1.f; - _mission_item.lat = pos_sp_triplet->current.lat; - _mission_item.lon = pos_sp_triplet->current.lon; + _mission_item.setLatEncoded(pos_sp_triplet->current.lat); + _mission_item.setLonEncoded(pos_sp_triplet->current.lon); _navigator->set_position_setpoint_triplet_updated(); @@ -172,8 +172,8 @@ VtolTakeoff::set_takeoff_position() _takeoff_alt_msl = _navigator->get_global_position()->alt; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.setLatEncoded(_navigator->get_global_position()->lat); + _mission_item.setLonEncoded(_navigator->get_global_position()->lon); _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); diff --git a/src/systemcmds/tests/test_dataman.cpp b/src/systemcmds/tests/test_dataman.cpp index bea7a7ca41..fdede366eb 100644 --- a/src/systemcmds/tests/test_dataman.cpp +++ b/src/systemcmds/tests/test_dataman.cpp @@ -105,7 +105,7 @@ private: static void *testAsyncThread(void *arg); - static constexpr uint32_t DM_MAX_DATA_SIZE{MISSION_ITEM_SIZE}; + static constexpr uint32_t DM_MAX_DATA_SIZE{MISSION_SIZE}; static_assert(sizeof(dataman_response_s::data) == DM_MAX_DATA_SIZE, "data size != DM_MAX_DATA_SIZE"); uint8_t _buffer_read[DM_MAX_DATA_SIZE];