Merged parameter changes

This commit is contained in:
Lorenz Meier
2012-08-22 09:19:43 +02:00
15 changed files with 375 additions and 72 deletions
+3 -1
View File
@@ -25,7 +25,9 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \
$(SRCROOT)/mixers/FMU_delta.mix~mixers/FMU_delta.mix \
$(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \
$(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \
$(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix
$(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
$(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
$(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix
#
# Add the PX4IO firmware to the spec if someone has dropped it into the
+7
View File
@@ -0,0 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a quadrotor in the + configuration, with 10% contribution from
roll and pitch and 20% contribution from yaw and no deadband.
R: 4+ 1000 1000 2000 0
+1 -1
View File
@@ -37,7 +37,7 @@
APPNAME = commander
PRIORITY = SCHED_PRIORITY_MAX - 30
STACKSIZE = 4096
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
+92 -14
View File
@@ -69,7 +69,8 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <arch/board/up_cpuload.h>
@@ -101,9 +102,9 @@ static orb_advert_t stat_pub;
static uint16_t nofix_counter = 0;
static uint16_t gotfix_counter = 0;
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status);
static void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status);
static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/* pthread loops */
static void *command_handling_loop(void *arg);
@@ -111,9 +112,30 @@ static void *command_handling_loop(void *arg);
__EXPORT int commander_main(int argc, char *argv[]);
#ifdef CONFIG_TONE_ALARM
/**
* Mainloop of commander.
*/
int commander_thread_main(int argc, char *argv[]);
static int buzzer_init(void);
static void buzzer_deinit(void);
static int led_init(void);
static void led_deinit(void);
static int led_toggle(int led);
static int led_on(int led);
static int led_off(int led);
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status);
static void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status);
static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static int buzzer_init()
{
@@ -131,13 +153,7 @@ static void buzzer_deinit()
{
close(buzzer);
}
#endif
static int led_init(void);
static void led_deinit(void);
static int led_toggle(int led);
static int led_on(int led);
static int led_off(int led);
static int led_init()
{
@@ -581,6 +597,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
/*
* do not report an error for commands that are
* handled directly by MAVLink.
*/
case MAV_CMD_PREFLIGHT_STORAGE:
break;
default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED;
@@ -705,6 +728,9 @@ enum BAT_CHEM {
*/
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
PARAM_DEFINE_FLOAT(BAT_VOLT_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_VOLT_FULL, 4.05f);
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
{
float ret = 0;
@@ -721,11 +747,61 @@ float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage
return ret;
}
/****************************************************************************
* Name: commander
****************************************************************************/
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int commander_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("commander already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_create("commander", SCHED_PRIORITY_MAX - 50, 4096, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tcommander is running\n");
} else {
printf("\tcommander not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
commander_initialized = false;
@@ -1108,6 +1184,8 @@ int commander_main(int argc, char *argv[])
led_deinit();
buzzer_deinit();
thread_running = false;
return 0;
}
@@ -42,8 +42,8 @@
#include <stdio.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
@@ -94,7 +94,7 @@ usage(const char *reason)
*/
int px4_deamon_app_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
+26 -18
View File
@@ -60,10 +60,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/fixedwing_control.h>
#ifndef F_M_PI
#define F_M_PI ((float)M_PI)
#endif
#include <systemlib/param/param.h>
__EXPORT int fixedwing_control_main(int argc, char *argv[]);
@@ -291,6 +288,18 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
return output;
}
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_P, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_LIM, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_P, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_LIM, 0.0f);
/**
* Load parameters from global storage.
*
@@ -299,7 +308,6 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
* Fetches the current parameters from the global parameter storage and writes them
* to the plane_data structure
*/
static void get_parameters(plane_data_t * plane_data)
{
plane_data->Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P];
@@ -395,10 +403,10 @@ static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, fl
*/
static float calc_bearing()
{
float bearing = F_M_PI/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
float bearing = M_PI_F/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
if (bearing < 0.0f) {
bearing += 2*F_M_PI;
bearing += 2*M_PI_F;
}
return bearing;
@@ -543,11 +551,11 @@ static float calc_roll_setpoint()
} else {
setpoint = calc_bearing() - plane_data.yaw;
if (setpoint < (-35.0f/180.0f)*F_M_PI)
return (-35.0f/180.0f)*F_M_PI;
if (setpoint < (-35.0f/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
if (setpoint > (35/180.0f)*F_M_PI)
return (-35.0f/180.0f)*F_M_PI;
if (setpoint > (35/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
}
return setpoint;
@@ -568,16 +576,16 @@ static float calc_pitch_setpoint()
float setpoint = 0.0f;
if (plane_data.mode == TAKEOFF) {
setpoint = ((35/180.0f)*F_M_PI);
setpoint = ((35/180.0f)*M_PI_F);
} else {
setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance());
if (setpoint < (-35.0f/180.0f)*F_M_PI)
return (-35.0f/180.0f)*F_M_PI;
if (setpoint < (-35.0f/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
if (setpoint > (35/180.0f)*F_M_PI)
return (-35.0f/180.0f)*F_M_PI;
if (setpoint > (35/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
}
return setpoint;
@@ -797,8 +805,8 @@ int fixedwing_control_main(int argc, char *argv[])
// printf("Current Altitude: %i\n\n", (int)plane_data.alt);
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
(int)(180.0f * plane_data.roll/F_M_PI), (int)(180.0f * plane_data.pitch/F_M_PI), (int)(180.0f * plane_data.yaw/F_M_PI),
(int)(180.0f * plane_data.rollspeed/F_M_PI), (int)(180.0f * plane_data.pitchspeed/F_M_PI), (int)(180.0f * plane_data.yawspeed)/F_M_PI);
(int)(180.0f * plane_data.roll/M_PI_F), (int)(180.0f * plane_data.pitch/M_PI_F), (int)(180.0f * plane_data.yaw/M_PI_F),
(int)(180.0f * plane_data.rollspeed/M_PI_F), (int)(180.0f * plane_data.pitchspeed/M_PI_F), (int)(180.0f * plane_data.yawspeed)/M_PI_F);
// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
+1 -1
View File
@@ -37,7 +37,7 @@
APPNAME = mavlink
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
+1 -1
View File
@@ -1447,7 +1447,7 @@ int mavlink_main(int argc, char *argv[])
}
thread_should_exit = false;
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4096, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
+46 -23
View File
@@ -43,6 +43,7 @@
#include <assert.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdbool.h>
#include <string.h>
#include <systemlib/param/param.h>
@@ -145,39 +146,54 @@ int mavlink_pm_send_param(param_t param)
return mavlink_missionlib_send_message(&tx_msg);
}
static const char *mavlink_parameter_file = "/eeprom/parameters";
/**
* @return 0 on success, -1 if device open failed, -2 if writing parameters failed
*/
static int
mavlink_pm_save_eeprom()
{
const char* name = "/eeprom";
int fd = open("/eeprom", O_WRONLY | O_CREAT | O_EXCL);
unlink(mavlink_parameter_file);
if (fd < 0)
warn(1, "opening '%s' failed", name);
int fd = open(mavlink_parameter_file, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0) {
warn("opening '%s' failed", mavlink_parameter_file);
return -1;
}
int result = param_export(fd, false);
close(fd);
if (result < 0) {
warn(1, "error exporting to '%s'", name);
unlink(mavlink_parameter_file);
warn("error exporting to '%s'", mavlink_parameter_file);
return -2;
}
return 0;
}
/**
* @return 0 on success, -1 if device open failed, -2 if writing parameters failed
*/
static int
mavlink_pm_load_eeprom()
{
const char* name = "/eeprom";
int fd = open(name, O_RDONLY);
int fd = open(mavlink_parameter_file, O_RDONLY);
if (fd < 0)
warn(1, "open '%s' failed", name);
if (fd < 0) {
warn("open '%s' failed", mavlink_parameter_file);
return -1;
}
int result = param_import(fd);
close(fd);
if (result < 0)
warn(1, "error importing from '%s'", name);
if (result < 0) {
warn("error importing from '%s'", mavlink_parameter_file);
return -2;
}
return 0;
}
@@ -258,35 +274,42 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
if (((int)(cmd_mavlink.param1)) == 0) {
if (OK == mavlink_pm_load_eeprom()) {
/* read all parameters from EEPROM to RAM */
int read_ret = mavlink_pm_load_eeprom();
if (read_ret == OK) {
//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] CMD Loaded EEPROM params in RAM");
mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params");
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");
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");
}
result = MAV_RESULT_FAILED;
}
/* Write all parameters from RAM to EEPROM */
} else if (((int)(cmd_mavlink.param1)) == 1) {
if (OK == mavlink_pm_save_eeprom()) {
//printf("[mavlink pm] RAM params written to EEPROM\n");
mavlink_missionlib_send_gcs_string("[mavlink pm] RAM params written to EEPROM");
/* 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");
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");
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");
}
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");
mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request");
result = MAV_RESULT_UNSUPPORTED;
}
}
@@ -51,8 +51,27 @@
#include <float.h>
#include <math.h>
#include <systemlib/pid/pid.h>
#include <systemlib/param/param.h>
#include <arch/board/up_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.0f);
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
struct actuator_controls_s *actuators, bool verbose)
@@ -96,16 +96,16 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
mag_values.y = raw->magnetometer_ga[1]*456.0f;
mag_values.z = raw->magnetometer_ga[2]*456.0f;
static int i = 0;
// static int i = 0;
if (i == 500) {
printf("gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n",
gyro_values.x, gyro_values.y, gyro_values.z,
accel_values.x, accel_values.y, accel_values.z,
mag_values.x, mag_values.y, mag_values.z);
i = 0;
}
i++;
// if (i == 500) {
// printf("[att estim bm] gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n",
// gyro_values.x, gyro_values.y, gyro_values.z,
// accel_values.x, accel_values.y, accel_values.z,
// mag_values.x, mag_values.y, mag_values.z);
// i = 0;
// }
// i++;
attitude_blackmagic(&accel_values, &mag_values, &gyro_values);
+62
View File
@@ -60,6 +60,8 @@
#include <arch/board/up_adc.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
@@ -127,6 +129,66 @@ extern uint64_t ppm_last_valid_decode;
/* ORB topic publishing our results */
static orb_advert_t sensor_pub;
PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
/**
* Sensor readout and publishing.
*
+46 -1
View File
@@ -35,6 +35,10 @@
* @file param.c
*
* Global parameter store.
*
* Note that it might make sense to convert this into a driver. That would
* offer some interesting options regarding state for e.g. ORB advertisements
* and background parameter saving.
*/
#include <debug.h>
@@ -46,10 +50,15 @@
#include <sys/stat.h>
#include <arch/board/up_hrt.h>
#include "systemlib/param/param.h"
#include "systemlib/uthash/utarray.h"
#include "systemlib/bson/tinybson.h"
#include "uORB/uORB.h"
#include "uORB/topics/parameters.h"
#if 1
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
#else
@@ -79,6 +88,9 @@ UT_array *param_values;
/** array info for the modified parameters array */
UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);
/** lock the parameter store */
static void
param_lock(void)
@@ -369,6 +381,24 @@ param_set(param_t param, const void *val)
out:
param_unlock();
/*
* If we set something, now that we have unlocked, go ahead and advertise that
* a thing has been set.
*/
if (result != 0) {
struct parameter_update_s pup = { .timestamp = hrt_absolute_time() };
/*
* Because we're a library, we can't keep a persistent advertisement
* around, so if we succeed in updating the topic, we have to toss
* the descriptor straight away.
*/
int param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
if (param_topic != -1)
close(param_topic);
}
return result;
}
@@ -407,6 +437,7 @@ param_export(int fd, bool only_unsaved)
switch (param_type(s->param)) {
case PARAM_TYPE_INT32:
param_get(s->param, &i);
if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
@@ -416,6 +447,7 @@ param_export(int fd, bool only_unsaved)
case PARAM_TYPE_FLOAT:
param_get(s->param, &f);
if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
@@ -424,7 +456,7 @@ param_export(int fd, bool only_unsaved)
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
if (bson_encoder_append_binary(&encoder,
if (bson_encoder_append_binary(&encoder,
param_name(s->param),
BSON_BIN_BINARY,
param_size(s->param),
@@ -474,6 +506,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
* ignore the node.
*/
param_t param = param_find(node->name);
if (param == PARAM_INVALID)
return 0;
@@ -487,6 +520,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
debug("unexpected type for '%s", node->name);
goto out;
}
i = node->i;
v = &i;
break;
@@ -496,6 +530,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
debug("unexpected type for '%s", node->name);
goto out;
}
f = node->d;
v = &f;
break;
@@ -505,22 +540,28 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
debug("unexpected subtype for '%s", node->name);
goto out;
}
if (bson_decoder_data_pending(decoder) != param_size(param)) {
debug("bad size for '%s'", node->name);
goto out;
}
/* XXX check actual file data size? */
tmp = malloc(param_size(param));
if (tmp == NULL) {
debug("failed allocating for '%s'", node->name);
goto out;
}
if (bson_decoder_copy_data(decoder, tmp)) {
debug("failed copying data for '%s'", node->name);
goto out;
}
v = tmp;
break;
default:
debug("unrecognised node type");
goto out;
@@ -530,6 +571,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
debug("error setting value for '%s'", node->name);
goto out;
}
if (tmp != NULL) {
free(tmp);
tmp = NULL;
@@ -538,8 +580,10 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
result = 0;
out:
if (tmp != NULL)
free(tmp);
return result;
}
@@ -559,6 +603,7 @@ param_import(int fd)
while (!done) {
result = bson_decoder_next(&decoder);
if (result != 0) {
debug("error during BSON decode");
break;
+59
View File
@@ -0,0 +1,59 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* 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 parameters.h
*
* Actuator control topics - mixer inputs.
*
* Values published to these topics are the outputs of the vehicle control
* system, and are expected to be mixed and used to drive the actuators
* (servos, speed controls, etc.) that operate the vehicle.
*
* Each topic can be published by a single controller
*/
#ifndef TOPIC_PARAMETERS_H
#define TOPIC_PARAMETERS_H
#include <stdint.h>
#include "../uORB.h"
struct parameter_update_s {
/** time at which the latest parameter was updated */
uint64_t timestamp;
};
ORB_DECLARE(parameter_update);
#endif