From 2d2548e714505b1a9d1074c0f9a78c44c8363314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Aug 2012 22:57:20 +0200 Subject: Final parameter interface cleanup - removed last bit of old cruft, fixed a bug on parameter update notification, cleaned up API slightly in naming --- apps/commander/commander.c | 18 +- apps/commander/state_machine_helper.c | 17 +- apps/drivers/mpu6000/mpu6000.cpp | 15 +- apps/fixedwing_control/fixedwing_control.c | 70 ++---- apps/sensors/sensors.cpp | 26 +- apps/systemlib/Makefile | 3 +- apps/systemlib/param/param.c | 7 +- apps/systemlib/systemlib.c | 4 +- apps/systemlib/systemlib.h | 2 +- apps/uORB/objects_common.cpp | 2 +- apps/uORB/parameter_storage.c | 379 ----------------------------- apps/uORB/parameter_storage.h | 152 ------------ apps/uORB/topics/parameter_update.h | 52 ++++ apps/uORB/topics/parameters.h | 59 ----- apps/uORB/uORB.cpp | 3 +- apps/uORB/uORB.h | 5 +- 16 files changed, 130 insertions(+), 684 deletions(-) delete mode 100644 apps/uORB/parameter_storage.c delete mode 100644 apps/uORB/parameter_storage.h create mode 100644 apps/uORB/topics/parameter_update.h delete mode 100644 apps/uORB/topics/parameters.h 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 -#include "state_machine_helper.h" +#include #include #include @@ -47,6 +47,8 @@ #include #include +#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 #include +#include #include #include @@ -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 #include #include +#include #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 */ @@ -280,6 +282,11 @@ private: */ void vehicle_status_poll(); + /** + * Check for changes in parameters. + */ + void parameter_update_poll(); + /** * Poll the ADC and update readings to suit. * @@ -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, ¶m_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 * - * * 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 @@ -47,7 +46,6 @@ #include #include #include -#include #include #include 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 - - -/* 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 -#include - -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/parameter_update.h b/apps/uORB/topics/parameter_update.h new file mode 100644 index 000000000..300e895c7 --- /dev/null +++ b/apps/uORB/topics/parameter_update.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 parameter_update.h + * Notification about a parameter update. + */ + +#ifndef TOPIC_PARAMETER_UPDATE_H +#define TOPIC_PARAMETER_UPDATE_H + +#include +#include "../uORB.h" + +struct parameter_update_s { + /** time at which the latest parameter was updated */ + uint64_t timestamp; +}; + +ORB_DECLARE(parameter_update); + +#endif \ No newline at end of file diff --git a/apps/uORB/topics/parameters.h b/apps/uORB/topics/parameters.h deleted file mode 100644 index 9dd76e8bc..000000000 --- a/apps/uORB/topics/parameters.h +++ /dev/null @@ -1,59 +0,0 @@ -/**************************************************************************** - * - * 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 -#include "../uORB.h" - -struct parameter_update_s { - /** time at which the latest parameter was updated */ - uint64_t timestamp; -}; - -ORB_DECLARE(parameter_update); - -#endif \ No newline at end of file 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 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 #include #include - // Hack until parameter storage is cleaned up -#include "parameter_storage.h" // Hack until everything is using this header #include -- cgit v1.2.3