// function to call after a MISSION_REQUEST_LIST command
COMM_MAV_RESULT navigation_tx_mission_items_start(COMM_MAV_MSG_TARGET *target, mavlink_message_t *msg, mavlink_message_t *answer_msg)
{
mavlink_msg_mission_count_pack(mavlink_system.sysid, target->component, answer_msg,
msg->sysid, target->component, mission_items.count);
return REPLY_TO_SENDER;
}
void GCS_Mavlink<T>::handle_mission_count(mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message
uint16_t len = mavlink_msg_mission_count_pack(_systemId, 0, msg, _systemId, 0, 0);
len = mavlink_msg_to_send_buffer(buf, msg);
_serial.write(buf, len);
}
void MavLinkSendMissionCount(void)
{
uint8_t missionCount;
mavlink_message_t msg;
GetMissionCount(&missionCount);
mavlink_msg_mission_count_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
groundStationSystemId, groundStationComponentId, missionCount);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
void mavlink_send_waypoints (void) {
mavlink_message_t msg;
mavlink_msg_mission_clear_all_pack(127, 0, &msg, ModelData.sysid, ModelData.compid);
mavlink_send_message(&msg);
usleep(100000);
uint16_t n = 0;
for (n = 1; n < MAX_WAYPOINTS; n++) {
if (WayPoints[n].p_lat == 0.0) {
break;
}
}
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
SDL_Log("mavlink: WORKAROUND: MEGAPIRATE_NG: fake one WP\n");
n++;
}
SDL_Log("mavlink: sending Waypoints (%i)\n", n - 1);
mavlink_msg_mission_count_pack(127, 0, &msg, ModelData.sysid, ModelData.compid, n - 1);
mavlink_send_message(&msg);
}
void Create_packets(ExtU_ANN_EKF_NMPC_2_T *data,int sock)
{ int i=0;
int bytes_sent;
uint8_t buf[BUFFER_LENGTH];
uint8_t buf1[BUFFER_LENGTH];
uint8_t buf2[BUFFER_LENGTH];
mavlink_message_t msg,msg1,msg2;
uint16_t len;
mavlink_msg_heartbeat_pack(2, 200, &msg, 2, 12, 65, 0, 3);
len = mavlink_msg_to_send_buffer(buf, &msg);
int len1 = strlen(buf);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
mavlink_msg_attitude_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAnglesmeas.phi, data->EulerAnglesmeas.theta, data->EulerAnglesmeas.psi, data->BodyRatesmeas.P,data->BodyRatesmeas.Q, data->BodyRatesmeas.R);
len = mavlink_msg_to_send_buffer(buf, &msg);
len1 = strlen(buf);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
// mavlink_msg_hil_state_pack(1, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,data->GPSPosition.Latitude,data->GPSPosition.Longitude,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
// mavlink_msg_hil_state_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,39,-95,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
//len = mavlink_msg_to_send_buffer(buf, &msg);
// bytes_sent = write(tty_fd1,buf,len);
mavlink_msg_gps_raw_int_pack(2, 200, &msg, microsSinceEpoch(),2,data->GPSPositionmeas.Latitude,data->GPSPositionmeas.Longitude,data->GPSPositionmeas.Altitude,UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,255);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
//mavlink_message_t msg;
mavlink_status_t status,status1;
mavlink_mission_count_t mission_count;
int cn;
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
//temp = buf[i];
// printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
printf("\n\n Incomming packet\n\n");
if(msg.msgid == 44)
{
mavlink_msg_mission_count_decode( &msg, &mission_count);
printf("\n\n the # of wypts received is %d########\n\n",mission_count.count);
data->wcn=mission_count.count;
for(cn=1;cn<=data->wcn;cn++)
{
//sendrequest(sock,cn);
memset(buf1, 0, BUFFER_LENGTH);
mavlink_msg_mission_request_pack(2, 200, &msg1,255,0,(cn-1));
len = mavlink_msg_to_send_buffer(buf1, &msg1);
bytes_sent = sendto(sock, buf1, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
//printf("\n\n @@@@@ sent successfull\n\n");
memset(buf2, 0, BUFFER_LENGTH);
//receive_waypoints(sock,cn);
while( (recsize1 = recvfrom(sock, (void *)buf2, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen)) <= 0);
if (recsize1 > 0)
{
int ii;
mavlink_mission_item_t mission_item;
for (ii = 0; ii < recsize1; ++ii)
{
if (mavlink_parse_char(MAVLINK_COMM_0, buf2[ii], &msg2, &status1))
{
printf("\n\n reading waypoint # %d",(cn-1));
}
}//if (msg2.msgid==0)
//goto loop;
if(msg2.msgid == 39)
{
mavlink_msg_mission_item_decode(&msg2, &mission_item);
printf("\n\nthe waypoint # %d is lat:%f lon:%f alt:%f",cn,mission_item.x,mission_item.y,mission_item.z);
data->lat[cn]=mission_item.x;
data->alt[cn] =mission_item.z;
data->lon[cn] = mission_item.y;
data->WaypointsIN.v[cn] = mission_item.param1;
latlon(&ANN_EKF_NMPC_2_U,cn);
}
}else printf("!!!!!!!!\n\n receiving failed \n\n ");
}
if((cn-1) == mission_count.count)
{
//sendack(sock,cn);
mavlink_msg_mission_ack_pack(2, 200, &msg2,255,0,0);
memset(buf2, 0, BUFFER_LENGTH);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
printf("\n\n ***** sent acknowledgement *****\n\n");
}
} // for msg id 44
if(msg.msgid == 43)
{
int j;
//sendcn(sock,cn);
mavlink_msg_mission_count_pack(2, 200, &msg2,255,0,data->wcn);
memset(buf2, 0, BUFFER_LENGTH);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf2, 0, BUFFER_LENGTH);
for(j=1; j<=data->wcn;j++)
{
//receive_request(sock);
//sendwyp(sock,-95,95);
mavlink_msg_mission_item_pack(2, 200, &msg2,255,0,(j-1),0,16,0,1,data->WaypointsIN.v[j],0,0,0,data->lat[j],data->lon[j],data->alt[j]);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
printf("\n\n @@@@@ sent count successfull\n\n");
}
//rec_ack(sock);
}// for msg id 43
} //parse - if
}//for
}//else printf("\n\n 12121212not good");//recsize - if
// printf("\n");
//memset(buf, 0, BUFFER_LENGTH);
//printf("\n\n !!!@@##$$ end of a loop\n\n");
}//for Createpacket
uint16_t MissionInterfaceResponse(uint8_t system_id, uint8_t component_id){
mavlink_message_t msg;
uint16_t bytes2Send = 0;
uint8_t CopyMsgToBuff = 0;
char vr_message[50];
// if (mlPending.wpProtState == WP_PROT_TX_WP) { //SLUGS Nav DBG info
// memset(vr_message, 0, sizeof (vr_message));
// sprintf(vr_message, "%d: y =%2.2f x =%2.2f z =%2.2f o =%d t =%d", mlPending.wpCurrentWpInTransaction, (double)mlWpValues.lat[mlPending.wpCurrentWpInTransaction],
// (double)mlWpValues.lon[mlPending.wpCurrentWpInTransaction],
// (double)mlWpValues.alt[mlPending.wpCurrentWpInTransaction],
// mlWpValues.orbit[mlPending.wpCurrentWpInTransaction],
// mlWpValues.type[mlPending.wpCurrentWpInTransaction]);
// bytes2Send += sendQGCDebugMessage(vr_message, 0, UartOutBuff, bytes2Send + 1);
// }
//if (mlPending.wpProtState == WP_PROT_GETTING_WP_IDLE) { //SLUGS Nav DBG info
// memset(vr_message, 0, sizeof (vr_message));
// sprintf(vr_message, "com = %d, tb =%2.2f ta = %2.2f", sw_intTemp, (double)fl_temp1, (double)fl_temp2);
// bytes2Send += sendQGCDebugMessage(vr_message, 0, UartOutBuff, bytes2Send + 1);
//}
// Current mission item (1 off indexing issue in qgc vs et)
if (mlPending.wpSendCurrent) { //--DBG TODO AM implement with nav implamntation
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_mission_current_pack(system_id,
component_id,
&msg, ((uint16_t)mlNavigation.toWP) - 1);
bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
mlPending.wpSendCurrent = FALSE;
}
// clear the msg
memset(&msg, 0, sizeof (mavlink_message_t));
//if (mlPending.wpTransaction){ //DBG
CopyMsgToBuff = 1; //Set send msg flag - if nothing to send the switch statement will reset this
switch (mlPending.wpProtState) {
case WP_PROT_LIST_REQUESTED:
//mlPending.statustext++;
//mlStatustext.severity = MAV_SEVERITY_INFO;
//strncpy(mlStatustext.text, "Got mission list request. Sending count...", 49);
mavlink_msg_mission_count_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlWpValues.wpCount);
// Change the state machine state
mlPending.wpProtState = WP_PROT_NUM_SENT;
// Reset the timeout
mlPending.wpTimeOut = 0;
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
break;
case WP_PROT_GETTING_WP_IDLE:
if (mlPending.wpCurrentWpInTransaction < mlPending.wpTotalWps) {
mavlink_msg_mission_request_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlPending.wpCurrentWpInTransaction++);
// Change the state machine state
mlPending.wpProtState = WP_PROT_RX_WP;
} else {
mavlink_msg_mission_ack_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
MAV_MISSION_ACCEPTED); // 0 is success
// Update the waypoint count
mlWpValues.wpCount = mlPending.wpTotalWps;
// End the transaction
mlPending.wpTransaction = 0;
mlPending.wpProtState = WP_PROT_IDLE;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTotalWps = 0;
mlPending.wpSendCurrent = TRUE; // send current waypoint index
// put zeros in the rest of the waypoints;
//clearWaypointsFrom(mlWpValues.wpCount); //this clears the EEPROM WP storage
}
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
// Reset the timeout
mlPending.wpTimeOut = 0;
break;
case WP_PROT_TX_WP:
//mlPending.statustext++;
//mlStatustext.severity = MAV_SEVERITY_INFO;
//strncpy(mlStatustext.text, "Sending waypoint.", 25);
// Send WP
mavlink_msg_mission_item_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlPending.wpCurrentWpInTransaction,
MAV_FRAME_GLOBAL,
mlWpValues.type[mlPending.wpCurrentWpInTransaction],
0, // not current
1, // autocontinue
0.0, // Param 1 not used
0.0, // Param 2 not used
(float) mlWpValues.orbit[mlPending.wpCurrentWpInTransaction],
0.0, // Param 4 not used
mlWpValues.lat[mlPending.wpCurrentWpInTransaction],
mlWpValues.lon[mlPending.wpCurrentWpInTransaction],
mlWpValues.alt[mlPending.wpCurrentWpInTransaction]); // always autocontinue
// Switch the state waiting for the next request
// Change the state machine state
mlPending.wpProtState = WP_PROT_SENDING_WP_IDLE;
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
// Reset the timeout
mlPending.wpTimeOut = 0;
break;
default:
CopyMsgToBuff = 0;
break;
} // switch wpProtState
// if (CopyMsgToBuff) // Copy the message to the send buffer
// bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
//}//if DBG
mlPending.wpTimeOut++;
// if Timed out reset the state machine and send an error
if (mlPending.wpTimeOut > PROTOCOL_TIMEOUT_TICKS) {
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_mission_ack_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
1); // 1 is failure
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
// reset the state machine
mlPending.wpTransaction = 0;
mlPending.wpProtState = WP_PROT_IDLE;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
mlPending.wpTotalWps = 0;
}
return(bytes2Send);
}
void MAVLinkProxy::dataReceived(const boost::system::error_code &error, size_t received_bytes)
{
static mavlink_message_t msg;
static mavlink_status_t status;
// Response message and buffer
static mavlink_message_t msg_response;
static uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Waypoint reception variables
static int waypoints_to_receive = 0;
static uint16_t waypoints_received = 0;
static int waypoint_timeout = 200;
static unsigned long last_waypoint_time = 0;
for(int i = 0; i < (int) received_bytes; i++)
{
if(mavlink_parse_char(MAVLINK_COMM_0, _received_msg_buf[i], &msg, &status))
{
// Handle received messages
msg_response.magic = 0;
// Waypoint reception
if(waypoints_to_receive > 0 && (last_waypoint_time + waypoint_timeout <= (std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1))))
{
// Timeout
printf("Timeout receiving waypoints!\n");
waypoints_to_receive = 0;
last_waypoint_time = 0;
}
if(waypoints_to_receive > 0 && msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM)
{
// Got waypoint
waypoints_received++;
waypoints_to_receive--;
mavlink_mission_item_t waypoint;
mavlink_msg_mission_item_decode(&msg, &waypoint);
handleWaypoint(&waypoint);
last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);
if(waypoints_to_receive > 0)
{
mavlink_mission_request_t request;
request.seq = waypoints_received;
request.target_system = msg.sysid;
request.target_component = msg.compid;
mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request);
}
else
{
mavlink_mission_ack_t ack;
ack.target_system = msg.sysid;
ack.target_component = msg.compid;
ack.type = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_ack_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &ack);
processPartialMission();
}
}
else if(msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST)
{
printf("Waypoint count request\n");
uint16_t mission_length = 0;
mavlink_msg_mission_count_pack(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, msg.sysid, msg.compid, mission_length);
}
else if(msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST)
{
printf("Parameter request\n");
mavlink_param_value_t param;
param.param_count = 0;
param.param_id[0] = '\0';
param.param_index = 0;
param.param_type = MAV_PARAM_TYPE_UINT8;
param.param_value = 0;
mavlink_msg_param_value_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, ¶m);
}
else if(msg.msgid == MAVLINK_MSG_ID_MISSION_COUNT)
{
mavlink_mission_count_t mission_count;
mavlink_msg_mission_count_decode(&msg, &mission_count);
printf("%d waypoints available\n", mission_count.count);
_mission_ready = false;
_partial_mission.clear();
waypoints_to_receive = mission_count.count;
waypoints_received = 0;
last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);
mavlink_mission_request_t request;
request.seq = 0;
request.target_system = msg.sysid;
request.target_component = msg.compid;
mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request);
}
else if(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT)
{
// Heartbeat from MAVLink ground station
}
else
{
// Unhandled message type
printf("Unknown MAVLink packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
}
// Send a response if the message has been populated
if(msg_response.magic != 0)
{
mavlink_msg_to_send_buffer(buf, &msg_response);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
}
}
}
// Listen for next packet
_socket.async_receive_from(boost::asio::buffer(_received_msg_buf), _endpoint, boost::bind(&MAVLinkProxy::dataReceived, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
}
void mission_read_waypoint_list(void)
{
uint32_t start_time, cur_time;
waypoint_t *cur_wp = waypoint_info.waypoint_list; //First node of the waypoint list
mavlink_mission_request_t mmrt;
mavlink_msg_mission_count_pack(
1, 0, &msg, 255, 0, waypoint_info.waypoint_count /* Waypoint count */
);
send_package(&msg);
int i;
for(i = 0; i < waypoint_info.waypoint_count; i++) {
start_time = get_system_time_ms();
/* Waiting for mission request command */
while(received_msg.msgid != 40) {
cur_time = get_system_time_ms();
//Suspend the task to read the new message
vTaskDelay(100);
/* Time out, leave */
if((cur_time - start_time) > TIMEOUT_CNT)
return;
}
/* Waiting for mission request command */
mavlink_msg_mission_request_decode(&received_msg, &mmrt);
/* Clear the received message */
clear_message_id(&received_msg);
/* Send the waypoint to the ground station */
mavlink_msg_mission_item_pack(
1, 0, &msg, 255, 0,
cur_wp->data.seq,
cur_wp->data.frame,
cur_wp->data.command,
cur_wp->data.current,
cur_wp->data.autocontinue,
cur_wp->data.param1,
cur_wp->data.param2,
cur_wp->data.param3,
cur_wp->data.param4,
cur_wp->data.x,
cur_wp->data.y,
cur_wp->data.z
);
send_package(&msg);
cur_wp = cur_wp->next;
}
/* Send a mission ack Message at the end */
mavlink_msg_mission_ack_pack(1, 0, &msg, 255, 0, 0);
send_package(&msg);
}
