dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Reworked MAVLink parameter interface to support new parameter storage, tested.

This commit is contained in:
Lorenz Meier 2012-08-20 09:07:33 +02:00
parent 4a7f92fad0
commit 2c8fafd12a
5 changed files with 186 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
apps/mavlink/mavlink.c
View File

@ -151,8 +151,8 @@ static enum {
void mavlink_wpm_send_message(mavlink_message_t *msg);
void mavlink_wpm_send_gcs_string(const char *string);
uint64_t mavlink_wpm_get_system_timestamp(void);
void mavlink_missionlib_send_message(mavlink_message_t *msg);
void mavlink_missionlib_send_gcs_string(const char *string);
int mavlink_missionlib_send_message(mavlink_message_t *msg);
int mavlink_missionlib_send_gcs_string(const char *string);
uint64_t mavlink_missionlib_get_system_timestamp(void);
void handleMessage(mavlink_message_t *msg);
@ -183,13 +183,18 @@ static void usage(const char *reason);
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
void mavlink_missionlib_send_message(mavlink_message_t *msg)
int mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
write(uart, missionlib_msg_buf, len);
int writelen = write(uart, missionlib_msg_buf, len);
if (writelen != len) {
return 1;
} else {
return 0;
}
}
void mavlink_missionlib_send_gcs_string(const char *string)
int mavlink_missionlib_send_gcs_string(const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
@ -210,7 +215,9 @@ void mavlink_missionlib_send_gcs_string(const char *string)
mavlink_message_t msg;
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
mavlink_missionlib_send_message(&msg);
return mavlink_missionlib_send_message(&msg);
} else {
return 1;
}
}

202
apps/mavlink/mavlink_parameters.c
View File

@ -45,40 +45,104 @@
#include <fcntl.h>
#include <stdbool.h>
#include <string.h>
#include <systemlib/param/param.h>
extern mavlink_system_t mavlink_system;
extern void mavlink_missionlib_send_message(mavlink_message_t *msg);
extern void mavlink_missionlib_send_gcs_string(const char *string);
extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
extern int mavlink_missionlib_send_gcs_string(const char *string);
/* send one parameter, assume lock on global_data_parameter_storage */
void mavlink_pm_send_one_parameter(uint16_t next_param)
/**
* 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);
void mavlink_pm_callback(void *arg, param_t param)
{
if (next_param < global_data_parameter_storage->pm.size) {
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
mavlink_message_t tx_msg;
mavlink_pm_send_param(param);
usleep(*(unsigned int*)arg);
}
strncpy((char *)name_buf, global_data_parameter_storage->pm.param_names[next_param], MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
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;
}
}
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;
}
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &val_buf) != OK) return;
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
MAVLINK_COMM_0,
&tx_msg,
name_buf,
global_data_parameter_storage->pm.param_values[next_param],
MAVLINK_TYPE_FLOAT,
global_data_parameter_storage->pm.size,
next_param);
mavlink_missionlib_send_message(&tx_msg);
// mavlink_msg_param_value_send(MAVLINK_COMM_0,
// name_buf,
// global_data_parameter_storage->pm.param_values[next_param],
// MAVLINK_TYPE_FLOAT,
// global_data_parameter_storage->pm.size,
// next_param);
}
val_buf,
mavlink_type,
param_count(),
param_get_index(param));
return mavlink_missionlib_send_message(&tx_msg);
}
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
@ -86,8 +150,8 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */
global_data_parameter_storage->pm.next_param = 0;
mavlink_missionlib_send_gcs_string("[pm] sending list");
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
@ -99,32 +163,22 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
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))) {
/* 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);
uint16_t i; //parameters
uint16_t j; //chars
bool match;
for (i = 0; i < PARAM_MAX_COUNT; i++) {
match = true;
for (j = 0; j < MAX_PARAM_NAME_LEN; j++) {
/* Compare char by char */
if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_set.param_id[j]) {
match = false;
}
/* End matching if null termination is reached */
if (global_data_parameter_storage->pm.param_names[i][j] == '\0') {
break;
}
}
/* Check if matched */
if (match) {
// XXX handle param type as well, assuming float here
global_data_parameter_storage->pm.param_values[i] = mavlink_param_set.param_value;
mavlink_pm_send_one_parameter(i);
}
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);
}
}
}
@ -137,53 +191,19 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
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) {
uint16_t i; //parameters
uint16_t j; //chars
bool match;
for (i = 0; i < PARAM_MAX_COUNT; i++) {
match = true;
for (j = 0; j < MAX_PARAM_NAME_LEN; j++) {
/* Compare char by char */
if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_request_read.param_id[j]) {
match = false;
}
/* End matching if null termination is reached */
if (global_data_parameter_storage->pm.param_names[i][j] == '\0') {
break;
}
}
/* Check if matched */
if (match) {
mavlink_pm_send_one_parameter(i);
}
}
/* 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_one_parameter(mavlink_param_request_read.param_index);
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
}
}
} break;
}
}
/**
* Send low-priority messages at a maximum rate of xx Hertz.
*
* This function sends messages at a lower rate to not exceed the wireless
* bandwidth. It sends one message each time it is called until the buffer is empty.
* Call this function with xx Hertz to increase/decrease the bandwidth.
*/
void mavlink_pm_queued_send(void)
{
//send parameters one by one:
mavlink_pm_send_one_parameter(global_data_parameter_storage->pm.next_param);
global_data_parameter_storage->pm.next_param++;
}

44
apps/mavlink/mavlink_parameters.h
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 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
@ -43,6 +43,44 @@
#include "v1.0/common/mavlink.h"
#include <stdbool.h>
#include <systemlib/param/param.h>
/**
* Handle parameter related messages.
*/
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void mavlink_pm_queued_send(void);
/**
* Send all parameters at once.
*
* This function blocks until all parameters have been sent.
* it delays each parameter by the passed amount of microseconds.
*
* @param delay The delay in us between sending all parameters.
*/
void mavlink_pm_send_all_params(unsigned int delay);
/**
* Send one parameter.
*
* @param param The parameter id to send
* @return zero on success, nonzero on failure
*/
int mavlink_pm_send_param(param_t param);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send();
/**
* Start sending the parameter queue.
*
* This function will not directly send parameters, but instead
* activate the sending of one parameter on each call of
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();

9
apps/systemlib/param/param.c
View File

@ -193,6 +193,15 @@ param_for_index(unsigned index)
return PARAM_INVALID;
}
int
param_get_index(param_t param)
{
if (handle_in_range(param))
return (unsigned)param;
return -1;
}
const char *
param_name(param_t param)
{

8
apps/systemlib/param/param.h
View File

@ -102,6 +102,14 @@ __EXPORT unsigned param_count(void) __attribute__((const));
*/
__EXPORT param_t param_for_index(unsigned index) __attribute__((const));
/**
* Look up the index of a parameter.
*
* @param param The parameter to obtain the index for.
* @return The index, or -1 if the parameter does not exist.
*/
__EXPORT int param_get_index(param_t param) __attribute__((const));
/**
* Obtain the name of a parameter.
*