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.
41 lines
1.2 KiB
41 lines
1.2 KiB
|
|
|
|
#include <AP_HAL.h> |
|
|
|
#include "downstream.h" |
|
#include "state.h" |
|
|
|
extern const AP_HAL::HAL& hal; |
|
extern mavlink_channel_t downstream_channel; |
|
|
|
extern FMStateMachine sm; |
|
|
|
static void downstream_handle_heartbeat(mavlink_message_t* msg) __attribute__((noinline)); |
|
static void downstream_handle_heartbeat(mavlink_message_t* msg) { |
|
mavlink_heartbeat_t pkt; |
|
mavlink_msg_heartbeat_decode(msg, &pkt); |
|
sm.on_downstream_heartbeat(&pkt); |
|
} |
|
|
|
static void downstream_handle_gps(mavlink_message_t* msg) __attribute__((noinline)); |
|
static void downstream_handle_gps(mavlink_message_t* msg) { |
|
mavlink_gps_raw_int_t pkt; |
|
mavlink_msg_gps_raw_int_decode(msg, &pkt); |
|
sm.on_downstream_gps_raw_int(&pkt); |
|
} |
|
|
|
void downstream_handler(mavlink_channel_t from, mavlink_message_t* msg) { |
|
switch (msg->msgid) { |
|
case MAVLINK_MSG_ID_HEARTBEAT: |
|
downstream_handle_heartbeat(msg); |
|
_mavlink_resend_uart(downstream_channel, msg); |
|
break; |
|
case MAVLINK_MSG_ID_GPS_RAW_INT: |
|
downstream_handle_gps(msg); |
|
_mavlink_resend_uart(downstream_channel, msg); |
|
break; |
|
default: |
|
_mavlink_resend_uart(downstream_channel, msg); |
|
} |
|
} |
|
|
|
|