Esempio n. 1
0
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;
    }
}
Esempio n. 2
0
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;
}