You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
39 lines
1.1 KiB
39 lines
1.1 KiB
|
|
#include <AP_HAL.h> |
|
|
|
#include "upstream.h" |
|
#include "state.h" |
|
|
|
extern const AP_HAL::HAL& hal; |
|
extern mavlink_channel_t upstream_channel; |
|
extern FMStateMachine sm; |
|
|
|
static void upstream_handle_command_long(mavlink_message_t* msg) __attribute__((noinline)); |
|
static void upstream_handle_command_long(mavlink_message_t* msg) { |
|
mavlink_command_long_t pkt; |
|
mavlink_msg_command_long_decode(msg, &pkt); |
|
sm.on_upstream_command_long(&pkt); |
|
} |
|
|
|
static void upstream_handle_set_mode(mavlink_message_t* msg) __attribute__((noinline)); |
|
static void upstream_handle_set_mode(mavlink_message_t* msg) { |
|
mavlink_set_mode_t pkt; |
|
mavlink_msg_set_mode_decode(msg, &pkt); |
|
sm.on_upstream_set_mode(&pkt); |
|
} |
|
|
|
void upstream_handler(mavlink_channel_t from, mavlink_message_t* msg) { |
|
switch (msg->msgid) { |
|
case MAVLINK_MSG_ID_COMMAND_LONG: |
|
upstream_handle_command_long(msg); |
|
_mavlink_resend_uart(upstream_channel, msg); |
|
break; |
|
case MAVLINK_MSG_ID_SET_MODE: |
|
upstream_handle_set_mode(msg); |
|
_mavlink_resend_uart(upstream_channel, msg); |
|
break; |
|
default: |
|
_mavlink_resend_uart(upstream_channel, msg); |
|
} |
|
} |
|
|
|
|