Skip to content
Snippets Groups Projects
Commit 3a267082 authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Resolved wrong TX drop display

parent 3932bad1
No related branches found
No related tags found
No related merge requests found
......@@ -728,6 +728,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* result of the command */
uint8_t result = MAV_RESULT_UNSUPPORTED;
/* announce command handling */
ioctl(buzzer, TONE_SET_ALARM, 1);
/* supported command handling start */
......@@ -907,6 +910,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED;
usleep(200000);
/* announce command rejection */
ioctl(buzzer, TONE_SET_ALARM, 4);
}
break;
}
......
......@@ -454,6 +454,24 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
}
/*
* Internal function to give access to the channel status for each channel
*/
mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[chan];
}
/*
* Internal function to give access to the channel buffer for each channel
*/
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
{
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_buffer[chan];
}
void mavlink_update_system(void)
{
static bool initialized = false;
......
......@@ -43,9 +43,12 @@
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
//use efficient approach, see mavlink_helpers.h
/* use efficient approach, see mavlink_helpers.h */
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
#include "v1.0/mavlink_types.h"
#include <unistd.h>
......@@ -70,4 +73,7 @@ extern mavlink_system_t mavlink_system;
*/
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
#endif /* MAVLINK_BRIDGE_HEADER_H */
......@@ -12,15 +12,18 @@
/*
* Internal function to give access to the channel status for each channel
*/
#ifndef MAVLINK_GET_CHANNEL_STATUS
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[chan];
}
#endif
/*
* Internal function to give access to the channel buffer for each channel
*/
#ifndef MAVLINK_GET_CHANNEL_BUFFER
MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
{
......@@ -35,6 +38,7 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
#endif
return &m_mavlink_buffer[chan];
}
#endif
/**
* @brief Reset the status of a channel.
......
......@@ -42,7 +42,9 @@
#endif // MAVLINK_SEPARATE_HELPERS
/* always include the prototypes to ensure we don't get out of sync */
#ifndef MAVLINK_GET_CHANNEL_STATUS
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
#endif
MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment