Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1702 | - | 1 | #define MAVLINK_COMM_NUM_BUFFERS 1 |
2 | #define MAVLINK_USE_CONVENIENCE_FUNCTIONS |
||
3 | |||
4 | // this code was moved from libraries/GCS_MAVLink to allow compile |
||
5 | // time selection of MAVLink 1.0 |
||
6 | BetterStream *mavlink_comm_0_port; |
||
7 | BetterStream *mavlink_comm_1_port; |
||
8 | |||
9 | mavlink_system_t mavlink_system = {12,1,0,0}; |
||
10 | |||
11 | #include "Mavlink_compat.h" |
||
12 | |||
13 | #ifdef MAVLINK10 |
||
14 | #include "../GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h" |
||
15 | #include "../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h" |
||
16 | #else |
||
17 | #include "../GCS_MAVLink/include/mavlink/v0.9/mavlink_types.h" |
||
18 | #include "../GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink.h" |
||
19 | #endif |
||
20 | |||
21 | // true when we have received at least 1 MAVLink packet |
||
22 | static bool mavlink_active; |
||
23 | static uint8_t crlf_count = 0; |
||
24 | |||
25 | static int packet_drops = 0; |
||
26 | static int parse_error = 0; |
||
27 | |||
28 | void request_mavlink_rates() |
||
29 | { |
||
30 | const int maxStreams = 6; |
||
31 | const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_RAW_SENSORS, |
||
32 | MAV_DATA_STREAM_EXTENDED_STATUS, |
||
33 | MAV_DATA_STREAM_RC_CHANNELS, |
||
34 | MAV_DATA_STREAM_POSITION, |
||
35 | MAV_DATA_STREAM_EXTRA1, |
||
36 | MAV_DATA_STREAM_EXTRA2}; |
||
37 | const uint16_t MAVRates[maxStreams] = {0x02, 0x02, 0x05, 0x02, 0x05, 0x02}; |
||
38 | for (int i=0; i < maxStreams; i++) { |
||
39 | mavlink_msg_request_data_stream_send(MAVLINK_COMM_0, |
||
40 | apm_mav_system, apm_mav_component, |
||
41 | MAVStreams[i], MAVRates[i], 1); |
||
42 | } |
||
43 | } |
||
44 | |||
45 | void read_mavlink(){ |
||
46 | mavlink_message_t msg; |
||
47 | mavlink_status_t status; |
||
48 | |||
49 | //grabing data |
||
50 | while(Serial.available() > 0) { |
||
51 | uint8_t c = Serial.read(); |
||
52 | |||
53 | /* allow CLI to be started by hitting enter 3 times, if no |
||
54 | heartbeat packets have been received */ |
||
55 | if (mavlink_active == 0 && millis() < 20000) { |
||
56 | if (c == '\n' || c == '\r') { |
||
57 | crlf_count++; |
||
58 | } else { |
||
59 | crlf_count = 0; |
||
60 | } |
||
61 | if (crlf_count == 3) { |
||
62 | uploadFont(); |
||
63 | } |
||
64 | } |
||
65 | |||
66 | //trying to grab msg |
||
67 | if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { |
||
68 | mavlink_active = 1; |
||
69 | //handle msg |
||
70 | switch(msg.msgid) { |
||
71 | case MAVLINK_MSG_ID_HEARTBEAT: |
||
72 | { |
||
73 | mavbeat = 1; |
||
74 | apm_mav_system = msg.sysid; |
||
75 | apm_mav_component = msg.compid; |
||
76 | apm_mav_type = mavlink_msg_heartbeat_get_type(&msg); |
||
77 | #ifdef MAVLINK10 |
||
78 | osd_mode = mavlink_msg_heartbeat_get_custom_mode(&msg); |
||
79 | osd_nav_mode = 0; |
||
80 | #endif |
||
81 | lastMAVBeat = millis(); |
||
82 | if(waitingMAVBeats == 1){ |
||
83 | enable_mav_request = 1; |
||
84 | } |
||
85 | } |
||
86 | break; |
||
87 | case MAVLINK_MSG_ID_SYS_STATUS: |
||
88 | { |
||
89 | #ifndef MAVLINK10 |
||
90 | osd_vbat_A = (mavlink_msg_sys_status_get_vbat(&msg) / 1000.0f); |
||
91 | osd_mode = mavlink_msg_sys_status_get_mode(&msg); |
||
92 | osd_nav_mode = mavlink_msg_sys_status_get_nav_mode(&msg); |
||
93 | #else |
||
94 | osd_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f); |
||
95 | #endif |
||
96 | osd_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg); |
||
97 | //osd_mode = apm_mav_component;//Debug |
||
98 | //osd_nav_mode = apm_mav_system;//Debug |
||
99 | } |
||
100 | break; |
||
101 | #ifndef MAVLINK10 |
||
102 | case MAVLINK_MSG_ID_GPS_RAW: |
||
103 | { |
||
104 | osd_lat = mavlink_msg_gps_raw_get_lat(&msg); |
||
105 | osd_lon = mavlink_msg_gps_raw_get_lon(&msg); |
||
106 | osd_fix_type = mavlink_msg_gps_raw_get_fix_type(&msg); |
||
107 | } |
||
108 | break; |
||
109 | case MAVLINK_MSG_ID_GPS_STATUS: |
||
110 | { |
||
111 | osd_satellites_visible = mavlink_msg_gps_status_get_satellites_visible(&msg); |
||
112 | } |
||
113 | break; |
||
114 | #else |
||
115 | case MAVLINK_MSG_ID_GPS_RAW_INT: |
||
116 | { |
||
117 | osd_lat = mavlink_msg_gps_raw_int_get_lat(&msg) / 10000000.0f; |
||
118 | osd_lon = mavlink_msg_gps_raw_int_get_lon(&msg) / 10000000.0f; |
||
119 | osd_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg); |
||
120 | osd_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg); |
||
121 | } |
||
122 | break; |
||
123 | #endif |
||
124 | |||
125 | case MAVLINK_MSG_ID_VFR_HUD: |
||
126 | { |
||
127 | osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); |
||
128 | osd_heading = mavlink_msg_vfr_hud_get_heading(&msg);// * 3.60f;//0-100% of 360 |
||
129 | osd_throttle = mavlink_msg_vfr_hud_get_throttle(&msg); |
||
130 | if(osd_throttle > 100 && osd_throttle < 150) osd_throttle = 100;//Temporary fix for ArduPlane 2.28 |
||
131 | if(osd_throttle < 0 || osd_throttle > 150) osd_throttle = 0;//Temporary fix for ArduPlane 2.28 |
||
132 | osd_alt = mavlink_msg_vfr_hud_get_alt(&msg); |
||
133 | } |
||
134 | break; |
||
135 | case MAVLINK_MSG_ID_ATTITUDE: |
||
136 | { |
||
137 | osd_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg)); |
||
138 | osd_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg)); |
||
139 | osd_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg)); |
||
140 | } |
||
141 | break; |
||
142 | default: |
||
143 | //Do nothing |
||
144 | break; |
||
145 | } |
||
146 | } |
||
147 | delayMicroseconds(138); |
||
148 | //next one |
||
149 | } |
||
150 | // Update global packet drops counter |
||
151 | packet_drops += status.packet_rx_drop_count; |
||
152 | parse_error += status.parse_error; |
||
153 | |||
154 | } |