From 5846a22a8f283faf99995f849c91921a6bb693a1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 20 Jan 2016 22:19:45 -0800 Subject: [PATCH] Code format fixes Signed-off-by: Mark Charlebois --- .../muorb/krait/px4muorb_KraitRpcWrapper.cpp | 2 +- .../muorb/krait/px4muorb_KraitRpcWrapper.hpp | 56 +++---- src/modules/systemlib/param/param_shmem.c | 63 +++++--- .../posix/px4_layer/px4_posix_impl.cpp | 6 +- src/platforms/posix/px4_layer/shmem_posix.c | 150 ++++++++++-------- .../tests/vcdev_test/vcdevtest_example.cpp | 3 +- src/platforms/qurt/px4_layer/main.cpp | 67 ++++---- .../qurt/px4_layer/px4_qurt_impl.cpp | 2 +- src/platforms/qurt/px4_layer/shmem_qurt.c | 135 +++++++++------- src/platforms/shmem.h | 9 +- src/systemcmds/mixer/mixer.cpp | 29 ++-- 11 files changed, 284 insertions(+), 238 deletions(-) diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp index 51e7125a7f..11638ab3fd 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -50,7 +50,7 @@ KraitRpcWrapper::~KraitRpcWrapper() {} * Initiatizes the rpc channel px4 muorb */ bool KraitRpcWrapper::Initialize() -{ +{ return (px4muorb_orb_initialize() == 0); } diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp index fa1ad36e17..3600ee69a3 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp @@ -31,41 +31,41 @@ namespace px4muorb { - class KraitRpcWrapper; +class KraitRpcWrapper; } class px4muorb::KraitRpcWrapper { public: - /** - * Constructor - */ - KraitRpcWrapper(); - - /** - * destructor - */ - ~KraitRpcWrapper(); + /** + * Constructor + */ + KraitRpcWrapper(); - /** - * Initiatizes the rpc channel px4 muorb - */ - bool Initialize(); + /** + * destructor + */ + ~KraitRpcWrapper(); - /** - * Terminate to clean up the resources. This should be called at program exit - */ - bool Terminate(); + /** + * Initiatizes the rpc channel px4 muorb + */ + bool Initialize(); - /** - * Muorb related functions to pub/sub of orb topic from krait to adsp - */ - int32_t AddSubscriber( const char* topic ); - int32_t RemoveSubscriber( const char* topic ); - int32_t SendData( const char* topic, int32_t length_in_bytes, const uint8_t* data ); - int32_t ReceiveData( int32_t* msg_type, char** topic, int32_t* length_in_bytes, uint8_t** data ); - int32_t IsSubscriberPresent( const char* topic, int32_t* status ); - int32_t ReceiveBulkData( uint8_t** bulk_data, int32_t* length_in_bytes, int32_t* topic_count ); - int32_t UnblockReceiveData(); + /** + * Terminate to clean up the resources. This should be called at program exit + */ + bool Terminate(); + + /** + * Muorb related functions to pub/sub of orb topic from krait to adsp + */ + int32_t AddSubscriber(const char *topic); + int32_t RemoveSubscriber(const char *topic); + int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data); + int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data); + int32_t IsSubscriberPresent(const char *topic, int32_t *status); + int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count); + int32_t UnblockReceiveData(); }; #endif // _px4muorb_KraitWrapper_hpp_ diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c index 2fd59f5f3d..36c827e751 100644 --- a/src/modules/systemlib/param/param_shmem.c +++ b/src/modules/systemlib/param/param_shmem.c @@ -114,22 +114,23 @@ const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8); extern int get_shmem_lock(void); extern void release_shmem_lock(void); -struct param_wbuf_s * param_find_changed(param_t param); +struct param_wbuf_s *param_find_changed(param_t param); void init_params(void); extern void init_shared_memory(void); extern void copy_params_to_shmem(struct param_info_s *param_info_base); -extern struct shmem_info* shmem_info_p; -uint64_t sync_other_prev_time=0, sync_other_current_time=0; +extern struct shmem_info *shmem_info_p; +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); -unsigned char set_called_from_get=0; +unsigned char set_called_from_get = 0; -static int param_import_done=0; /*at startup, params are loaded from file, if present. we dont want to send notifications that time since muorb is not ready*/ +static int param_import_done = + 0; /*at startup, params are loaded from file, if present. we dont want to send notifications that time since muorb is not ready*/ static int param_load_default_no_notify(void); @@ -508,16 +509,16 @@ param_get(param_t param, void *val) param_lock(); - if(!handle_in_range(param)) + if (!handle_in_range(param)) { return result; + } union param_value_u value; - if(update_from_shmem(param, &value)) - { - set_called_from_get=1; + if (update_from_shmem(param, &value)) { + set_called_from_get = 1; param_set_internal(param, &value, true, false); - set_called_from_get=0; + set_called_from_get = 0; } @@ -529,9 +530,13 @@ param_get(param_t param, void *val) } #ifdef ENABLE_SHMEM_DEBUG - if(param_type(param)==PARAM_TYPE_INT32) { PX4_INFO("param_get for %s : %d\n", param_name(param), *(int*)val); } - else if(param_type(param)==PARAM_TYPE_FLOAT) { PX4_INFO("param_get for %s : %f\n", param_name(param), *(double*)val); } + + if (param_type(param) == PARAM_TYPE_INT32) { PX4_INFO("param_get for %s : %d\n", param_name(param), *(int *)val); } + + else if (param_type(param) == PARAM_TYPE_FLOAT) { PX4_INFO("param_get for %s : %f\n", param_name(param), *(double *)val); } + else { PX4_INFO("Unknown param type for %s\n", param_name(param)); } + #endif param_unlock(); @@ -547,8 +552,9 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ param_lock(); - if(!handle_in_range(param)) + if (!handle_in_range(param)) { return result; + } mark_saved = true; //mark all params as saved @@ -623,19 +629,24 @@ out: * a thing has been set. */ - if(!param_import_done) notify_changes=0; + if (!param_import_done) { notify_changes = 0; } if (params_changed && notify_changes) { param_notify_changes(); } - if(result==0 && !set_called_from_get) - update_to_shmem(param, *(union param_value_u*)val); + if (result == 0 && !set_called_from_get) { + update_to_shmem(param, *(union param_value_u *)val); + } #ifdef ENABLE_SHMEM_DEBUG - if(param_type(param)==PARAM_TYPE_INT32) {PX4_INFO("param_set for %s : %d\n", param_name(param), *(int*)val);} - else if(param_type(param)==PARAM_TYPE_FLOAT) {PX4_INFO("param_set for %s : %f\n", param_name(param), *(double*)val);} + + if (param_type(param) == PARAM_TYPE_INT32) {PX4_INFO("param_set for %s : %d\n", param_name(param), *(int *)val);} + + else if (param_type(param) == PARAM_TYPE_FLOAT) {PX4_INFO("param_set for %s : %f\n", param_name(param), *(double *)val);} + else {PX4_INFO("Unknown param type for %s\n", param_name(param));} + #endif return result; @@ -794,10 +805,10 @@ param_save_default(void) const char *filename = param_get_default_file(); - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { PX4_ERR("Could not get shmem lock\n"); return 0; - } + } fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); @@ -858,15 +869,16 @@ param_load_default(void) static int param_load_default_no_notify(void) { - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { PX4_ERR("Could not get shmem lock\n"); return 0; - } + } int fd_load = open(param_get_default_file(), O_RDONLY); if (fd_load < 0) { release_shmem_lock(); + /* no parameter file is OK, otherwise this is an error */ if (errno != ENOENT) { debug("open '%s' for reading failed", param_get_default_file()); @@ -1185,14 +1197,17 @@ void init_params(void) #ifdef __PX4_POSIX param_load_default_no_notify(); #endif - param_import_done=1; + param_import_done = 1; copy_params_to_shmem(param_info_base); #ifdef ENABLE_SHMEM_DEBUG PX4_INFO("Offsets: \n"); - PX4_INFO("params_val %lu, krait_changed %lu, adsp_changed %lu\n", (unsigned char*)shmem_info_p->params_val - (unsigned char*)shmem_info_p, (unsigned char*)&shmem_info_p->krait_changed_index - (unsigned char*)shmem_info_p, (unsigned char*)&shmem_info_p->adsp_changed_index - (unsigned char*)shmem_info_p); + PX4_INFO("params_val %lu, krait_changed %lu, adsp_changed %lu\n", + (unsigned char *)shmem_info_p->params_val - (unsigned char *)shmem_info_p, + (unsigned char *)&shmem_info_p->krait_changed_index - (unsigned char *)shmem_info_p, + (unsigned char *)&shmem_info_p->adsp_changed_index - (unsigned char *)shmem_info_p); #endif } diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 514cf260c0..51e286e1dd 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -59,9 +59,9 @@ __BEGIN_DECLS long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); #ifdef ENABLE_SHMEM - extern void init_params(void); +extern void init_params(void); #endif - + #ifdef ENABLE_SHMEM extern void init_own_params(void); extern unsigned int init_other_params(void); @@ -86,7 +86,7 @@ void init_once(void) #ifdef ENABLE_SHMEM PX4_INFO("Starting shared memory param sync\n"); init_own_params(); - param_sync_done=init_other_params(); + param_sync_done = init_other_params(); #endif } diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c index cd8c460548..a61d5298d2 100644 --- a/src/platforms/posix/px4_layer/shmem_posix.c +++ b/src/platforms/posix/px4_layer/shmem_posix.c @@ -58,8 +58,8 @@ int mem_fd; unsigned char *map_base, *virt_addr; -struct shmem_info* shmem_info_p; -static void* map_memory(off_t target); +struct shmem_info *shmem_info_p; +static void *map_memory(off_t target); int get_shmem_lock(void); void release_shmem_lock(void); @@ -67,65 +67,66 @@ void init_shared_memory(void); void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); int update_from_shmem(param_t param, union param_value_u *value); -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]; +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; }; -extern struct param_wbuf_s * param_find_changed(param_t param); +extern struct param_wbuf_s *param_find_changed(param_t param); #define MEMDEVICE "/dev/mem" -static void* map_memory(off_t target) +static void *map_memory(off_t target) { - if((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) - { - PX4_ERR("Cannot open %s\n", MEMDEVICE); - exit(1); - } + if ((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) { + PX4_ERR("Cannot open %s\n", MEMDEVICE); + exit(1); + } - /* Map one page */ - 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"); - exit(1); - } + /* Map one page */ + map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, target & ~MAP_MASK); - return (map_base + (target & MAP_MASK) + LOCK_SIZE); + if (map_base == (void *) - 1) { + PX4_ERR("Cannot mmap /dev/atl_mem\n"); + exit(1); + } + + return (map_base + (target & MAP_MASK) + LOCK_SIZE); } int get_shmem_lock(void) { - int i=0; - + int i = 0; + /*ioctl calls cmpxchg*/ - while(ioctl(mem_fd, LOCK_MEM)!=0) - { + while (ioctl(mem_fd, LOCK_MEM) != 0) { PX4_INFO("Could not get lock, spinning\n"); 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 } void release_shmem_lock(void) { - *(virt_addr-LOCK_SIZE)=1; + *(virt_addr - LOCK_SIZE) = 1; } 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"); } @@ -135,32 +136,36 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) param_t param; unsigned int i; - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { PX4_ERR("Could not get shmem lock\n"); return; - } + } //PX4_INFO("%d krait params allocated\n", param_count()); - for (param = 0; paramparams_val[param] = param_info_base[param].val; - else shmem_info_p->params_val[param] = s->val; + + if (s == NULL) { shmem_info_p->params_val[param] = param_info_base[param].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));} - } - 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));} + + 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));} + + } 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));} } + #endif } - + //PX4_INFO("written %u params to shmem offset %lu\n", param_count(), (unsigned char*)&shmem_info_p->params_count-(unsigned char*)shmem_info_p); - for(i=0;ikrait_changed_index[i]=0; - adsp_changed_index[i]=0; + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + shmem_info_p->krait_changed_index[i] = 0; + adsp_changed_index[i] = 0; } release_shmem_lock(); @@ -171,43 +176,47 @@ void update_to_shmem(param_t param, union param_value_u value) { unsigned int byte_changed, bit_changed; - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { fprintf(stderr, "Could not get shmem lock\n"); return; - } + } shmem_info_p->params_val[param] = value; - byte_changed = param/8; - bit_changed = 1 << param%8; - shmem_info_p->krait_changed_index[byte_changed]|=bit_changed; + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->krait_changed_index[byte_changed] |= bit_changed; //PX4_INFO("set %d bit on krait changed index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->krait_changed_index[byte_changed]); #ifdef SHMEM_DEBUG - if(param_type(param)==PARAM_TYPE_INT32) + + 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) + + 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 release_shmem_lock(); - -} + +} static void update_index_from_shmem(void) { unsigned int i; - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { fprintf(stderr, "Could not get shmem lock\n"); return; - } + } //PX4_INFO("Updating index from shmem\n"); - for(i=0;iadsp_changed_index[i]; + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i]; + } release_shmem_lock(); } @@ -217,25 +226,28 @@ static void update_value_from_shmem(param_t param, union param_value_u *value) { unsigned int byte_changed, bit_changed; - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { fprintf(stderr, "Could not get shmem lock\n"); return; - } + } *value = shmem_info_p->params_val[param]; /*also clear the index since we are holding the lock*/ - byte_changed = param/8; - bit_changed = 1 << param%8; - shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed; + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed; release_shmem_lock(); #ifdef SHMEM_DEBUG - if(param_type(param)==PARAM_TYPE_INT32) + + 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) + + 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 } @@ -244,22 +256,22 @@ int update_from_shmem(param_t param, union param_value_u *value) unsigned int byte_changed, bit_changed; unsigned int retval = 0; - update_from_shmem_current_time = hrt_absolute_time(); - if((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) //update every 1 second - { + update_from_shmem_current_time = hrt_absolute_time(); + + 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(); } - byte_changed = param/8; - bit_changed = 1 << param%8; + byte_changed = param / 8; + bit_changed = 1 << param % 8; - if(adsp_changed_index[byte_changed] & bit_changed) - { + if (adsp_changed_index[byte_changed] & bit_changed) { update_value_from_shmem(param, value); adsp_changed_index[byte_changed] &= ~bit_changed; //clear the bit retval = 1; } + //else {PX4_INFO("no change to param %s\n", param_name(param));} //PX4_INFO("%s %d bit on adsp index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed); diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index 4d82e3dc88..3009908b2b 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -331,7 +331,7 @@ int VCDevExample::main() PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - if(do_poll(fd, 0, 3, 0)) { + if (do_poll(fd, 0, 3, 0)) { ret = 1; goto fail2; goto fail2; @@ -357,6 +357,7 @@ int VCDevExample::main() ret = 1; goto fail2; } + PX4_INFO("TEST: waiting for writer to stop"); fail2: g_exit = true; diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index d5169793b3..2ef02a5ac5 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -68,28 +68,30 @@ static void run_cmd(map &apps, const vector &appargs string command = appargs[0]; //replaces app.find with iterator code to avoid null pointer exception - for (map::iterator it=apps.begin(); it!=apps.end(); ++it) - if (it->first == command) { - const char *arg[2 + 1]; + for (map::iterator it = apps.begin(); it != apps.end(); ++it) + if (it->first == command) { + const char *arg[2 + 1]; - unsigned int i = 0; + unsigned int i = 0; + + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { + arg[i] = (char *)appargs[i].c_str(); + PX4_WARN(" arg%d = '%s'\n", i, arg[i]); + ++i; + } + + arg[i] = (char *)0; + + //PX4_DEBUG_PRINTF(i); + if (apps[command] == NULL) { + PX4_ERR("Null function !!\n"); + + } else { + apps[command](i, (char **)arg); + break; + } - while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { - arg[i] = (char *)appargs[i].c_str(); - PX4_WARN(" arg%d = '%s'\n", i, arg[i]); - ++i; } - - arg[i] = (char *)0; - //PX4_DEBUG_PRINTF(i); - if(apps[command]==NULL) - PX4_ERR("Null function !!\n"); - else { - apps[command](i, (char **)arg); - break; - } - - } } void eat_whitespace(const char *&b, int &i) @@ -108,7 +110,7 @@ static void process_commands(map &apps, const char *cmds) const char *b = cmds; char arg[256]; - + // Eat leading whitespace eat_whitespace(b, i); @@ -170,26 +172,27 @@ const char *get_commands() { int fd = open(COMMANDS_ADSP_FILE, O_RDONLY); - if(fd>0) - { + if (fd > 0) { static char *commands; char buf[4096]; - int bytes_read, total_bytes=0; + int bytes_read, total_bytes = 0; PX4_INFO("reading commands from %s\n", COMMANDS_ADSP_FILE); - do{ - bytes_read = read(fd, (void*)buf, sizeof(buf)); - if(bytes_read>0) - { - commands=(char*)realloc(commands, total_bytes+bytes_read); - memcpy(commands+total_bytes, buf, bytes_read); + + do { + bytes_read = read(fd, (void *)buf, sizeof(buf)); + + if (bytes_read > 0) { + commands = (char *)realloc(commands, total_bytes + bytes_read); + memcpy(commands + total_bytes, buf, bytes_read); total_bytes += bytes_read; } - }while((unsigned int)bytes_read>0); + } while ((unsigned int)bytes_read > 0); + close(fd); - return (const char*)commands; + return (const char *)commands; } - + PX4_ERR("Could not open %s\n", COMMANDS_ADSP_FILE); static const char *commands = diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 071e6d88ce..dcbff32ece 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -114,7 +114,7 @@ void init_once(void) /*Shared memory param sync*/ init_own_params(); - param_sync_done=init_other_params(); + param_sync_done = init_other_params(); } void init(int argc, char *argv[], const char *app_name) diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c index 0fc24c0945..5e644a6a88 100644 --- a/src/platforms/qurt/px4_layer/shmem_qurt.c +++ b/src/platforms/qurt/px4_layer/shmem_qurt.c @@ -51,45 +51,45 @@ int mem_fd; unsigned char *map_base, *virt_addr; -struct shmem_info* shmem_info_p; -static void* map_memory(off_t target); +struct shmem_info *shmem_info_p; +static void *map_memory(off_t target); int get_shmem_lock(void); void release_shmem_lock(void); void init_shared_memory(void); void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); int update_from_shmem(param_t param, union param_value_u *value); -uint64_t update_from_shmem_prev_time=0, update_from_shmem_current_time=0; -static unsigned char krait_changed_index[MAX_SHMEM_PARAMS/8+1]; +uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; +static unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; struct param_wbuf_s { param_t param; union param_value_u val; bool unsaved; }; -extern struct param_wbuf_s * param_find_changed(param_t param); +extern struct param_wbuf_s *param_find_changed(param_t param); -static void* map_memory(off_t target) +static void *map_memory(off_t target) { - return (void*)(target+LOCK_SIZE); + return (void *)(target + LOCK_SIZE); } int get_shmem_lock(void) { - unsigned char *lock = (unsigned char*)(MAP_ADDRESS+LOCK_OFFSET); - unsigned int i=0; + unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); + unsigned int i = 0; - while(!atomic_compare_and_set(lock, 1, 0)) - { + while (!atomic_compare_and_set(lock, 1, 0)) { PX4_INFO("Could not get lock. spinning\n"); i++; usleep(1000); - if(i>100) break; + + if (i > 100) { break; } } - if(i>100) return -1; + if (i > 100) { return -1; } return 0; //got the lock @@ -97,9 +97,9 @@ int get_shmem_lock(void) void release_shmem_lock(void) { - unsigned char *lock = (unsigned char*)(MAP_ADDRESS+LOCK_OFFSET); + unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); - *lock=1; + *lock = 1; return; } @@ -108,7 +108,7 @@ void init_shared_memory(void) //PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000); virt_addr = map_memory(MAP_ADDRESS); - shmem_info_p = (struct shmem_info*)virt_addr; + shmem_info_p = (struct shmem_info *)virt_addr; //PX4_INFO("adsp memory mapped\n"); } @@ -118,30 +118,36 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) param_t param; unsigned int i; - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { PX4_INFO("Could not get shmem lock\n"); return; - } + } + //else PX4_INFO("Got lock\n"); - for (param = 0; paramadsp_params[param].name)-(unsigned char*)shmem_info_p);} struct param_wbuf_s *s = param_find_changed(param); - if(s==NULL) shmem_info_p->params_val[param] = param_info_base[param].val; - else shmem_info_p->params_val[param] = s->val; + + if (s == NULL) { shmem_info_p->params_val[param] = param_info_base[param].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));} - else if(param_type(param)==PARAM_TYPE_FLOAT){ - PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param));} + + 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)); + + } else if (param_type(param) == PARAM_TYPE_FLOAT) { + PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param)); + } + #endif } - - for(i=0;iadsp_changed_index[i]=0; - krait_changed_index[i]=0; + + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + shmem_info_p->adsp_changed_index[i] = 0; + krait_changed_index[i] = 0; } release_shmem_lock(); @@ -155,46 +161,51 @@ void update_to_shmem(param_t param, union param_value_u value) { unsigned int byte_changed, bit_changed; - if(!handle_in_range(param)) + if (!handle_in_range(param)) { return; + } - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { PX4_ERR("Could not get shmem lock\n"); return; - } + } shmem_info_p->params_val[param] = value; - byte_changed = param/8; - bit_changed = 1 << param%8; - shmem_info_p->adsp_changed_index[byte_changed]|=bit_changed; + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->adsp_changed_index[byte_changed] |= bit_changed; //PX4_INFO("set %d bit on adsp index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->adsp_changed_index[byte_changed]); #ifdef SHMEM_DEBUG - if(param_type(param)==PARAM_TYPE_INT32) + + if (param_type(param) == PARAM_TYPE_INT32) {PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);} - else if(param_type(param)==PARAM_TYPE_FLOAT) + + else if (param_type(param) == PARAM_TYPE_FLOAT) {PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed, bit_changed);} + #endif release_shmem_lock(); - -} + +} static void update_index_from_shmem(void) { unsigned int i; - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { PX4_ERR("Could not get shmem lock\n"); return; - } + } //PX4_INFO("Updating index from shmem\n"); - for(i=0;ikrait_changed_index[i]; + for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { + krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; + } release_shmem_lock(); } @@ -204,25 +215,28 @@ static void update_value_from_shmem(param_t param, union param_value_u *value) { unsigned int byte_changed, bit_changed; - if(get_shmem_lock()!=0) { + if (get_shmem_lock() != 0) { PX4_ERR("Could not get shmem lock\n"); return; - } + } *value = shmem_info_p->params_val[param]; /*also clear the index since we are holding the lock*/ - byte_changed = param/8; - bit_changed = 1 << param%8; - shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed; + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed; release_shmem_lock(); #ifdef SHMEM_DEBUG - if(param_type(param)==PARAM_TYPE_INT32) + + if (param_type(param) == PARAM_TYPE_INT32) {PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);} - else if(param_type(param)==PARAM_TYPE_FLOAT) + + else if (param_type(param) == PARAM_TYPE_FLOAT) {PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed, bit_changed);} + #endif } @@ -231,25 +245,26 @@ int update_from_shmem(param_t param, union param_value_u *value) unsigned int byte_changed, bit_changed; unsigned int retval = 0; - if(!handle_in_range(param) || value==NULL) + if (!handle_in_range(param) || value == NULL) { return retval; + } - update_from_shmem_current_time = hrt_absolute_time(); - if((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) //update every 1 second - { + update_from_shmem_current_time = hrt_absolute_time(); + + 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(); } - byte_changed = param/8; - bit_changed = 1 << param%8; + byte_changed = param / 8; + bit_changed = 1 << param % 8; - if(krait_changed_index[byte_changed] & bit_changed) - { + if (krait_changed_index[byte_changed] & bit_changed) { update_value_from_shmem(param, value); - krait_changed_index[byte_changed] &= ~bit_changed; + krait_changed_index[byte_changed] &= ~bit_changed; retval = 1; } + //else {PX4_INFO("no change to param %s\n", param_name(param));} //PX4_INFO("%s %d bit on krait changed index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed); diff --git a/src/platforms/shmem.h b/src/platforms/shmem.h index 2704836587..17e582654c 100644 --- a/src/platforms/shmem.h +++ b/src/platforms/shmem.h @@ -34,16 +34,15 @@ #define MAX_SHMEM_PARAMS 3850 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info)) -struct shmem_info -{ +struct shmem_info { union param_value_u params_val[MAX_SHMEM_PARAMS]; - unsigned char krait_changed_index[MAX_SHMEM_PARAMS/8+1]; /*bit map of all params changed by krait*/ - unsigned char adsp_changed_index[MAX_SHMEM_PARAMS/8+1]; /*bit map of all params changed by adsp*/ + unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by krait*/ + unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by adsp*/ #ifdef __PX4_NUTTX }; #else -}__attribute__((packed)); +} __attribute__((packed)); #endif #define MAP_ADDRESS 0xfbfc000 diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index ca77594e91..d378958f0f 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -124,6 +124,7 @@ load(const char *devname, const char *fname) #ifndef __PX4_QURT char buf[2048]; + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { warnx("can't load mixer: %s", fname); return 1; @@ -132,20 +133,20 @@ load(const char *devname, const char *fname) /* XXX pass the buffer to the device */ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); #else - char newbuf[] = - "R: 4x 10000 10000 10000 0\n" - "M: 1\n" - "O: 10000 10000 0 -10000 10000\n" - "S: 0 4 10000 10000 0 -10000 10000\n" - "M: 1\n" - "O: 10000 10000 0 -10000 10000\n" - "S: 0 5 10000 10000 0 -10000 10000\n" - "M: 1\n" - "O: 10000 10000 0 -10000 10000\n" - "S: 0 6 10000 10000 0 -10000 10000\n" - "M: 1\n" - "O: 10000 10000 0 -10000 10000\n" - "S: 0 7 10000 10000 0 -10000 10000\n"; + char newbuf[] = + "R: 4x 10000 10000 10000 0\n" + "M: 1\n" + "O: 10000 10000 0 -10000 10000\n" + "S: 0 4 10000 10000 0 -10000 10000\n" + "M: 1\n" + "O: 10000 10000 0 -10000 10000\n" + "S: 0 5 10000 10000 0 -10000 10000\n" + "M: 1\n" + "O: 10000 10000 0 -10000 10000\n" + "S: 0 6 10000 10000 0 -10000 10000\n" + "M: 1\n" + "O: 10000 10000 0 -10000 10000\n" + "S: 0 7 10000 10000 0 -10000 10000\n"; /* XXX pass the buffer to the device */ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf); #endif