mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-19 20:09:06 +08:00
Compress mission_item_s
This commit is contained in:
alexcekay
parent
24da87db12
commit
06521d3948
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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<const mavlink_mission_item_int_t *>(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<const mavlink_mission_item_int_t *>(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_int_t *>(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;
|
||||
|
||||
@ -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. */
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -280,8 +280,8 @@ MissionBase::on_active()
|
||||
reinterpret_cast<uint8_t *>(&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,
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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<int16_t>(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info},
|
||||
|
||||
@ -43,9 +43,14 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -596,8 +596,8 @@ void RtlDirect::publish_rtl_direct_navigator_mission_item()
|
||||
|
||||
navigator_mission_item.sequence_current = static_cast<uint16_t>(_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);
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
|
||||
@ -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) &&
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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];
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user