From 4adfea7fa9bc7ab7a0b251280f62ecd7d14f2a38 Mon Sep 17 00:00:00 2001 From: jwilson Date: Tue, 16 Feb 2016 20:01:27 -0800 Subject: [PATCH] Resolved shared memory parameter problems and removed additional debug code. --- .../mc_att_control/mc_att_control_main.cpp | 15 -------- src/modules/sensors/sensors.cpp | 4 -- src/modules/systemlib/param/param_shmem.c | 38 ++++++++++++------- src/systemcmds/param/param.c | 14 ++----- 4 files changed, 29 insertions(+), 42 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 67884716d2..50fbc9420b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -839,9 +839,6 @@ MulticopterAttitudeControl::task_main() continue; } - // TODO-JYW: TESTING-TESTING: - warnx("_ctrl_state_sub is producing data"); - /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("mc att ctrl: poll error %d, %d", pret, errno); @@ -887,9 +884,6 @@ MulticopterAttitudeControl::task_main() } } - // TODO-JYW: TESTING-TESTING: - warnx("_v_control_mode.flag_control_attitude_enabled: %d", _v_control_mode.flag_control_attitude_enabled); - if (_v_control_mode.flag_control_attitude_enabled) { if (_ts_opt_recovery == nullptr) { @@ -959,9 +953,6 @@ MulticopterAttitudeControl::task_main() } } - // TODO-JYW: TESTING-TESTING: - warnx("_v_control_mode.flag_control_rates_enabled: %d", _v_control_mode.flag_control_rates_enabled); - if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); @@ -978,15 +969,9 @@ MulticopterAttitudeControl::task_main() _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); - // TODO-JYW: TESTING-TESTING: - warnx("_actuators_0_circuit_breaker_enabled: %d", _actuators_0_circuit_breaker_enabled); - if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { - // TODO-JYW: TESTING-TESTING: - warnx("publishing actuator controls"); - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a44f8d76f5..bf39233ba9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2231,9 +2231,6 @@ Sensors::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warnx("poll error: pret: %d, errno: %d, gyro_sub: %d", pret, errno, fds[0].fd); - - // TODO-JYW: TESTING-TESTING /* if the polling operation failed because no gyro sensor is available yet, * then attempt to subscribe once again */ @@ -2241,7 +2238,6 @@ Sensors::task_main() _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); fds[0].fd = _gyro_sub[0]; - warnx("poll error: gyro count: %d, gyro sub: %d", _gyro_count, fds[0].fd); } continue; } diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c index 44d96d7d3b..7af6644968 100644 --- a/src/modules/systemlib/param/param_shmem.c +++ b/src/modules/systemlib/param/param_shmem.c @@ -33,6 +33,8 @@ * ****************************************************************************/ +// TODO-JYW: TESTING-TESTING +#define DEBUG_BUILD 1 /** * @file param.c @@ -126,7 +128,7 @@ uint64_t sync_other_prev_time = 0, sync_other_current_time = 0; extern void update_to_shmem(param_t param, union param_value_u value); extern int update_from_shmem(param_t param, union param_value_u *value); -static int param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes); +static int param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved); unsigned char set_called_from_get = 0; static int param_import_done = @@ -251,9 +253,12 @@ param_find_changed(param_t param) } static void -param_notify_changes(void) +param_notify_changes(bool is_saved) { - struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; + struct parameter_update_s pup = { .timestamp = hrt_absolute_time(), .saved = is_saved }; + + // TODO-JYW: TESTING-TESTING: + warnx("param_notify_changes: notifying of changes, timestamp: %lld, is_saved: %d", pup.timestamp, pup.saved); /* * If we don't have a handle to our topic, create one now; otherwise @@ -507,7 +512,7 @@ param_get(param_t param, void *val) if (update_from_shmem(param, &value)) { set_called_from_get = 1; - param_set_internal(param, &value, true, false); + param_set_internal(param, &value, true, false, false); set_called_from_get = 0; } @@ -541,7 +546,7 @@ param_get(param_t param, void *val) } static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) +param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved) { int result = -1; bool params_changed = false; @@ -631,7 +636,7 @@ out: if (!param_import_done) { notify_changes = 0; } if (params_changed && notify_changes) { - param_notify_changes(); + param_notify_changes(is_saved); } if (result == 0 && !set_called_from_get) { @@ -660,13 +665,19 @@ out: int param_set(param_t param, const void *val) { - return param_set_internal(param, val, false, true); + return param_set_internal(param, val, false, true, false); +} + +int +param_set_no_autosave(param_t param, const void *val) +{ + return param_set_internal(param, val, false, true, true); } int param_set_no_notification(param_t param, const void *val) { - return param_set_internal(param, val, false, false); + return param_set_internal(param, val, false, false, false); } bool @@ -719,7 +730,7 @@ param_reset(param_t param) param_unlock(); if (s != NULL) { - param_notify_changes(); + param_notify_changes(false); } return (!param_found); @@ -739,7 +750,7 @@ param_reset_all(void) param_unlock(); - param_notify_changes(); + param_notify_changes(false); } void @@ -771,7 +782,7 @@ param_reset_excludes(const char *excludes[], int num_excludes) param_unlock(); - param_notify_changes(); + param_notify_changes(false); } #ifdef __PX4_QURT @@ -823,7 +834,7 @@ param_save_default(void) if (fd < 0) { PX4_ERR("failed to open param file: %s", filename); - return ERROR; + goto exit; } res = param_export(fd, false); @@ -834,6 +845,7 @@ param_save_default(void) } PARAM_CLOSE(fd); + fd = -1; exit: @@ -1095,7 +1107,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) goto out; } - if (param_set_internal(param, v, state->mark_saved, true)) { + if (param_set_internal(param, v, state->mark_saved, true, false)) { debug("error setting value for '%s'", node->name); goto out; } diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index b3d897b0f0..49f6554994 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -448,12 +448,10 @@ do_set(const char *name, const char *val, bool fail_on_not_found) param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); printf("curr: %ld", (long)i); -// FIXME: the call paramset_no_autosave doesn't exist on Eagle yet. -#ifndef CONFIG_ARCH_BOARD_EAGLE param_set_no_autosave(param, &newval); -#else - param_set(param, &newval); -#endif + // TODO-JYW: TESTING-TESTING: Reenabling param_set functionality. + param_set_no_autosave(param, &newval); +>>>>>>> bce2306... Resolved shared memory parameter problems and removed additional debug code. printf(" -> new: %ld\n", (long)newval); } } @@ -475,12 +473,8 @@ do_set(const char *name, const char *val, bool fail_on_not_found) param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); printf("curr: %4.4f", (double)f); -// FIXME: the call paramset_no_autosave doesn't exist on Eagle yet. -#ifndef CONFIG_ARCH_BOARD_EAGLE + // TODO-JYW: TESTING-TESTING: Reenabling param_set functionality. param_set_no_autosave(param, &newval); -#else - param_set(param, &newval); -#endif printf(" -> new: %4.4f\n", (double)newval); }