void request_data_stream(short stream_id, short mav_speed, unsigned char mav_mode) {
mavlink_message_t smsg;
uint8_t sbuf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_request_data_stream_pack(0, MAV_COMP_ID_IMU, &smsg, 1, 0, stream_id, mav_speed, mav_mode);
uint16_t len = mavlink_msg_to_send_buffer(sbuf, &smsg);
if (write(tty_fd, sbuf, len) != len) {perror("LOOP: Es konnte kein Datenstream abbonniert werden."); exit(1);}
}
//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);
}
void baseflight_mavlink_send_message(mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
uint16_t i;
for (i = 0; i < len; i++) uartWrite(buf[i]);
}
void Mavlink_send_Test_data(uint8_t uart_id, uint8_t data){
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_test_data_pack(MAV_NUMBER, COMP_ID, &msg, data);
uint16_t length = mavlink_msg_to_send_buffer(buf, &msg);
UART_putString(uart_id, buf, length);
}
void GCS_Mavlink<T>::handle_param_set(mavlink_message_t *msg)
{
Mav_Param *param;
enum var_type vtype;
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
param = Mav_Param::get_param_by_name(packet.param_id, vtype);
if (param != NULL)
{
param->set_value(packet.param_value, vtype);
#ifdef DEBUG_ALL
Serial.print(packet.param_id);Serial.print(",");
Serial.print(param->cast_to_float_mav(vtype));Serial.print(",");
Serial.println(vtype);
#endif
mavlink_msg_param_value_pack( _systemId,
MAV_COMP_ID_ALL,
msg,
packet.param_id,
param->cast_to_float_mav(vtype),
vtype,
_param_count,
Mav_Param::get_id_by_name(packet.param_id));
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
_serial.write(buf, len);
}
}
void ofxMavlink::sendMissionLand()
{
//cout << "Send mission land" << endl;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_mission_item_t mission;
mavlink_message_t message;
mission.autocontinue = 0;
mission.current = 2; //2 for guided mode
mission.param1 = 0;
mission.param2 = 0;
mission.param3 = 0;
mission.param4 = 0;
mission.frame = 0;// MAV_FRAME_GLOBAL;
mission.command = MAV_CMD_NAV_LAND;
mission.seq = 0; // don't read out the sequence number of the waypoint class
mission.x = 0; //latitude
mission.y = 0; //longitude
mission.z = 3; //altitude
mission.target_system = 1;
mission.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_msg_mission_item_encode(sysId, compId, &message, &mission);
unsigned 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);
}
//81
void MavSerialPort::send_manual_setpoint(){
mavlink_message_t msg;
uint8_t buffer[2048];
//todo: timestamp
uint32_t time_boot_ms = 100000; ///< Timestamp in milliseconds since system boot
float roll = 0; ///< Desired roll rate in radians per second
float pitch = 0; ///< Desired pitch rate in radians per second
float yaw = 1; ///< Desired yaw rate in radians per second
float thrust = 0.2; ///< Collective thrust, normalized to 0 .. 1
uint8_t mode_switch = 0; ///< Flight mode switch position, 0.. 255
uint8_t manual_override_switch = 0; ///< Override mode switch position, 0.. 255
mavlink_msg_manual_setpoint_pack(0,0, &msg, time_boot_ms, roll, pitch, yaw, thrust, mode_switch, manual_override_switch);
// Pack a message to send it over a serial byte stream
// MAVLINK_HELPER uint16_t
// return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
//(uint8_t *buffer, const mavlink_message_t *msg)
int size = mavlink_msg_to_send_buffer(buffer, &msg);
QByteArray ba((char*)buffer,size);
write(ba);
qDebug() << "send out manual setpoint\n";
}
void Mavlink_send_ACK(uint8_t uart_id, uint8_t Message_Name){
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_mavlink_ack_pack(MAV_NUMBER, COMP_ID, &msg, Message_Name);
uint16_t length = mavlink_msg_to_send_buffer(buf, &msg);
UART_putString(uart_id, buf, length);
}
/**
* @brief -- pass-through non-needed message to specified channel
* @param chan -- channel to send (apm or gcs)
* @param msg -- the msg to be sent
* @return -- NULL
*/
void UAS_comm::bypassMessage(uint8_t chan, mavlink_message_t* pMsg){
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, pMsg);
if (chan == chan_apm){
pthread_mutex_lock(&mutex_apm_send_lock);
for (int i = 0; i < len; ++i)
{
apm_tx_buf[i] = buf[i];
}
apm_tx_buf_len = len;
pthread_cond_signal(&condition_apm_send_var);
pthread_mutex_unlock(&mutex_apm_send_lock);
}
if (chan == chan_gcs){
pthread_mutex_lock(&mutex_gcs_send_lock);
for (int i = 0; i < len; ++i)
{
gcs_tx_buf[i] = buf[i];
}
gcs_tx_buf_len = len;
pthread_cond_signal(&condition_gcs_send_var);
pthread_mutex_unlock(&mutex_gcs_send_lock);
}
}
//---------------------------------------------------------------------------------
//-- Forward message(s) to the GCS
int
MavESP8266GCS::sendMessage(mavlink_message_t* message, int count) {
int sentCount = 0;
_udp.beginPacket(_ip, _udp_port);
for(int i = 0; i < count; i++) {
// Translate message to buffer
char buf[300];
unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message[i]);
// Send it
_status.packets_sent++;
size_t sent = _udp.write((uint8_t*)(void*)buf, len);
if(sent != len) {
break;
//-- Fibble attempt at not losing data until we get access to the socket TX buffer
// status before we try to send.
_udp.endPacket();
delay(2);
_udp.beginPacket(_ip, _udp_port);
_udp.write((uint8_t*)(void*)&buf[sent], len - sent);
_udp.endPacket();
return sentCount;
}
sentCount++;
}
_udp.endPacket();
return sentCount;
}
void GCS_Mavlink<T>::send_param_list_next()
{
if (_next_id < _param_count)
{
var_type vtype;
char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
float value = Mav_Param::get_value_name_by_id(vtype, name, _next_id);
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
#ifdef DEBUG_ALL
Serial.print("param_next:");
Serial.print(name);
Serial.print(",");
Serial.print(value);
Serial.print(",");
Serial.print(vtype);
Serial.print(",");
Serial.print(_param_count);
Serial.print(",");
Serial.println(_next_id);
#endif
_next_id++;
mavlink_msg_param_value_pack( _systemId,
MAV_COMP_ID_ALL,
&msg,
name,
value,
vtype,
_param_count,
_next_id);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
}
void GCS_Mavlink<T>::handle_mission_count(mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message
uint16_t len = mavlink_msg_mission_count_pack(_systemId, 0, msg, _systemId, 0, 0);
len = mavlink_msg_to_send_buffer(buf, msg);
_serial.write(buf, len);
}
int mavlink_connector::send_msg(mavlink_message_t * msg)
{
int len = mavlink_msg_to_send_buffer((uint8_t *) buffer, msg);
write(buffer, len);
return 0;
}
void mavlinkSendRCChannelsAndRSSI(void)
{
uint16_t msgLength;
mavlink_msg_rc_channels_raw_pack(0, 200, &mavMsg,
// time_boot_ms Timestamp (milliseconds since system boot)
millis(),
// 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
(rxRuntimeConfig.channelCount >= 1) ? rcData[0] : 0,
// chan2_raw RC channel 2 value, in microseconds
(rxRuntimeConfig.channelCount >= 2) ? rcData[1] : 0,
// chan3_raw RC channel 3 value, in microseconds
(rxRuntimeConfig.channelCount >= 3) ? rcData[2] : 0,
// chan4_raw RC channel 4 value, in microseconds
(rxRuntimeConfig.channelCount >= 4) ? rcData[3] : 0,
// chan5_raw RC channel 5 value, in microseconds
(rxRuntimeConfig.channelCount >= 5) ? rcData[4] : 0,
// chan6_raw RC channel 6 value, in microseconds
(rxRuntimeConfig.channelCount >= 6) ? rcData[5] : 0,
// chan7_raw RC channel 7 value, in microseconds
(rxRuntimeConfig.channelCount >= 7) ? rcData[6] : 0,
// chan8_raw RC channel 8 value, in microseconds
(rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
scaleRange(rssi, 0, 1023, 0, 255));
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
//84
void MavSerialPort::send_position_target_local_ned(){
mavlink_message_t msg;
uint8_t buffer[2048];
uint32_t time_boot_ms = 10000; ///< Timestamp in milliseconds since system boot
float x = 1; ///< X Position in NED frame in meters
float y = 0; ///< Y Position in NED frame in meters
float z = 0; ///< Z Position in NED frame in meters (note, altitude is negative in NED)
float vx = 0; ///< X velocity in NED frame in meter / s
float vy = 1; ///< Y velocity in NED frame in meter / s
float vz = 0; ///< Z velocity in NED frame in meter / s
float afx = 0; ///< X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N
float afy = 0; ///< Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N
float afz = 1; ///< Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N
float yaw =1; ///< yaw setpoint in rad
float yaw_rate = 3; ///< yaw rate setpoint in rad/s
uint16_t type_mask = 0; ///< Bitmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 10 is set the floats afx afy afz should be interpreted as force instead of acceleration. Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate
uint8_t coordinate_frame = 0; ///
mavlink_msg_set_position_target_local_ned_pack(system_id, component_id, &msg, time_boot_ms, target_system, target_component, coordinate_frame, type_mask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
int size = mavlink_msg_to_send_buffer(buffer, &msg);
QByteArray ba((char*)buffer,size);
write(ba);
qDebug() << "send out set position target local ned\n";
}
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);
}
void mavlink_missionlib_send_message(mavlink_message_t* msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
printf("SENT %d bytes\n", bytes_sent);
}
void GCS_Mavlink<T>::send_rc_channels_scaled(mavlink_rc_channels_scaled_t &rc_channels_scaled)
{
mavlink_message_t msg;
mavlink_msg_rc_channels_scaled_encode(_systemId, MAV_COMP_ID_ALL, &msg, &rc_channels_scaled);
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
void
Mavlink::send_message(const mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
}
int
mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
return 0;
}
void MavLinkSendDst800Data(void)
{
mavlink_message_t msg;
mavlink_msg_dst800_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
waterDataStore.speed.flData, waterDataStore.temp.flData, waterDataStore.depth.flData);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
void MavLinkSendMissionRequest(uint8_t currentMissionIndex)
{
mavlink_message_t msg;
mavlink_msg_mission_request_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
groundStationSystemId, groundStationComponentId, currentMissionIndex);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
/**
* Transmit a mission acknowledgement message. The type of message is the sole argument to this
* function (see enum MAV_MISSIONRESULT).
*/
void MavLinkSendMissionAck(uint8_t type)
{
mavlink_message_t msg;
mavlink_msg_mission_ack_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
groundStationSystemId, groundStationComponentId, type);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
void GCS_Mavlink<T>::send_attitude(mavlink_attitude_t &attitude)
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_attitude_encode(_systemId, MAV_COMP_ID_ALL, &msg, &attitude);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
void MavLinkSendWindAirData(void)
{
mavlink_message_t msg;
mavlink_msg_wso100_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
windDataStore.speed.flData, windDataStore.direction.flData,
airDataStore.temp.flData, airDataStore.pressure.flData, airDataStore.humidity.flData);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
void send_package(mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PAYLOAD_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
int i;
for(i = 0; i < len; i++)
usart3_send(buf[i]);
}
void GCS_Mavlink<T>::send_scaled_imu(mavlink_scaled_imu_t &scaled_imu)
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_scaled_imu_encode(_systemId, MAV_COMP_ID_ALL, &msg, &scaled_imu);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
_serial.write(buf, len);
}
void IO::operator<<(mavlink_message_t* msg) {
int len = mavlink_msg_to_send_buffer(send_buffer, msg);
memset(&WriteOverlapped, 0, sizeof(OVERLAPPED));
if (!WriteFile(h, send_buffer, len, NULL, &WriteOverlapped)) {
if (GetLastError() != ERROR_IO_PENDING){
throw runtime_error("Write Failed");
}
}
}
static uint16_t _to_mmc_buf(void *out, uint8_t *sendbuf, mavlink_message_t *mavlink_message_struct){
uint16_t len = 0;
len = mavlink_msg_to_send_buffer(sendbuf, mavlink_message_struct);
memcpy(out, &len, sizeof(len));
out += sizeof(len);
memcpy(out, sendbuf, len);
return len + sizeof(len);
}
void MavLinkInterpreter::ConfigureConnection()
{
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tSetting up Mavlink streams\r\n", millis());
debugSerial.printf("%d\tSerial available for write? %d bytes avail\r\n", millis(), _MavLinkSerial.availableForWrite());
#endif
digitalWrite(LED_BOARD_P, HIGH);
// mavlink_msg_request_data_stream_pack(0xFF,0xBE,&_msg,1,1,MAV_DATA_STREAM_EXTENDED_STATUS, MSG_RATE, START);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_EXTENDED_STATUS, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_EXTENDED_STATUS request\r\n", millis());
#endif
delay(10);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_EXTRA2, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_EXTRA2 request\r\n", millis());
#endif
delay(10);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_RAW_SENSORS, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_RAW_SENSORS request\r\n", millis());
#endif
#ifdef USE_RC_CHANNELS
delay(10);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_RC_CHANNELS, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_RC_CHANNELS request\r\n", millis());
#endif
#endif
digitalWrite(LED_BOARD_P, LOW);
_send_mavlink_connection_config++;
}
