static void uavtalk_handle_msg(struct uavtalk_message *msg)
{
mavlink_message_t mav_msg;
switch (msg->objid) {
case UAVTALK_OBJID_ATTITUDESTATE:
mavlink_msg_attitude_pack(1, 1, &mav_msg, 0,
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_ROLL)),
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_PITCH)),
DEG2RAD(uavtalk_get_float(msg, UAVTALK_OBJID_ATTITUDESTATE_YAW)),
0, 0, 0);
mavlink_handle_msg(255, &mav_msg, NULL);
break;
case UAVTALK_OBJID_MANUALCONTROLCOMMAND:
case UAVTALK_OBJID_MANUALCONTROLCOMMAND_001:
case UAVTALK_OBJID_MANUALCONTROLCOMMAND_002:
mavlink_msg_rc_channels_raw_pack(1, 1, &mav_msg, 0, 0,
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_1),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_2),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_3),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_4),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_5),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_6),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_7),
(unsigned int) uavtalk_get_int16(msg, UAVTALK_OBJID_MANUALCONTROLCOMMAND_CHANNEL_8),
0);
mavlink_handle_msg(255, &mav_msg, NULL);
break;
}
}
static unsigned int mavlink_receive(struct uart_client *cli, unsigned char *buf, unsigned int len)
{
mavlink_message_t msg __attribute__ ((aligned(2)));
mavlink_status_t status;
unsigned int i = len;
while (i--) {
if (mavlink_parse_char(cli->ch, *(buf++), &msg, &status)) {
mavlink_handle_msg(cli->ch, &msg, &status);
}
}
return len;
}