diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 7a424970f6..71b670888e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -48,6 +48,7 @@ then echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV" else echo "[i] Error loading mixer: $MIXER_FILE" + echo "ERROR: Could not load mixer: $MIXER_FILE" >> $LOG_FILE tone_alarm $TUNE_ERR fi @@ -56,6 +57,7 @@ else if [ $MIXER != skip ] then echo "[i] Mixer not defined" + echo "ERROR: Mixer not defined" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -125,8 +127,10 @@ then echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" else echo "[i] Error loading mixer: $MIXER_AUX_FILE" + echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE fi else + echo "ERROR: Could not start: fmu mode_pwm" >> $LOG_FILE tone_alarm $TUNE_ERR fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1237d9bb11..e100e6b4b7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -233,11 +233,11 @@ then set IO_PRESENT yes else - echo "PX4IO update failed" >> $LOG_FILE + echo "ERROR: PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi else - echo "PX4IO update failed" >> $LOG_FILE + echo "ERROR: PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -246,6 +246,7 @@ then if [ $IO_PRESENT == no ] then echo "[i] ERROR: PX4IO not found" + echo "ERROR: PX4IO not found" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -328,6 +329,7 @@ then then sh /etc/init.d/rc.io else + echo "ERROR: PX4IO start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -339,6 +341,7 @@ then echo "[i] FMU mode_$FMU_MODE started" else echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + echo "ERROR: FMU start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi @@ -372,6 +375,7 @@ then echo "[i] MK started" else echo "[i] ERROR: MK start failed" + echo "ERROR: MK start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi unset MKBLCTRL_ARG @@ -385,6 +389,7 @@ then echo "[i] HIL output started" else echo "[i] ERROR: HIL output start failed" + echo "ERROR: HIL output start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -402,6 +407,7 @@ then sh /etc/init.d/rc.io else echo "[i] ERROR: PX4IO start failed" + echo "ERROR: PX4IO start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -413,6 +419,7 @@ then echo "[i] FMU mode_$FMU_MODE started" else echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + echo "ERROR: FMU mode_$FMU_MODE start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 2a7e493a39..430f7dd768 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -147,6 +147,8 @@ float32 load # processor load from 0 to 1 float32 battery_voltage float32 battery_current float32 battery_remaining +float32 battery_discharged_mah +uint32 battery_cell_count uint8 battery_warning # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum uint16 drop_rate_comm diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bab6bd2076..4995374125 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1509,7 +1509,9 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; + status.battery_discharged_mah = battery.discharged_mah; status.condition_battery_voltage_valid = true; + status.battery_cell_count = battery_get_n_cells(); /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c1d6b08423..b902952ae2 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -332,6 +332,10 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } +unsigned battery_get_n_cells() { + return bat_n_cells; +} + float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index bf0c0505d2..d2aace2a40 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -89,4 +89,6 @@ int battery_init(); */ float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); +unsigned battery_get_n_cells(); + #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 3030bc1fd4..fdb4e18c57 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -310,8 +310,12 @@ MavlinkFTP::_workList(PayloadHeader* payload) DIR *dp = opendir(dirPath); if (dp == nullptr) { +#ifdef MAVLINK_FTP_UNIT_TEST + warnx("File open failed"); +#else _mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)"); _mavlink->send_statustext_critical(dirPath); +#endif // this is not an FTP error, abort directory read and continue payload->data[offset++] = kDirentSkip; @@ -334,8 +338,12 @@ MavlinkFTP::_workList(PayloadHeader* payload) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { +#ifdef MAVLINK_FTP_UNIT_TEST + warnx("readdir_r failed"); +#else _mavlink->send_statustext_critical("FTP: list readdir_r failure"); _mavlink->send_statustext_critical(dirPath); +#endif payload->data[offset++] = kDirentSkip; *((char *)&payload->data[offset]) = '\0'; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 33871a048e..e62d223e0c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -546,6 +546,27 @@ protected: status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); + + /* battery status message with higher resolution */ + mavlink_battery_status_t bat_msg; + bat_msg.id = 0; + bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; + bat_msg.type = MAV_BATTERY_TYPE_LIPO; + bat_msg.temperature = INT16_MAX; + for (unsigned i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) { + if (i < status.battery_cell_count) { + bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f; + } else { + bat_msg.voltages[i] = 0; + } + } + bat_msg.current_battery = status.battery_current * 100.0f; + bat_msg.current_consumed = status.battery_discharged_mah; + bat_msg.energy_consumed = -1.0f; + bat_msg.battery_remaining = (status.battery_voltage > 0) ? + status.battery_remaining * 100.0f : -1; + + _mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ca6b226934..3c4fa76a0c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1388,7 +1388,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); + map_projection_init(&_hil_local_proj_ref, lat, lon); hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8d04c0b966..00c1d7be40 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -145,6 +145,19 @@ PARAM_DEFINE_INT32(SDLOG_RATE, -1); */ PARAM_DEFINE_INT32(SDLOG_EXT, -1); +/** + * Use timestamps only if GPS 3D fix is available + * + * A value of 1 constrains the log folder creation + * to only use the time stamp if a 3D GPS lock is + * present. + * + * @min 0 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0); + #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ } else { \ @@ -164,6 +177,7 @@ static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; +static bool _gpstime_only = false; #define MOUNTPOINT "/fs/microsd" static const char *mountpoint = MOUNTPOINT; @@ -273,6 +287,11 @@ static void handle_status(struct vehicle_status_s *cmd); */ static int create_log_dir(void); +/** + * Get the time struct from the currently preferred time source + */ +static bool get_log_time_utc_tt(struct tm *tt, bool boot_time); + /** * Select first free log file name and open it. */ @@ -287,7 +306,7 @@ sdlog2_usage(const char *reason) fprintf(stderr, "%s\n", reason); } - warnx("usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t -x\n" + warnx("usage: sdlog2 {start|stop|status|on|off} [-r ] [-b ] -e -a -t -x\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" @@ -351,6 +370,8 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "on")) { struct vehicle_command_s cmd; cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param1 = -1; + cmd.param2 = -1; cmd.param3 = 1; int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); close(fd); @@ -360,7 +381,9 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "off")) { struct vehicle_command_s cmd; cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; - cmd.param3 = 0; + cmd.param1 = -1; + cmd.param2 = -1; + cmd.param3 = 2; int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); close(fd); return 0; @@ -370,20 +393,41 @@ int sdlog2_main(int argc, char *argv[]) return 1; } +bool get_log_time_utc_tt(struct tm *tt, bool boot_time) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ + time_t utc_time_sec; + + if (_gpstime_only) { + utc_time_sec = gps_time / 1e6; + } else { + utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + } + if (utc_time_sec > PX4_EPOCH_SECS) { + + /* strip the time elapsed since boot */ + if (boot_time) { + utc_time_sec -= hrt_absolute_time() / 1e6; + } + + struct tm *ttp = gmtime_r(&utc_time_sec, tt); + return (ttp != NULL); + } else { + return false; + } +} + int create_log_dir() { /* create dir on sdcard if needed */ uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ - time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; - struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + bool time_ok = get_log_time_utc_tt(&tt, true); - if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + if (log_name_timestamp && time_ok) { int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); @@ -432,15 +476,11 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ - time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; - struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + bool time_ok = get_log_time_utc_tt(&tt, false); /* start logging if we have a valid time and the time is not in the past */ - if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + if (log_name_timestamp && time_ok) { strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); @@ -485,14 +525,10 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.txt */ - time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; - struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + bool time_ok = get_log_time_utc_tt(&tt, false); - if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + if (log_name_timestamp && time_ok) { strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); @@ -965,6 +1001,22 @@ int sdlog2_thread_main(int argc, char *argv[]) } + param_t log_gpstime_ph = param_find("SDLOG_GPSTIME"); + + if (log_gpstime_ph != PARAM_INVALID) { + + int32_t param_log_gpstime; + param_get(log_gpstime_ph, ¶m_log_gpstime); + + if (param_log_gpstime > 0) { + _gpstime_only = true; + } else if (param_log_gpstime == 0) { + _gpstime_only = false; + } + /* any other value means to ignore the parameter, so no else case */ + + } + if (check_free_space() != OK) { warnx("ERR: MicroSD almost full"); @@ -1209,7 +1261,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_utc_usec; + gps_time = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1237,7 +1289,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_utc_usec; + gps_time = buf_gps_pos.time_utc_usec / 1e6; } if (!logging_enabled) { @@ -1878,8 +1930,9 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); + warnx("time: gps: %u seconds", (unsigned)gps_time); if (!logging_enabled) { - warnx("standing by"); + warnx("not logging"); } else { float kibibytes = log_bytes_written / 1024.0f; @@ -1982,7 +2035,7 @@ void handle_command(struct vehicle_command_s *cmd) if (param == 1) { sdlog2_start_log(); - } else if (param == -1) { + } else if (param == 2) { sdlog2_stop_log(); } else { // Silently ignore non-matching command values, as they could be for params. diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 19e91b4b2f..ca0b7296c3 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -72,14 +73,14 @@ /** * Array of static parameter info. */ -#if defined(_UNIT_TEST) - extern struct param_info_s param_array[]; - extern struct param_info_s *param_info_base; - extern struct param_info_s *param_info_limit; +#ifdef _UNIT_TEST +extern struct param_info_s param_array[]; +extern struct param_info_s *param_info_base; +extern struct param_info_s *param_info_limit; #else - extern char __param_start, __param_end; - static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; - static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; +extern char __param_start, __param_end; +static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; +static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; #endif #define param_info_count ((unsigned)(param_info_limit - param_info_base)) @@ -93,8 +94,23 @@ struct param_wbuf_s { bool unsaved; }; -// XXX this should be param_info_count, but need to work out linking -uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {}; + +uint8_t *param_changed_storage = 0; +int size_param_changed_storage_bytes = 0; +const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8); + + +static unsigned +get_param_info_count(void) +{ + /* Singleton creation of and array of bits to track changed values */ + if (!param_changed_storage) { + size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit) + 1; + param_changed_storage = calloc(size_param_changed_storage_bytes, 1); + } + + return param_info_count; +} /** flexible array holding modified parameter values */ UT_array *param_values; @@ -142,7 +158,7 @@ param_assert_locked(void) static bool handle_in_range(param_t param) { - return (param < param_info_count); + return (param < get_param_info_count()); } /** @@ -156,11 +172,13 @@ param_compare_values(const void *a, const void *b) struct param_wbuf_s *pa = (struct param_wbuf_s *)a; struct param_wbuf_s *pb = (struct param_wbuf_s *)b; - if (pa->param < pb->param) + if (pa->param < pb->param) { return -1; + } - if (pa->param > pb->param) + if (pa->param > pb->param) { return 1; + } return 0; } @@ -173,7 +191,8 @@ param_compare_values(const void *a, const void *b) * NULL if the parameter has not been modified. */ static struct param_wbuf_s * -param_find_changed(param_t param) { +param_find_changed(param_t param) +{ struct param_wbuf_s *s = NULL; param_assert_locked(); @@ -186,8 +205,9 @@ param_find_changed(param_t param) { #else while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { - if (s->param == param) + if (s->param == param) { break; + } } #endif @@ -224,6 +244,7 @@ param_find_internal(const char *name, bool notification) if (notification) { param_set_used_internal(param); } + return param; } } @@ -247,27 +268,31 @@ param_find_no_notification(const char *name) unsigned param_count(void) { - return param_info_count; + return get_param_info_count(); } unsigned param_count_used(void) { + // ensure the allocation has been done + get_param_info_count(); unsigned count = 0; - for (unsigned i = 0; i < sizeof(param_changed_storage) / sizeof(param_changed_storage[0]); i++) { - for (unsigned j = 0; j < 8; j++) { + + for (unsigned i = 0; i < size_param_changed_storage_bytes; i++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { count++; } } } + return count; } param_t param_for_index(unsigned index) { - if (index < param_info_count) { + if (index < get_param_info_count()) { return (param_t)index; } @@ -277,12 +302,12 @@ param_for_index(unsigned index) param_t param_for_used_index(unsigned index) { - if (index < param_info_count) { + if (index < get_param_info_count()) { /* walk all params and count */ int count = 0; - for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) { + for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { for (unsigned j = 0; j < 8; j++) { if (param_changed_storage[i] & (1 << j)) { @@ -290,7 +315,7 @@ param_for_used_index(unsigned index) * return the param value */ if (index == count) { - return (param_t)i; + return (param_t)(i * 8 + j); } count++; @@ -315,20 +340,19 @@ param_get_index(param_t param) int param_get_used_index(param_t param) { - int param_storage_index = param_get_index(param); - - if (param_storage_index < 0) { + /* this tests for out of bounds and does a constant time lookup */ + if (!param_used(param)) { return -1; } - /* walk all params and count */ + /* walk all params and count, now knowing that it has a valid index */ int count = 0; - for (unsigned i = 0; i < (unsigned)param + 1; i++) { + for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { for (unsigned j = 0; j < 8; j++) { if (param_changed_storage[i] & (1 << j)) { - if (param_storage_index == i) { + if ((unsigned)param == i * 8 + j) { return count; } @@ -343,10 +367,7 @@ param_get_used_index(param_t param) const char * param_name(param_t param) { - if (handle_in_range(param)) - return param_info_base[param].name; - - return NULL; + return handle_in_range(param) ? param_info_base[param].name : NULL; } bool @@ -359,29 +380,22 @@ bool param_value_unsaved(param_t param) { static struct param_wbuf_s *s; - s = param_find_changed(param); - - if (s && s->unsaved) - return true; - - return false; + return (s && s->unsaved) ? true : false; } enum param_type_e -param_type(param_t param) -{ - if (handle_in_range(param)) - return param_info_base[param].type; - - return PARAM_TYPE_UNKNOWN; +param_type(param_t param) { + return handle_in_range(param) ? param_info_base[param].type : PARAM_TYPE_UNKNOWN; } size_t param_size(param_t param) { if (handle_in_range(param)) { + switch (param_type(param)) { + case PARAM_TYPE_INT32: case PARAM_TYPE_FLOAT: return 4; @@ -426,8 +440,9 @@ param_get_value_ptr(param_t param) v = ¶m_info_base[param].val; } - if (param_type(param) >= PARAM_TYPE_STRUCT - && param_type(param) <= PARAM_TYPE_STRUCT_MAX) { + if (param_type(param) >= PARAM_TYPE_STRUCT && + param_type(param) <= PARAM_TYPE_STRUCT_MAX) { + result = v->p; } else { @@ -465,8 +480,9 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ param_lock(); - if (param_values == NULL) + if (param_values == NULL) { utarray_new(param_values, ¶m_icd); + } if (param_values == NULL) { debug("failed to allocate modified values array"); @@ -496,6 +512,7 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ /* update the changed value */ switch (param_type(param)) { + case PARAM_TYPE_INT32: s->val.i = *(int32_t *)val; break; @@ -527,16 +544,15 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } out: - param_set_used_internal(param); - param_unlock(); /* * If we set something, now that we have unlocked, go ahead and advertise that * a thing has been set. */ - if (params_changed && notify_changes) + if (params_changed && notify_changes) { param_notify_changes(); + } return result; } @@ -557,23 +573,25 @@ bool param_used(param_t param) { int param_index = param_get_index(param); + if (param_index < 0) { return false; } - unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); - return param_changed_storage[param_index / sizeof(param_changed_storage[0])] & (1 << bitindex); + return param_changed_storage[param_index / bits_per_allocation_unit] & + (1 << param_index % bits_per_allocation_unit); } void param_set_used_internal(param_t param) { int param_index = param_get_index(param); + if (param_index < 0) { return; } - unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); - param_changed_storage[param_index / sizeof(param_changed_storage[0])] |= (1 << bitindex); + param_changed_storage[param_index / bits_per_allocation_unit] |= + (1 << param_index % bits_per_allocation_unit); } int @@ -600,8 +618,9 @@ param_reset(param_t param) param_unlock(); - if (s != NULL) + if (s != NULL) { param_notify_changes(); + } return (!param_found); } @@ -624,28 +643,28 @@ param_reset_all(void) } void -param_reset_excludes(const char* excludes[], int num_excludes) +param_reset_excludes(const char *excludes[], int num_excludes) { param_lock(); param_t param; for (param = 0; handle_in_range(param); param++) { - const char* name = param_name(param); + const char *name = param_name(param); bool exclude = false; for (int index = 0; index < num_excludes; index ++) { int len = strlen(excludes[index]); - if((excludes[index][len - 1] == '*' - && strncmp(name, excludes[index], len - 1) == 0) - || strcmp(name, excludes[index]) == 0) { + if ((excludes[index][len - 1] == '*' + && strncmp(name, excludes[index], len - 1) == 0) + || strcmp(name, excludes[index]) == 0) { exclude = true; break; } } - if(!exclude) { + if (!exclude) { param_reset(param); } } @@ -659,14 +678,17 @@ static const char *param_default_file = "/eeprom/parameters"; static char *param_user_file = NULL; int -param_set_default_file(const char* filename) +param_set_default_file(const char *filename) { if (param_user_file != NULL) { free(param_user_file); param_user_file = NULL; } - if (filename) + + if (filename) { param_user_file = strdup(filename); + } + return 0; } @@ -718,6 +740,7 @@ param_load_default(void) warn("open '%s' for reading failed", param_get_default_file()); return -1; } + return 1; } @@ -758,13 +781,16 @@ param_export(int fd, bool only_unsaved) * If we are only saving values changed since last save, and this * one hasn't, then skip it */ - if (only_unsaved && !s->unsaved) + if (only_unsaved && !s->unsaved) { continue; + } s->unsaved = false; /* append the appropriate BSON type object */ + switch (param_type(s->param)) { + case PARAM_TYPE_INT32: param_get(s->param, &i); @@ -808,8 +834,9 @@ param_export(int fd, bool only_unsaved) out: param_unlock(); - if (result == 0) + if (result == 0) { result = bson_encoder_fini(&encoder); + } return result; } @@ -919,8 +946,9 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) out: - if (tmp != NULL) + if (tmp != NULL) { free(tmp); + } return result; } @@ -946,8 +974,9 @@ param_import_internal(int fd, bool mark_saved) out: - if (result < 0) + if (result < 0) { debug("BSON error decoding parameters"); + } return result; } diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 50d7df8dbd..04a37a229a 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -249,7 +249,7 @@ __EXPORT void param_reset_all(void); * at the end to exclude parameters with a certain prefix. * @param num_excludes The number of excludes provided. */ - __EXPORT void param_reset_excludes(const char* excludes[], int num_excludes); +__EXPORT void param_reset_excludes(const char *excludes[], int num_excludes); /** * Export changed parameters to a file. @@ -306,16 +306,16 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, * exist. * @return Zero on success. */ -__EXPORT int param_set_default_file(const char* filename); +__EXPORT int param_set_default_file(const char *filename); /** * Get the default parameter file name. * * @return The path to the current default parameter file; either as - * a result of a call to param_set_default_file, or the + * a result of a call to param_set_default_file, or the * built-in default. */ -__EXPORT const char* param_get_default_file(void); +__EXPORT const char *param_get_default_file(void); /** * Save parameters to the default file. diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 20de5fd7a7..b1044d71ea 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,6 +63,7 @@ static int do_save(const char *param_file_name); static int do_load(const char *param_file_name); static int do_import(const char *param_file_name); static void do_show(const char *search_string); +static void do_show_index(const char *index, bool used_index); static void do_show_print(void *arg, param_t param); static int do_set(const char *name, const char *val, bool fail_on_not_found); static int do_compare(const char *name, char *vals[], unsigned comparisons); @@ -173,6 +174,24 @@ param_main(int argc, char *argv[]) return do_reset_nostart(NULL, 0); } } + + if (!strcmp(argv[1], "index_used")) { + if (argc >= 3) { + do_show_index(argv[2], true); + } else { + warnx("no index provided"); + return 1; + } + } + + if (!strcmp(argv[1], "index")) { + if (argc >= 3) { + do_show_index(argv[2], false); + } else { + warnx("no index provided"); + return 1; + } + } } warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); @@ -252,6 +271,50 @@ do_show(const char *search_string) printf("\n %u parameters total, %u used.\n", param_count(), param_count_used()); } +static void +do_show_index(const char *index, bool used_index) +{ + char *end; + int i = strtol(index, &end, 10); + param_t param; + int32_t ii; + float ff; + + if (used_index) { + param = param_for_used_index(i); + } else { + param = param_for_index(i); + } + + if (param == PARAM_INVALID) { + return; + } + + printf("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '), + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param), param_get_used_index(param), param_get_index(param)); + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &ii)) { + printf("%d\n", ii); + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &ff)) { + printf("%4.4f\n", (double)ff); + } + + break; + default: + printf("\n", 0 + param_type(param)); + } + + exit(0); +} + static void do_show_print(void *arg, param_t param) {