mirror of https://github.com/ArduPilot/ardupilot
40 lines
1.1 KiB
C++
40 lines
1.1 KiB
C++
|
|
||
|
|
#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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|