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(); }