mavlink_parameters.c 10.9 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
/****************************************************************************
 *
 *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
 *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file mavlink_parameters.c
 * MAVLink parameter protocol implementation (BSD-relicensed).
 */

#include "mavlink_parameters.h"
#include <uORB/uORB.h>
#include "math.h" /* isinf / isnan checks */
#include <assert.h>
#include <stdio.h>
#include <fcntl.h>
46
#include <unistd.h>
47
48
#include <stdbool.h>
#include <string.h>
49
#include <errno.h>
50
#include <systemlib/param/param.h>
51
52
#include <systemlib/err.h>
#include <sys/stat.h>
53
54
55

extern mavlink_system_t mavlink_system;

56
57
extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
extern int mavlink_missionlib_send_gcs_string(const char *string);
58

59
60
61
62
63
64
65
66
67
68
69
70
/**
 * If the queue index is not at 0, the queue sending
 * logic will send parameters from the current index
 * to len - 1, the end of the param list.
 */
static unsigned int mavlink_param_queue_index = 0;

/**
 * Callback for param interface.
 */
void mavlink_pm_callback(void *arg, param_t param);

71
72
73
74
75
76
77
78
79
80
81
82
83
84
/**
 * Save parameters to EEPROM.
 *
 * Stores the parameters to /eeprom/parameters
 */
static int mavlink_pm_save_eeprom(void);

/**
 * Load parameters from EEPROM.
 *
 * Loads the parameters from /eeprom/parameters
 */
static int mavlink_pm_load_eeprom(void);

85
void mavlink_pm_callback(void *arg, param_t param)
86
{
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
	mavlink_pm_send_param(param);
	usleep(*(unsigned int*)arg);
}

void mavlink_pm_send_all_params(unsigned int delay)
{
	unsigned int dbuf = delay;
	param_foreach(&mavlink_pm_callback, &dbuf, false);
}

int mavlink_pm_queued_send()
{
	if (mavlink_param_queue_index < param_count()) {
		mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
		mavlink_param_queue_index++;
		return 0;
	} else {
		return 1;
105
106
107
	}
}

108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
void mavlink_pm_start_queued_send()
{
	mavlink_param_queue_index = 0;
}

int mavlink_pm_send_param_for_index(uint16_t index)
{
	return mavlink_pm_send_param(param_for_index(index));
}

int mavlink_pm_send_param_for_name(const char* name)
{
	return mavlink_pm_send_param(param_find(name));
}

int mavlink_pm_send_param(param_t param)
{
	if (param == PARAM_INVALID) return 1;

	/* buffers for param transmission */
	static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
	float val_buf;
	static mavlink_message_t tx_msg;

	/* query parameter type */
	param_type_t type = param_type(param);
	/* copy parameter name */
	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
	
	/*
	 * Map onboard parameter type to MAVLink type,
	 * endianess matches (both little endian)
	 */
	uint8_t mavlink_type;
	if (type == PARAM_TYPE_INT32) {
		mavlink_type = MAVLINK_TYPE_INT32_T;
	} else if (type == PARAM_TYPE_FLOAT) {
		mavlink_type = MAVLINK_TYPE_FLOAT;
146
147
	} else {
		mavlink_type = MAVLINK_TYPE_FLOAT;
148
149
150
151
152
153
	}

	/*
	 * get param value, since MAVLink encodes float and int params in the same
	 * space during transmission, copy param onto float val_buf
	 */
154
155
156

	int ret;
	if ((ret = param_get(param, &val_buf)) != OK) return ret;
157
158
159
160
161
162
163
164
165
166

	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
					  mavlink_system.compid,
					  MAVLINK_COMM_0,
					  &tx_msg,
					  name_buf,
					  val_buf,
					  mavlink_type,
					  param_count(),
					  param_get_index(param));
167
168
	ret = mavlink_missionlib_send_message(&tx_msg);
	return ret;
169
170
}

171
static const char *mavlink_parameter_file = "/eeprom/parameters";
172

173
174
175
/**
 * @return 0 on success, -1 if device open failed, -2 if writing parameters failed
 */
176
static int mavlink_pm_save_eeprom()
Lorenz Meier's avatar
Lorenz Meier committed
177
{
178
	/* delete the file in case it exists */
179
180
	unlink(mavlink_parameter_file);

181
	/* create the file */
182
	int fd = open(mavlink_parameter_file, O_WRONLY | O_CREAT | O_EXCL);
Lorenz Meier's avatar
Lorenz Meier committed
183

184
	if (fd < 0) {
185
		warn("opening '%s' for writing failed", mavlink_parameter_file);
186
		return -1;
187
	}
Lorenz Meier's avatar
Lorenz Meier committed
188
189
190
191

	int result = param_export(fd, false);
	close(fd);

192
	if (result != 0) {
193
		unlink(mavlink_parameter_file);
194
		warn("error exporting parameters to '%s'", mavlink_parameter_file);
195
		return -2;
Lorenz Meier's avatar
Lorenz Meier committed
196
197
198
199
200
	}

	return 0;
}

201
/**
202
 * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
203
 */
Lorenz Meier's avatar
Lorenz Meier committed
204
205
206
static int
mavlink_pm_load_eeprom()
{
207
	int fd = open(mavlink_parameter_file, O_RDONLY);
Lorenz Meier's avatar
Lorenz Meier committed
208

209
	if (fd < 0) {
210
211
		/* no parameter file is OK, otherwise this is an error */
		if (errno != ENOENT) {
212
213
214
			warn("open '%s' for reading failed", mavlink_parameter_file);
			return -1;
		}
215
		return 1;
216
	}
Lorenz Meier's avatar
Lorenz Meier committed
217

218
	int result = param_load(fd);
Lorenz Meier's avatar
Lorenz Meier committed
219
220
	close(fd);

221
	if (result != 0) {
222
		warn("error reading parameters from '%s'", mavlink_parameter_file);
223
		return -2;
224
	}
Lorenz Meier's avatar
Lorenz Meier committed
225
226
227
228

	return 0;
}

229
230
231
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{
	switch (msg->msgid) {
232
		case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
233
			/* Start sending parameters */
234
235
			mavlink_pm_start_queued_send();
			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
236
237
		} break;

238
		case MAVLINK_MSG_ID_PARAM_SET: {
239
240
241
242
243
244
245
246

			/* Handle parameter setting */

			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
				mavlink_param_set_t mavlink_param_set;
				mavlink_msg_param_set_decode(msg, &mavlink_param_set);

				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
					/* local name buffer to enforce null-terminated string */
					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1];
					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
					/* enforce null termination */
					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
					/* attempt to find parameter, set and send it */
					param_t param = param_find(name);

					if (param == PARAM_INVALID) {
						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
						sprintf(buf, "[mavlink pm] unknown: %s", name);
						mavlink_missionlib_send_gcs_string(buf);
					} else {
						/* set and send parameter */
						param_set(param, &(mavlink_param_set.param_value));
						mavlink_pm_send_param(param);
263
264
265
266
267
					}
				}
			}
		} break;

268
		case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
269
270
271
			mavlink_param_request_read_t mavlink_param_request_read;
			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);

272
273
274
275
276
277
278
279
280
281
282
283
284
285
				if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
					/* when no index is given, loop through string ids and compare them */
					if (mavlink_param_request_read.param_index == -1) {
						/* local name buffer to enforce null-terminated string */
						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1];
						strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
						/* enforce null termination */
						name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
						/* attempt to find parameter and send it */
						mavlink_pm_send_param_for_name(name);
					} else {
						/* when index is >= 0, send this parameter again */
						mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
					}
286
287
288
				}

		} break;
289
290
291
292
293

		case MAVLINK_MSG_ID_COMMAND_LONG: {
			mavlink_command_long_t cmd_mavlink;
			mavlink_msg_command_long_decode(msg, &cmd_mavlink);

294
			uint8_t result = MAV_RESULT_UNSUPPORTED;
295
296
297
298
299
300
301
302
303
304

			if (cmd_mavlink.target_system == mavlink_system.sysid &&
				((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {

				/* preflight parameter load / store */
				if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
					/* Read all parameters from EEPROM to RAM */

					if (((int)(cmd_mavlink.param1)) == 0)	{

305
306
307
						/* read all parameters from EEPROM to RAM */
						int read_ret = mavlink_pm_load_eeprom();
						if (read_ret == OK) {
308
							//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
309
							mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params");
310
							result = MAV_RESULT_ACCEPTED;
311
312
313
						} else if (read_ret == 1) {
							mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load");
							result = MAV_RESULT_ACCEPTED;
314
						} else {
315
316
317
318
319
							if (read_ret < -1) {
								mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM");
							} else {
								mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found");
							}
320
321
322
323
324
							result = MAV_RESULT_FAILED;
						}

					} else if (((int)(cmd_mavlink.param1)) == 1)	{

325
326
327
328
						/* write all parameters from RAM to EEPROM */
						int write_ret = mavlink_pm_save_eeprom();
						if (write_ret == OK) {
							mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM");
329
330
331
							result = MAV_RESULT_ACCEPTED;

						} else {
332
333
334
335
336
							if (write_ret < -1) {
								mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM");
							} else {
								mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found");
							}
337
338
339
340
341
							result = MAV_RESULT_FAILED;
						}

					} else {
						//fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
342
						mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request");
343
344
345
346
						result = MAV_RESULT_UNSUPPORTED;
					}
				}
			}
347
348

			/* send back command result */
349
			//mavlink_msg_command_ack_send(chan, cmd.command, result);
350
		} break;
351
352
	}
}