diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-22 09:19:43 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-22 09:19:43 +0200 |
commit | d2e757aa3c9c352701f935ea269bcf862042b9a2 (patch) | |
tree | a92d6a4d09e77ff8ea6211ee11042afe30954ca7 | |
parent | 72979032e9bfef200809e97663c613b7b530b011 (diff) | |
parent | c8645a7e530e0adcfafb17325ea05fbdd7c61ae2 (diff) | |
download | px4-firmware-d2e757aa3c9c352701f935ea269bcf862042b9a2.tar.gz px4-firmware-d2e757aa3c9c352701f935ea269bcf862042b9a2.tar.bz2 px4-firmware-d2e757aa3c9c352701f935ea269bcf862042b9a2.zip |
Merged parameter changes
-rw-r--r-- | ROMFS/Makefile | 4 | ||||
-rw-r--r-- | ROMFS/mixers/FMU_quad_+.mix | 7 | ||||
-rw-r--r-- | ROMFS/mixers/FMU_quad_x.mix (renamed from ROMFS/mixers/FMU_multirotor.mix) | 0 | ||||
-rw-r--r-- | apps/commander/Makefile | 2 | ||||
-rw-r--r-- | apps/commander/commander.c | 106 | ||||
-rw-r--r-- | apps/examples/px4_deamon_app/px4_deamon_app.c | 6 | ||||
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 44 | ||||
-rw-r--r-- | apps/mavlink/Makefile | 2 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 2 | ||||
-rw-r--r-- | apps/mavlink/mavlink_parameters.c | 69 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 19 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 20 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 62 | ||||
-rw-r--r-- | apps/systemlib/param/param.c | 47 | ||||
-rw-r--r-- | apps/uORB/topics/parameters.h | 59 |
15 files changed, 376 insertions, 73 deletions
diff --git a/ROMFS/Makefile b/ROMFS/Makefile index 6437a1e97..cc419a639 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -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 diff --git a/ROMFS/mixers/FMU_quad_+.mix b/ROMFS/mixers/FMU_quad_+.mix new file mode 100644 index 000000000..550ed8316 --- /dev/null +++ b/ROMFS/mixers/FMU_quad_+.mix @@ -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 diff --git a/ROMFS/mixers/FMU_multirotor.mix b/ROMFS/mixers/FMU_quad_x.mix index 285a40e1a..285a40e1a 100644 --- a/ROMFS/mixers/FMU_multirotor.mix +++ b/ROMFS/mixers/FMU_quad_x.mix diff --git a/apps/commander/Makefile b/apps/commander/Makefile index f2972ac4e..d12697274 100644 --- a/apps/commander/Makefile +++ b/apps/commander/Makefile @@ -37,7 +37,7 @@ APPNAME = commander PRIORITY = SCHED_PRIORITY_MAX - 30 -STACKSIZE = 4096 +STACKSIZE = 2048 INCLUDES = $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a642ac093..75946fb24 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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,12 +747,62 @@ 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; } diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c index 91e60a8f9..052a36cb9 100644 --- a/apps/examples/px4_deamon_app/px4_deamon_app.c +++ b/apps/examples/px4_deamon_app/px4_deamon_app.c @@ -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")) { diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 7ea4101ab..42534a0a7 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -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)); diff --git a/apps/mavlink/Makefile b/apps/mavlink/Makefile index d4e9a5762..8ddb69b71 100644 --- a/apps/mavlink/Makefile +++ b/apps/mavlink/Makefile @@ -37,7 +37,7 @@ APPNAME = mavlink PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +STACKSIZE = 2048 INCLUDES = $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 596953789..84ea0aae2 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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); } diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index ab788c461..99df42738 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -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; } } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 367a0c3e1..b21a20ad1 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -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) diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index fd3ed1137..97d7d39b7 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -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; - - 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++; + // static int i = 0; + + // 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); diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 891167d1c..b5875cb68 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -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. * diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 692489cd4..7d0c75670 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -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; diff --git a/apps/uORB/topics/parameters.h b/apps/uORB/topics/parameters.h new file mode 100644 index 000000000..9dd76e8bc --- /dev/null +++ b/apps/uORB/topics/parameters.h @@ -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
\ No newline at end of file |