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

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
parent cc7a9c9b66 41172f24d5
commit 56bba7816f
6 changed files with 263 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
apps/commander/commander.c
View File

@ -581,46 +581,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
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: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED;

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;
}
}

274
apps/mavlink/mavlink_parameters.c
View File

@ -45,52 +45,116 @@
#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);
}
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);
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,
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)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
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: {
case MAVLINK_MSG_ID_PARAM_SET: {
/* 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);
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);
}
}
}
} 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_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))) {
/* 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);
}
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);
}
}
} else {
/* when index is >= 0, send this parameter again */
mavlink_pm_send_one_parameter(mavlink_param_request_read.param_index);
} break;
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;
}
}
/**
* 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++;
}

60
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,60 @@
#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 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);

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)
{

14
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.
*
@ -156,7 +164,7 @@ __EXPORT int param_set(param_t param, const void *val);
__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.)
* @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.
*
* 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 arg Argument passed to the function.