/*
handle a MISSION_SET_CURRENT mavlink packet
*/
void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_set_current_t packet;
mavlink_msg_mission_set_current_decode(msg, &packet);
// set current command
if (mission.set_current_cmd(packet.seq)) {
mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
}
}
/*
handle a MISSION_SET_CURRENT mavlink packet
*/
void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_set_current_t packet;
mavlink_msg_mission_set_current_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// set current command
if (mission.set_current_cmd(packet.seq)) {
mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
}
}
static inline void MissionSetCurrent(mavlink_message_t* handle_msg)
{
mavlink_mission_set_current_t packet;
//send_text((uint8_t*)"waypoint set current\r\n");
DPRINT("mission set current\r\n");
// decode
mavlink_msg_mission_set_current_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
DPRINT("mission set current: %u\r\n", packet.seq);
// set current waypoint
set(PARAM_WP_INDEX, packet.seq);
set_waypoint(packet.seq);
{
//Location temp; // XXX this is gross
//temp = get_wp_with_index(packet.seq);
//set_next_WP(&temp);
}
mavlink_msg_mission_current_send(MAVLINK_COMM_0, get(PARAM_WP_INDEX));
}
void Rover::send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
}
void MAVMissionOutput_40hz(void)
{
#if (FLIGHT_PLAN_TYPE == FP_WAYPOINTS) // LOGO_WAYPOINTS cannot be uploaded / downloaded
vect3_32t wp;
if (mavlink_flags.mavlink_send_waypoint_reached == 1)
{
mavlink_flags.mavlink_send_waypoint_reached = 0;
mavlink_msg_mission_item_reached_send(MAVLINK_COMM_0, mav_waypoint_reached);
}
if (mavlink_flags.mavlink_send_waypoint_changed == 1)
{
mavlink_flags.mavlink_send_waypoint_changed = 0;
mavlink_msg_mission_current_send(MAVLINK_COMM_0, mav_waypoint_changed);
}
//static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, uint16_t seq)
//static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq)
// CHECK WHETHER WAYPOINT PROTOCOL HAS TIMED OUT WAITING ON A RESPONSE
if (mavlink_waypoint_timeout <= 0)
{
if (mavlink_flags.mavlink_sending_waypoints || mavlink_flags.mavlink_receiving_waypoints)
{
//send_text((uint8_t *)"Timeout on waypoint protocol.\r\n");
DPRINT("Timeout on waypoint protocol.\r\n");
}
mavlink_flags.mavlink_sending_waypoints = false;
mavlink_flags.mavlink_receiving_waypoints = false;
}
// if (mavlink_flags.mavlink_receiving_waypoints == 1)
if (mavlink_flags.mavlink_request_specific_waypoint == 1)
{
DPRINT("requesting waypoint: %u\r\n", waypoint_request_i);
//mavlink_flags.waypoint_request_i = 0;
//static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq)
mavlink_msg_mission_request_send(MAVLINK_COMM_0, mavlink_waypoint_dest_sysid, mavlink_waypoint_dest_compid, waypoint_request_i);
mavlink_flags.mavlink_request_specific_waypoint = 0;
}
// SEND NUMBER OF WAYPOINTS IN WAYPOINTS LIST
if (mavlink_flags.mavlink_send_waypoint_count == 1)
{
int16_t number_of_waypoints = waypoint_count();
//send_text((uint8_t *)"Sending waypoint count\r\n");
DPRINT("Sending waypoint count: %u\r\n", number_of_waypoints);
mavlink_msg_mission_count_send(MAVLINK_COMM_0, mavlink_waypoint_dest_sysid, mavlink_waypoint_dest_compid, number_of_waypoints);
mavlink_flags.mavlink_send_waypoint_count = 0;
}
// SEND DETAILS OF A SPECIFIC WAYPOINT
if (mavlink_flags.mavlink_send_specific_waypoint == 1)
{
//send_text((uint8_t *)"Time to send a specific waypoint\r\n");
DPRINT("Time to send a specific waypoint: %u\r\n", mavlink_waypoint_requested_sequence_number);
// mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component,
// uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue,
// float param1, float param2, float param3, float param4,
// float x, float y, float z)
//BUILDING
//struct waypoint3D { int32_t x; int32_t y; int16_t z; };
// struct waypoint3D getWaypoint3D(uint16_t wp);
// struct waypoint3D wp;
// wp = getWaypoint3D(mavlink_waypoint_requested_sequence_number);
wp = getWaypoint3D(mavlink_waypoint_requested_sequence_number);
//float lat_float, lon_float, alt_float = 0.0;
//uint32_t accum_long = IMUlocationy._.W1 + (lat_origin.WW / 90); // meters North from Equator
//lat_float = (float)((accum_long * 90) / 10000000.0); // degrees North from Equator
//lon_float = (float)((float) lon_origin.WW + ((float)(IMUlocationx._.W1) * 90.0) / (float)(cos_lat / 16384.0)) / 10000000.0;
//extern struct relWaypointDef wp_to_relative(struct waypointDef wp);
//struct relWaypointDef current_waypoint = wp_to_relative(waypoints[waypointIndex]);
//alt_float = ((float)(IMUlocationz._.W1)) + (float)(alt_origin.WW / 100.0);
mavlink_msg_mission_item_send(MAVLINK_COMM_0, mavlink_waypoint_dest_sysid, mavlink_waypoint_dest_compid, \
mavlink_waypoint_requested_sequence_number, mavlink_waypoint_frame, MAV_CMD_NAV_WAYPOINT, mavlink_waypoint_current, true, \
0.0, 0.0, 0.0, 0.0, \
(float)wp.y / 10000000.0, (float)wp.x / 10000000.0, wp.z);
DPRINT("waypoint %f %f %f\r\n", (double)wp.y / 10000000.0, (double)wp.x / 10000000.0, (double)wp.z);
mavlink_flags.mavlink_send_specific_waypoint = 0;
}
if (mavlink_waypoint_timeout > 0) mavlink_waypoint_timeout--;
#endif // (FLIGHT_PLAN_TYPE == FP_WAYPOINTS)
/*
// Acknowledge a command if flaged to do so.
if (mavlink_send_command_ack == true)
{
mavlink_msg_command_ack_send(MAVLINK_COMM_0, mavlink_command_ack_command, mavlink_command_ack_result);
mavlink_send_command_ack = false;
}
*/
}
