void GazeboMavlinkInterface::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
component_ID = 0;
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* no idea which numbers should be here*/
buf[2] = 100;
buf[3] = 0;
buf[4] = component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
if (len <= 0) {
printf("Failed sending mavlink message");
}
}
bool com_checksum(uint8_t *data, int32_t length) //Gets an array, length of its payload
//and checksums on the com-ports
{
if (length < 5)
return 0;
uint16_t res = crc_calculate(data, length + 6);
crc_accumulate(crcs[data[5]], &res);
uint8_t ck_a = (uint8_t)(res & 0xFF); //< High byte
uint8_t ck_b = (uint8_t)(res >> 8); //< Low byte
if ((ck_a == (uint8_t)data[length + 6]) && (ck_b == (uint8_t)data[length + 7])) //If checksums are equal to each other, then
return true; //return true
else
return false;
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (buf_free < packet_len) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* use mavlink's internal counter for the TX seq */
buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
buf[3] = mavlink_system.sysid;
buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
pthread_mutex_unlock(&_send_mutex);
}
// send actuator controls message to Pixhawk
void send_outputs_mavlink(const uint16_t *pwm, const unsigned num_pwm)
{
// Fill up to number of outputs.
mavlink_actuator_control_target_t controls_message;
for (unsigned i = 0; i < num_pwm && i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; ++i) {
controls_message.controls[i] = pwm[i];
}
// And the rest with NAN.
for (unsigned i = _outputs.noutputs; (i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS)
&& (i < actuator_controls_s::NUM_ACTUATOR_CONTROLS); ++i) {
controls_message.controls[i] = NAN;
}
controls_message.time_usec = _controls.timestamp;
const uint8_t msgid = MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
const uint8_t component_ID = 0;
const uint8_t payload_len = mavlink_message_lengths[msgid];
const unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
// TODO FIXME: no idea which numbers should be here.
buf[2] = 100;
buf[3] = 0;
buf[4] = component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], (const void *)&controls_message, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
int ret = ::write(_fd, &buf[0], packet_len);
//static unsigned counter = 0;
//if (counter++ % 250 == 0) {
// PX4_INFO("send motor controls %d bytes %.2f %.2f %.2f %.2f",
// ret,
// controls_message.controls[0],
// controls_message.controls[1],
// controls_message.controls[2],
// controls_message.controls[3]);
//}
if (ret < 1) {
PX4_WARN("Failed sending rc mavlink message, ret: %d, errno: %d", ret, errno);
}
}
void send_rc_mavlink()
{
mavlink_rc_channels_t rc_message;
rc_message.time_boot_ms = _rc.timestamp_publication / 1000;
rc_message.chancount = _rc.channel_count;
rc_message.chan1_raw = (_rc.channel_count > 0) ? _rc.values[0] : UINT16_MAX;
rc_message.chan2_raw = (_rc.channel_count > 1) ? _rc.values[1] : UINT16_MAX;
rc_message.chan3_raw = (_rc.channel_count > 2) ? _rc.values[2] : UINT16_MAX;
rc_message.chan4_raw = (_rc.channel_count > 3) ? _rc.values[3] : UINT16_MAX;
rc_message.chan5_raw = (_rc.channel_count > 4) ? _rc.values[4] : UINT16_MAX;
rc_message.chan6_raw = (_rc.channel_count > 5) ? _rc.values[5] : UINT16_MAX;
rc_message.chan7_raw = (_rc.channel_count > 6) ? _rc.values[6] : UINT16_MAX;
rc_message.chan8_raw = (_rc.channel_count > 7) ? _rc.values[7] : UINT16_MAX;
rc_message.chan9_raw = (_rc.channel_count > 8) ? _rc.values[8] : UINT16_MAX;
rc_message.chan10_raw = (_rc.channel_count > 9) ? _rc.values[9] : UINT16_MAX;
rc_message.chan11_raw = (_rc.channel_count > 10) ? _rc.values[10] : UINT16_MAX;
rc_message.chan12_raw = (_rc.channel_count > 11) ? _rc.values[11] : UINT16_MAX;
rc_message.chan13_raw = (_rc.channel_count > 12) ? _rc.values[12] : UINT16_MAX;
rc_message.chan14_raw = (_rc.channel_count > 13) ? _rc.values[13] : UINT16_MAX;
rc_message.chan15_raw = (_rc.channel_count > 14) ? _rc.values[14] : UINT16_MAX;
rc_message.chan16_raw = (_rc.channel_count > 15) ? _rc.values[15] : UINT16_MAX;
rc_message.chan17_raw = (_rc.channel_count > 16) ? _rc.values[16] : UINT16_MAX;
rc_message.chan18_raw = (_rc.channel_count > 17) ? _rc.values[17] : UINT16_MAX;
rc_message.rssi = _rc.rssi;
const uint8_t msgid = MAVLINK_MSG_ID_RC_CHANNELS;
uint8_t component_ID = 0;
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
/* no idea which numbers should be here*/
buf[2] = 100;
buf[3] = 0;
buf[4] = component_ID;
buf[5] = msgid;
/* payload */
memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], (const void *)&rc_message, payload_len);
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
int len = ::write(_uart_fd, &buf[0], packet_len);
//static unsigned counter = 0;
//if (counter++ % 100 == 0) {
// PX4_INFO("sent %d %d %d %d %d %d", len,
// rc_message.chan1_raw,
// rc_message.chan2_raw,
// rc_message.chan3_raw,
// rc_message.chan4_raw,
// rc_message.chan5_raw);
//}
if (len < 1) {
PX4_WARN("failed sending rc mavlink message");
}
}
void mavlink_parse(){
int payload_length = 0;
uint8_t *p_checksum;
uint16_t checksum, checksum_calc;
uint8_t msg_id;
bool payload_received;
pos_front = MAVLINK_BUFFER_SIZE - __HAL_DMA_GET_COUNTER((Uart3Handle.hdmarx));
//printf("%d\r\n", sizeof(mavlink_buffer));
while(1){
payload_received = false;
if( pos_front >= pos_back ){
distance = pos_front - pos_back;
}else{
distance = MAVLINK_BUFFER_SIZE + pos_front - pos_back;
}
if( distance >= 8 ){
//printf("distance: %d\r\n", distance);
payload_length = *(mavlink_buffer + (pos_back + P_LEN)%MAVLINK_BUFFER_SIZE );
msg_id = *(mavlink_buffer + (pos_back + P_MSG)%MAVLINK_BUFFER_SIZE );
if( 0xfe == *(mavlink_buffer + pos_back) && MAVLINK_MSG_ID_ATTITUDE == msg_id ){ // search sycn code and msg_id I want
//printf("sync found: %d\r\n", pos_back);
//printf("payload_length: %d\r\n", payload_length);
//printf("msg_id: %x\r\n", msg_id);
if( payload_length + 8 <= distance ){ // a full attitude payload has been received
//printf("length enough\r\n");
// copy the payload
if( pos_back + 6 + payload_length < MAVLINK_BUFFER_SIZE ){
memcpy( msg, mavlink_buffer + pos_back + 1, payload_length + 5 );
}else{
memcpy( msg, mavlink_buffer + pos_back + 1, MAVLINK_BUFFER_SIZE - (pos_back + 1) );
memcpy( msg + MAVLINK_BUFFER_SIZE - (pos_back + 1), mavlink_buffer, (payload_length + 5) - (MAVLINK_BUFFER_SIZE - (pos_back + 1)) );
}
p_checksum = (mavlink_buffer + (pos_back + 6 + payload_length) % MAVLINK_BUFFER_SIZE);
checksum = *p_checksum + 0x100 * ( *(p_checksum + 1) );
//printf("checksum: %x\r\n", checksum);
checksum_calc = crc_calculate( msg, payload_length + 5 );
crc_accumulate(39, &checksum_calc);
//printf("checksum: %x\r\n", checksum_calc);
// crc check
if ( checksum == checksum_calc){
//printf("checksum ok\r\n");
payload_received = true;
payload = (mavlink_attitude_t *)((uint8_t *)msg + 5);
real_roll = payload->roll;
real_pitch = payload->pitch;
}else{
payload_received = false;
}
}else{
//printf("length not enough\r\n");
return; // length is not enough, so return;
}
}else{
//printf("not found\r\n");
}
}else{
return;// length is not enough, so return;
}
if( payload_received ){
pos_back = ( pos_back + payload_length + 8 ) % MAVLINK_BUFFER_SIZE;
}else{
pos_back =( pos_back + 1 ) % MAVLINK_BUFFER_SIZE;
}
}
}
