PageRenderTime 58ms CodeModel.GetById 27ms RepoModel.GetById 0ms app.codeStats 1ms

/software/onboard/aqv5/aq_mavlink.c

http://autoquad.googlecode.com/
C | 586 lines | 460 code | 83 blank | 43 comment | 74 complexity | 2ff90888f7c250489142187d87f0fe91 MD5 | raw file
Possible License(s): GPL-3.0 
    

  1. /*
    2. 

  2.     This file is part of AutoQuad.
    3. 

    4. 

  4.     AutoQuad is free software: you can redistribute it and/or modify
    5. 

  5.     it under the terms of the GNU General Public License as published by
    6. 

  6.     the Free Software Foundation, either version 3 of the License, or
    7. 

  7.     (at your option) any later version.
    8. 

    9. 

  9.     AutoQuad is distributed in the hope that it will be useful,
    10. 

  10.     but WITHOUT ANY WARRANTY; without even the implied warranty of
    11. 

  11.     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    12. 

  12.     GNU General Public License for more details.
    13. 

  13.     You should have received a copy of the GNU General Public License
    14. 

  14.     along with AutoQuad.  If not, see <http://www.gnu.org/licenses/>.
    15. 

    16. 

  16.     Copyright Š 2011  Bill Nesbitt
    17. 

  17. */
    18. 

  18. 

  19. #include "aq_mavlink.h"
    20. 

  20. #include "mavlink.h"
    21. 

  21. #include "config.h"
    22. 

  22. #include "timer.h"
    23. 

  23. #include "adc.h"
    24. 

  24. #include "spektrum.h"
    25. 

  25. #include "quat.h"
    26. 

  26. #include "gps.h"
    27. 

  27. #include "flash.h"
    28. 

  28. #include "motors.h"
    29. 

  29. #include "control.h"
    30. 

  30. #include "nav.h"
    31. 

  31. #include "math.h"
    32. 

  32. #include <string.h>
    33. 

  33. 

  34. mavlinkStruct_t mavlinkData;
    35. 

  35. mavlink_system_t mavlink_system;
    36. 

  36. 

  37. unsigned mavlinkSendTaskStack[1+STACKSIZE+1];
    38. 

  38. unsigned mavlinkRecvTaskStack[1+STACKSIZE+1];
    39. 

  39. 

  40. void comm_send_ch(mavlink_channel_t chan, uint8_t ch) {
    41. 

  41.     serialWrite(mavlinkData.serialPort, ch);
    42. 

  42. }
    43. 

  43. 

  44. void mavlinkNotice(const char *s) {
    45. 

  45.     // queue message, notify and leave
    46. 

  46.     ctl_message_queue_post_nb(&mavlinkData.notices, (void *)s);
    47. 

  47.     ctl_events_pulse(&publicEvents, AQ_TELEM_RDY);
    48. 

  48. }
    49. 

  49. 

  50. void mavlinkSendTaskCode(void *unused) {
    51. 

  51.     serialPort_t *s = mavlinkData.serialPort;
    52. 

  52.     void *msg;
    53. 

  53.     long radioHits, radioMisses;
    54. 

  54.     unsigned long mavCounter;
    55. 

  55.     unsigned long micros;
    56. 

  56.     unsigned long lastMicros = 0;
    57. 

  57. 

  58.     AQ_NOTICE("Mavlink send task started...\n");
    59. 

  59. 

  60.     while (1) {
    61. 

  61. 	ctl_events_wait(CTL_EVENT_WAIT_ANY_EVENTS, &publicEvents, AQ_TELEM_RDY, CTL_TIMEOUT_DELAY, 100);	// 10Hz minimum
    62. 

  62. 

  63. 	micros = timerMicros();
    64. 

  64. 	mavCounter = counter;
    65. 

  65. 

  66. 	// handle rollover
    67. 

  67. 	if (micros < lastMicros) {
    68. 

  68. 	    mavlinkData.nextHeartbeat = 0;
    69. 

  69. 	    mavlinkData.nextParam = 0;
    70. 

  70. 	    memset(mavlinkData.streamNext, 0, sizeof(mavlinkData.streamInterval));
    71. 

  71. 	}
    72. 

  72. 	lastMicros = micros;
    73. 

  73. 

  74. 	// calculate radio reception quality
    75. 

  75. 	if (spektrumData.frameCount > radioHits)
    76. 

  76. 	    mavlinkData.radioQuality = (spektrumData.frameCount - radioHits) * 255 /
    77. 

  77. 		((spektrumData.frameCount - radioHits) + (spektrumData.errorCount - radioMisses));
    78. 

  78. 	else
    79. 

  79. 	    if (mavlinkData.radioQuality)
    80. 

  80. 		mavlinkData.radioQuality--;
    81. 

  81. 

  82. 	radioHits = spektrumData.frameCount;
    83. 

  83. 	radioMisses = spektrumData.errorCount;
    84. 

  84. 

  85. 	// calculate idle time
    86. 

  86. 	mavlinkData.idlePercent = (mavCounter - mavlinkData.lastCounter) * 1000 * (1e6 / 2 / adcData.sampleTime) / (72e6 / minCycles);
    87. 

  87. 	mavlinkData.lastCounter = mavCounter;
    88. 

  88. 

  89. 	ctl_mutex_lock(&mavlinkData.serialPortMutex, CTL_TIMEOUT_NONE, 0);
    90. 

  90. 

  91. 	// heartbeat & status
    92. 

  92. 	if (mavlinkData.nextHeartbeat < micros) {
    93. 

  93. 	    mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, MAV_AUTOPILOT_GENERIC);
    94. 

  94. 	    mavlink_msg_sys_status_send(MAVLINK_COMM_0, mavlinkData.mode, mavlinkData.nav_mode, mavlinkData.status, 1000-mavlinkData.idlePercent, AQ_VIN * 1000, (AQ_VIN - 9.8f) / 12.6f * 1000, mavlinkData.packetDrops);
    95. 

  95. 	    mavlinkData.nextHeartbeat = micros + MAVLINK_HEARTBEAT_INTERVAL;
    96. 

  96. 	}
    97. 

  97. 	// raw sensors
    98. 

  98. 	else if ((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL] || mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_SENSORS]) && mavlinkData.streamNext[MAV_DATA_STREAM_RAW_SENSORS] < micros) {
    99. 

  99. 	    mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, micros, AQ_ACCX*1000.0, AQ_ACCY*1000.0, AQ_ACCZ*1000.0, AQ_RATEX*1000.0, AQ_RATEY*1000.0, AQ_RATEZ*1000.0, AQ_MAGX*1000.0, AQ_MAGY*1000.0, AQ_MAGZ*1000.0);
    100. 

  100. 	    mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, micros, AQ_PRESSURE*0.01f, 0.0f, AQ_TEMP*100);
    101. 

  101. 	    mavlinkData.streamNext[MAV_DATA_STREAM_RAW_SENSORS] = micros + mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_SENSORS];
    102. 

  102. 	}
    103. 

  103. 	// position
    104. 

  104. 	else if ((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL] || mavlinkData.streamInterval[MAV_DATA_STREAM_POSITION]) && mavlinkData.streamNext[MAV_DATA_STREAM_POSITION] < micros) {
    105. 

  105. 	    mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, micros, 3, gpsData.lat*1e7, gpsData.lon*1e7, gpsData.height*1e3, gpsData.hDOP, gpsData.vDOP, gpsData.speed, gpsData.heading);
    106. 

  106. //	    mavlink_msg_gps_raw_send(MAVLINK_COMM_0, micros, 3, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hDOP, gpsData.vDOP, gpsData.speed, gpsData.heading);
    107. 

  107. 	    mavlinkData.streamNext[MAV_DATA_STREAM_POSITION] = micros + mavlinkData.streamInterval[MAV_DATA_STREAM_POSITION];
    108. 

  108. 	}
    109. 

  109. 	// extended status
    110. 

  110. //	else if ((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL] || mavlinkData.streamInterval[MAV_DATA_STREAM_EXTENDED_STATUS]) && mavlinkData.streamNext[MAV_DATA_STREAM_EXTENDED_STATUS] < micros) {
    111. 

  111. //	    mavlinkData.streamNext[MAV_DATA_STREAM_EXTENDED_STATUS] = micros + mavlinkData.streamInterval[MAV_DATA_STREAM_EXTENDED_STATUS];
    112. 

  112. //	}
    113. 

  113. 	// rc channels
    114. 

  114. 	else if ((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL] || mavlinkData.streamInterval[MAV_DATA_STREAM_RC_CHANNELS]) && mavlinkData.streamNext[MAV_DATA_STREAM_RC_CHANNELS] < micros) {
    115. 

  115. 	    mavlink_msg_rc_channels_raw_send(MAVLINK_COMM_0, SPEKTRUM_THROT+1024, SPEKTRUM_ROLL+1024, SPEKTRUM_PITCH+1024, SPEKTRUM_RUDD+1024, SPEKTRUM_GEAR+1024, SPEKTRUM_FLAPS+1024, SPEKTRUM_AUX2+1024, SPEKTRUM_AUX3+1024, mavlinkData.radioQuality);
    116. 

  116. 	    mavlink_msg_rc_channels_scaled_send(MAVLINK_COMM_0, (SPEKTRUM_THROT-750)*13, SPEKTRUM_ROLL*13, SPEKTRUM_PITCH*13, SPEKTRUM_RUDD*13, SPEKTRUM_GEAR*13, SPEKTRUM_FLAPS*13, SPEKTRUM_AUX2*13, SPEKTRUM_AUX3*13, mavlinkData.radioQuality);
    117. 

  117. 	    mavlinkData.streamNext[MAV_DATA_STREAM_RC_CHANNELS] = micros + mavlinkData.streamInterval[MAV_DATA_STREAM_RC_CHANNELS];
    118. 

  118. 	}
    119. 

  119. 	// raw controller
    120. 

  120. 	else if ((mavlinkData.streamInterval[MAV_DATA_STREAM_ALL] || mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_CONTROLLER]) && mavlinkData.streamNext[MAV_DATA_STREAM_RAW_CONTROLLER] < micros) {
    121. 

  121. 	    mavlink_msg_attitude_send(MAVLINK_COMM_0, micros, -AQ_ROLL*DEG_TO_RAD, AQ_PITCH*DEG_TO_RAD, AQ_YAW*DEG_TO_RAD, -(AQ_RATEX - quatData.rateBias[0])*DEG_TO_RAD, (AQ_RATEY - quatData.rateBias[0])*DEG_TO_RAD, (AQ_RATEZ - quatData.rateBias[0])*DEG_TO_RAD);
    122. 

  122. 	    mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, motorsData.m1, motorsData.m2, motorsData.m3, motorsData.m4, motorsData.m5, motorsData.m6, motorsData.m7, motorsData.m8);
    123. 

  123. 	    mavlinkData.streamNext[MAV_DATA_STREAM_RAW_CONTROLLER] = micros + mavlinkData.streamInterval[MAV_DATA_STREAM_RAW_CONTROLLER];
    124. 

  124. 	}
    125. 

  125. 

  126. 	// send pending notices
    127. 

  127. 	if (ctl_message_queue_receive_nb(&mavlinkData.notices, &msg) != 0)
    128. 

  128. 		mavlink_msg_statustext_send(MAVLINK_COMM_0, 0, (const int8_t*)msg);
    129. 

  129. 

  130. 	// list all parameters
    131. 

  131. 	if (mavlinkData.currentParam < mavlinkData.numParams && mavlinkData.nextParam < micros) {
    132. 

  132. 	    mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlinkData.paramIds[mavlinkData.currentParam], *mavlinkData.paramValues[mavlinkData.currentParam], mavlinkData.numParams, mavlinkData.currentParam++);
    133. 

  133. 	    mavlinkData.nextParam = micros + MABLINK_PARAM_INTERVAL;
    134. 

  134. 	}
    135. 

  135. 

  136. 	ctl_mutex_unlock(&mavlinkData.serialPortMutex);
    137. 

  137.     }
    138. 

  138. }
    139. 

  139. 

  140. void mavlinkTakeAction(mavlink_message_t *msg) {
    141. 

  141.     uint8_t action;
    142. 

  142.     uint8_t ack = 0;
    143. 

  143. 

  144.     action = mavlink_msg_action_get_action(msg);
    145. 

  145. 

  146.     switch (action) {
    147. 

  147. 	case MAV_ACTION_STORAGE_READ:
    148. 

  148. 	    if (!controlData.flying) {
    149. 

  149. 		configFlashRead();
    150. 

  150. 		ack = 1;
    151. 

  151. 	    }
    152. 

  152. 	    break;
    153. 

  153. 

  154. 	case MAV_ACTION_STORAGE_WRITE:
    155. 

  155. 	    if (!controlData.flying && configFlashWrite())
    156. 

  156. 		ack = 1;
    157. 

  157. 	    break;
    158. 

  158. 

  159. 	default:
    160. 

  160. 	    break;
    161. 

  161.     }
    162. 

  162. 

  163.     mavlink_msg_action_ack_send(MAVLINK_COMM_0, action, ack);
    164. 

  164. }
    165. 

  165. 

  166. void mavlinkRecvTaskCode(void *unused) {
    167. 

  167.     serialPort_t *s = mavlinkData.serialPort;
    168. 

  168.     mavlink_message_t msg;
    169. 

  169.     mavlink_status_t status;
    170. 

  170.     int8_t paramId[16];
    171. 

  171.     uint8_t c;
    172. 

  172. 

  173.     AQ_NOTICE("Mavlink receive task started...\n");
    174. 

  174. 

  175.     while (1) {
    176. 

  176. 	ctl_events_wait(CTL_EVENT_WAIT_ANY_EVENTS, &serialAvailableEvent, (1<<s->waitFlag), CTL_TIMEOUT_NONE, 0);
    177. 

  177. 

  178. 	// process incoming data
    179. 

  179. 	while (serialAvailable(s)) {
    180. 

  180. 	    c = serialRead(s);
    181. 

  181. 	    // Try to get a new message
    182. 

  182. 	    if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
    183. 

  183. 		ctl_mutex_lock(&mavlinkData.serialPortMutex, CTL_TIMEOUT_NONE, 0);
    184. 

  184. 		digitalHi(mavlinkData.debugLed);
    185. 

  185. 

  186. 		// Handle message
    187. 

  187. 		switch(msg.msgid) {
    188. 

  188. 		    // TODO: finish this block
    189. 

  189. 		    case MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL:
    190. 

  190. 			if (mavlink_msg_set_mode_get_target(&msg) == mavlink_system.sysid) {
    191. 

  191. 			    mavlink_msg_change_operator_control_ack_send(MAVLINK_COMM_0, msg.sysid, mavlink_msg_change_operator_control_get_control_request(&msg), 0);
    192. 

  192. 			}
    193. 

  193. 			break;
    194. 

  194. 

  195. 		    case MAVLINK_MSG_ID_SET_MODE:
    196. 

  196. 			if (mavlink_msg_set_mode_get_target(&msg) == mavlink_system.sysid) {
    197. 

  197. 			    mavlinkData.mode = mavlink_msg_set_mode_get_mode(&msg);
    198. 

  198. 			    mavlink_msg_sys_status_send(MAVLINK_COMM_0, mavlinkData.mode, mavlinkData.nav_mode, mavlinkData.status, mavlinkData.idlePercent, AQ_VIN * 1000, 500, mavlinkData.packetDrops);
    199. 

  199. 			}
    200. 

  200. 			break;
    201. 

  201. 

  202. 		    case MAVLINK_MSG_ID_ACTION:
    203. 

  203. 			if (mavlink_msg_action_get_target(&msg) == mavlink_system.sysid) {
    204. 

  204. 			    mavlinkTakeAction(&msg);
    205. 

  205. 			}
    206. 

  206. 			break;
    207. 

  207. 

  208. 		    case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
    209. 

  209. 			if (mavlink_msg_waypoint_request_list_get_target_system(&msg) == mavlink_system.sysid) {
    210. 

  210. 			    mavlink_msg_waypoint_count_send(MAVLINK_COMM_0, msg.sysid, msg.compid, navGetWaypointCount());
    211. 

  211. 			}
    212. 

  212. 			break;
    213. 

  213. 

  214. 		    case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
    215. 

  215. 			if (mavlink_msg_waypoint_clear_all_get_target_system(&msg) == mavlink_system.sysid) {
    216. 

  216. 			    mavlink_msg_waypoint_ack_send(MAVLINK_COMM_0, mavlink_system.sysid, mavlink_system.compid, navClearWaypoints());
    217. 

  217. 			}
    218. 

  218. 			break;
    219. 

  219. 

  220. 		    case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
    221. 

  221. 			if (mavlink_msg_waypoint_request_get_target_system(&msg) == mavlink_system.sysid) {
    222. 

  222. 			    uint16_t seqId;
    223. 

  223. 			    navMission_t *wp;
    224. 

  224. 

  225. 			    seqId = mavlink_msg_waypoint_request_get_seq(&msg);
    226. 

  226. 			    wp = navGetWaypoint(seqId);
    227. 

  227. 			    mavlink_msg_waypoint_send(MAVLINK_COMM_0, msg.sysid, msg.compid,
    228. 

  228. 				seqId, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 0, 1,
    229. 

  229. 				wp->targetRadius, wp->loiterTime, 0.0, wp->poiHeading, wp->targetLat, wp->targetLon, wp->targetAlt);
    230. 

  230. 			}
    231. 

  231. 			break;
    232. 

  232. 

  233. 		    case MAVLINK_MSG_ID_WAYPOINT_COUNT:
    234. 

  234. 			if (mavlink_msg_waypoint_count_get_target_system(&msg) == mavlink_system.sysid) {
    235. 

  235. 			    mavlink_msg_waypoint_ack_send(MAVLINK_COMM_0, mavlink_system.sysid, mavlink_system.compid, 0);
    236. 

  236. 			}
    237. 

  237. 			break;
    238. 

  238. 

  239. 		    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
    240. 

  240. 			if (mavlink_msg_param_request_read_get_target_system(&msg) == mavlink_system.sysid) {
    241. 

  241. 			    uint16_t paramIndex;
    242. 

  242. 

  243. 			    paramIndex = mavlink_msg_param_request_read_get_param_index(&msg);
    244. 

  244. 			    if (paramIndex < mavlinkData.numParams)
    245. 

  245. 				mavlink_msg_param_value_send(MAVLINK_COMM_0, mavlinkData.paramIds[paramIndex], *mavlinkData.paramValues[paramIndex], mavlinkData.numParams, paramIndex);
    246. 

  246. 			}
    247. 

  247. 			break;
    248. 

  248. 

  249. 		    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
    250. 

  250. 			if (mavlink_msg_param_request_list_get_target_system(&msg) == mavlink_system.sysid) {
    251. 

  251. 			    mavlinkData.currentParam = 0;
    252. 

  252. 			    mavlinkData.nextParam = 0;
    253. 

  253. 			}
    254. 

  254. 			break;
    255. 

  255. 

  256. 		    case MAVLINK_MSG_ID_PARAM_SET:
    257. 

  257. 			if (mavlink_msg_param_set_get_target_system(&msg) == mavlink_system.sysid) {
    258. 

  258. 			    int paramIndex = -1;
    259. 

  259. 			    float paramValue;
    260. 

  260. 			    int i, j;
    261. 

  261. 

  262. 			    mavlink_msg_param_set_get_param_id(&msg, paramId);
    263. 

  263. 			    for (i = 0; i < mavlinkData.numParams; i++) {
    264. 

  264. 				for (j = 0; j < MAVLINK_PARAMID_LEN; j++)
    265. 

  265. 				    if (paramId[j] != mavlinkData.paramIds[i][j] || paramId[j] == 0)
    266. 

  266. 					break;
    267. 

  267. 				if (paramId[j] == 0) {
    268. 

  268. 				    paramIndex = i;
    269. 

  269. 				    break;
    270. 

  270. 				}
    271. 

  271. 			    }
    272. 

  272. 			    if (paramIndex >= 0 && paramIndex < mavlinkData.numParams) {
    273. 

  273. 				paramValue = mavlink_msg_param_set_get_param_value(&msg);
    274. 

  274. 				if (!isnan(paramValue) && !isinf(paramValue) && !controlData.flying)
    275. 

  275. 				    *mavlinkData.paramValues[i] = paramValue;
    276. 

  276. 				// send back what we have no matter what
    277. 

  277. 				mavlink_msg_param_value_send(MAVLINK_COMM_0, paramId, *mavlinkData.paramValues[i], mavlinkData.numParams, i);
    278. 

  278. 			    }
    279. 

  279. 			}
    280. 

  280. 			break;
    281. 

  281. 

  282. 		    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
    283. 

  283. 			if (mavlink_msg_request_data_stream_get_target_system(&msg) == mavlink_system.sysid) {
    284. 

  284. 			    uint16_t rate;
    285. 

  285. 			    uint8_t stream_id;
    286. 

  286. 

  287. 			    stream_id = mavlink_msg_request_data_stream_get_req_stream_id(&msg);
    288. 

  288. 			    rate = mavlink_msg_request_data_stream_get_req_message_rate(&msg);
    289. 

  289. 

  290. 			    if (rate > 0 && rate < 200 && stream_id < MAV_DATA_STREAM_ENUM_END && mavlink_msg_request_data_stream_get_start_stop(&msg)) {
    291. 

  291. 				mavlinkData.streamInterval[stream_id] = 1e6 / rate;
    292. 

  292. 			    }
    293. 

  293. 			    else {
    294. 

  294. 				mavlinkData.streamInterval[stream_id] = 0;
    295. 

  295. 			    }
    296. 

  296. 			}
    297. 

  297. 			break;
    298. 

  298. 

  299. 		    default:
    300. 

  300. 			//Do nothing
    301. 

  301. 			break;
    302. 

  302. 		}
    303. 

  303. 

  304. 		digitalLo(mavlinkData.debugLed);
    305. 

  305. 		ctl_mutex_unlock(&mavlinkData.serialPortMutex);
    306. 

  306. 	    }
    307. 

  307. 

  308. 	    // Update global packet drops counter
    309. 

  309. 	    mavlinkData.packetDrops += status.packet_rx_drop_count;
    310. 

  310. 	}
    311. 

  311.     }
    312. 

  312. }
    313. 

  313. 

  314. int mavlinkInitParams(void) {
    315. 

  315.     int i = 0;
    316. 

  316. 

  317.     mavlinkData.paramIds[i] = "CTRL_FACT_THRO"; mavlinkData.paramValues[i] = &configData.controlThroFactor; i++;
    318. 

  318.     mavlinkData.paramIds[i] = "CTRL_FACT_PITC"; mavlinkData.paramValues[i] = &configData.controlPitchFactor; i++;
    319. 

  319.     mavlinkData.paramIds[i] = "CTRL_FACT_ROLL"; mavlinkData.paramValues[i] = &configData.controlRollFactor; i++;
    320. 

  320.     mavlinkData.paramIds[i] = "CTRL_FACT_RUDD"; mavlinkData.paramValues[i] = &configData.controlRuddFactor; i++;
    321. 

  321.     mavlinkData.paramIds[i] = "CTRL_DEAD_BAND"; mavlinkData.paramValues[i] = &configData.controlDeadBand; i++;
    322. 

  322.     mavlinkData.paramIds[i] = "CTRL_MIN_THROT"; mavlinkData.paramValues[i] = &configData.controlMinThro; i++;
    323. 

  323.     mavlinkData.paramIds[i] = "CTRL_MAX"; mavlinkData.paramValues[i] = &configData.controlMax; i++;
    324. 

  324. 

  325.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_P"; mavlinkData.paramValues[i] = &configData.controlTiltRateP; i++;
    326. 

  326.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_I"; mavlinkData.paramValues[i] = &configData.controlTiltRateI; i++;
    327. 

  327.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_D"; mavlinkData.paramValues[i] = &configData.controlTiltRateD; i++;
    328. 

  328.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_F"; mavlinkData.paramValues[i] = &configData.controlTiltRateF; i++;
    329. 

  329.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_PM"; mavlinkData.paramValues[i] = &configData.controlTiltRateMaxP; i++;
    330. 

  330.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_IM"; mavlinkData.paramValues[i] = &configData.controlTiltRateMaxI; i++;
    331. 

  331.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_DM"; mavlinkData.paramValues[i] = &configData.controlTiltRateMaxD; i++;
    332. 

  332.     mavlinkData.paramIds[i] = "CTRL_TLT_RTE_OM"; mavlinkData.paramValues[i] = &configData.controlTiltRateMaxO; i++;
    333. 

  333. 

  334.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_P"; mavlinkData.paramValues[i] = &configData.controlYawRateP; i++;
    335. 

  335.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_I"; mavlinkData.paramValues[i] = &configData.controlYawRateI; i++;
    336. 

  336.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_D"; mavlinkData.paramValues[i] = &configData.controlYawRateD; i++;
    337. 

  337.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_F"; mavlinkData.paramValues[i] = &configData.controlYawRateF; i++;
    338. 

  338.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_PM"; mavlinkData.paramValues[i] = &configData.controlYawRateMaxP; i++;
    339. 

  339.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_IM"; mavlinkData.paramValues[i] = &configData.controlYawRateMaxI; i++;
    340. 

  340.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_DM"; mavlinkData.paramValues[i] = &configData.controlYawRateMaxD; i++;
    341. 

  341.     mavlinkData.paramIds[i] = "CTRL_YAW_RTE_OM"; mavlinkData.paramValues[i] = &configData.controlYawRateMaxO; i++;
    342. 

  342. 

  343.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_P"; mavlinkData.paramValues[i] = &configData.controlTiltAngleP; i++;
    344. 

  344.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_I"; mavlinkData.paramValues[i] = &configData.controlTiltAngleI; i++;
    345. 

  345.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_D"; mavlinkData.paramValues[i] = &configData.controlTiltAngleD; i++;
    346. 

  346.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_F"; mavlinkData.paramValues[i] = &configData.controlTiltAngleF; i++;
    347. 

  347.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_PM"; mavlinkData.paramValues[i] = &configData.controlTiltAngleMaxP; i++;
    348. 

  348.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_IM"; mavlinkData.paramValues[i] = &configData.controlTiltAngleMaxI; i++;
    349. 

  349.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_DM"; mavlinkData.paramValues[i] = &configData.controlTiltAngleMaxD; i++;
    350. 

  350.     mavlinkData.paramIds[i] = "CTRL_TLT_ANG_OM"; mavlinkData.paramValues[i] = &configData.controlTiltAngleMaxO; i++;
    351. 

  351. 

  352.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_P"; mavlinkData.paramValues[i] = &configData.controlYawAngleP; i++;
    353. 

  353.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_I"; mavlinkData.paramValues[i] = &configData.controlYawAngleI; i++;
    354. 

  354.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_D"; mavlinkData.paramValues[i] = &configData.controlYawAngleD; i++;
    355. 

  355.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_F"; mavlinkData.paramValues[i] = &configData.controlYawAngleF; i++;
    356. 

  356.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_PM"; mavlinkData.paramValues[i] = &configData.controlYawAngleMaxP; i++;
    357. 

  357.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_IM"; mavlinkData.paramValues[i] = &configData.controlYawAngleMaxI; i++;
    358. 

  358.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_DM"; mavlinkData.paramValues[i] = &configData.controlYawAngleMaxD; i++;
    359. 

  359.     mavlinkData.paramIds[i] = "CTRL_YAW_ANG_OM"; mavlinkData.paramValues[i] = &configData.controlYawAngleMaxO; i++;
    360. 

  360. 

  361.     mavlinkData.paramIds[i] = "GPS_BAUD_RATE"; mavlinkData.paramValues[i] = &configData.gpsBaud; i++;
    362. 

  362.     mavlinkData.paramIds[i] = "GPS_POS_DEV"; mavlinkData.paramValues[i] = &configData.gpsPosDev; i++;
    363. 

  363.     mavlinkData.paramIds[i] = "GPS_VEL_DEV"; mavlinkData.paramValues[i] = &configData.gpsVelDev; i++;
    364. 

  364.     mavlinkData.paramIds[i] = "GPS_RATE"; mavlinkData.paramValues[i] = &configData.gpsRate; i++;
    365. 

  365. 

  366.     mavlinkData.paramIds[i] = "MOT_START"; mavlinkData.paramValues[i] = &configData.motorsStart; i++;
    367. 

  367.     mavlinkData.paramIds[i] = "MOT_MIN"; mavlinkData.paramValues[i] = &configData.motorsMin; i++;
    368. 

  368.     mavlinkData.paramIds[i] = "MOT_MAX"; mavlinkData.paramValues[i] = &configData.motorsMax; i++;
    369. 

  369.     mavlinkData.paramIds[i] = "MOT_HOV_THROT"; mavlinkData.paramValues[i] = &configData.motorsHoverThrot; i++;
    370. 

  370.     mavlinkData.paramIds[i] = "MOT_EXP_FACT"; mavlinkData.paramValues[i] = &configData.motorsExpFactor; i++;
    371. 

  371.     mavlinkData.paramIds[i] = "MOT_EXP_MIN"; mavlinkData.paramValues[i] = &configData.motorsExpMin; i++;
    372. 

  372.     mavlinkData.paramIds[i] = "MOT_EXP_MAX"; mavlinkData.paramValues[i] = &configData.motorsExpMax; i++;
    373. 

  373.     mavlinkData.paramIds[i] = "MOT_ROLL_ASYM"; mavlinkData.paramValues[i] = &configData.motorsRollAsym; i++;
    374. 

  374.     mavlinkData.paramIds[i] = "MOT_PITCH_ASYM"; mavlinkData.paramValues[i] = &configData.motorsPitchAsym; i++;
    375. 

  375. 

  376.     mavlinkData.paramIds[i] = "MOT_MAG_BIAS_X"; mavlinkData.paramValues[i] = &configData.motorsMagBiasX; i++;
    377. 

  377.     mavlinkData.paramIds[i] = "MOT_MAG_BIAS_Y"; mavlinkData.paramValues[i] = &configData.motorsMagBiasY; i++;
    378. 

  378.     mavlinkData.paramIds[i] = "MOT_MAG_BIAS_Z"; mavlinkData.paramValues[i] = &configData.motorsMagBiasZ; i++;
    379. 

  379.     mavlinkData.paramIds[i] = "MOT_MAG_SCAL_X"; mavlinkData.paramValues[i] = &configData.motorsMagScaleX; i++;
    380. 

  380.     mavlinkData.paramIds[i] = "MOT_MAG_SCAL_Y"; mavlinkData.paramValues[i] = &configData.motorsMagScaleY; i++;
    381. 

  381.     mavlinkData.paramIds[i] = "MOT_MAG_SCAL_Z"; mavlinkData.paramValues[i] = &configData.motorsMagScaleZ; i++;
    382. 

  382.     mavlinkData.paramIds[i] = "MOT_MAG_ALGN_XY"; mavlinkData.paramValues[i] = &configData.motorsMagAlgnXY; i++;
    383. 

  383.     mavlinkData.paramIds[i] = "MOT_MAG_ALGN_XZ"; mavlinkData.paramValues[i] = &configData.motorsMagAlgnXZ; i++;
    384. 

  384.     mavlinkData.paramIds[i] = "MOT_MAG_ALGN_YX"; mavlinkData.paramValues[i] = &configData.motorsMagAlgnYX; i++;
    385. 

  385.     mavlinkData.paramIds[i] = "MOT_MAG_ALGN_YZ"; mavlinkData.paramValues[i] = &configData.motorsMagAlgnYZ; i++;
    386. 

  386.     mavlinkData.paramIds[i] = "MOT_MAG_ALGN_ZX"; mavlinkData.paramValues[i] = &configData.motorsMagAlgnZX; i++;
    387. 

  387.     mavlinkData.paramIds[i] = "MOT_MAG_ALGN_ZY"; mavlinkData.paramValues[i] = &configData.motorsMagAlgnZY; i++;
    388. 

  388. 

  389.     mavlinkData.paramIds[i] = "DOWNLINK_BAUD"; mavlinkData.paramValues[i] = &configData.downlinkBaud; i++;
    390. 

  390. 

  391.     mavlinkData.paramIds[i] = "TELEMETRY_RATE"; mavlinkData.paramValues[i] = &configData.telemetryRate; i++;
    392. 

  392. 

  393.     mavlinkData.paramIds[i] = "NAV_MIN_PDOP"; mavlinkData.paramValues[i] = &configData.navMinPDOP; i++;
    394. 

  394.     mavlinkData.paramIds[i] = "NAV_MAX_FIXAGE"; mavlinkData.paramValues[i] = &configData.navMaxFixAge; i++;
    395. 

  395.     mavlinkData.paramIds[i] = "NAV_MAX_TILT"; mavlinkData.paramValues[i] = &configData.navMaxTilt; i++;
    396. 

  396.     mavlinkData.paramIds[i] = "NAV_MAX_SPEED"; mavlinkData.paramValues[i] = &configData.navMaxSpeed; i++;
    397. 

  397. 

  398.     mavlinkData.paramIds[i] = "NAV_SPEED_P"; mavlinkData.paramValues[i] = &configData.navSpeedP; i++;
    399. 

  399.     mavlinkData.paramIds[i] = "NAV_SPEED_I"; mavlinkData.paramValues[i] = &configData.navSpeedI; i++;
    400. 

  400.     mavlinkData.paramIds[i] = "NAV_SPEED_PM"; mavlinkData.paramValues[i] = &configData.navSpeedMaxP; i++;
    401. 

  401.     mavlinkData.paramIds[i] = "NAV_SPEED_IM"; mavlinkData.paramValues[i] = &configData.navSpeedMaxI; i++;
    402. 

  402.     mavlinkData.paramIds[i] = "NAV_SPEED_OM"; mavlinkData.paramValues[i] = &configData.navSpeedMaxO; i++;
    403. 

  403. 

  404.     mavlinkData.paramIds[i] = "NAV_DIST_P"; mavlinkData.paramValues[i] = &configData.navDistP; i++;
    405. 

  405.     mavlinkData.paramIds[i] = "NAV_DIST_I"; mavlinkData.paramValues[i] = &configData.navDistI; i++;
    406. 

  406.     mavlinkData.paramIds[i] = "NAV_DIST_PM"; mavlinkData.paramValues[i] = &configData.navDistMaxP; i++;
    407. 

  407.     mavlinkData.paramIds[i] = "NAV_DIST_IM"; mavlinkData.paramValues[i] = &configData.navDistMaxI; i++;
    408. 

  408.     mavlinkData.paramIds[i] = "NAV_DIST_OM"; mavlinkData.paramValues[i] = &configData.navDistMaxO; i++;
    409. 

  409. 

  410.     mavlinkData.paramIds[i] = "NAV_ATL_SPED_P"; mavlinkData.paramValues[i] = &configData.navAltSpeedP; i++;
    411. 

  411.     mavlinkData.paramIds[i] = "NAV_ATL_SPED_I"; mavlinkData.paramValues[i] = &configData.navAltSpeedI; i++;
    412. 

  412.     mavlinkData.paramIds[i] = "NAV_ATL_SPED_PM"; mavlinkData.paramValues[i] = &configData.navAltSpeedMaxP; i++;
    413. 

  413.     mavlinkData.paramIds[i] = "NAV_ATL_SPED_IM"; mavlinkData.paramValues[i] = &configData.navAltSpeedMaxI; i++;
    414. 

  414.     mavlinkData.paramIds[i] = "NAV_ATL_SPED_OM"; mavlinkData.paramValues[i] = &configData.navAltSpeedMaxO; i++;
    415. 

  415. 

  416.     mavlinkData.paramIds[i] = "NAV_ALT_POS_P"; mavlinkData.paramValues[i] = &configData.navAltPosP; i++;
    417. 

  417.     mavlinkData.paramIds[i] = "NAV_ALT_POS_I"; mavlinkData.paramValues[i] = &configData.navAltPosI; i++;
    418. 

  418.     mavlinkData.paramIds[i] = "NAV_ALT_POS_PM"; mavlinkData.paramValues[i] = &configData.navAltPosMaxP; i++;
    419. 

  419.     mavlinkData.paramIds[i] = "NAV_ALT_POS_IM"; mavlinkData.paramValues[i] = &configData.navAltPosMaxI; i++;
    420. 

  420.     mavlinkData.paramIds[i] = "NAV_ALT_POS_OM"; mavlinkData.paramValues[i] = &configData.navAltPosMaxO; i++;
    421. 

  421. 

  422.     mavlinkData.paramIds[i] = "NAV_FILTER_PX"; mavlinkData.paramValues[i] = &configData.navFiltPx; i++;
    423. 

  423.     mavlinkData.paramIds[i] = "NAV_FILTER_VX"; mavlinkData.paramValues[i] = &configData.navFiltVx; i++;
    424. 

  424.     mavlinkData.paramIds[i] = "NAV_FILTER_BX"; mavlinkData.paramValues[i] = &configData.navFiltBx; i++;
    425. 

  425.     mavlinkData.paramIds[i] = "NAV_FILTER_PZ"; mavlinkData.paramValues[i] = &configData.navFiltPz; i++;
    426. 

  426.     mavlinkData.paramIds[i] = "NAV_FILTER_VZ"; mavlinkData.paramValues[i] = &configData.navFiltVz; i++;
    427. 

  427.     mavlinkData.paramIds[i] = "NAV_FILTER_BZ"; mavlinkData.paramValues[i] = &configData.navFiltBz; i++;
    428. 

  428. 

  429.     mavlinkData.paramIds[i] = "IMU_ROT"; mavlinkData.paramValues[i] = &configData.adcIMURot; i++;
    430. 

  430. 

  431.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS_X"; mavlinkData.paramValues[i] = &configData.adcAccBiasX; i++;
    432. 

  432.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS_Y"; mavlinkData.paramValues[i] = &configData.adcAccBiasY; i++;
    433. 

  433.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS_Z"; mavlinkData.paramValues[i] = &configData.adcAccBiasZ; i++;
    434. 

  434.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS1_X"; mavlinkData.paramValues[i] = &configData.adcAccBias1X; i++;
    435. 

  435.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS1_Y"; mavlinkData.paramValues[i] = &configData.adcAccBias1Y; i++;
    436. 

  436.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS1_Z"; mavlinkData.paramValues[i] = &configData.adcAccBias1Z; i++;
    437. 

  437.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS2_X"; mavlinkData.paramValues[i] = &configData.adcAccBias2X; i++;
    438. 

  438.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS2_Y"; mavlinkData.paramValues[i] = &configData.adcAccBias2Y; i++;
    439. 

  439.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS2_Z"; mavlinkData.paramValues[i] = &configData.adcAccBias2Z; i++;
    440. 

  440.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS3_X"; mavlinkData.paramValues[i] = &configData.adcAccBias3X; i++;
    441. 

  441.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS3_Y"; mavlinkData.paramValues[i] = &configData.adcAccBias3Y; i++;
    442. 

  442.     mavlinkData.paramIds[i] = "IMU_ACC_BIAS3_Z"; mavlinkData.paramValues[i] = &configData.adcAccBias3Z; i++;
    443. 

  443.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL_X"; mavlinkData.paramValues[i] = &configData.adcAccScaleX; i++;
    444. 

  444.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL_Y"; mavlinkData.paramValues[i] = &configData.adcAccScaleY; i++;
    445. 

  445.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL_Z"; mavlinkData.paramValues[i] = &configData.adcAccScaleZ; i++;
    446. 

  446.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL1_X"; mavlinkData.paramValues[i] = &configData.adcAccScale1X; i++;
    447. 

  447.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL1_Y"; mavlinkData.paramValues[i] = &configData.adcAccScale1Y; i++;
    448. 

  448.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL1_Z"; mavlinkData.paramValues[i] = &configData.adcAccScale1Z; i++;
    449. 

  449.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL2_X"; mavlinkData.paramValues[i] = &configData.adcAccScale2X; i++;
    450. 

  450.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL2_Y"; mavlinkData.paramValues[i] = &configData.adcAccScale2Y; i++;
    451. 

  451.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL2_Z"; mavlinkData.paramValues[i] = &configData.adcAccScale2Z; i++;
    452. 

  452.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL3_X"; mavlinkData.paramValues[i] = &configData.adcAccScale3X; i++;
    453. 

  453.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL3_Y"; mavlinkData.paramValues[i] = &configData.adcAccScale3Y; i++;
    454. 

  454.     mavlinkData.paramIds[i] = "IMU_ACC_SCAL3_Z"; mavlinkData.paramValues[i] = &configData.adcAccScale3Z; i++;
    455. 

  455.     mavlinkData.paramIds[i] = "IMU_ACC_ALGN_XY"; mavlinkData.paramValues[i] = &configData.adcAccAlgnXY; i++;
    456. 

  456.     mavlinkData.paramIds[i] = "IMU_ACC_ALGN_XZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgnXZ; i++;
    457. 

  457.     mavlinkData.paramIds[i] = "IMU_ACC_ALGN_YX"; mavlinkData.paramValues[i] = &configData.adcAccAlgnYX; i++;
    458. 

  458.     mavlinkData.paramIds[i] = "IMU_ACC_ALGN_YZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgnYZ; i++;
    459. 

  459.     mavlinkData.paramIds[i] = "IMU_ACC_ALGN_ZX"; mavlinkData.paramValues[i] = &configData.adcAccAlgnZX; i++;
    460. 

  460.     mavlinkData.paramIds[i] = "IMU_ACC_ALGN_ZY"; mavlinkData.paramValues[i] = &configData.adcAccAlgnZY; i++;
    461. 

  461.     mavlinkData.paramIds[i] = "IMU_ACC_ALN1_XY"; mavlinkData.paramValues[i] = &configData.adcAccAlgn1XY; i++;
    462. 

  462.     mavlinkData.paramIds[i] = "IMU_ACC_ALN1_XZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgn1XZ; i++;
    463. 

  463.     mavlinkData.paramIds[i] = "IMU_ACC_ALN1_YX"; mavlinkData.paramValues[i] = &configData.adcAccAlgn1YX; i++;
    464. 

  464.     mavlinkData.paramIds[i] = "IMU_ACC_ALN1_YZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgn1YZ; i++;
    465. 

  465.     mavlinkData.paramIds[i] = "IMU_ACC_ALN1_ZX"; mavlinkData.paramValues[i] = &configData.adcAccAlgn1ZX; i++;
    466. 

  466.     mavlinkData.paramIds[i] = "IMU_ACC_ALN1_ZY"; mavlinkData.paramValues[i] = &configData.adcAccAlgn1ZY; i++;
    467. 

  467.     mavlinkData.paramIds[i] = "IMU_ACC_ALN2_XY"; mavlinkData.paramValues[i] = &configData.adcAccAlgn2XY; i++;
    468. 

  468.     mavlinkData.paramIds[i] = "IMU_ACC_ALN2_XZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgn2XZ; i++;
    469. 

  469.     mavlinkData.paramIds[i] = "IMU_ACC_ALN2_YX"; mavlinkData.paramValues[i] = &configData.adcAccAlgn2YX; i++;
    470. 

  470.     mavlinkData.paramIds[i] = "IMU_ACC_ALN2_YZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgn2YZ; i++;
    471. 

  471.     mavlinkData.paramIds[i] = "IMU_ACC_ALN2_ZX"; mavlinkData.paramValues[i] = &configData.adcAccAlgn2ZX; i++;
    472. 

  472.     mavlinkData.paramIds[i] = "IMU_ACC_ALN2_ZY"; mavlinkData.paramValues[i] = &configData.adcAccAlgn2ZY; i++;
    473. 

  473.     mavlinkData.paramIds[i] = "IMU_ACC_ALN3_XY"; mavlinkData.paramValues[i] = &configData.adcAccAlgn3XY; i++;
    474. 

  474.     mavlinkData.paramIds[i] = "IMU_ACC_ALN3_XZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgn3XZ; i++;
    475. 

  475.     mavlinkData.paramIds[i] = "IMU_ACC_ALN3_YX"; mavlinkData.paramValues[i] = &configData.adcAccAlgn3YX; i++;
    476. 

  476.     mavlinkData.paramIds[i] = "IMU_ACC_ALN3_YZ"; mavlinkData.paramValues[i] = &configData.adcAccAlgn3YZ; i++;
    477. 

  477.     mavlinkData.paramIds[i] = "IMU_ACC_ALN3_ZX"; mavlinkData.paramValues[i] = &configData.adcAccAlgn3ZX; i++;
    478. 

  478.     mavlinkData.paramIds[i] = "IMU_ACC_ALN3_ZY"; mavlinkData.paramValues[i] = &configData.adcAccAlgn3ZY; i++;
    479. 

  479. 

  480.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS_X"; mavlinkData.paramValues[i] = &configData.adcMagBiasX; i++;
    481. 

  481.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS_Y"; mavlinkData.paramValues[i] = &configData.adcMagBiasY; i++;
    482. 

  482.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS_Z"; mavlinkData.paramValues[i] = &configData.adcMagBiasZ; i++;
    483. 

  483.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS1_X"; mavlinkData.paramValues[i] = &configData.adcMagBias1X; i++;
    484. 

  484.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS1_Y"; mavlinkData.paramValues[i] = &configData.adcMagBias1Y; i++;
    485. 

  485.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS1_Z"; mavlinkData.paramValues[i] = &configData.adcMagBias1Z; i++;
    486. 

  486.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS2_X"; mavlinkData.paramValues[i] = &configData.adcMagBias2X; i++;
    487. 

  487.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS2_Y"; mavlinkData.paramValues[i] = &configData.adcMagBias2Y; i++;
    488. 

  488.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS2_Z"; mavlinkData.paramValues[i] = &configData.adcMagBias2Z; i++;
    489. 

  489.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS3_X"; mavlinkData.paramValues[i] = &configData.adcMagBias3X; i++;
    490. 

  490.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS3_Y"; mavlinkData.paramValues[i] = &configData.adcMagBias3Y; i++;
    491. 

  491.     mavlinkData.paramIds[i] = "IMU_MAG_BIAS3_Z"; mavlinkData.paramValues[i] = &configData.adcMagBias3Z; i++;
    492. 

  492.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL_X"; mavlinkData.paramValues[i] = &configData.adcMagScaleX; i++;
    493. 

  493.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL_Y"; mavlinkData.paramValues[i] = &configData.adcMagScaleY; i++;
    494. 

  494.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL_Z"; mavlinkData.paramValues[i] = &configData.adcMagScaleZ; i++;
    495. 

  495.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL1_X"; mavlinkData.paramValues[i] = &configData.adcMagScale1X; i++;
    496. 

  496.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL1_Y"; mavlinkData.paramValues[i] = &configData.adcMagScale1Y; i++;
    497. 

  497.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL1_Z"; mavlinkData.paramValues[i] = &configData.adcMagScale1Z; i++;
    498. 

  498.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL2_X"; mavlinkData.paramValues[i] = &configData.adcMagScale2X; i++;
    499. 

  499.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL2_Y"; mavlinkData.paramValues[i] = &configData.adcMagScale2Y; i++;
    500. 

  500.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL2_Z"; mavlinkData.paramValues[i] = &configData.adcMagScale2Z; i++;
    501. 

  501.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL3_X"; mavlinkData.paramValues[i] = &configData.adcMagScale3X; i++;
    502. 

  502.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL3_Y"; mavlinkData.paramValues[i] = &configData.adcMagScale3Y; i++;
    503. 

  503.     mavlinkData.paramIds[i] = "IMU_MAG_SCAL3_Z"; mavlinkData.paramValues[i] = &configData.adcMagScale3Z; i++;
    504. 

  504.     mavlinkData.paramIds[i] = "IMU_MAG_ALGN_XY"; mavlinkData.paramValues[i] = &configData.adcMagAlgnXY; i++;
    505. 

  505.     mavlinkData.paramIds[i] = "IMU_MAG_ALGN_XZ"; mavlinkData.paramValues[i] = &configData.adcMagAlgnXZ; i++;
    506. 

  506.     mavlinkData.paramIds[i] = "IMU_MAG_ALGN_YX"; mavlinkData.paramValues[i] = &configData.adcMagAlgnYX; i++;
    507. 

  507.     mavlinkData.paramIds[i] = "IMU_MAG_ALGN_YZ"; mavlinkData.paramValues[i] = &configData.adcMagAlgnYZ; i++;
    508. 

  508.     mavlinkData.paramIds[i] = "IMU_MAG_ALGN_ZX"; mavlinkData.paramValues[i] = &configData.adcMagAlgnZX; i++;
    509. 

  509.     mavlinkData.paramIds[i] = "IMU_MAG_ALGN_ZY"; mavlinkData.paramValues[i] = &configData.adcMagAlgnZY; i++;
    510. 

  510. 

  511.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS_X"; mavlinkData.paramValues[i] = &configData.adcGyoBiasX; i++;
    512. 

  512.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS_Y"; mavlinkData.paramValues[i] = &configData.adcGyoBiasY; i++;
    513. 

  513.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS_Z"; mavlinkData.paramValues[i] = &configData.adcGyoBiasZ; i++;
    514. 

  514.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS1_X"; mavlinkData.paramValues[i] = &configData.adcGyoBias1X; i++;
    515. 

  515.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS1_Y"; mavlinkData.paramValues[i] = &configData.adcGyoBias1Y; i++;
    516. 

  516.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS1_Z"; mavlinkData.paramValues[i] = &configData.adcGyoBias1Z; i++;
    517. 

  517.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS2_X"; mavlinkData.paramValues[i] = &configData.adcGyoBias2X; i++;
    518. 

  518.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS2_Y"; mavlinkData.paramValues[i] = &configData.adcGyoBias2Y; i++;
    519. 

  519.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS2_Z"; mavlinkData.paramValues[i] = &configData.adcGyoBias2Z; i++;
    520. 

  520.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS3_X"; mavlinkData.paramValues[i] = &configData.adcGyoBias3X; i++;
    521. 

  521.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS3_Y"; mavlinkData.paramValues[i] = &configData.adcGyoBias3Y; i++;
    522. 

  522.     mavlinkData.paramIds[i] = "IMU_GYO_BIAS3_Z"; mavlinkData.paramValues[i] = &configData.adcGyoBias3Z; i++;
    523. 

  523.     mavlinkData.paramIds[i] = "IMU_GYO_SCAL_X"; mavlinkData.paramValues[i] = &configData.adcGyoScaleX; i++;
    524. 

  524.     mavlinkData.paramIds[i] = "IMU_GYO_SCAL_Y"; mavlinkData.paramValues[i] = &configData.adcGyoScaleY; i++;
    525. 

  525.     mavlinkData.paramIds[i] = "IMU_GYO_SCAL_Z"; mavlinkData.paramValues[i] = &configData.adcGyoScaleZ; i++;
    526. 

  526.     mavlinkData.paramIds[i] = "IMU_GYO_ALGN_XY"; mavlinkData.paramValues[i] = &configData.adcGyoAlgnXY; i++;
    527. 

  527.     mavlinkData.paramIds[i] = "IMU_GYO_ALGN_XZ"; mavlinkData.paramValues[i] = &configData.adcGyoAlgnXZ; i++;
    528. 

  528.     mavlinkData.paramIds[i] = "IMU_GYO_ALGN_YX"; mavlinkData.paramValues[i] = &configData.adcGyoAlgnYX; i++;
    529. 

  529.     mavlinkData.paramIds[i] = "IMU_GYO_ALGN_YZ"; mavlinkData.paramValues[i] = &configData.adcGyoAlgnYZ; i++;
    530. 

  530.     mavlinkData.paramIds[i] = "IMU_GYO_ALGN_ZX"; mavlinkData.paramValues[i] = &configData.adcGyoAlgnZX; i++;
    531. 

  531.     mavlinkData.paramIds[i] = "IMU_GYO_ALGN_ZY"; mavlinkData.paramValues[i] = &configData.adcGyoAlgnZY; i++;
    532. 

  532. 

  533.     mavlinkData.paramIds[i] = "QUAT_MAG_INCL"; mavlinkData.paramValues[i] = &configData.quatMagIncl; i++;
    534. 

  534.     mavlinkData.paramIds[i] = "QUAT_ACC_DIST"; mavlinkData.paramValues[i] = &configData.quatAccDist; i++;
    535. 

  535.     mavlinkData.paramIds[i] = "QUAT_KP"; mavlinkData.paramValues[i] = &configData.quatKp; i++;
    536. 

  536.     mavlinkData.paramIds[i] = "QUAT_KI"; mavlinkData.paramValues[i] = &configData.quatKi; i++;
    537. 

  537.     mavlinkData.paramIds[i] = "QUAT_KA"; mavlinkData.paramValues[i] = &configData.quatKa; i++;
    538. 

  538.     mavlinkData.paramIds[i] = "QUAT_KM1"; mavlinkData.paramValues[i] = &configData.quatKm1; i++;
    539. 

  539.     mavlinkData.paramIds[i] = "QUAT_KM2"; mavlinkData.paramValues[i] = &configData.quatKm2; i++;
    540. 

  540. 

  541.     return i;
    542. 

  542. }
    543. 

  543. 

  544. void mavlinkInit(void) {
    545. 

  545.     unsigned long micros;
    546. 

  546.     int i;
    547. 

  547. 

  548.     mavlinkData.serialPort = serialOpen(MAVLINK_USART, configData.downlinkBaud, USART_HardwareFlowControl_RTS_CTS);
    549. 

  549. 

  550.     mavlinkData.debugLed = digitalInit(MAVLINK_LED_PORT, MAVLINK_LED_PIN);
    551. 

  551. 

  552.     // setup mutex for serial port access
    553. 

  553.     ctl_mutex_init(&mavlinkData.serialPortMutex);
    554. 

  554. 

  555.     ctl_message_queue_init(&mavlinkData.notices, mavlinkData.noticeQueue, MAVLINK_NOTICE_DEPTH);
    556. 

  556. 

  557.     AQ_NOTICE("Mavlink init... ");
    558. 

  558. 

  559.     mavlinkData.numParams = mavlinkInitParams();
    560. 

  560.     mavlinkData.currentParam = mavlinkData.numParams;
    561. 

  561. 

  562.     // start send thread with high priority so that it can process startup notices - will drop priority later
    563. 

  563.     memset(mavlinkSendTaskStack, 0xcd, sizeof(mavlinkSendTaskStack));  // write known values into the stack
    564. 

  564.     mavlinkSendTaskStack[0] = mavlinkSendTaskStack[1+STACKSIZE] = 0xfacefeed; // put marker values at the words before/after the stack
    565. 

  565.     ctl_task_run(&mavlinkData.sendTask, 175, mavlinkSendTaskCode, 0, "mavlinkSendTask", STACKSIZE, mavlinkSendTaskStack+1, 0);
    566. 

  566. 

  567.     memset(mavlinkRecvTaskStack, 0xcd, sizeof(mavlinkRecvTaskStack));  // write known values into the stack
    568. 

  568.     mavlinkRecvTaskStack[0] = mavlinkRecvTaskStack[1+STACKSIZE] = 0xfacefeed; // put marker values at the words before/after the stack
    569. 

  569.     ctl_task_run(&mavlinkData.recvTask, 20, mavlinkRecvTaskCode, 0, "mavlinkRecvTask", STACKSIZE, mavlinkRecvTaskStack+1, 0);
    570. 

  570. 

  571.     mavlink_system.sysid = flashSerno() % 250;
    572. 

  572.     mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER;
    573. 

  573.     mavlink_system.type = MAV_QUADROTOR;
    574. 

  574. 

  575.     mavlinkData.mode = MAV_MODE_MANUAL;
    576. 

  576.     mavlinkData.nav_mode = MAV_NAV_GROUNDED;
    577. 

  577.     mavlinkData.status = MAV_STATE_ACTIVE;
    578. 

  578. 

  579.     // turn on all streams at 1Hz & spread them out
    580. 

  580.     micros = timerMicros();
    581. 

  581.     for (i = 1; i < 13; i++) {	// TODO: remove hardcoded value
    582. 

  582. 	mavlinkData.streamInterval[i] = 1e6;
    583. 

  583. 	mavlinkData.streamNext[i] = micros + 5e6 + i * 5e3;
    584. 

  584.     }
    585. 

  585. }
    586. 

  586. 

  587.