int set_vehicle_global_position_setpoint (int index, float yaw_sp, struct vehicle_global_position_setpoint_s* g_sp) { mission_command_t cmd = mission.command_list[index]; float navigation_alt, navigation_lat, navigation_lon; navigation_alt = cmd.option1; navigation_lat = (cmd.name == accepted_command_rtl || cmd.name == accepted_command_land)? home_position.latitude : cmd.option2; navigation_lon = (cmd.name == accepted_command_rtl || cmd.name == accepted_command_land)? home_position.longitude : cmd.option3; /* Update setpoints */ g_sp->altitude = navigation_alt; g_sp->latitude = navigation_lat * 1e7f; g_sp->longitude = navigation_lon * 1e7f; g_sp->altitude_is_relative = 0; g_sp->yaw = _wrap_pi(yaw_sp / 180.0f * M_PI); return set_special_fields(cmd.name, cmd.option4, cmd.option5, g_sp); }
/** * This callback is executed each time a waypoint changes. * * It publishes the vehicle_global_position_setpoint_s or the * vehicle_local_position_setpoint_s topic, depending on the type of waypoint */ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param2, float param3, float param4, float param5_lat_x, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { static orb_advert_t global_position_setpoint_pub = -1; static orb_advert_t global_position_set_triplet_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; static unsigned last_waypoint_index = -1; char buf[50] = {0}; // XXX include check if WP is supported, jump to next if not /* Update controller setpoints */ if (frame == (int)MAV_FRAME_GLOBAL) { /* global, absolute waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize setpoint publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } /* fill triplet: previous, current, next waypoint */ struct vehicle_global_position_set_triplet_s triplet; /* current waypoint is same as sp */ memcpy(&(triplet.current), &sp, sizeof(sp)); /* * Check if previous WP (in mission, not in execution order) * is available and identify correct index */ int last_setpoint_index = -1; bool last_setpoint_valid = false; if (index > 0) { last_setpoint_index = index - 1; } while (last_setpoint_index >= 0) { if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { last_setpoint_valid = true; break; } last_setpoint_index--; } /* * Check if next WP (in mission, not in execution order) * is available and identify correct index */ int next_setpoint_index = -1; bool next_setpoint_valid = false; /* next waypoint */ if (wpm->size > 1) { next_setpoint_index = index + 1; } while (next_setpoint_index < wpm->size - 1) { if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { next_setpoint_valid = true; break; } next_setpoint_index++; } /* populate last and next */ triplet.previous_valid = false; triplet.next_valid = false; if (last_setpoint_valid) { triplet.previous_valid = true; struct vehicle_global_position_setpoint_s sp; sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[last_setpoint_index].z; sp.altitude_is_relative = false; sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(wpm->waypoints[last_setpoint_index].param1, wpm->waypoints[last_setpoint_index].param2, wpm->waypoints[last_setpoint_index].param3, wpm->waypoints[last_setpoint_index].param4, wpm->waypoints[last_setpoint_index].command, &sp); memcpy(&(triplet.previous), &sp, sizeof(sp)); } if (next_setpoint_valid) { triplet.next_valid = true; struct vehicle_global_position_setpoint_s sp; sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[next_setpoint_index].z; sp.altitude_is_relative = false; sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(wpm->waypoints[next_setpoint_index].param1, wpm->waypoints[next_setpoint_index].param2, wpm->waypoints[next_setpoint_index].param3, wpm->waypoints[next_setpoint_index].param4, wpm->waypoints[next_setpoint_index].command, &sp); memcpy(&(triplet.next), &sp, sizeof(sp)); } /* Initialize triplet publication if necessary */ if (global_position_set_triplet_pub < 0) { global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); } else { orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); } sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { /* global, relative alt (in relation to HOME) waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { /* local, absolute waypoint */ struct vehicle_local_position_setpoint_s sp; sp.x = param5_lat_x; sp.y = param6_lon_y; sp.z = param7_alt_z; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else { warnx("non-navigation WP, ignoring"); mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); return; } /* only set this for known waypoint types (non-navigation types would have returned earlier) */ last_waypoint_index = index; mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); }