Merge branch 'px4dev_new_param' of https://github.com/PX4/Firmware into px4dev_new_param

This commit is contained in:
px4dev 2012-08-20 01:53:37 -07:00
commit 56bba7816f
6 changed files with 263 additions and 153 deletions

View File

@ -581,46 +581,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
} }
break; break;
/* preflight parameter load / store */
case MAV_CMD_PREFLIGHT_STORAGE: {
/* Read all parameters from EEPROM to RAM */
if (((int)cmd->param1) == 0) {
if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
//printf("[commander] Loaded EEPROM params in RAM\n");
mavlink_log_info(mavlink_fd, "[commander] CMD Loaded EEPROM params in RAM");
result = MAV_RESULT_ACCEPTED;
} else {
//fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
result = MAV_RESULT_FAILED;
}
/* Write all parameters from RAM to EEPROM */
} else if (((int)cmd->param1) == 1) {
if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
//printf("[commander] RAM params written to EEPROM\n");
mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM");
result = MAV_RESULT_ACCEPTED;
} else {
//fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM");
result = MAV_RESULT_FAILED;
}
} else {
//fprintf(stderr, "[commander] refusing unsupported storage request\n");
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request");
result = MAV_RESULT_UNSUPPORTED;
}
}
break;
default: { default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED; result = MAV_RESULT_UNSUPPORTED;

View File

@ -151,8 +151,8 @@ static enum {
void mavlink_wpm_send_message(mavlink_message_t *msg); void mavlink_wpm_send_message(mavlink_message_t *msg);
void mavlink_wpm_send_gcs_string(const char *string); void mavlink_wpm_send_gcs_string(const char *string);
uint64_t mavlink_wpm_get_system_timestamp(void); uint64_t mavlink_wpm_get_system_timestamp(void);
void mavlink_missionlib_send_message(mavlink_message_t *msg); int mavlink_missionlib_send_message(mavlink_message_t *msg);
void mavlink_missionlib_send_gcs_string(const char *string); int mavlink_missionlib_send_gcs_string(const char *string);
uint64_t mavlink_missionlib_get_system_timestamp(void); uint64_t mavlink_missionlib_get_system_timestamp(void);
void handleMessage(mavlink_message_t *msg); 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]; 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); 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; const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext; mavlink_statustext_t statustext;
@ -210,7 +215,9 @@ void mavlink_missionlib_send_gcs_string(const char *string)
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); 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;
} }
} }

View File

@ -45,52 +45,116 @@
#include <fcntl.h> #include <fcntl.h>
#include <stdbool.h> #include <stdbool.h>
#include <string.h> #include <string.h>
#include <systemlib/param/param.h>
extern mavlink_system_t mavlink_system; extern mavlink_system_t mavlink_system;
extern void mavlink_missionlib_send_message(mavlink_message_t *msg); extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
extern void mavlink_missionlib_send_gcs_string(const char *string); 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) { mavlink_pm_send_param(param);
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; usleep(*(unsigned int*)arg);
mavlink_message_t tx_msg; }
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);
}
mavlink_msg_param_value_pack_chan(mavlink_system.sysid, int mavlink_pm_queued_send()
mavlink_system.compid, {
MAVLINK_COMM_0, if (mavlink_param_queue_index < param_count()) {
&tx_msg, mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
name_buf, mavlink_param_queue_index++;
global_data_parameter_storage->pm.param_values[next_param], return 0;
MAVLINK_TYPE_FLOAT, } else {
global_data_parameter_storage->pm.size, return 1;
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);
} }
} }
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,
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) void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{ {
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */ /* Start sending parameters */
global_data_parameter_storage->pm.next_param = 0; mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[pm] sending list"); mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
} break; } break;
case MAVLINK_MSG_ID_PARAM_SET: { case MAVLINK_MSG_ID_PARAM_SET: {
/* Handle parameter setting */ /* Handle parameter setting */
@ -99,91 +163,97 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
mavlink_msg_param_set_decode(msg, &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))) { 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 if (param == PARAM_INVALID) {
uint16_t j; //chars char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
bool match; sprintf(buf, "[mavlink pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(buf);
for (i = 0; i < PARAM_MAX_COUNT; i++) { } else {
match = true; /* set and send parameter */
param_set(param, &(mavlink_param_set.param_value));
for (j = 0; j < MAX_PARAM_NAME_LEN; j++) { mavlink_pm_send_param(param);
/* 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);
}
} }
} }
} }
} break; } break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_param_request_read_t mavlink_param_request_read; mavlink_param_request_read_t mavlink_param_request_read;
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
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))) { 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 */ /* when no index is given, loop through string ids and compare them */
if (mavlink_param_request_read.param_index == -1) { if (mavlink_param_request_read.param_index == -1) {
/* local name buffer to enforce null-terminated string */
uint16_t i; //parameters char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1];
uint16_t j; //chars strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
bool match; /* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
for (i = 0; i < PARAM_MAX_COUNT; i++) { /* attempt to find parameter and send it */
match = true; mavlink_pm_send_param_for_name(name);
} else {
for (j = 0; j < MAX_PARAM_NAME_LEN; j++) { /* when index is >= 0, send this parameter again */
/* Compare char by char */ mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
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);
}
} }
}
} else { } break;
/* when index is >= 0, send this parameter again */
mavlink_pm_send_one_parameter(mavlink_param_request_read.param_index); case MAVLINK_MSG_ID_COMMAND_LONG: {
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
uint8_t result;
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) {
if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] CMD Loaded EEPROM params in RAM");
result = MAV_RESULT_ACCEPTED;
} else {
//fprintf(stderr, "[mavlink pm] ERROR loading EEPROM params in RAM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] ERROR loading EEPROM params in RAM");
result = MAV_RESULT_FAILED;
}
/* Write all parameters from RAM to EEPROM */
} else if (((int)(cmd_mavlink.param1)) == 1) {
if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
//printf("[mavlink pm] RAM params written to EEPROM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] RAM params written to EEPROM");
result = MAV_RESULT_ACCEPTED;
} else {
//fprintf(stderr, "[mavlink pm] ERROR writing RAM params to EEPROM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] ERROR writing RAM params to EEPROM");
result = MAV_RESULT_FAILED;
}
} else {
//fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported storage request");
result = MAV_RESULT_UNSUPPORTED;
}
} }
} }
} break; } 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++;
}

View File

@ -1,7 +1,7 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch> * Author: @author Lorenz Meier <lm@inf.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -43,6 +43,60 @@
#include "v1.0/common/mavlink.h" #include "v1.0/common/mavlink.h"
#include <stdbool.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_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 one parameter identified by index.
*
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_index(uint16_t index);
/**
* Send one parameter identified by name.
*
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char* name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
/**
* 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(void);

View File

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

View File

@ -102,6 +102,14 @@ __EXPORT unsigned param_count(void) __attribute__((const));
*/ */
__EXPORT param_t param_for_index(unsigned index) __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. * Obtain the name of a parameter.
* *
@ -156,7 +164,7 @@ __EXPORT int param_set(param_t param, const void *val);
__EXPORT int param_export(int fd, bool only_unsaved); __EXPORT int param_export(int fd, bool only_unsaved);
/** /**
* Import parameters from a file, discarding any unrecognised parameters. * Import parameters from a file, discarding any unrecognized parameters.
* *
* @param fd File descriptor to import from. (Currently expected to be a file.) * @param fd File descriptor to import from. (Currently expected to be a file.)
* @return Zero on success, nonzero if an error occurred during import. * @return Zero on success, nonzero if an error occurred during import.
@ -167,7 +175,9 @@ __EXPORT int param_import(int fd);
/** /**
* Apply a function to each parameter. * Apply a function to each parameter.
* *
* Note that the parameter set is not locked during the traversal. * Note that the parameter set is not locked during the traversal. It also does
* not hold an internal state, so the callback function can block or sleep between
* parameter callbacks.
* *
* @param func The function to invoke for each parameter. * @param func The function to invoke for each parameter.
* @param arg Argument passed to the function. * @param arg Argument passed to the function.