static void postHeartbeat( SOCKET sock, const sockaddr_in& gcAddr, float position[] )
{
mavlink_message_t msg;
uint16_t len;
int bytes_sent;
uint8_t buf[BUFFER_LENGTH];
//#define OLD_MAV
/*Send Heartbeat */
#ifdef OLD_MAV
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC);
#else
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
#endif
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(gcAddr));
/* Send Status */
#ifdef OLD_MAV
mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 7500, 0, 0, 0);
#else
mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
#endif
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof (gcAddr));
/* Send Local Position */
#ifdef OLD_MAV
mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
#else
mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
#endif
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
//old and new, do difference.
static float roll = 1.2;
float pitch = 1.7;
float yaw = 3.14;
#ifdef OLD_MAV
mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, 0.01, 0.02, 0.03);
#else
mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, 0.01, 0.02, 0.03);
#endif
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(gcAddr));
//Beep( 880, 100 );
}
void Create_packets(ExtU_quadrotor_FCS_0_T *data,int sock)
{
int bytes_sent;
unsigned int temp = 0;
int i;
uint8_t buf[BUFFER_LENGTH];
mavlink_message_t msg;
uint16_t len;
mavlink_msg_heartbeat_pack(3, 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(3, 200, &msg, microsSinceEpoch(),data->Internalstates[3], data->Internalstates[4], data->Internalstates[5], data->Internalstates[6], data->Internalstates[7], data->Internalstates[8]);
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_gps_raw_int_pack(3, 200, &msg, microsSinceEpoch(),2,data->lla[0]*10000000,data->lla[1]*10000000,data->lla[2],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));
mavlink_msg_rc_channels_pack(3, 200, &msg, microsSinceEpoch(),4,data->RC[1]*100,data->RC[2]*100,data->RC[3]*100,data->RC[4]*100,data->ORC[0],data->ORC[1],data->ORC[2],data->ORC[3],data->rpm[0],data->rpm[1],data->rpm[2],data->rpm[3],UINT16_MAX,UINT16_MAX,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);
}
void
poseStampedCallback(const geometry_msgs::PoseStamped& poseStampedMsg)
{
// set timestamp (get NSec from ROS and convert to us)
uint64_t timestamp = poseStampedMsg.header.stamp.toNSec() / 1000;
// send MAVLINK attitude and local position messages
mavlink_message_t msg;
//convert quaternion to euler angles
const btQuaternion quat(poseStampedMsg.pose.orientation.x,
poseStampedMsg.pose.orientation.y,
poseStampedMsg.pose.orientation.z,
poseStampedMsg.pose.orientation.w);
const btMatrix3x3 mat(quat);
double roll, pitch, yaw;
mat.getEulerYPR(yaw, pitch, roll);
mavlink_msg_attitude_pack(sysid, compid, &msg, timestamp, roll, pitch, yaw, 0.0f, 0.0f, 0.0f);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
float x = poseStampedMsg.pose.position.x;
float y = poseStampedMsg.pose.position.y;
float z = poseStampedMsg.pose.position.z;
mavlink_msg_local_position_pack(sysid, compid, &msg, timestamp, x, y, z, 0.0f, 0.0f, 0.0f);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
if (verbose)
{
ROS_INFO("Sent Mavlink local position and attitude messages.");
}
}
static void uavtalk_handle_msg(struct uavtalk_message *msg)
{
mavlink_message_t mav_msg;
switch (msg->objid) {
case UAVTALK_OBJID_ATTITUDESTATE:
mavlink_msg_attitude_pack(1, 1, &mav_msg, 0,
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_ROLL)),
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_PITCH)),
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_YAW)),
0, 0, 0);
mavlink_handle_msg(255, &mav_msg, NULL);
break;
case UAVTALK_OBJID_MANUALCONTROLCOMMAND:
case UAVTALK_OBJID_MANUALCONTROLCOMMAND_001:
case UAVTALK_OBJID_MANUALCONTROLCOMMAND_002:
mavlink_msg_rc_channels_raw_pack(1, 1, &mav_msg, 0, 0,
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_1),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_2),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_3),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_4),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_5),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_6),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_7),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_8),
0);
mavlink_handle_msg(255, &mav_msg, NULL);
break;
}
}
/*****************************************************************
* mavlink_sender()
* send mavlink heartbeat and IMU attitude packets
******************************************************************/
void* mavlink_sender(void* ptr){
uint8_t buf[MAV_BUF_LEN];
mavlink_message_t msg;
uint16_t len;
while(get_state() != EXITING){
// send heartbeat
memset(buf, 0, MAV_BUF_LEN);
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr,\
sizeof(struct sockaddr_in));
//send attitude
memset(&buf, 0, MAV_BUF_LEN);
mavlink_msg_attitude_pack(1, 200, &msg,\
microsSinceEpoch(),
mpu.fusedEuler[VEC3_X],\
mpu.fusedEuler[VEC3_Y],\
mpu.fusedEuler[VEC3_Z],\
0, 0, 0);
//set gyro rates to 0 for simplicity
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr,\
sizeof(struct sockaddr_in));
usleep(100000); // 10 hz
}
return NULL;
}
/**
* Transmits the vehicle attitude. Right now just the yaw value.
* Expects systemStatus.time to be in centiseconds which are then converted
* to ms for transmission.
* Yaw should be in radians where positive is eastward from north.
*/
void MavLinkSendAttitude(void)
{
mavlink_message_t msg;
mavlink_msg_attitude_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
systemStatus.time*10, 0.0, 0.0, internalVariables.Heading, 0.0, 0.0, 0.0);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
uint16_t PackRawAttitude(uint8_t system_id, uint8_t component_id, mavlink_attitude_t mlAttitudeData ,uint32_t time_usec){
mavlink_system_t mavlink_system;
// if (!(mlPending.wpProtState == WP_PROT_IDLE))
// return 0;
mavlink_system.sysid = system_id; ///< ID 20 for this airplane
mavlink_system.compid = component_id;//MAV_COMP_ID_IMU; ///< The component sending the message is the IMU, it could be also a Linux process
//////////////////////////////////////////////////////////////////////////
mavlink_message_t msg;
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_attitude_pack(mavlink_system.sysid, mavlink_system.compid, &msg , time_usec , mlAttitudeData.roll, mlAttitudeData.pitch, mlAttitudeData.yaw, mlAttitudeData.rollspeed, mlAttitudeData.pitchspeed, mlAttitudeData.yawspeed );
return( mavlink_msg_to_send_buffer(UartOutBuff, &msg));
}
void cDataLink::SendAttMsg(float roll, float pitch, float yaw,
float rollspeed, float pitchspeed, float yawspeed)
{
int bytes_sent;
uint16_t len;
/* Send attitude */
mavlink_msg_attitude_pack(1, MAV_COMP_ID_ALL, &m_msg, microsSinceEpoch(),
roll, pitch, yaw,
rollspeed, pitchspeed, yawspeed);
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
void Dialog::on_pushButton_clicked()
{
mavlink_msg_attitude_pack (100, 200, &msg, 47, 0.1, 0.2, 0.3, 0, 0, 0) ;
len = mavlink_msg_to_send_buffer(buf, &msg);
udpSocket->writeDatagram((char*)buf,len, *host, 14550);
//ui->textEdit->setText((char *)buf);
// QString strNum = QString().setNum(num);
// QString strMsgId = QString().setNum(msg.msgid);
// QString *str = new QString("Sent No: ");//<< msg.sysid <<"COMP:"<<msg.compid<<"LEN:"<<msg.len<<"MSG ID:"<<msg.msgid);
// str->append(strNum);
// str->append(" MSG ID : ");
// str->append(strMsgId);
// ui->textEditSendMessage->setText(*str);
}
static void send_attitude_info(void)
{
mavlink_message_t msg;
float attitude_roll, attitude_pitch, attitude_yaw;
/* Prepare the attitude data */
read_global_data_value(TRUE_ROLL, DATA_POINTER_CAST(&attitude_roll));
read_global_data_value(TRUE_PITCH, DATA_POINTER_CAST(&attitude_pitch));
read_global_data_value(TRUE_YAW, DATA_POINTER_CAST(&attitude_yaw));
mavlink_msg_attitude_pack(1, 200, &msg,
get_system_time_ms(),
toRad(attitude_roll),
toRad(attitude_pitch),
toRad(attitude_yaw),
0.0, 0.0, 0.0
);
send_package(&msg);
}
void mavlinkSendAttitude(void)
{
uint16_t msgLength;
mavlink_msg_attitude_pack(0, 200, &mavMsg,
// time_boot_ms Timestamp (milliseconds since system boot)
millis(),
// roll Roll angle (rad)
DECIDEGREES_TO_RADIANS(attitude.values.roll),
// pitch Pitch angle (rad)
DECIDEGREES_TO_RADIANS(-attitude.values.pitch),
// yaw Yaw angle (rad)
DECIDEGREES_TO_RADIANS(attitude.values.yaw),
// rollspeed Roll angular speed (rad/s)
0,
// pitchspeed Pitch angular speed (rad/s)
0,
// yawspeed Yaw angular speed (rad/s)
0);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
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
int main(int argc, char* argv[])
{
char help[] = "--help";
char target_ip[100];
float position[6] = {};
int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
struct sockaddr_in gcAddr;
struct sockaddr_in locAddr;
//struct sockaddr_in fromAddr;
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
socklen_t fromlen;
int bytes_sent;
mavlink_message_t msg;
uint16_t len;
int i = 0;
//int success = 0;
unsigned int temp = 0;
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
for (;;)
{
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
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);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
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");
}
memset(buf, 0, BUFFER_LENGTH);
sleep(1); // Sleep one second
}
}
static void uavoMavlinkBridgeTask(void *parameters) {
uint32_t lastSysTime;
// Main task loop
lastSysTime = PIOS_Thread_Systime();
FlightBatterySettingsData batSettings = {};
if (FlightBatterySettingsHandle() != NULL )
FlightBatterySettingsGet(&batSettings);
SystemStatsData systemStats;
while (1) {
PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ);
if (stream_trigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
FlightBatteryStateData batState = {};
if (FlightBatteryStateHandle() != NULL )
FlightBatteryStateGet(&batState);
SystemStatsGet(&systemStats);
int8_t battery_remaining = 0;
if (batSettings.Capacity != 0) {
if (batState.ConsumedEnergy < batSettings.Capacity) {
battery_remaining = 100 - lroundf(batState.ConsumedEnergy / batSettings.Capacity * 100);
}
}
uint16_t voltage = 0;
if (batSettings.VoltagePin != FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE)
voltage = lroundf(batState.Voltage * 1000);
uint16_t current = 0;
if (batSettings.CurrentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
current = lroundf(batState.Current * 100);
mavlink_msg_sys_status_pack(0, 200, mav_msg,
// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
0,
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
(uint16_t)systemStats.CPULoad * 10,
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
voltage,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
current,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
battery_remaining,
// drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_count1 Autopilot-specific errors
0,
// errors_count2 Autopilot-specific errors
0,
// errors_count3 Autopilot-specific errors
0,
// errors_count4 Autopilot-specific errors
0);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_RC_CHANNELS)) {
ManualControlCommandData manualState;
FlightStatusData flightStatus;
ManualControlCommandGet(&manualState);
FlightStatusGet(&flightStatus);
SystemStatsGet(&systemStats);
//TODO connect with RSSI object and pass in last argument
mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
0,
// chan1_raw RC channel 1 value, in microseconds
manualState.Channel[0],
// chan2_raw RC channel 2 value, in microseconds
manualState.Channel[1],
// chan3_raw RC channel 3 value, in microseconds
manualState.Channel[2],
// chan4_raw RC channel 4 value, in microseconds
manualState.Channel[3],
// chan5_raw RC channel 5 value, in microseconds
manualState.Channel[4],
// chan6_raw RC channel 6 value, in microseconds
manualState.Channel[5],
// chan7_raw RC channel 7 value, in microseconds
manualState.Channel[6],
// chan8_raw RC channel 8 value, in microseconds
manualState.Channel[7],
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
manualState.Rssi);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_POSITION)) {
GPSPositionData gpsPosData = {};
HomeLocationData homeLocation = {};
SystemStatsGet(&systemStats);
if (GPSPositionHandle() != NULL )
GPSPositionGet(&gpsPosData);
if (HomeLocationHandle() != NULL )
HomeLocationGet(&homeLocation);
SystemStatsGet(&systemStats);
uint8_t gps_fix_type;
switch (gpsPosData.Status)
{
case GPSPOSITION_STATUS_NOGPS:
gps_fix_type = 0;
break;
case GPSPOSITION_STATUS_NOFIX:
gps_fix_type = 1;
break;
case GPSPOSITION_STATUS_FIX2D:
gps_fix_type = 2;
break;
case GPSPOSITION_STATUS_FIX3D:
case GPSPOSITION_STATUS_DIFF3D:
gps_fix_type = 3;
break;
default:
gps_fix_type = 0;
break;
}
mavlink_msg_gps_raw_int_pack(0, 200, mav_msg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
(uint64_t)systemStats.FlightTime * 1000,
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gps_fix_type,
// lat Latitude in 1E7 degrees
gpsPosData.Latitude,
// lon Longitude in 1E7 degrees
gpsPosData.Longitude,
// alt Altitude in 1E3 meters (millimeters) above MSL
gpsPosData.Altitude * 1000,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.HDOP * 100,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
gpsPosData.VDOP * 100,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
gpsPosData.Groundspeed * 100,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
gpsPosData.Heading * 100,
// satellites_visible Number of satellites visible. If unknown, set to 255
gpsPosData.Satellites);
send_message();
mavlink_msg_gps_global_origin_pack(0, 200, mav_msg,
// latitude Latitude (WGS84), expressed as * 1E7
homeLocation.Latitude,
// longitude Longitude (WGS84), expressed as * 1E7
homeLocation.Longitude,
// altitude Altitude(WGS84), expressed as * 1000
homeLocation.Altitude * 1000);
send_message();
//TODO add waypoint nav stuff
//wp_target_bearing
//wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
//alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
//aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
//xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
//mavlink_msg_nav_controller_output_pack
//wp_number
//mavlink_msg_mission_current_pack
}
if (stream_trigger(MAV_DATA_STREAM_EXTRA1)) {
AttitudeActualData attActual;
SystemStatsData systemStats;
AttitudeActualGet(&attActual);
SystemStatsGet(&systemStats);
mavlink_msg_attitude_pack(0, 200, mav_msg,
// time_boot_ms Timestamp (milliseconds since system boot)
systemStats.FlightTime,
// roll Roll angle (rad)
attActual.Roll * DEG2RAD,
// pitch Pitch angle (rad)
attActual.Pitch * DEG2RAD,
// yaw Yaw angle (rad)
attActual.Yaw * DEG2RAD,
// rollspeed Roll angular speed (rad/s)
0,
// pitchspeed Pitch angular speed (rad/s)
0,
// yawspeed Yaw angular speed (rad/s)
0);
send_message();
}
if (stream_trigger(MAV_DATA_STREAM_EXTRA2)) {
ActuatorDesiredData actDesired;
AttitudeActualData attActual;
AirspeedActualData airspeedActual = {};
GPSPositionData gpsPosData = {};
BaroAltitudeData baroAltitude = {};
FlightStatusData flightStatus;
if (AirspeedActualHandle() != NULL )
AirspeedActualGet(&airspeedActual);
if (GPSPositionHandle() != NULL )
GPSPositionGet(&gpsPosData);
if (BaroAltitudeHandle() != NULL )
BaroAltitudeGet(&baroAltitude);
ActuatorDesiredGet(&actDesired);
AttitudeActualGet(&attActual);
FlightStatusGet(&flightStatus);
float altitude = 0;
if (BaroAltitudeHandle() != NULL)
altitude = baroAltitude.Altitude;
else if (GPSPositionHandle() != NULL)
altitude = gpsPosData.Altitude;
// round attActual.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360)
int16_t heading = lroundf(attActual.Yaw);
if (heading < 0)
heading += 360;
mavlink_msg_vfr_hud_pack(0, 200, mav_msg,
// airspeed Current airspeed in m/s
airspeedActual.TrueAirspeed,
// groundspeed Current ground speed in m/s
gpsPosData.Groundspeed,
// heading Current heading in degrees, in compass units (0..360, 0=north)
heading,
// throttle Current throttle setting in integer percent, 0 to 100
actDesired.Throttle * 100,
// alt Current altitude (MSL), in meters
altitude,
// climb Current climb rate in meters/second
0);
send_message();
uint8_t armed_mode = 0;
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED)
armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t custom_mode = CUSTOM_MODE_STAB;
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
case FLIGHTSTATUS_FLIGHTMODE_MWRATE:
case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR:
case FLIGHTSTATUS_FLIGHTMODE_HORIZON:
/* Kinda a catch all */
custom_mode = CUSTOM_MODE_SPORT;
break;
case FLIGHTSTATUS_FLIGHTMODE_ACRO:
case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK:
custom_mode = CUSTOM_MODE_ACRO;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
/* May want these three to try and
* infer based on roll axis */
case FLIGHTSTATUS_FLIGHTMODE_LEVELING:
custom_mode = CUSTOM_MODE_STAB;
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
custom_mode = CUSTOM_MODE_DRIFT;
break;
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
custom_mode = CUSTOM_MODE_ALTH;
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
custom_mode = CUSTOM_MODE_RTL;
break;
case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
custom_mode = CUSTOM_MODE_POSH;
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
custom_mode = CUSTOM_MODE_AUTO;
break;
}
mavlink_msg_heartbeat_pack(0, 200, mav_msg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
MAV_TYPE_GENERIC,
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
MAV_AUTOPILOT_GENERIC,
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
armed_mode,
// custom_mode A bitfield for use for autopilot-specific flags.
custom_mode,
// system_status System status flag, see MAV_STATE ENUM
0);
send_message();
}
}
}
/* Attitude Sender */
bool MAVLink::sendAttitude(float roll, float pitch, float yaw, float p, float q, float r) {
mavlink_message_t msg;
mavlink_msg_attitude_pack(mySystemId,myComponentId,&msg,getTime_ms(),roll,pitch,yaw,p,q,r);
return sendMessage(msg);
}
struct mavlink_drone *mavlink_drone_new(struct mavlink_drone_cfg *cfg)
{
struct mavlink_drone *c;
struct epoll_event ev;
struct epoll_event events[3];
struct itimerspec timeout;
struct mavlink_comm_cfg comm_cfg = { .local_port = 14551,
.remote_port = 14550,
.cb = &callback};
if (!cfg)
return NULL;
c = calloc(1, sizeof(struct mavlink_drone));
if (!c)
return NULL;
c->remote_system_id = cfg->remote_system_id;
c->remote_component_id = cfg->remote_component_id;
c->update_status_cb = cfg->update_status_cb;
comm_cfg.remote_addr = NULL;
c->comm = mavlink_comm_new(&comm_cfg);
if (!c->comm) {
printf("bad init comm\n");
goto error;
}
c->exitfd = eventfd(0,0);
bzero(&timeout, sizeof(timeout));
timeout.it_value.tv_sec = 1;
timeout.it_value.tv_nsec = 0;
timeout.it_interval.tv_sec = 1;
timeout.it_interval.tv_nsec = 0;
c->fd_1hz = timerfd_init(&timeout);
if (c->fd_1hz < 0) {
printf("Failed to create timerfd 1hz\n");
goto error;
}
printf("size %lu\n", sizeof(events) / sizeof(events[0]));
c->epollfd = epoll_create(sizeof(events) / sizeof(events[0]));
if (c->epollfd < 0) {
printf("epoll_create failed\n");
goto error;
}
ev.events = EPOLLIN;
ev.data.fd = c->fd_1hz;
if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, c->fd_1hz, &ev) == -1) {
printf("failed to add fd_1hz\n");
goto error;
}
ev.events = EPOLLIN;
ev.data.fd = mavlink_comm_get_sockfd(c->comm);
if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, ev.data.fd, &ev) == -1) {
printf ("failed to add sockfd\n");
goto error;
}
ev.events = EPOLLIN;
ev.data.fd = c->exitfd;
if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, c->exitfd, &ev) == -1) {
printf("failed to add eventfd\n");
goto error;
}
return c;
error:
if (c->comm)
mavlink_comm_destroy(c->comm);
if (c->fd_1hz > 0)
close(c->fd_1hz);
if (c->epollfd > 0)
close(c->epollfd);
free(c);
return NULL;
}
void mavlink_drone_destroy(struct mavlink_drone *c)
{
if (!c)
return;
if (c->epollfd > 0)
close(c->epollfd);
free(c);
}
static void mavlink_drone_send_status(struct mavlink_drone *ctrl)
{
mavlink_message_t msg;
struct mavlink_info_cache *c = &ctrl->cache;
int ret;
if (ctrl->update_status_cb)
(ctrl->update_status_cb)(c);
mavlink_msg_heartbeat_pack(ctrl->remote_system_id,
ctrl->remote_component_id,
&msg, c->hb.type, c->hb.autopilot, c->hb.base_mode,
0, c->hb.system_status);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
/* Send Status */
mavlink_msg_sys_status_pack(ctrl->remote_system_id,
ctrl->remote_component_id,
&msg, c->sys_stat.onboard_control_sensors_present,
c->sys_stat.onboard_control_sensors_enabled,
c->sys_stat.onboard_control_sensors_health,
c->sys_stat.load,
c->sys_stat.voltage_battery,
c->sys_stat.current_battery,
c->sys_stat.drop_rate_comm,
c->sys_stat.errors_comm,
c->sys_stat.errors_count1,
c->sys_stat.errors_count2,
c->sys_stat.errors_count3,
c->sys_stat.errors_count4,
c->sys_stat.battery_remaining);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
/* Send Local Position */
mavlink_msg_local_position_ned_pack(ctrl->remote_system_id,
ctrl->remote_component_id, &msg, c->lpn.time_boot_ms,
c->lpn.x, c->lpn.y, c->lpn.z,
c->lpn.vx, c->lpn.vy, c->lpn.vz);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
/* Send attitude */
mavlink_msg_attitude_pack(ctrl->remote_system_id,
ctrl->remote_component_id,
&msg, c->att.time_boot_ms, c->att.roll, c->att.pitch,
c->att.yaw, c->att.rollspeed, c->att.pitchspeed,
c->att.yawspeed);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
}
void *MavlinkStatusTask(void *ptr) {
MavlinkStruct *Mavlink = (MavlinkStruct *) ptr;
AttitudeData *AttitudeDesire = Control.AttitudeDesire;
AttitudeData *AttitudeMesure = Mavlink->AttitudeMesure;
AttData Data, Speed;
AttData DataD, SpeedD;
AttData DataM, SpeedM;
double Error[4] = {0.0, 0.0, 0.0, 0.0};
uint32_t TimeStamp;
mavlink_message_t msg;
uint16_t len;
uint8_t buf[BUFFER_LENGTH];
int bytes_sent;
uint32_t sensor = 0xf00f;
printf("%s : Mavlink Status démarré\n", __FUNCTION__);
pthread_barrier_wait(&(MavlinkStartBarrier));
while (MavlinkActivated) {
sem_wait(&MavlinkStatusTimerSem);
if (MavlinkActivated == 0)
break;
memset(buf, 0, BUFFER_LENGTH);
pthread_spin_lock(&(AttitudeMesure->AttitudeLock));
memcpy((void *) &Data, (void *) &(AttitudeMesure->Data), sizeof(AttData));
memcpy((void *) &Speed, (void *) &(AttitudeMesure->Speed), sizeof(AttData));
TimeStamp = AttitudeMesure->timestamp_s*1000 + AttitudeMesure->timestamp_n/1000000L;
pthread_spin_unlock(&(AttitudeMesure->AttitudeLock));
//Send Heartbeat
mavlink_msg_heartbeat_pack(SYSTEM_ID, COMPONENT_ID, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in));
// Send Status
mavlink_msg_sys_status_pack(SYSTEM_ID, COMPONENT_ID, &msg, sensor, sensor, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof (struct sockaddr_in));
// Send Local Position
mavlink_msg_local_position_ned_pack(SYSTEM_ID, COMPONENT_ID, &msg, TimeStamp, 0, 0, (float) Data.Elevation, 0, 0, (float) Speed.Elevation);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in));
pthread_spin_lock(&(AttitudeDesire->AttitudeLock));
memcpy((void *) &DataD, (void *) &(AttitudeDesire->Data), sizeof(AttData));
memcpy((void *) &SpeedD, (void *) &(AttitudeDesire->Speed), sizeof(AttData));
pthread_spin_unlock(&(AttitudeDesire->AttitudeLock));
pthread_spin_lock(&(AttitudeMesure->AttitudeLock));
memcpy((void *) &DataM, (void *) &(AttitudeMesure->Data), sizeof(AttData));
memcpy((void *) &SpeedM, (void *) &(AttitudeMesure->Speed), sizeof(AttData));
pthread_spin_unlock(&(AttitudeMesure->AttitudeLock));
Error[HEIGHT] = DataD.Elevation - DataM.Elevation;
Error[ROLL] = DataD.Roll - DataM.Roll;
Error[PITCH] = DataD.Pitch - DataM.Pitch;
Error[YAW] = DataD.Yaw - DataM.Yaw;
// Send Attitude
mavlink_msg_attitude_pack(SYSTEM_ID, COMPONENT_ID, &msg, TimeStamp, (float) Data.Roll, (float) Data.Pitch, (float) Data.Yaw, (float) Speed.Roll, (float) Speed.Pitch, (float) Speed.Yaw);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in));
}
printf("%s : Mavlink Status Arrêté\n", __FUNCTION__);
pthread_exit(NULL);
}
int main(int argc, char* argv[])
{
// Initialize MAVLink
mavlink_wpm_init(&wpm);
mavlink_system.sysid = 5;
mavlink_system.compid = 20;
mavlink_pm_reset_params(&pm);
int32_t ground_distance;
uint32_t time_ms;
// Create socket
sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
for (;;)
{
bytes_sent = 0;
/* Send Heartbeat */
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_ned_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send global position */
if (hilEnabled)
{
mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
}
/* Send attitude */
mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send HIL outputs */
float roll_ailerons = 0; // -1 .. 1
float pitch_elevator = 0.2; // -1 .. 1
float yaw_rudder = 0.1; // -1 .. 1
float throttle = 0.9; // 0 .. 1
mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0);
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);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
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);
// Handle packet with waypoint component
mavlink_wpm_message_handler(&msg);
// Handle packet with parameter component
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
// Print HIL values sent to system
if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE)
{
mavlink_hil_state_t hil;
mavlink_msg_hil_state_decode(&msg, &hil);
printf("Received HIL state:\n");
printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw);
roll = hil.roll;
pitch = hil.pitch;
yaw = hil.yaw;
rollspeed = hil.rollspeed;
pitchspeed = hil.pitchspeed;
yawspeed = hil.yawspeed;
speedx = hil.vx;
speedy = hil.vy;
speedz = hil.vz;
latitude = hil.lat;
longitude = hil.lon;
altitude = hil.alt;
hilEnabled = true;
}
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
usleep(10000); // Sleep 10 ms
// Send one parameter
mavlink_pm_queued_send();
}
}
int main(int argc, char* argv[])
{
// Initialize MAVLink
mavlink_wpm_init(&wpm);
mavlink_system.sysid = 1;
mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER;
// Create socket
sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
for (;;)
{
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
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);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
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);
// Handle packet with waypoint component
mavlink_wpm_message_handler(&msg);
// Handle packet with parameter component
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
usleep(50000); // Sleep one second
}
}
