aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink/waypoints.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink/waypoints.c')
-rw-r--r--src/modules/mavlink/waypoints.c301
1 files changed, 94 insertions, 207 deletions
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 16a7c2d35..7e4a2688f 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -246,55 +246,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
-//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
-//{
-// if (seq < wpm->size)
-// {
-// mavlink_mission_item_t *cur = waypoints->at(seq);
-//
-// const PxVector3 A(cur->x, cur->y, cur->z);
-// const PxVector3 C(x, y, z);
-//
-// // seq not the second last waypoint
-// if ((uint16_t)(seq+1) < wpm->size)
-// {
-// mavlink_mission_item_t *next = waypoints->at(seq+1);
-// const PxVector3 B(next->x, next->y, next->z);
-// const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
-// if (r >= 0 && r <= 1)
-// {
-// const PxVector3 P(A + r*(B-A));
-// return (P-C).length();
-// }
-// else if (r < 0.f)
-// {
-// return (C-A).length();
-// }
-// else
-// {
-// return (C-B).length();
-// }
-// }
-// else
-// {
-// return (C-A).length();
-// }
-// }
-// else
-// {
-// // if (verbose) // printf("ERROR: index out of bounds\n");
-// }
-// return -1.f;
-//}
-
/*
* Calculate distance in global frame.
*
* The distance calculation is based on the WGS84 geoid (GPS)
*/
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
+float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
{
- static uint16_t counter;
if (seq < wpm->size) {
mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
@@ -315,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
float dxy = radius_earth * c;
float dz = alt - wp->z;
+ *dist_xy = fabsf(dxy);
+ *dist_z = fabsf(dz);
+
return sqrtf(dxy * dxy + dz * dz);
} else {
return -1.0f;
}
- counter++;
-
}
/*
* Calculate distance in local frame (NED)
*/
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z)
+float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
{
if (seq < wpm->size) {
mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
@@ -337,17 +296,27 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
float dy = (cur->y - y);
float dz = (cur->z - z);
- return sqrt(dx * dx + dy * dy + dz * dz);
+ *dist_xy = sqrtf(dx * dx + dy * dy);
+ *dist_z = fabsf(dz);
+
+ return sqrtf(dx * dx + dy * dy + dz * dz);
} else {
return -1.0f;
}
}
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos)
+void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
{
static uint16_t counter;
+ if ((!global_pos->valid && !local_pos->xy_valid) ||
+ /* no waypoint */
+ wpm->size == 0) {
+ /* nothing to check here, return */
+ return;
+ }
+
if (wpm->current_active_wp_id < wpm->size) {
float orbit;
@@ -366,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
orbit = 15.0f;
}
+ /* keep vertical orbit */
+ float vertical_switch_distance = orbit;
+
+ /* Take the larger turn distance - orbit or turn_distance */
+ if (orbit < turn_distance)
+ orbit = turn_distance;
+
int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
float dist = -1.0f;
+ float dist_xy = -1.0f;
+ float dist_z = -1.0f;
+
if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
- dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
+ dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
- dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
+ dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
- dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
+ dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
/* Check if conditions of mission item are satisfied */
// XXX TODO
}
- if (dist >= 0.f && dist <= orbit) {
+ if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
wpm->pos_reached = true;
}
+
// check if required yaw reached
float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
@@ -417,17 +397,70 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
}
if (time_elapsed) {
+
+ /* safeguard against invalid missions with last wp autocontinue on */
+ if (wpm->current_active_wp_id == wpm->size - 1) {
+ /* stop handling missions here */
+ cur_wp->autocontinue = false;
+ }
+
if (cur_wp->autocontinue) {
+
cur_wp->current = 0;
+ float navigation_lat = -1.0f;
+ float navigation_lon = -1.0f;
+ float navigation_alt = -1.0f;
+ int navigation_frame = -1;
+
+ /* initialize to current position in case we don't find a suitable navigation waypoint */
+ if (global_pos->valid) {
+ navigation_lat = global_pos->lat/1e7;
+ navigation_lon = global_pos->lon/1e7;
+ navigation_alt = global_pos->alt;
+ navigation_frame = MAV_FRAME_GLOBAL;
+ } else if (local_pos->xy_valid && local_pos->z_valid) {
+ navigation_lat = local_pos->x;
+ navigation_lon = local_pos->y;
+ navigation_alt = local_pos->z;
+ navigation_frame = MAV_FRAME_LOCAL_NED;
+ }
+
+ /* guard against missions without final land waypoint */
/* only accept supported navigation waypoints, skip unknown ones */
do {
- if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
- /* the last waypoint was reached, if auto continue is
- * activated restart the waypoint list from the beginning
- */
- wpm->current_active_wp_id = 0;
+ /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
+ if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+ wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+ wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+ wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
+ wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
+
+ /* this is a navigation waypoint */
+ navigation_frame = cur_wp->frame;
+ navigation_lat = cur_wp->x;
+ navigation_lon = cur_wp->y;
+ navigation_alt = cur_wp->z;
+ }
+
+ if (wpm->current_active_wp_id == wpm->size - 1) {
+
+ /* if we're not landing at the last nav waypoint, we're falling back to loiter */
+ if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
+ /* the last waypoint was reached, if auto continue is
+ * activated AND it is NOT a land waypoint, keep the system loitering there.
+ */
+ cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+ cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+ cur_wp->frame = navigation_frame;
+ cur_wp->x = navigation_lat;
+ cur_wp->y = navigation_lon;
+ cur_wp->z = navigation_alt;
+ }
+
+ /* we risk an endless loop for missions without navigation waypoints, abort. */
+ break;
} else {
if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
@@ -459,7 +492,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
}
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position)
+int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
{
/* check for timed-out operations */
if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
@@ -482,12 +515,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
}
}
- // Do NOT continously send the current WP, since we're not loosing messages
- // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) {
- // mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
- // }
-
- check_waypoints_reached(now, global_position, local_position);
+ check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
return OK;
}
@@ -498,115 +526,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
uint64_t now = mavlink_missionlib_get_system_timestamp();
switch (msg->msgid) {
-// case MAVLINK_MSG_ID_ATTITUDE:
-// {
-// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-// {
-// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
-// {
-// mavlink_attitude_t att;
-// mavlink_msg_attitude_decode(msg, &att);
-// float yaw_tolerance = wpm->accept_range_yaw;
-// //compare current yaw
-// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI)
-// {
-// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
-// wpm->yaw_reached = true;
-// }
-// else if(att.yaw - yaw_tolerance < 0.0f)
-// {
-// float lowerBound = 360.0f + att.yaw - yaw_tolerance;
-// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
-// wpm->yaw_reached = true;
-// }
-// else
-// {
-// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI;
-// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
-// wpm->yaw_reached = true;
-// }
-// }
-// }
-// break;
-// }
-//
-// case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
-// {
-// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size)
-// {
-// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]);
-//
-// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
-// {
-// mavlink_local_position_ned_t pos;
-// mavlink_msg_local_position_ned_decode(msg, &pos);
-// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
-//
-// wpm->pos_reached = false;
-//
-// // compare current position (given in message) with current waypoint
-// float orbit = wp->param1;
-//
-// float dist;
-//// if (wp->param2 == 0)
-//// {
-//// // FIXME segment distance
-//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
-//// }
-//// else
-//// {
-// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z);
-//// }
-//
-// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached)
-// {
-// wpm->pos_reached = true;
-// }
-// }
-// }
-// break;
-// }
-
-// case MAVLINK_MSG_ID_CMD: // special action from ground station
-// {
-// mavlink_cmd_t action;
-// mavlink_msg_cmd_decode(msg, &action);
-// if(action.target == mavlink_system.sysid)
-// {
-// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
-// switch (action.action)
-// {
-// // case MAV_ACTION_LAUNCH:
-// // // if (verbose) std::cerr << "Launch received" << std::endl;
-// // current_active_wp_id = 0;
-// // if (wpm->size>0)
-// // {
-// // setActive(waypoints[current_active_wp_id]);
-// // }
-// // else
-// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
-// // break;
-//
-// // case MAV_ACTION_CONTINUE:
-// // // if (verbose) std::c
-// // err << "Continue received" << std::endl;
-// // idle = false;
-// // setActive(waypoints[current_active_wp_id]);
-// // break;
-//
-// // case MAV_ACTION_HALT:
-// // // if (verbose) std::cerr << "Halt received" << std::endl;
-// // idle = true;
-// // break;
-//
-// // default:
-// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
-// // break;
-// }
-// }
-// break;
-// }
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_mission_ack_t wpa;
@@ -617,26 +536,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
if (wpm->current_wp_id == wpm->size - 1) {
-#ifdef MAVLINK_WPM_NO_PRINTF
- mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#else
- if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
+ mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
-#endif
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
wpm->current_wp_id = 0;
}
}
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
}
break;
@@ -664,13 +573,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
}
}
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("NEW WP SET");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id);
-#endif
wpm->yaw_reached = false;
wpm->pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
@@ -678,33 +582,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
wpm->timestamp_firstinside_orbit = 0;
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
-
-#endif
}
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-#else
- if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
-
-#endif
}
} else {
-#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-#else
-
- if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
-
-#endif
}
break;
@@ -1151,5 +1038,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
}
}
- check_waypoints_reached(now, global_pos, local_pos);
+ // check_waypoints_reached(now, global_pos, local_pos);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 09:20:31 +0000