aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2012-08-27 22:57:20 +0200
committerLorenz Meier <lm@inf.ethz.ch>2012-08-27 22:57:20 +0200
commit2d2548e714505b1a9d1074c0f9a78c44c8363314 (patch)
treedf4b20758f361f00d40268a4fae4137c77067d0d /apps
parent2a6a151342905377c60711c1cade166ffa058e86 (diff)
downloadpx4-firmware-2d2548e714505b1a9d1074c0f9a78c44c8363314.tar.gz
px4-firmware-2d2548e714505b1a9d1074c0f9a78c44c8363314.tar.bz2
px4-firmware-2d2548e714505b1a9d1074c0f9a78c44c8363314.zip
Final parameter interface cleanup - removed last bit of old cruft, fixed a bug on parameter update notification, cleaned up API slightly in naming
Diffstat (limited to 'apps')
-rw-r--r--apps/commander/commander.c18
-rw-r--r--apps/commander/state_machine_helper.c17
-rw-r--r--apps/drivers/mpu6000/mpu6000.cpp15
-rw-r--r--apps/fixedwing_control/fixedwing_control.c70
-rw-r--r--apps/sensors/sensors.cpp26
-rw-r--r--apps/systemlib/Makefile3
-rw-r--r--apps/systemlib/param/param.c7
-rw-r--r--apps/systemlib/systemlib.c4
-rw-r--r--apps/systemlib/systemlib.h2
-rw-r--r--apps/uORB/objects_common.cpp2
-rw-r--r--apps/uORB/parameter_storage.c379
-rw-r--r--apps/uORB/parameter_storage.h152
-rw-r--r--apps/uORB/topics/parameter_update.h (renamed from apps/uORB/topics/parameters.h)15
-rw-r--r--apps/uORB/uORB.cpp3
-rw-r--r--apps/uORB/uORB.h5
15 files changed, 82 insertions, 636 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index e2d197648..352cf68f6 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -125,8 +125,8 @@ 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 do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
+static void do_mag_calibration(int status_pub, struct vehicle_status_s *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);
@@ -444,7 +444,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
close(sub_sensor_combined);
}
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status)
+void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
{
const int calibration_count = 3000;
@@ -942,13 +942,13 @@ int commander_thread_main(int argc, char *argv[])
led_toggle(LED_AMBER);
} else {
- /* Constant error indication in standby mode without GPS */
- if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR && !current_status.gps_valid) {
- led_on(LED_AMBER);
+ // /* Constant error indication in standby mode without GPS */
+ // if (!current_status.gps_valid) {
+ // led_on(LED_AMBER);
- } else {
- led_off(LED_AMBER);
- }
+ // } else {
+ // led_off(LED_AMBER);
+ // }
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 8abf427f7..462406648 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -39,7 +39,7 @@
*/
#include <stdio.h>
-#include "state_machine_helper.h"
+#include <unistd.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -47,6 +47,8 @@
#include <arch/board/up_hrt.h>
#include <mavlink/mavlink_log.h>
+#include "state_machine_helper.h"
+
static const char* system_state_txt[] = {
"SYSTEM_STATE_PREFLIGHT",
"SYSTEM_STATE_STANDBY",
@@ -75,14 +77,12 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
switch (new_state) {
case SYSTEM_STATE_MISSION_ABORT: {
/* Indoor or outdoor */
- uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
- if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+ // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
- } else {
- ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
- }
+ // } else {
+ // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
+ // }
}
break;
@@ -198,10 +198,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (invalid_state == false || old_state != new_state) {
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
+ ret = OK;
}
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
+ ret = ERROR;
}
+ return ret;
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 6060a80f8..7c5ff2ee0 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -55,6 +55,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
+#include <systemlib/conversions.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
@@ -761,20 +762,6 @@ MPU6000::measure_trampoline(void *arg)
dev->measure();
}
-int16_t
-int16_t_from_bytes(uint8_t bytes[])
-{
- union {
- uint8_t b[2];
- int16_t w;
- } u;
-
- u.b[1] = bytes[0];
- u.b[0] = bytes[1];
-
- return u.w;
-}
-
void
MPU6000::measure()
{
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index 73a72185d..a52ecfb6e 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -575,18 +575,13 @@ static float calc_roll_setpoint()
{
float setpoint = 0;
- if (plane_data.mode == TAKEOFF) {
- setpoint = 0;
+ setpoint = calc_bearing() - plane_data.yaw;
- } else {
- setpoint = calc_bearing() - plane_data.yaw;
+ if (setpoint < (-35.0f/180.0f)*M_PI_F)
+ return (-35.0f/180.0f)*M_PI_F;
- if (setpoint < (-35.0f/180.0f)*M_PI_F)
- return (-35.0f/180.0f)*M_PI_F;
-
- if (setpoint > (35/180.0f)*M_PI_F)
- return (-35.0f/180.0f)*M_PI_F;
- }
+ if (setpoint > (35/180.0f)*M_PI_F)
+ return (-35.0f/180.0f)*M_PI_F;
return setpoint;
}
@@ -605,10 +600,10 @@ static float calc_pitch_setpoint()
{
float setpoint = 0.0f;
- if (plane_data.mode == TAKEOFF) {
- setpoint = ((35/180.0f)*M_PI_F);
+ // if (plane_data.mode == TAKEOFF) {
+ // setpoint = ((35/180.0f)*M_PI_F);
- } else {
+ // } else {
setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance());
if (setpoint < (-35.0f/180.0f)*M_PI_F)
@@ -616,7 +611,7 @@ static float calc_pitch_setpoint()
if (setpoint > (35/180.0f)*M_PI_F)
return (-35.0f/180.0f)*M_PI_F;
- }
+ // }
return setpoint;
}
@@ -634,46 +629,24 @@ static float calc_throttle_setpoint()
{
float setpoint = 0;
- // if TAKEOFF full throttle
- if (plane_data.mode == TAKEOFF) {
- setpoint = 60.0f;
- }
+ // // if TAKEOFF full throttle
+ // if (plane_data.mode == TAKEOFF) {
+ // setpoint = 60.0f;
+ // }
- // if CRUISE - parameter value
- if (plane_data.mode == CRUISE) {
+ // // if CRUISE - parameter value
+ // if (plane_data.mode == CRUISE) {
setpoint = plane_data.airspeed;
- }
+ // }
- // if LAND no throttle
- if (plane_data.mode == LAND) {
- setpoint = 0.0f;
- }
+ // // if LAND no throttle
+ // if (plane_data.mode == LAND) {
+ // setpoint = 0.0f;
+ // }
return setpoint;
}
-/**
- * set_plane_mode
- *
- * Sets the plane mode
- * (TAKEOFF, CRUISE, LOITER or LAND)
- *
- */
-
-static void set_plane_mode()
-{
- if (plane_data.alt < 10.0f) {
- plane_data.mode = TAKEOFF;
-
- } else {
- plane_data.mode = CRUISE;
- // TODO: if reached waypoint and no further waypoint exists, go to LOITER mode
- }
-
- // Debug override - don't need TAKEOFF mode for now
- plane_data.mode = CRUISE;
-}
-
/*
* fixedwing_control_main
*
@@ -835,9 +808,6 @@ int fixedwing_control_main(int argc, char *argv[])
/************************************************************************************************************/
}
- /* Set plane mode */
- set_plane_mode();
-
/* Calculate the P,Q,R body rates of the aircraft */
//calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw,
// plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index b3fc5b642..bcde4b14f 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -69,6 +69,7 @@
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
@@ -162,6 +163,7 @@ private:
int _mag_sub; /**< raw mag data subscription */
int _baro_sub; /**< raw baro data subscription */
int _vstatus_sub; /**< vehicle status subscription */
+ int _params_sub; /**< notification of parameter updates */
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
@@ -281,6 +283,11 @@ private:
void vehicle_status_poll();
/**
+ * Check for changes in parameters.
+ */
+ void parameter_update_poll();
+
+ /**
* Poll the ADC and update readings to suit.
*
* @param raw Combined sensor data structure into which
@@ -330,6 +337,7 @@ Sensors::Sensors() :
_mag_sub(-1),
_baro_sub(-1),
_vstatus_sub(-1),
+ _params_sub(-1),
/* publications */
_sensor_pub(-1),
@@ -724,9 +732,21 @@ Sensors::vehicle_status_poll()
_publishing = true;
}
}
+}
- /* XXX run the param update more often right now */
+void
+Sensors::parameter_update_poll()
+{
+ bool param_updated;
+ /* Check if any parameter has changed */
+ orb_check(_params_sub, &param_updated);
+ if (param_updated)
{
+ /* read from param to clear updated flag */
+ struct parameter_update_s update;
+ orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+ printf("PARAMS UPDATED\n");
/* update parameters */
parameters_update();
@@ -886,6 +906,7 @@ Sensors::task_main()
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ _params_sub = orb_subscribe(ORB_ID(parameter_update));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
@@ -957,6 +978,9 @@ Sensors::task_main()
/* check vehicle status for changes to publication state */
vehicle_status_poll();
+ /* check parameters for updates */
+ parameter_update_poll();
+
/* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */
raw.timestamp = hrt_absolute_time();
diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile
index 0b3cb3c7b..8a14f0f6e 100644
--- a/apps/systemlib/Makefile
+++ b/apps/systemlib/Makefile
@@ -39,7 +39,8 @@ CSRCS = err.c \
hx_stream.c \
perf_counter.c \
param/param.c \
- bson/tinybson.c
+ bson/tinybson.c \
+ conversions.c
#
# XXX this really should be a CONFIG_* test
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index acd5674ab..9391558e9 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -57,7 +57,7 @@
#include "systemlib/bson/tinybson.h"
#include "uORB/uORB.h"
-#include "uORB/topics/parameters.h"
+#include "uORB/topics/parameter_update.h"
#if 1
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
@@ -318,6 +318,7 @@ int
param_set(param_t param, const void *val)
{
int result = -1;
+ bool params_changed = false;
param_lock();
@@ -378,7 +379,7 @@ param_set(param_t param, const void *val)
}
s->unsaved = true;
-
+ params_changed = true;
result = 0;
}
@@ -389,7 +390,7 @@ out:
* If we set something, now that we have unlocked, go ahead and advertise that
* a thing has been set.
*/
- if (result != 0) {
+ if (params_changed) {
struct parameter_update_s pup = { .timestamp = hrt_absolute_time() };
/*
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index 1433d1113..bed5b1a10 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -3,7 +3,6 @@
* 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
* are met:
@@ -35,7 +34,7 @@
/**
* @file systemlib.c
- * Implementation of commonly used low-level system-call like functions
+ * Implementation of commonly used low-level system-call like functions.
*/
#include <nuttx/config.h>
@@ -47,7 +46,6 @@
#include <sys/stat.h>
#include <unistd.h>
#include <arch/board/drv_eeprom.h>
-#include <uORB/parameter_storage.h>
#include <float.h>
#include <string.h>
diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h
index 2824cbec9..add47f440 100644
--- a/apps/systemlib/systemlib.h
+++ b/apps/systemlib/systemlib.h
@@ -34,7 +34,7 @@
/**
* @file systemlib.h
- * Definition of commonly used low-level system-call like functions
+ * Definition of commonly used low-level system-call like functions.
*/
#ifndef SYSTEMLIB_H_
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index c60a93fcb..de26dcf01 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -32,7 +32,7 @@
****************************************************************************/
/**
- * @file objects_common.h
+ * @file objects_common.cpp
*
* Common object definitions without a better home.
*/
diff --git a/apps/uORB/parameter_storage.c b/apps/uORB/parameter_storage.c
deleted file mode 100644
index f4bd1c712..000000000
--- a/apps/uORB/parameter_storage.c
+++ /dev/null
@@ -1,379 +0,0 @@
-
-#include "parameter_storage.h"
-#include <stdbool.h>
-
-
-/* Global symbols / flags */
-
-struct global_data_parameter_storage_t global_data_parameter_storage_d = { /*.counter = 0, .timestamp = 0,*/ .pm = {.size = PARAM_MAX_COUNT,
- .param_values[PARAM_SYSTEM_ID] = 12,
- .param_names[PARAM_SYSTEM_ID] = "SYS_ID",
- .param_needs_write[PARAM_SYSTEM_ID] = false,
-
- .param_values[PARAM_COMP_ID] = 200,
- .param_names[PARAM_COMP_ID] = "COMP_ID",
- .param_needs_write[PARAM_COMP_ID] = false,
-
- .param_values[PARAM_FLIGHT_ENV] = (float)PX4_FLIGHT_ENVIRONMENT_INDOOR,
- .param_names[PARAM_FLIGHT_ENV] = "FLIGHT_ENV",
- .param_needs_write[PARAM_FLIGHT_ENV] = false,
-
- .param_values[PARAM_BATTERYVOLTAGE_CONVERSION] = -1.f,
- .param_names[PARAM_BATTERYVOLTAGE_CONVERSION] = "BATVOLTAG_CONV",
- .param_needs_write[PARAM_BATTERYVOLTAGE_CONVERSION] = false,
-
- .param_values[PARAM_PID_YAWPOS_P] = 0.3f,
- .param_names[PARAM_PID_YAWPOS_P] = "PID_YAWPOS_P",
- .param_needs_write[PARAM_PID_YAWPOS_P] = false,
-
- .param_values[PARAM_PID_YAWPOS_I] = 0.15f,
- .param_names[PARAM_PID_YAWPOS_I] = "PID_YAWPOS_I",
- .param_needs_write[PARAM_PID_YAWPOS_I] = false,
-
- .param_values[PARAM_PID_YAWPOS_D] = 0.0f,
- .param_names[PARAM_PID_YAWPOS_D] = "PID_YAWPOS_D",
- .param_needs_write[PARAM_PID_YAWPOS_D] = false,
-
- .param_values[PARAM_PID_YAWPOS_AWU] = 1.0f,
- .param_names[PARAM_PID_YAWPOS_AWU] = "PID_YAWPOS_AWU",
- .param_needs_write[PARAM_PID_YAWPOS_AWU] = false,
-
- .param_values[PARAM_PID_YAWPOS_LIM] = 3.0f,
- .param_names[PARAM_PID_YAWPOS_LIM] = "PID_YAWPOS_LIM",
- .param_needs_write[PARAM_PID_YAWPOS_LIM] = false,
-
- .param_values[PARAM_PID_YAWSPEED_P] = 0.1f,
- .param_names[PARAM_PID_YAWSPEED_P] = "PID_YAWSPD_P",
- .param_needs_write[PARAM_PID_YAWSPEED_P] = false,
-
- .param_values[PARAM_PID_YAWSPEED_I] = 0.02f,
- .param_names[PARAM_PID_YAWSPEED_I] = "PID_YAWSPD_I",
- .param_needs_write[PARAM_PID_YAWSPEED_I] = false,
-
- .param_values[PARAM_PID_YAWSPEED_D] = 0.0f,
- .param_names[PARAM_PID_YAWSPEED_D] = "PID_YAWSPD_D",
- .param_needs_write[PARAM_PID_YAWSPEED_D] = false,
-
- .param_values[PARAM_PID_YAWSPEED_AWU] = 0.02f,
- .param_names[PARAM_PID_YAWSPEED_AWU] = "PID_YAWSPD_AWU",
- .param_needs_write[PARAM_PID_YAWSPEED_AWU] = false,
-
- .param_values[PARAM_PID_YAWSPEED_LIM] = 0.1f,
- .param_names[PARAM_PID_YAWSPEED_LIM] = "PID_YAWSPD_LIM",
- .param_needs_write[PARAM_PID_YAWSPEED_LIM] = false,
-
- .param_values[PARAM_PID_ATT_P] = 0.3f,
- .param_names[PARAM_PID_ATT_P] = "PID_ATT_P",
- .param_needs_write[PARAM_PID_ATT_P] = false,
-
- .param_values[PARAM_PID_ATT_I] = 0.0f,
- .param_names[PARAM_PID_ATT_I] = "PID_ATT_I",
- .param_needs_write[PARAM_PID_ATT_I] = false,
-
- .param_values[PARAM_PID_ATT_D] = 0.1f,
- .param_names[PARAM_PID_ATT_D] = "PID_ATT_D",
- .param_needs_write[PARAM_PID_ATT_D] = false,
-
- .param_values[PARAM_PID_ATT_AWU] = 0.05f,
- .param_names[PARAM_PID_ATT_AWU] = "PID_ATT_AWU",
- .param_needs_write[PARAM_PID_ATT_AWU] = false,
-
- .param_values[PARAM_PID_ATT_LIM] = 0.3f,
- .param_names[PARAM_PID_ATT_LIM] = "PID_ATT_LIM",
- .param_needs_write[PARAM_PID_ATT_LIM] = false,
-
- .param_values[PARAM_PID_POS_P] = 40.0f,
- .param_names[PARAM_PID_POS_P] = "PID_POS_P",
- .param_needs_write[PARAM_PID_POS_P] = false,
-
- .param_values[PARAM_PID_POS_I] = 0.0f,
- .param_names[PARAM_PID_POS_I] = "PID_POS_I",
- .param_needs_write[PARAM_PID_POS_I] = false,
-
- .param_values[PARAM_PID_POS_D] = 0.0f,
- .param_names[PARAM_PID_POS_D] = "PID_POS_D",
- .param_needs_write[PARAM_PID_POS_D] = false,
-
- .param_values[PARAM_PID_POS_AWU] = 5.0f,
- .param_names[PARAM_PID_POS_AWU] = "PID_POS_AWU",
- .param_needs_write[PARAM_PID_POS_AWU] = false,
-
- .param_values[PARAM_PID_POS_LIM] = 0.3f,
- .param_names[PARAM_PID_POS_LIM] = "PID_POS_LIM",
- .param_needs_write[PARAM_PID_POS_LIM] = false,
-
- .param_values[PARAM_PID_POS_Z_P] = 10.0f,
- .param_names[PARAM_PID_POS_Z_P] = "PID_POS_Z_P",
- .param_needs_write[PARAM_PID_POS_Z_P] = false,
-
- .param_values[PARAM_PID_POS_Z_I] = 0.0f,
- .param_names[PARAM_PID_POS_Z_I] = "PID_POS_Z_I",
- .param_needs_write[PARAM_PID_POS_Z_I] = false,
-
- .param_values[PARAM_PID_POS_Z_D] = 0.0f,
- .param_names[PARAM_PID_POS_Z_D] = "PID_POS_Z_D",
- .param_needs_write[PARAM_PID_POS_Z_D] = false,
-
- .param_values[PARAM_PID_POS_Z_AWU] = 3.0f,
- .param_names[PARAM_PID_POS_Z_AWU] = "PID_POS_Z_AWU",
- .param_needs_write[PARAM_PID_POS_Z_AWU] = false,
-
- .param_values[PARAM_PID_POS_Z_LIM] = 0.3f,
- .param_names[PARAM_PID_POS_Z_LIM] = "PID_POS_Z_LIM",
- .param_needs_write[PARAM_PID_POS_Z_LIM] = false,
-
- .param_values[PARAM_AIRSPEED] = 30.0f,
- .param_names[PARAM_AIRSPEED] = "AIRSPEED",
- .param_needs_write[PARAM_AIRSPEED] = false,
-
- .param_values[PARAM_WPLON] = -120.0f,
- .param_names[PARAM_WPLON] = "WPLON",
- .param_needs_write[PARAM_WPLON] = false,
-
- .param_values[PARAM_WPLAT] = 38.0f,
- .param_names[PARAM_WPLAT] = "WPLAT",
- .param_needs_write[PARAM_WPLAT] = false,
-
- .param_values[PARAM_WPALT] = 500.0f,
- .param_names[PARAM_WPALT] = "WPALT",
- .param_needs_write[PARAM_WPALT] = false,
-
- .param_values[PARAM_FLIGHTMODE] = CRUISE,
- .param_names[PARAM_FLIGHTMODE] = "FLIGHTMODE",
- .param_needs_write[PARAM_FLIGHTMODE] = false,
-
- .param_values[PARAM_SENSOR_GYRO_XOFFSET] = 700.f,
- .param_names[PARAM_SENSOR_GYRO_XOFFSET] = "SENSOR_GYRO_XOF",
- .param_needs_write[PARAM_SENSOR_GYRO_XOFFSET] = false,
-
- .param_values[PARAM_SENSOR_GYRO_YOFFSET] = 1400.0f,
- .param_names[PARAM_SENSOR_GYRO_YOFFSET] = "SENSOR_GYRO_YOF",
- .param_needs_write[PARAM_SENSOR_GYRO_YOFFSET] = false,
-
- .param_values[PARAM_SENSOR_GYRO_ZOFFSET] = 0.0f,
- .param_names[PARAM_SENSOR_GYRO_ZOFFSET] = "SENSOR_GYRO_ZOF",
- .param_needs_write[PARAM_SENSOR_GYRO_ZOFFSET] = false,
-
- .param_values[PARAM_SENSOR_MAG_XOFFSET] = 422.0f,
- .param_names[PARAM_SENSOR_MAG_XOFFSET] = "SENSOR_MAG_XOF",
- .param_needs_write[PARAM_SENSOR_MAG_XOFFSET] = false,
-
- .param_values[PARAM_SENSOR_MAG_YOFFSET] = -85.0f,
- .param_names[PARAM_SENSOR_MAG_YOFFSET] = "SENSOR_MAG_YOF",
- .param_needs_write[PARAM_SENSOR_MAG_YOFFSET] = false,
-
- .param_values[PARAM_SENSOR_MAG_ZOFFSET] = -370.0f,
- .param_names[PARAM_SENSOR_MAG_ZOFFSET] = "SENSOR_MAG_ZOF",
- .param_needs_write[PARAM_SENSOR_MAG_ZOFFSET] = false,
-
- .param_values[PARAM_ATT_XOFFSET] = 0.0f,
- .param_names[PARAM_ATT_XOFFSET] = "ATT_XOFF",
- .param_needs_write[PARAM_ATT_XOFFSET] = false,
-
- .param_values[PARAM_ATT_YOFFSET] = 0.0f,
- .param_names[PARAM_ATT_YOFFSET] = "ATT_YOFF",
- .param_needs_write[PARAM_ATT_YOFFSET] = false,
-
- .param_values[PARAM_RC1_MIN] = 1000.0f,
- .param_names[PARAM_RC1_MIN] = "RC1_MIN",
- .param_needs_write[PARAM_RC1_MIN] = false,
-
- .param_values[PARAM_RC1_MAX] = 2000.0f,
- .param_names[PARAM_RC1_MAX] = "RC1_MAX",
- .param_needs_write[PARAM_RC1_MAX] = false,
-
- .param_values[PARAM_RC1_TRIM] = 1500.0f,
- .param_names[PARAM_RC1_TRIM] = "RC1_TRIM",
- .param_needs_write[PARAM_RC1_TRIM] = false,
-
- .param_values[PARAM_RC1_REV] = 1.0f,
- .param_names[PARAM_RC1_REV] = "RC1_REV",
- .param_needs_write[PARAM_RC1_REV] = false,
-
- .param_values[PARAM_RC2_MIN] = 1000.0f,
- .param_names[PARAM_RC2_MIN] = "RC2_MIN",
- .param_needs_write[PARAM_RC2_MIN] = false,
-
- .param_values[PARAM_RC2_MAX] = 2000.0f,
- .param_names[PARAM_RC2_MAX] = "RC2_MAX",
- .param_needs_write[PARAM_RC2_MAX] = false,
-
- .param_values[PARAM_RC2_TRIM] = 1500.0f,
- .param_names[PARAM_RC2_TRIM] = "RC2_TRIM",
- .param_needs_write[PARAM_RC2_TRIM] = false,
-
- .param_values[PARAM_RC2_REV] = 1.0f,
- .param_names[PARAM_RC2_REV] = "RC2_REV",
- .param_needs_write[PARAM_RC2_REV] = false,
-
- .param_values[PARAM_RC3_MIN] = 1000.0f,
- .param_names[PARAM_RC3_MIN] = "RC3_MIN",
- .param_needs_write[PARAM_RC3_MIN] = false,
-
- .param_values[PARAM_RC3_MAX] = 2000.0f,
- .param_names[PARAM_RC3_MAX] = "RC3_MAX",
- .param_needs_write[PARAM_RC3_MAX] = false,
-
- .param_values[PARAM_RC3_TRIM] = 1500.0f,
- .param_names[PARAM_RC3_TRIM] = "RC3_TRIM",
- .param_needs_write[PARAM_RC3_TRIM] = false,
-
- .param_values[PARAM_RC3_REV] = 1.0f,
- .param_names[PARAM_RC3_REV] = "RC3_REV",
- .param_needs_write[PARAM_RC3_REV] = false,
-
- .param_values[PARAM_RC4_MIN] = 1000.0f,
- .param_names[PARAM_RC4_MIN] = "RC4_MIN",
- .param_needs_write[PARAM_RC4_MIN] = false,
-
- .param_values[PARAM_RC4_MAX] = 2000.0f,
- .param_names[PARAM_RC4_MAX] = "RC4_MAX",
- .param_needs_write[PARAM_RC4_MAX] = false,
-
- .param_values[PARAM_RC4_TRIM] = 1500.0f,
- .param_names[PARAM_RC4_TRIM] = "RC4_TRIM",
- .param_needs_write[PARAM_RC4_TRIM] = false,
-
- .param_values[PARAM_RC4_REV] = 1.0f,
- .param_names[PARAM_RC4_REV] = "RC4_REV",
- .param_needs_write[PARAM_RC4_MIN] = false,
-
- .param_values[PARAM_RC5_MIN] = 1000.0f,
- .param_names[PARAM_RC5_MIN] = "RC5_MIN",
- .param_needs_write[PARAM_RC5_MIN] = false,
-
- .param_values[PARAM_RC5_MAX] = 2000.0f,
- .param_names[PARAM_RC5_MAX] = "RC5_MAX",
- .param_needs_write[PARAM_RC5_MAX] = false,
-
- .param_values[PARAM_RC5_TRIM] = 1500.0f,
- .param_names[PARAM_RC5_TRIM] = "RC5_TRIM",
- .param_needs_write[PARAM_RC5_TRIM] = false,
-
- .param_values[PARAM_RC5_REV] = 1.0f,
- .param_names[PARAM_RC5_REV] = "RC5_REV",
- .param_needs_write[PARAM_RC5_REV] = false,
-
- .param_values[PARAM_RC6_MIN] = 1000.0f,
- .param_names[PARAM_RC6_MIN] = "RC6_MIN",
- .param_needs_write[PARAM_RC6_MIN] = false,
-
- .param_values[PARAM_RC6_MAX] = 2000.0f,
- .param_names[PARAM_RC6_MAX] = "RC6_MAX",
- .param_needs_write[PARAM_RC6_MAX] = false,
-
- .param_values[PARAM_RC6_TRIM] = 1500.0f,
- .param_names[PARAM_RC6_TRIM] = "RC6_TRIM",
- .param_needs_write[PARAM_RC6_TRIM] = false,
-
- .param_values[PARAM_RC6_REV] = 1.0f,
- .param_names[PARAM_RC6_REV] = "RC6_REV",
- .param_needs_write[PARAM_RC6_REV] = false,
-
- .param_values[PARAM_RC7_MIN] = 1000,
- .param_names[PARAM_RC7_MIN] = "RC7_MIN",
- .param_needs_write[PARAM_RC7_MIN] = false,
-
- .param_values[PARAM_RC7_MAX] = 2000,
- .param_names[PARAM_RC7_MAX] = "RC7_MAX",
- .param_needs_write[PARAM_RC7_MAX] = false,
-
- .param_values[PARAM_RC7_TRIM] = 1500,
- .param_names[PARAM_RC7_TRIM] = "RC7_TRIM",
- .param_needs_write[PARAM_RC7_TRIM] = false,
-
- .param_values[PARAM_RC7_REV] = 1.0f,
- .param_names[PARAM_RC7_REV] = "RC7_REV",
- .param_needs_write[PARAM_RC7_REV] = false,
-
- .param_values[PARAM_RC8_MIN] = 1000,
- .param_names[PARAM_RC8_MIN] = "RC8_MIN",
- .param_needs_write[PARAM_RC8_MIN] = false,
-
- .param_values[PARAM_RC8_MAX] = 2000,
- .param_names[PARAM_RC8_MAX] = "RC8_MAX",
- .param_needs_write[PARAM_RC8_MAX] = false,
-
- .param_values[PARAM_RC8_TRIM] = 1500,
- .param_names[PARAM_RC8_TRIM] = "RC8_TRIM",
- .param_needs_write[PARAM_RC8_TRIM] = false,
-
- .param_values[PARAM_RC8_REV] = 1.0f,
- .param_names[PARAM_RC8_REV] = "RC8_REV",
- .param_needs_write[PARAM_RC8_REV] = false,
-
- .param_values[PARAM_ROLL_CHAN] = 1,
- .param_names[PARAM_ROLL_CHAN] = "ROLL_CHAN",
- .param_needs_write[PARAM_ROLL_CHAN] = false,
-
- .param_values[PARAM_PITCH_CHAN] = 2,
- .param_names[PARAM_PITCH_CHAN] = "PITCH_CHAN",
- .param_needs_write[PARAM_PITCH_CHAN] = false,
-
- .param_values[PARAM_THROTTLE_CHAN] = 3,
- .param_names[PARAM_THROTTLE_CHAN] = "THROTTLE_CHAN",
- .param_needs_write[PARAM_THROTTLE_CHAN] = false,
-
- .param_values[PARAM_YAW_CHAN] = 4,
- .param_names[PARAM_YAW_CHAN] = "YAW_CHAN",
- .param_needs_write[PARAM_YAW_CHAN] = false,
-
- .param_values[PARAM_OVERRIDE_CHAN] = 5,
- .param_names[PARAM_OVERRIDE_CHAN] = "OVERRIDE_CHAN",
- .param_needs_write[PARAM_OVERRIDE_CHAN] = false,
-
- .param_values[PARAM_SERVO1_MIN] = 1000.0f,
- .param_names[PARAM_SERVO1_MIN] = "SERVO1_MIN",
- .param_needs_write[PARAM_SERVO1_MIN] = false,
-
- .param_values[PARAM_SERVO1_MAX] = 2000.0f,
- .param_names[PARAM_SERVO1_MAX] = "SERVO1_MAX",
- .param_needs_write[PARAM_SERVO1_MAX] = false,
-
- .param_values[PARAM_SERVO1_TRIM] = 1500.0f,
- .param_names[PARAM_SERVO1_TRIM] = "SERVO1_TRIM",
- .param_needs_write[PARAM_SERVO1_TRIM] = false,
-
- .param_values[PARAM_SERVO2_MIN] = 1000.0f,
- .param_names[PARAM_SERVO2_MIN] = "SERVO2_MIN",
- .param_needs_write[PARAM_SERVO2_MIN] = false,
-
- .param_values[PARAM_SERVO2_MAX] = 2000.0f,
- .param_names[PARAM_SERVO2_MAX] = "SERVO2_MAX",
- .param_needs_write[PARAM_SERVO2_MAX] = false,
-
- .param_values[PARAM_SERVO2_TRIM] = 1500.0f,
- .param_names[PARAM_SERVO2_TRIM] = "SERVO2_TRIM",
- .param_needs_write[PARAM_SERVO2_TRIM] = false,
-
- .param_values[PARAM_SERVO3_MIN] = 1000.0f,
- .param_names[PARAM_SERVO3_MIN] = "SERVO3_MIN",
- .param_needs_write[PARAM_SERVO3_MIN] = false,
-
- .param_values[PARAM_SERVO3_MAX] = 2000.0f,
- .param_names[PARAM_SERVO3_MAX] = "SERVO3_MAX",
- .param_needs_write[PARAM_SERVO3_MAX] = false,
-
- .param_values[PARAM_SERVO3_TRIM] = 1500.0f,
- .param_names[PARAM_SERVO3_TRIM] = "SERVO3_TRIM",
- .param_needs_write[PARAM_SERVO3_TRIM] = false,
-
- .param_values[PARAM_SERVO4_MIN] = 1000.0f,
- .param_names[PARAM_SERVO4_MIN] = "SERVO4_MIN",
- .param_needs_write[PARAM_SERVO4_MIN] = false,
-
- .param_values[PARAM_SERVO4_MAX] = 2000.0f,
- .param_names[PARAM_SERVO4_MAX] = "SERVO4_MAX",
- .param_needs_write[PARAM_SERVO4_MAX] = false,
-
- .param_values[PARAM_SERVO4_TRIM] = 1500.0f,
- .param_names[PARAM_SERVO4_TRIM] = "SERVO4_TRIM",
- .param_needs_write[PARAM_SERVO4_TRIM] = false,
-
- .param_values[PARAM_SERVO_SCALE] = 20.0f,
- .param_names[PARAM_SERVO_SCALE] = "SERVO_SCALE",
- .param_needs_write[PARAM_SERVO_SCALE] = false
- }
-};
-
-struct global_data_parameter_storage_t *global_data_parameter_storage = &global_data_parameter_storage_d;
diff --git a/apps/uORB/parameter_storage.h b/apps/uORB/parameter_storage.h
deleted file mode 100644
index aa4c1bb4f..000000000
--- a/apps/uORB/parameter_storage.h
+++ /dev/null
@@ -1,152 +0,0 @@
-/* Structure for storage of parameters */
-
-#ifndef GLOBAL_DATA_PARAMETER_STORAGE_T_H_ //adjust this line!
-#define GLOBAL_DATA_PARAMETER_STORAGE_T_H_ //adjust this line!
-
-#define PX4_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16
-#define MAX_PARAM_NAME_LEN 16
-
-#include <stdbool.h>
-#include <stdint.h>
-
-enum MODE {
- TAKEOFF = 1,
- CRUISE,
- LOITER,
- LAND
-};
-
-enum PARAM {
- PARAM_SYSTEM_ID = 0, ///< System ID used for communication
- PARAM_COMP_ID, ///< Component ID
- PARAM_FLIGHT_ENV, ///< Flight environment, indoor or outdoor
- PARAM_BATTERYVOLTAGE_CONVERSION, ///< Conversion factor from adc measurement to millivolts. if not set the sensor app will use the default value for the ardrone board
- PARAM_PID_YAWPOS_P,
- PARAM_PID_YAWPOS_I,
- PARAM_PID_YAWPOS_D,
- PARAM_PID_YAWPOS_AWU,
- PARAM_PID_YAWPOS_LIM,
- PARAM_PID_YAWSPEED_P,
- PARAM_PID_YAWSPEED_I,
- PARAM_PID_YAWSPEED_D,
- PARAM_PID_YAWSPEED_AWU,
- PARAM_PID_YAWSPEED_LIM,
- PARAM_PID_ATT_P,
- PARAM_PID_ATT_I,
- PARAM_PID_ATT_D,
- PARAM_PID_ATT_AWU,
- PARAM_PID_ATT_LIM,
- PARAM_PID_POS_P,
- PARAM_PID_POS_I,
- PARAM_PID_POS_D,
- PARAM_PID_POS_AWU,
- PARAM_PID_POS_LIM,
- PARAM_PID_POS_Z_P,
- PARAM_PID_POS_Z_I,
- PARAM_PID_POS_Z_D,
- PARAM_PID_POS_Z_AWU,
- PARAM_PID_POS_Z_LIM,
- PARAM_AIRSPEED,
- PARAM_WPLON,
- PARAM_WPLAT,
- PARAM_WPALT,
- PARAM_FLIGHTMODE,
- PARAM_SENSOR_GYRO_XOFFSET,
- PARAM_SENSOR_GYRO_YOFFSET,
- PARAM_SENSOR_GYRO_ZOFFSET,
- PARAM_SENSOR_MAG_XOFFSET,
- PARAM_SENSOR_MAG_YOFFSET,
- PARAM_SENSOR_MAG_ZOFFSET,
- PARAM_ATT_XOFFSET,
- PARAM_ATT_YOFFSET,
- PARAM_RC1_MIN,
- PARAM_RC1_MAX,
- PARAM_RC1_TRIM,
- PARAM_RC1_REV,
- PARAM_RC2_MIN,
- PARAM_RC2_MAX,
- PARAM_RC2_TRIM,
- PARAM_RC2_REV,
- PARAM_RC3_MIN,
- PARAM_RC3_MAX,
- PARAM_RC3_TRIM,
- PARAM_RC3_REV,
- PARAM_RC4_MIN,
- PARAM_RC4_MAX,
- PARAM_RC4_TRIM,
- PARAM_RC4_REV,
- PARAM_RC5_MIN,
- PARAM_RC5_MAX,
- PARAM_RC5_TRIM,
- PARAM_RC5_REV,
- PARAM_RC6_MIN,
- PARAM_RC6_MAX,
- PARAM_RC6_TRIM,
- PARAM_RC6_REV,
- PARAM_RC7_MIN,
- PARAM_RC7_MAX,
- PARAM_RC7_TRIM,
- PARAM_RC7_REV,
- PARAM_RC8_MIN,
- PARAM_RC8_MAX,
- PARAM_RC8_TRIM,
- PARAM_RC8_REV,
- PARAM_THROTTLE_CHAN,
- PARAM_ROLL_CHAN,
- PARAM_PITCH_CHAN,
- PARAM_YAW_CHAN,
- PARAM_OVERRIDE_CHAN,
- PARAM_SERVO1_MIN,
- PARAM_SERVO1_MAX,
- PARAM_SERVO1_TRIM,
- PARAM_SERVO2_MIN,
- PARAM_SERVO2_MAX,
- PARAM_SERVO2_TRIM,
- PARAM_SERVO3_MIN,
- PARAM_SERVO3_MAX,
- PARAM_SERVO3_TRIM,
- PARAM_SERVO4_MIN,
- PARAM_SERVO4_MAX,
- PARAM_SERVO4_TRIM,
- PARAM_SERVO_SCALE,
- PARAM_MAX_COUNT ///< LEAVE THIS IN PLACE AS LAST ELEMENT!
-};
-
-#pragma pack(push, 1)
-struct px4_parameter_storage_t {
- float param_values[PARAM_MAX_COUNT]; ///< Parameter values
- bool param_needs_write[PARAM_MAX_COUNT];
- const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names
- uint16_t next_param;
- uint16_t size;
-};
-#pragma pack(pop)
-
-
-#define PX4_FLIGHT_ENVIRONMENT_INDOOR 0
-#define PX4_FLIGHT_ENVIRONMENT_OUTDOOR 1
-#define PX4_FLIGHT_ENVIRONMENT_TESTING 2 //NO check for position fix in this environment
-
-#pragma pack(push, 1)
-struct global_data_parameter_storage_t {
-
- /* use of a counter and timestamp recommended (but not necessary) */
-
-// uint16_t counter; //incremented by the writing thread everytime new data is stored
- uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
-
- /* Actual data, this is specific to the type of data which is stored in this struct */
-
- //***** Start: Add your variables here *****
-
- /* Parameters (set by a param_set mavlink message */
- struct px4_parameter_storage_t pm;
-
- //*****END: Add your variables here *****
-
-};
-#pragma pack(pop)
-
-__attribute__ ((visibility ("default"))) extern struct global_data_parameter_storage_t *global_data_parameter_storage; //adjust this line!
-
-#endif
diff --git a/apps/uORB/topics/parameters.h b/apps/uORB/topics/parameter_update.h
index 9dd76e8bc..300e895c7 100644
--- a/apps/uORB/topics/parameters.h
+++ b/apps/uORB/topics/parameter_update.h
@@ -32,19 +32,12 @@
****************************************************************************/
/**
- * @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
+ * @file parameter_update.h
+ * Notification about a parameter update.
*/
-#ifndef TOPIC_PARAMETERS_H
-#define TOPIC_PARAMETERS_H
+#ifndef TOPIC_PARAMETER_UPDATE_H
+#define TOPIC_PARAMETER_UPDATE_H
#include <stdint.h>
#include "../uORB.h"
diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp
index c4434e4ed..72a60f871 100644
--- a/apps/uORB/uORB.cpp
+++ b/apps/uORB/uORB.cpp
@@ -32,7 +32,8 @@
****************************************************************************/
/**
- * @file A lightweight object broker.
+ * @file uORB.cpp
+ * A lightweight object broker.
*/
#include <nuttx/config.h>
diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h
index eb068d2b7..172c37d0f 100644
--- a/apps/uORB/uORB.h
+++ b/apps/uORB/uORB.h
@@ -35,14 +35,13 @@
#define _UORB_UORB_H
/**
- * @file API for the uORB lightweight object broker.
+ * @file uORB.h
+ * API for the uORB lightweight object broker.
*/
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
- // Hack until parameter storage is cleaned up
-#include "parameter_storage.h"
// Hack until everything is using this header
#include <systemlib/visibility.h>