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 );
}
Пример #2
0
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);
}
Пример #3
0
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));
}
Пример #4
0
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;
}
Пример #5
0
//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);
}
Пример #6
0
/*****************************************************************
*	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;
}
Пример #7
0
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);

}
Пример #9
0
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);
}
Пример #10
0
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(&current_flight_mode));
	read_global_data_value(SAFTY_BUTTON, DATA_POINTER_CAST(&current_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);
}
Пример #11
0
/**
 * 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);
}
Пример #12
0
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);
}
Пример #13
0
/**
 * 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);
}
Пример #14
0
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);
}
Пример #15
0
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);
}
Пример #16
0
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);
}
Пример #17
0
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
}
Пример #18
0
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)));
    }
}
Пример #19
0
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);
}
Пример #20
0
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);
}
Пример #21
0
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;
}
Пример #22
0
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);

}
Пример #23
0
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
    }
}
Пример #24
0
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);
	}

}
Пример #25
0
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
    }
}
Пример #26
0
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);
	}
}
Пример #27
0
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();
		}
	}
}
Пример #28
0
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);
}
Пример #29
0
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
Пример #30
0
/*-----------------------------------------------------------------------------
 * 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();
}