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 MAVLinkProxy::heartbeat()
{
static mavlink_message_t msg;
static uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// System definition
static uint8_t system_type = MAV_TYPE_QUADROTOR;
static uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY;
uint8_t system_mode = MAV_MODE_PREFLIGHT;
uint32_t custom_mode = 0;
uint8_t system_state = MAV_STATE_STANDBY;
if(_connected)
{
system_mode = MAV_MODE_STABILIZE_DISARMED;
}
if(_flying)
{
system_mode = MAV_MODE_STABILIZE_ARMED;
system_state = MAV_STATE_ACTIVE;
}
mavlink_msg_heartbeat_pack(_mavlink_system.sysid, _mavlink_system.compid, &msg, system_type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_to_send_buffer(buf, &msg);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
mavlink_msg_sys_status_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg, &_sys_status);
mavlink_msg_to_send_buffer(buf, &msg);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
_heartbeat_timer.expires_from_now(boost::posix_time::milliseconds(MAVLINK_HEARTBEAT_INTERVAL));
_heartbeat_timer.async_wait(boost::bind(&MAVLinkProxy::heartbeat, this));
}
bool baseflight_mavlink_send_1Hzheartbeat(void) // That mother is running at 1Hz and schedules/collects eeprom writes
{
mavlink_message_t msg2;
static uint32_t LastHeartbeat;
uint8_t autopilot_type = MAV_AUTOPILOT_ARDUPILOTMEGA; // uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC;
uint8_t system_mode = 0; // Is set below
uint32_t custom_mode = 0;
uint8_t system_state = MAV_STATE_STANDBY;
CheckWPrxtxTimeouts(); // Check for disconnects during WP stuff
if ((currentTimeMS - LastHeartbeat) < 1000) return false;
LastHeartbeat = currentTimeMS;
// Set this here if Automission: MAV_MODE_STABILIZE_DISARMED
if (f.ANGLE_MODE || f.HORIZON_MODE) system_mode = MAV_MODE_STABILIZE_DISARMED;
else system_mode = MAV_MODE_MANUAL_DISARMED;
if(f.ARMED)
{
system_mode |= 128; // Set the Armed bit here if necessary
system_state = MAV_STATE_ACTIVE;
}
mavlink_msg_heartbeat_pack(MLSystemID, MLComponentID, &msg2, system_type, autopilot_type, system_mode, custom_mode, system_state);
baseflight_mavlink_send_message(&msg2);
return true;
}
//private slots:
void Dialog::broadcastDatagram()
{
static int num = 0;
num++;
//qDebug()<<"broadcast"<<endl;
mavlink_msg_heartbeat_pack(100, 200, &msg, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC,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);
// mavlink_msg_sys_status_pack(2, 200, &msg, MAV_MODE_MANUAL, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 750, 12300,0,0);
// len = mavlink_msg_to_send_buffer(buf, &msg);
// udpSocket->writeDatagram((char*)buf,len, *host, 14550);
//mavlink_msg_manual_control_decode(&msg, manual_control);
// QHostAddress host = QHostAddress("127.0.0.1");
// messageNo++;
// QByteArray datagram = "Broadcast message " + QByteArray::number(messageNo);
// udpSocket->writeDatagram(datagram.data(), datagram.size(),host, 14550);
}
/*****************************************************************
* 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;
}
uint16_t PackHeartBeat(uint8_t system_id, uint8_t component_id){
mavlink_system_t mavlink_system;
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_system.type = MAV_TYPE_FIXED_WING; ///< This system is an airplane / fixed wing
// Define the system type, in this case an airplane
uint8_t system_type = MAV_TYPE_FIXED_WING;
uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC;
uint8_t system_mode = mlHeartbeatLocal.base_mode; ///< Booting up
uint32_t custom_mode = mlHeartbeatLocal.custom_mode; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = mlHeartbeatLocal.system_status; ///< System ready for flight
mavlink_message_t msg;
uint16_t bytes2Send = 0;
//////////////////////////////////////////////////////////////////////////
memset(&msg, 0, sizeof (mavlink_message_t));
// Pack the Heartbeat message
mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, system_type, autopilot_type, system_mode, custom_mode, system_state);
/*
mavlink_msg_heartbeat_pack(system_id,
component_id,
&msg,
MAV_TYPE_FIXED_WING,//MAV_TYPE_GENERIC,
MAV_AUTOPILOT_SLUGS,//MAV_AUTOPILOT_GENERIC,
MAV_MODE_PREFLIGHT,//mlHeartbeatLocal.base_mode,
MAV_MODE_PREFLIGHT,//mlHeartbeatLocal.custom_mode,
MAV_STATE_UNINIT//mlHeartbeatLocal.system_status
);
*/
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
return(bytes2Send);
}
// ----------------------------------------------------------------------------
// Sending some heartbeat
// ----------------------------------------------------------------------------
void Pixhawk_Interface::
send_heartbeat(mavlink_message_t msg)
{
mavlink_msg_heartbeat_pack(id.sysid, id.compid, &msg, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID,0, 0, 0);
serial_port->write_message(msg);
}
void _Mavlink::sendHeartbeat(void)
{
mavlink_message_t message;
mavlink_msg_heartbeat_pack(m_systemID, m_componentID, &message, m_type, 0,
0, 0, MAV_STATE_ACTIVE);
writeMessage(message);
}
static void send_heartbeat_info(void)
{
mavlink_message_t msg;
uint8_t current_flight_mode,current_safety_switch;
uint8_t current_MAV_mode = MAV_MODE_PREFLIGHT;
read_global_data_value(MODE_BUTTON, DATA_POINTER_CAST(¤t_flight_mode));
read_global_data_value(SAFTY_BUTTON, DATA_POINTER_CAST(¤t_safety_switch));
if(current_safety_switch == 0){
/* ENGINE ON */
if(current_flight_mode == 0){
/* Mode 1 */
current_MAV_mode = MAV_MODE_STABILIZE_ARMED;
}else if(current_flight_mode == 1){
/* Mode 2 */
current_MAV_mode = MAV_MODE_GUIDED_ARMED;
}else if(current_flight_mode == 2){
/* Mode 3 */
current_MAV_mode = MAV_MODE_AUTO_ARMED;
}
}else if(current_safety_switch == 1){
/* ENGINE OFF */
if(current_flight_mode == 0){
/* Mode 1 */
current_MAV_mode = MAV_MODE_STABILIZE_DISARMED;
}else if(current_flight_mode == 1){
/* Mode 2 */
current_MAV_mode = MAV_MODE_GUIDED_DISARMED;
}else if(current_flight_mode == 2){
/* Mode 3 */
current_MAV_mode = MAV_MODE_AUTO_DISARMED;
}
}
mavlink_msg_heartbeat_pack(1, 200, &msg,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_GENERIC,
current_MAV_mode,
0, MAV_STATE_ACTIVE
);
send_package(&msg);
}
/**
* This function creates a MAVLink heartbeat message with some basic parameters and
* caches that message (along with its size) in the module-level variables declared
* above. This buffer should be transmit at 1Hz back to the groundstation.
*/
void MavLinkSendHeartbeat(void)
{
mavlink_message_t msg;
// Pack the message
mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY, mavlink_system.mode, 0, mavlink_system.state);
// Copy the message to the send buffer
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
void state_telemetry_send_heartbeat(const state_t* state, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
mavlink_msg_heartbeat_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
state->autopilot_type,
state->autopilot_name,
state->mav_mode.byte,
state->mav_mode_custom,
state->mav_state);
}
/**
* send a heartbeat message.
*/
int gcs_heartbeat(int sock) {
int bytes_sent;
mavlink_message_t msg;
uint8_t buf[512];
uint16_t len;
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = gcs_udp_send(sock, buf, len);
return(bytes_sent);
}
void MAVLinkMKApp::send_heartbeat() {
Lock tx_lock(tx_mav_mutex);
mavlink_msg_heartbeat_pack(system_id(),
component_id,
&tx_mav_msg,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_GENERIC,
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, //base mode
0, //custom mode
MAV_STATE_ACTIVE); //system status
AppLayer<mavlink_message_t>::send(tx_mav_msg);
}
void MavLinkInterpreter::SendHeartbeat()
{
// Send a heartbeat over the mavlink
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tSending heartbeat message.\r\n", millis());
#endif
//mavlink_msg_heartbeat_pack(123, 123, &_msg, MAV_TYPE_ONBOARD_CONTROLLER, MAV_AUTOPILOT_INVALID, MAV_MODE_PREFLIGHT, <CUSTOM_MODE>, MAV_STATE_STANDBY);
mavlink_msg_heartbeat_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, 18, 8, 0, 0, 4);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
}
static void mav_heartbeat(struct timer *t, void *d)
{
mavlink_message_t msg;
mavlink_msg_heartbeat_pack(osd_sysid,
MAV_COMP_ID_ALCEOSD, &msg, MAV_TYPE_ALCEOSD,
MAV_AUTOPILOT_INVALID,
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, // base_mode
0, //custom_mode
MAV_STATE_ACTIVE);
mavlink_send_msg(&msg);
}
void loop()
{
delay(1000); // Wait 1 second between transmits, could also 'sleep' here!
// Create and pack the heartbeat data
mavlink_msg_heartbeat_pack(NODEID, NODEID, &data_pkt, 78.3, 45.1, 83.6, 75.4, 10234);
uint16_t buf_len = mavlink_msg_to_send_buffer(mavlink_buf, &data_pkt);
radio.send(RECEIVER, mavlink_buf, buf_len, false);
Blink(LED_BUILTIN, 50, 3);
/*if(radio.sendWithRetry(RECEIVER, radiopacket, strlen(radiopacket)))
{
Serial.println("OK");
Blink(LED_BUILTIN, 50, 3); //blink LED 3 times, 50ms between blinks
}*/
radio.receiveDone(); // Put radio in RX mode
Serial.flush(); // Make sure all serial data is clocked out before sleeping the MCU
}
void cDataLink::heartbeat_loop(void)
{
int bytes_sent;
uint16_t len;
while(execute_heartbeat_thread)
{
/*Send Heartbeat */
//MAV_TYPE_TRICOPTER
mavlink_msg_heartbeat_pack(1, MAV_COMP_ID_SYSTEM_CONTROL, &m_msg,
MAV_TYPE_TRICOPTER,
MAV_AUTOPILOT_GENERIC,
mav_mode,
0, //custom mode bitfield
mav_state);
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
boost::this_thread::sleep(boost::posix_time::time_duration(boost::posix_time::milliseconds(500)));
}
}
static void mav_heartbeat(struct timer *t, void *d)
{
mavlink_message_t msg;
LED = ~LED;
if (get_millis() - uav_last_seen > UAV_LAST_SEEN_TIMEOUT) {
set_timer_period(t, 5);
return;
} else {
set_timer_period(t, 10);
}
mavlink_msg_heartbeat_pack(config.mav.osd_sysid,
MAV_COMP_ID_OSD, &msg, MAV_TYPE_ALCEOSD,
MAV_AUTOPILOT_INVALID,
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, // base_mode
0, //custom_mode
MAV_STATE_ACTIVE);
mavlink_send_msg(&msg);
}
void GCS_Mavlink<T>::send_hearthbeat( MAV_STATE mav_state_define,
MAV_MODE mav_mode_define,
uint8_t mav_mode_flag_define)
{
_mav_state = mav_state_define;
_mav_mode = mav_mode_define;
_mav_mode_flag |= mav_mode_flag_define;
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message
mavlink_msg_heartbeat_pack( _systemId,
0,
&msg,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_PX4,
_mav_mode,
_mav_mode_flag,
_mav_state);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
bool GCS_Mavlink<T>::send_hearthbeat()
{
if ((millis() - _lastReceivedHearthbeat) > 3000)
_GCS_connection = false;
else
_GCS_connection = true;
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message
mavlink_msg_heartbeat_pack( _systemId,
0,
&msg,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_PX4,
_mav_mode,
_mav_mode_flag,
_mav_state);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
return _GCS_connection;
}
void ofxMavlink::sendHeartbeat() {
// Initialize the required buffers
mavlink_message_t message;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Define the system type, in this case an airplane
int system_type = MAV_TYPE_GCS;
int autopilot_type = MAV_AUTOPILOT_GENERIC;
// Pack the message
mavlink_msg_heartbeat_pack(sysId, compId, &message, system_type, autopilot_type, 0, 0, MAV_STATE_ACTIVE);
// mavlink_message_heartbeat_pack(system id, component id, message container, system type, MAV_AUTOPILOT_GENERIC)
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &message);
/* write packet via serial link */
int returnval = write(fd, buf, len);
if(returnval < 0) ofLogError("ofxMavlink") << "Error writing to serial port";
/* wait until all data has been written */
tcdrain(fd);
}
int main(int argc, char* argv[])
{
char help[] = "--help";
// IP 使用的是数组来存储
char target_ip[100];
float position[6] = {};
int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// socket的地址
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;
// channel
int chan1_raw;
int chan2_raw;
int chan3_raw;
int chan4_raw;
int chan5_raw;
int chan6_raw;
int chan7_raw;
int chan8_raw;
int osd_rssi;
// pitch roll yaw
int osd_pitch;
int osd_roll;
int osd_yaw;
// 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]);
}
// 配置socket的地址，这是本地的地址
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14550);
/* 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(14551);
for (;;)
{
// 这里有发送的代码模块， 发送的时候，并没有控制发送的速度
/*Send Heartbeat */
// 1,system_id component_id
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);
// linux socket函数，头文件#include <arpa/inet.h>
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;
mavlink_attitude_t attitude;
mavlink_vfr_hud_t hud;
#if SHOW
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
#endif
for (i = 0; i < recsize; ++i)
{
// temp 是一个byteBUF的形式
temp = buf[i];
#if SHOW
printf("%02x ", (unsigned char)temp);
#endif
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
#if SHOW
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
#endif
if(msg.msgid== MAVLINK_MSG_ID_ATTITUDE){
mavlink_msg_attitude_decode(&msg,&attitude);
//printf(" Received attitude packet: roll %f, pitch %f, yaw %f \n",attitude.roll,attitude.pitch,attitude.yaw);
}
if(msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_RAW){
chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(&msg);
chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(&msg);
chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg);
chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(&msg);
chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(&msg);
chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(&msg);
chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(&msg);
chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(&msg);
osd_rssi = mavlink_msg_rc_channels_raw_get_rssi(&msg);
printf("chan1: %d, chan2: %d,chan3: %d, chan4: %d\n",chan1_raw,chan2_raw,chan3_raw,chan4_raw);
}
if(msg.msgid == MAVLINK_MSG_ID_ATTITUDE){
//osd_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
//osd_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
//osd_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
//printf("pitch: %d,roll: %d,yaw: %d\n",osd_pitch,osd_roll,osd_yaw );
}
if(msg.msgid == MAVLINK_MSG_ID_VFR_HUD){
// 解码
mavlink_msg_vfr_hud_decode(&msg,&hud);
// osd_airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg);
// osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg);
// osd_heading = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north
// osd_throttle = (uint8_t)mavlink_msg_vfr_hud_get_throttle(&msg);
// osd_alt = mavlink_msg_vfr_hud_get_alt(&msg);
// osd_climb = mavlink_msg_vfr_hud_get_climb(&msg);
// printf("airspeed: %d,osd_throttle %d,osd_alt: %d\n",osd_airspeed,osd_throttle,osd_alt);
printf("airspeed: %f,hrottle %d,alt: %f\n",hud.airspeed,hud.throttle,hud.alt);
}
}
}
#if SHOW
printf("\n");
#endif
}
memset(buf, 0, BUFFER_LENGTH);
usleep(1000); // Sleep one second
}
}
int main(int argc, char **argv){
//open com port
char *portname = "/dev/ttyACM0";
char *baud = "57600";
int c;
while((c = getopt(argc, argv, "p:b:")) != -1)
switch(c){
case 'p':
portname = optarg;
break;
case 'b':
baud = optarg;
break;
default:
printf("error in command line arguments\n");
exit(1);
break;
}
printf("serial port: %s, baud rate: %s\n", portname, baud);
int fd = open (portname, O_RDWR | O_NOCTTY | O_SYNC);
//terminate if unable to open com port
if (fd < 0)
{
error_message ("error %d opening %s: %s\n", errno, portname, strerror (errno));
return;
}
//configure com port connection
if(baud == "57600")
set_interface_attribs (fd, B57600, 0); // set speed to 57600 bps, 8n1 (no parity)
else if (baud == "115200")
set_interface_attribs (fd, B115200, 0);
set_blocking (fd, 0); // set no blocking
printf("serial connection established\n");
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //buffer for sending and receiving messages
int recsize = 0; //size of received message
int i = 0; //loop variable
while(1){
//send heartbeat to ardupilot
mavlink_msg_heartbeat_pack(100, 200, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
write(fd, buf, len);
//printf("quadrotor: %i, mavautopilot: %i, mavmode: %i, mavstate: %i\n", MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, MAV_STATE_STANDBY);
//printf("LENGTH: %02x ", len);
// int i;
// for(i=0; i<len; i++)
// {
// printf("heartbeat: %02x ", (unsigned char) buf[i]);
// }
//clear buffer
memset(buf, 0, MAVLINK_MAX_PACKET_LEN);
//read from serial
recsize = read(fd, buf, sizeof buf);
//interpret message if the size of received data is greater than 0
if(recsize > 0){
mavlink_message_t msg;
mavlink_status_t status;
#ifdef DEBUG
printf("Bytes Received: %d\n Datagram: ", (int)recsize);
#endif
for(i = 0; i < recsize; i++){
#ifdef DEBUG
if(buf[i] == 254)
printf("\n");
printf("%02x ", (unsigned char) buf[i]);
#endif
//use mavlink function to parse message
if(mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)){
#ifdef DEBUG
printf("\nReceived packet: SYS: %d, COMP:%d, LEN:%d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
#endif
//extract data depending on the id number of the messages
switch(msg.msgid){
case MAVLINK_MSG_ID_GPS_RAW_INT:
printf("raw lat: %d, raw lon: %d\n", mavlink_msg_gps_raw_int_get_lat(&msg), mavlink_msg_gps_raw_int_get_lon(&msg));
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
printf("lat: %d, lon: %d, alt: %d compass: %d\n", mavlink_msg_global_position_int_get_lat(&msg), mavlink_msg_global_position_int_get_lon(&msg),
mavlink_msg_global_position_int_get_alt(&msg), mavlink_msg_global_position_int_get_hdg(&msg));
break;
//add more cases here for other messages (see https://pixhawk.ethz.ch/mavlink/#DATA_STREAM and header(.h) files in mavlink/include/common/)
}
}
}
printf("\n\n");
}
memset(buf, 0, MAVLINK_MAX_PACKET_LEN);
sleep(1);
}
}
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
}
}
void scheduleData (unsigned char hilOn, unsigned char* dataOut){
// Generic message container used to pack the messages
mavlink_message_t msg;
// Generic buffer used to hold data to be streamed via serial port
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Cycles from 1 to 10 to decide which
// message's turn is to be sent
static uint8_t samplePeriod = 1;
// Contains the total bytes to send via the serial port
uint8_t bytes2Send = 0;
memset(&msg,0,sizeof(mavlink_message_t));
switch (samplePeriod){
case 1: //GPS
mavlink_msg_heartbeat_pack(SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
MAV_FIXED_WING,
MAV_AUTOPILOT_SLUGS);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
memset(&msg,0,sizeof(mavlink_message_t));
// Pack the GPS message
mavlink_msg_gps_raw_pack(SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
mlGpsData.usec,
mlGpsData.fix_type,
mlGpsData.lat,
mlGpsData.lon,
mlGpsData.alt,
mlGpsData.eph,
mlGpsData.epv,
mlGpsData.v,
mlGpsData.hdg);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 2: // LOAD
mavlink_msg_cpu_load_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlCpuLoadData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 3: // XYZ
mavlink_msg_local_position_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlLocalPositionData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 4: // Dynamic and Reboot (if required)
mavlink_msg_scaled_pressure_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlAirData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
// clear the message
memset(&msg,0,sizeof(mavlink_message_t));
// it there has been a reboot
if(mlBoot.version == 1){
// Copy the message to the send buffer
mavlink_msg_boot_pack(SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
1);
mlBoot.version=0;
}
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 5: // Bias
mavlink_msg_sensor_bias_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlSensorBiasData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 6: // Diagnostic
mavlink_msg_diagnostic_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlDiagnosticData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 7: // Pilot Console Data
mavlink_msg_rc_channels_raw_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlPilotConsoleData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 8: // Sensor Data in meaningful units
mavlink_msg_scaled_imu_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlFilteredData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
case 9: // Raw Pressure
mavlink_msg_raw_pressure_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlRawPressureData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
break;
default:
*dataOut = 0;
break;
}
memset(&msg,0,sizeof(mavlink_message_t));
// Attitude data. Gets included every sample time
mavlink_msg_attitude_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlAttitudeData);
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
memset(&msg,0,sizeof(mavlink_message_t));
// Sensor Raw data. Gets included every sample time
mavlink_msg_raw_imu_encode( SLUGS_SYSTEMID,
SLUGS_COMPID,
&msg,
&mlRawImuData);
bytes2Send += mavlink_msg_to_send_buffer((dataOut+1+bytes2Send), &msg);
// Put the length of the message in the first byte of the outgoing array
*dataOut = bytes2Send;
// increment/overflow the samplePeriod counter
// configured for 10 Hz in non vital messages
samplePeriod = (samplePeriod >= 10)? 1: samplePeriod + 1;
// Send the data via the debug serial port
if (hilOn == 0){
send2DebugPort(dataOut, hilOn);
}
}
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();
}
}
}
void mavlinkSendHUDAndHeartbeat(void)
{
uint16_t msgLength;
float mavAltitude = 0;
float mavGroundSpeed = 0;
float mavAirSpeed = 0;
float mavClimbRate = 0;
#if defined(GPS)
// use ground speed if source available
if (sensors(SENSOR_GPS)) {
mavGroundSpeed = GPS_speed / 100.0f;
}
#endif
// select best source for altitude
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
// Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
mavAltitude = altitudeHoldGetEstimatedAltitude() / 100.0;
}
#if defined(GPS)
else if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#endif
#elif defined(GPS)
if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#endif
mavlink_msg_vfr_hud_pack(0, 200, &mavMsg,
// airspeed Current airspeed in m/s
mavAirSpeed,
// groundspeed Current ground speed in m/s
mavGroundSpeed,
// heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw),
// throttle Current throttle setting in integer percent, 0 to 100
scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
// alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate)
mavAltitude,
// climb Current climb rate in meters/second
mavClimbRate);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
if (ARMING_FLAG(ARMED))
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t mavSystemType;
switch(mixerConfig()->mixerMode)
{
case MIXER_TRI:
mavSystemType = MAV_TYPE_TRICOPTER;
break;
case MIXER_QUADP:
case MIXER_QUADX:
case MIXER_Y4:
case MIXER_VTAIL4:
mavSystemType = MAV_TYPE_QUADROTOR;
break;
case MIXER_Y6:
case MIXER_HEX6:
case MIXER_HEX6X:
mavSystemType = MAV_TYPE_HEXAROTOR;
break;
case MIXER_OCTOX8:
case MIXER_OCTOFLATP:
case MIXER_OCTOFLATX:
mavSystemType = MAV_TYPE_OCTOROTOR;
break;
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_CUSTOM_AIRPLANE:
mavSystemType = MAV_TYPE_FIXED_WING;
break;
case MIXER_HELI_120_CCPM:
case MIXER_HELI_90_DEG:
mavSystemType = MAV_TYPE_HELICOPTER;
break;
default:
mavSystemType = MAV_TYPE_GENERIC;
break;
}
// Custom mode for compatibility with APM OSDs
uint8_t mavCustomMode = 1; // Acro by default
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
mavCustomMode = 0; //Stabilize
mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE))
mavCustomMode = 2; //Alt Hold
if (FLIGHT_MODE(GPS_HOME_MODE))
mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(GPS_HOLD_MODE))
mavCustomMode = 16; //Position Hold (Earlier called Hybrid)
uint8_t mavSystemState = 0;
if (ARMING_FLAG(ARMED)) {
if (failsafeIsActive()) {
mavSystemState = MAV_STATE_CRITICAL;
}
else {
mavSystemState = MAV_STATE_ACTIVE;
}
}
else {
mavSystemState = MAV_STATE_STANDBY;
}
mavlink_msg_heartbeat_pack(0, 200, &mavMsg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
mavSystemType,
// 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
mavModes,
// custom_mode A bitfield for use for autopilot-specific flags.
mavCustomMode,
// system_status System status flag, see MAV_STATE ENUM
mavSystemState);
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
/*-----------------------------------------------------------------------------
* Service MAVlink protocol message routine
*/
void Timer1_IRQHandler() {
mavlink_message_t msg;
MadgwickAHRSupdateIMU(
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val);
mavlink_msg_heartbeat_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_TYPE_GROUND_ROVER,
MAV_AUTOPILOT_GENERIC,
mavlink_sys_mode,
0,
mavlink_sys_state);
mavlink_send_msg(&msg);
mavlink_msg_sys_status_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
50, // TODO remove hardoded values
0,
-1,
-1,
0,
0,
0,
0,
0,
0);
mavlink_send_msg(&msg);
mavlink_msg_autopilot_version_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
0, // No capabilities (MAV_PROTOCOL_CAPABILITY). TBD
42,
42,
42,
42,
"DEADBEEF",
"DEADBEEF",
"DEADBEEF",
42,
42,
42);
mavlink_send_msg(&msg);
mavlink_msg_highres_imu_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val,
params[PARAM_GX].val,
params[PARAM_GY].val,
params[PARAM_GZ].val,
params[PARAM_MX].val,
params[PARAM_MY].val,
params[PARAM_MZ].val,
0,
0,
0,
params[PARAM_T].val,
(1 << 12) | ((1 << 9) - 1));
mavlink_send_msg(&msg);
mavlink_msg_attitude_quaternion_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
q0,
q1,
q2,
q3,
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val));
mavlink_send_msg(&msg);
usec_service_routine();
MSS_TIM1_clear_irq();
}