void IO::operator<<(const string& msg) {
mavlink_message_t mavlink_msg;
static char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
strncpy(buffer, msg.c_str(), MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
mavlink_msg_statustext_pack(1, 1, &mavlink_msg, 0, buffer);
(*this) << &mavlink_msg;
}
/**
* \brief Makes this GroundLink send an info message to the GCS. Enable other UAVComponents to
* make visible whats going on to the drone operator. Should not be used intensively,
* since it consumes a significant amount of bandwidth (51 payload bytes). Messages are buffered
* and sent with 1 Hz.
*/
void GroundLink::sendInfoMessage(char* text){
// TODO We should not access the mavlink protocol here, since it is supposed to be hidden by the MAVlinkMAVion class.
// Maybe add buffer functionality to MAVLinkMAVion class instead (methods: bufferMessage(packMessageX([...])), sendMessage(packMessageX([...])) ).
static mavlink_message_t bufferMessage;
uint16_t l = mavlink_msg_statustext_pack(this->mavLink.mavlink_system.sysid,
this->mavLink.mavlink_system.compid,
&bufferMessage,
MAV_SEVERITY_INFO,
text);
this->mavLink.sendMessage(&bufferMessage);
}
char sendQGCDebugMessage(const char * dbgMessage, char severity, unsigned char* bytesToAdd, char positionStart) {
mavlink_message_t msg;
unsigned char bytes2Send = 0; // size in bytes of the mavlink packed message (return value)
mavlink_msg_statustext_pack(101,
1,
&msg,
severity,
dbgMessage);
bytes2Send = mavlink_msg_to_send_buffer(UartOutBuff, &msg);
return bytes2Send;
}
uint16_t PackTextMsg(uint8_t system_id, uint8_t component_id, unsigned char buf){
mavlink_message_t msg;
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
char vr_message[50];
memset(vr_message, 0, sizeof (vr_message));
sprintf(vr_message, "buffer %d",buf);
mavlink_msg_statustext_pack(mavlink_system.sysid,
mavlink_system.compid,
&msg,
0,
vr_message);
return(mavlink_msg_to_send_buffer(UartOutBuff, &msg));
}
//---------------------------------------------------------------------------------
//-- Send Debug Message
void
MavESP8266Component::_sendStatusMessage(MavESP8266Bridge* sender, uint8_t type, const char* text)
{
if(!getWorld()->getParameters()->getDebugEnabled() && type == MAV_SEVERITY_DEBUG) {
return;
}
//-- Build message
mavlink_message_t msg;
mavlink_msg_statustext_pack(
getWorld()->getVehicle()->systemID(),
MAV_COMP_ID_UDP_BRIDGE,
&msg,
type,
text
);
sender->sendMessage(&msg);
}
void ground_station_task(void)
{
uint32_t delay_t =(uint32_t) 50.0/(1000.0 / configTICK_RATE_HZ);
uint32_t cnt = 0;
uint8_t msg_buff[50];
mavlink_message_t msg;
while(1) {
if(cnt == 8) {
send_heartbeat_info();
send_gps_info();
//send_system_info();
cnt = 0;
}
if(cnt == 5) {
sprintf((char *)msg_buff, "Zd:%ld NAV: %d,%d,%ld,%ld",
__altitude_Zd,
__nav_roll,
__nav_pitch,
__pAcc,
__numSV);
mavlink_msg_statustext_pack(1,
0,
&msg,
0,
(const char *) &msg_buff);
//send_package(&msg);
}
send_attitude_info();
send_reached_waypoint();
send_current_waypoint();
vTaskDelay(delay_t);
mavlink_parse_received_cmd(&received_msg);
cnt++;
}
}
