From fea910d45ae80b7743e09dbce3b98cc989c4b33e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 Jan 2016 20:41:42 -0800 Subject: [PATCH] Code cleanup and ifdefs required for qurt build Code that was previously out of tree that was #if 0, is now #ifdef __PX4_QURT. These changes were required for flight using the qurt build. Changes include code cleanup for shmem_posix.c. Signed-off-by: Mark Charlebois --- .../commander/accelerometer_calibration.cpp | 8 +++ src/modules/commander/gyro_calibration.cpp | 15 +++- src/modules/commander/mag_calibration.cpp | 17 +++++ src/modules/systemlib/param/param_shmem.c | 20 +----- src/platforms/posix/px4_layer/shmem_posix.c | 71 ++++++++++++------- 5 files changed, 87 insertions(+), 44 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 488f662e11..4750ddbd56 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -175,7 +175,9 @@ typedef struct { int do_accel_calibration(int mavlink_fd) { +#ifndef __PX4_QURT int fd; +#endif mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -192,6 +194,7 @@ int do_accel_calibration(int mavlink_fd) char str[30]; +#ifndef __PX4_QURT /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); @@ -211,6 +214,7 @@ int do_accel_calibration(int mavlink_fd) mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); } } +#endif float accel_offs[max_accel_sens][3]; float accel_T[max_accel_sens][3][3]; @@ -277,14 +281,17 @@ int do_accel_calibration(int mavlink_fd) failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); +#ifndef __PX4_QURT (void)sprintf(str, "CAL_ACC%u_ID", i); failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); +#endif if (failed) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i); return ERROR; } +#ifndef __PX4_QURT sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); fd = px4_open(str, 0); @@ -299,6 +306,7 @@ int do_accel_calibration(int mavlink_fd) if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i); } +#endif } if (res == OK) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 1fcd48b088..f027ed0155 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -173,7 +173,7 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { char str[30]; - +#ifndef __PX4_QURT // Reset gyro ids to unavailable worker_data.device_id[s] = 0; (void)sprintf(str, "CAL_GYRO%u_ID", s); @@ -182,9 +182,18 @@ int do_gyro_calibration(int mavlink_fd) mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); return ERROR; } +#else + (void)sprintf(str, "CAL_GYRO%u_ID", s); + res = param_get(param_find(str), &(worker_data.device_id[s])); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "[cal] Unable to get CAL_GYRO%u_ID", s); + return ERROR; + } +#endif // Reset all offsets to 0 and scales to 1 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale)); +#ifndef __PX4_QURT sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); if (fd >= 0) { @@ -197,6 +206,7 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } } +#endif } @@ -286,6 +296,8 @@ int do_gyro_calibration(int mavlink_fd) failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset))); (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset))); + +#ifndef __PX4_QURT (void)sprintf(str, "CAL_GYRO%u_ID", s); failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s]))); @@ -304,6 +316,7 @@ int do_gyro_calibration(int mavlink_fd) if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1); } +#endif } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index ef0689bec2..64873ceb29 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -101,6 +101,7 @@ int do_mag_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); +#ifndef __PX4_QURT struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -109,6 +110,7 @@ int do_mag_calibration(int mavlink_fd) 0.0f, 1.0f, }; +#endif int result = OK; @@ -121,6 +123,7 @@ int do_mag_calibration(int mavlink_fd) } for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { +#ifndef __PX4_QURT // Reset mag id to mag not available (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); @@ -128,7 +131,17 @@ int do_mag_calibration(int mavlink_fd) mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); break; } +#else + (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); + result = param_get(param_find(str), &(device_ids[cur_mag])); + if (result != OK) { + mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); + break; + } +#endif +/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */ +#ifndef __PX4_QURT // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); int fd = px4_open(str, O_RDONLY); @@ -158,6 +171,7 @@ int do_mag_calibration(int mavlink_fd) } px4_close(fd); +#endif } // Calibrate all mags at the same time @@ -615,8 +629,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag bool failed = false; /* set parameters */ + +#ifndef __PX4_QURT (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); +#endif (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c index 36c827e751..fe52effbe3 100644 --- a/src/modules/systemlib/param/param_shmem.c +++ b/src/modules/systemlib/param/param_shmem.c @@ -72,11 +72,7 @@ #include "shmem.h" -#if 0 -# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) -#else -# define debug(fmt, args...) do { } while(0) -#endif +#define debug(fmt, args...) do { } while(0) #define PARAM_OPEN open #define PARAM_CLOSE close @@ -89,7 +85,7 @@ extern struct param_info_s param_array[]; extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; #else -// FIXME - start and end are reversed +// TODO: start and end are reversed static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters; #endif @@ -187,7 +183,7 @@ param_unlock(void) static void param_assert_locked(void) { - /* XXX */ + /* TODO */ } /** @@ -196,7 +192,6 @@ param_assert_locked(void) * @param param The parameter handle to test. * @return True if the handle is valid. */ -//static bool bool handle_in_range(param_t param) { @@ -241,19 +236,11 @@ param_find_changed(param_t param) param_assert_locked(); if (param_values != NULL) { -#if 0 /* utarray_find requires bsearch, not available */ - struct param_wbuf_s key; - key.param = param; - s = utarray_find(param_values, &key, param_compare_values); -#else - while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { if (s->param == param) { break; } } - -#endif } return s; @@ -282,7 +269,6 @@ param_find_internal(const char *name, bool notification) param_t param; /* perform a linear search of the known parameters */ - for (param = 0; handle_in_range(param); param++) { if (!strcmp(param_info_base[param].name, name)) { if (notification) { diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c index a61d5298d2..319d4287b6 100644 --- a/src/platforms/posix/px4_layer/shmem_posix.c +++ b/src/platforms/posix/px4_layer/shmem_posix.c @@ -1,7 +1,6 @@ - /**************************************************************************** * - * Copyright (c) 2015 Ramakrishna Kintada. All rights reserved. + * Copyright (c) 2015 Vijay Venkatraman. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,7 +31,6 @@ * ****************************************************************************/ - #include #include #include @@ -71,9 +69,9 @@ uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; struct param_wbuf_s { - param_t param; - union param_value_u val; - bool unsaved; + param_t param; + union param_value_u val; + bool unsaved; }; extern struct param_wbuf_s *param_find_changed(param_t param); @@ -88,7 +86,8 @@ static void *map_memory(off_t target) } /* Map one page */ - map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, target & ~MAP_MASK); + map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, + MAP_SHARED, mem_fd, target & ~MAP_MASK); if (map_base == (void *) - 1) { PX4_ERR("Cannot mmap /dev/atl_mem\n"); @@ -109,10 +108,14 @@ int get_shmem_lock(void) usleep(100000); //sleep for 100 msec i++; - if (i > 100) { break; } + if (i > 100) { + break; + } } - if (i > 100) { return -1; } + if (i > 100) { + return -1; + } return 0; //got the lock } @@ -126,14 +129,14 @@ void init_shared_memory(void) { virt_addr = map_memory(MAP_ADDRESS); //16K space - shmem_info_p = (struct shmem_info *)virt_addr; + shmem_info_p = (struct shmem_info *) virt_addr; //PX4_INFO("linux memory mapped\n"); } void copy_params_to_shmem(struct param_info_s *param_info_base) { - param_t param; + param_t param; unsigned int i; if (get_shmem_lock() != 0) { @@ -145,17 +148,26 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) for (param = 0; param < param_count(); param++) { struct param_wbuf_s *s = param_find_changed(param); - if (s == NULL) { shmem_info_p->params_val[param] = param_info_base[param].val; } + if (s == NULL) { + shmem_info_p->params_val[param] = param_info_base[param].val; - else { shmem_info_p->params_val[param] = s->val; } + } else { + shmem_info_p->params_val[param] = s->val; + } #ifdef SHMEM_DEBUG if (param_type(param) == PARAM_TYPE_INT32) { - {PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));} + { + PX4_INFO("%d: written %d for param %s to shared mem", + param, shmem_info_p->params_val[param].i, param_name(param)); + } } else if (param_type(param) == PARAM_TYPE_FLOAT) { - {PX4_INFO("%d: written %f for param %s to shared mem", param, (double)shmem_info_p->params_val[param].f, param_name(param));} + { + PX4_INFO("%d: written %f for param %s to shared mem", + param, (double)shmem_info_p->params_val[param].f, param_name(param)); + } } #endif @@ -191,11 +203,14 @@ void update_to_shmem(param_t param, union param_value_u value) #ifdef SHMEM_DEBUG - if (param_type(param) == PARAM_TYPE_INT32) - {PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);} + if (param_type(param) == PARAM_TYPE_INT32) { + PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n", + value.i, param_name(param), byte_changed, bit_changed); - else if (param_type(param) == PARAM_TYPE_FLOAT) - {PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n", (double)value.f, param_name(param), byte_changed, bit_changed);} + } else if (param_type(param) == PARAM_TYPE_FLOAT) { + PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n", + (double)value.f, param_name(param), byte_changed, bit_changed); + } #endif @@ -221,7 +236,6 @@ static void update_index_from_shmem(void) release_shmem_lock(); } - static void update_value_from_shmem(param_t param, union param_value_u *value) { unsigned int byte_changed, bit_changed; @@ -242,11 +256,16 @@ static void update_value_from_shmem(param_t param, union param_value_u *value) #ifdef SHMEM_DEBUG - if (param_type(param) == PARAM_TYPE_INT32) - {PX4_INFO("Got value %d for param %s from shmem, cleared adsp index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);} + if (param_type(param) == PARAM_TYPE_INT32) { + PX4_INFO( + "Got value %d for param %s from shmem, cleared adsp index %d:%d\n", + value->i, param_name(param), byte_changed, bit_changed); - else if (param_type(param) == PARAM_TYPE_FLOAT) - {PX4_INFO("Got value %f for param %s from shmem, cleared adsp index %d:%d\n", (double)value->f, param_name(param), byte_changed, bit_changed);} + } else if (param_type(param) == PARAM_TYPE_FLOAT) { + PX4_INFO( + "Got value %f for param %s from shmem, cleared adsp index %d:%d\n", + (double)value->f, param_name(param), byte_changed, bit_changed); + } #endif } @@ -258,7 +277,8 @@ int update_from_shmem(param_t param, union param_value_u *value) update_from_shmem_current_time = hrt_absolute_time(); - if ((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) { //update every 1 second + if ((update_from_shmem_current_time - update_from_shmem_prev_time) + > 1000000) { //update every 1 second update_from_shmem_prev_time = update_from_shmem_current_time; update_index_from_shmem(); } @@ -279,4 +299,3 @@ int update_from_shmem(param_t param, union param_value_u *value) return retval; } -