diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp index dad971ac7a..4e25989b17 100644 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ b/src/modules/muorb/adsp/px4muorb.cpp @@ -50,7 +50,7 @@ extern int dspal_main(int argc, char *argv[]); extern struct shmem_info *shmem_info_p; extern int get_shmem_lock(const char *caller_file_name, int caller_line_number); extern void release_shmem_lock(const char *caller_file_name, - int caller_line_number); + int caller_line_number); extern void init_shared_memory(void); __END_DECLS int px4muorb_orb_initialize() @@ -61,7 +61,7 @@ int px4muorb_orb_initialize() uORB::Manager::initialize(); // Register the fastrpc muorb with uORBManager. uORB::Manager::get_instance()->set_uorb_communicator( - uORB::FastRpcChannel::GetInstance()); + uORB::FastRpcChannel::GetInstance()); // Now continue with the usual dspal startup. const char *argv[] = { "dspal", "start" }; @@ -85,7 +85,7 @@ int px4muorb_get_absolute_time(uint64_t *time_us) /*update value and param's change bit in shared memory*/ int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, - int data_len_in_bytes) + int data_len_in_bytes) { unsigned int byte_changed, bit_changed; union param_value_u *param_value = (union param_value_u *) value; @@ -115,8 +115,9 @@ int px4muorb_param_update_index_from_shmem(unsigned char *data, { unsigned int i; - if (!shmem_info_p) + if (!shmem_info_p) { return -1; + } if (get_shmem_lock(__FILE__, __LINE__) != 0) { PX4_INFO("Could not get shmem lock\n"); @@ -138,8 +139,9 @@ int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, unsigned int byte_changed, bit_changed; union param_value_u *param_value = (union param_value_u *) value; - if (!shmem_info_p) + if (!shmem_info_p) { return -1; + } if (get_shmem_lock(__FILE__, __LINE__) != 0) { PX4_INFO("Could not get shmem lock\n"); @@ -197,7 +199,7 @@ int px4muorb_remove_subscriber(const char *name) } int px4muorb_send_topic_data(const char *name, const uint8_t *data, - int data_len_in_bytes) + int data_len_in_bytes) { int rc = 0; uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); @@ -229,7 +231,7 @@ int px4muorb_is_subscriber_present(const char *topic_name, int *status) } int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, - uint8_t *data, int data_len_in_bytes, int *bytes_returned) + uint8_t *data, int data_len_in_bytes, int *bytes_returned) { int rc = 0; int32_t local_msg_type = 0; @@ -237,14 +239,14 @@ int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, - data_len_in_bytes, &local_bytes_returned); + data_len_in_bytes, &local_bytes_returned); *msg_type = (int) local_msg_type; *bytes_returned = (int) local_bytes_returned; return rc; } int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, - int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count) + int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count) { int rc = 0; int32_t local_bytes_returned = 0; @@ -252,7 +254,7 @@ int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes, - &local_bytes_returned, &local_topic_count); + &local_bytes_returned, &local_topic_count); *returned_length_in_bytes = (int) local_bytes_returned; *topic_count = (int) local_topic_count; return rc; diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp index 8d7c2db5f2..b7bea9cad9 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -62,7 +62,7 @@ static bool _Initialized = false; static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024; static const uint32_t _MAX_TOPICS = 64; static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = - _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; + _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; static uint8_t *_BulkTransferBuffer = 0; unsigned char *adsp_changed_index = 0; @@ -104,7 +104,7 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu - + (ts.tv_nsec / 1000); + + (ts.tv_nsec / 1000); close(fd); @@ -122,8 +122,8 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) // Before casting to in32_t, check if it fits. uint64_t abs_diff = - (time_appsproc > time_dsp) ? - (time_appsproc - time_dsp) : (time_dsp - time_appsproc); + (time_appsproc > time_dsp) ? + (time_appsproc - time_dsp) : (time_dsp - time_appsproc); if (abs_diff > INT32_MAX) { PX4_ERR("Timer difference too big"); @@ -133,9 +133,9 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) *time_diff_us = time_appsproc - time_dsp; PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", - time_dsp, time_appsproc); + time_dsp, time_appsproc); PX4_DEBUG("found time_diff: %li us, %.6f s", - *time_diff_us, ((double)*time_diff_us) / 1e6); + *time_diff_us, ((double)*time_diff_us) / 1e6); return 0; } @@ -158,24 +158,24 @@ bool px4muorb::KraitRpcWrapper::Initialize() PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__); _BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, - _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)); + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)); rc = (_BulkTransferBuffer != NULL) ? true : false; if (!rc) { PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", - __FUNCTION__); + __FUNCTION__); return rc; } else { PX4_DEBUG( - "%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p", - __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer); + "%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p", + __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer); } _TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, - _MAX_TOPIC_NAME_BUFFER * sizeof(char)); + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_TOPIC_NAME_BUFFER * sizeof(char)); rc = (_TopicNameBuffer != NULL) ? true : false; @@ -190,8 +190,8 @@ bool px4muorb::KraitRpcWrapper::Initialize() // now allocate the data buffer. _DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, - _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t)); + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t)); rc = (_DataBuffer != NULL) ? true : false; @@ -208,12 +208,13 @@ bool px4muorb::KraitRpcWrapper::Initialize() } adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, - MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t)); + MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t)); rc = (adsp_changed_index != NULL) ? true : false; if (!rc) { PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__); + } else { memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t)); } @@ -228,7 +229,7 @@ bool px4muorb::KraitRpcWrapper::Initialize() // call muorb initialize routine. if (px4muorb_orb_initialize() != 0) { PX4_ERR("%s Error calling the uorb fastrpc initalize method..", - __FUNCTION__); + __FUNCTION__); rc = false; return rc; } @@ -244,7 +245,7 @@ bool px4muorb::KraitRpcWrapper::Initialize() int diff = (time_dsp - time_appsproc); PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", - time_dsp, time_appsproc, diff); + time_dsp, time_appsproc, diff); _Initialized = true; return rc; @@ -296,7 +297,7 @@ int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) { return (_Initialized ? - px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); + px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); } int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, @@ -306,8 +307,8 @@ int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, if (_Initialized) { rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, - _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, - length_in_bytes); + _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, + length_in_bytes); if (rc == 0) { *topic = _TopicNameBuffer; @@ -332,7 +333,7 @@ int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, if (_Initialized) { //rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes ); rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, - _MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count); + _MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count); if (rc == 0) { *bulk_data = _BulkTransferBuffer; diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c index aa0ef2b959..66cf5ba9bf 100644 --- a/src/platforms/posix/px4_layer/shmem_posix.c +++ b/src/platforms/posix/px4_layer/shmem_posix.c @@ -60,8 +60,7 @@ void update_index_from_shmem(void); uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; extern unsigned char *adsp_changed_index; -struct param_wbuf_s -{ +struct param_wbuf_s { param_t param; union param_value_u val; bool unsaved; @@ -70,9 +69,10 @@ struct param_wbuf_s /*update value and param's change bit in shared memory*/ void update_to_shmem(param_t param, union param_value_u value) { - if (px4muorb_param_update_to_shmem(param, (unsigned char*) &value, - sizeof(value))) + if (px4muorb_param_update_to_shmem(param, (unsigned char *) &value, + sizeof(value))) { PX4_ERR("krait update param %u failed", param); + } } void update_index_from_shmem(void) @@ -83,14 +83,15 @@ void update_index_from_shmem(void) } px4muorb_param_update_index_from_shmem(adsp_changed_index, - PARAM_BUFFER_SIZE); + PARAM_BUFFER_SIZE); } static void update_value_from_shmem(param_t param, union param_value_u *value) { - if (px4muorb_param_update_value_from_shmem(param, (unsigned char*) value, - sizeof(union param_value_u))) + if (px4muorb_param_update_value_from_shmem(param, (unsigned char *) value, + sizeof(union param_value_u))) { PX4_ERR("%s get param failed", __FUNCTION__); + } } int update_from_shmem(param_t param, union param_value_u *value) @@ -106,7 +107,7 @@ 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 + > 1000000) { //update every 1 second update_from_shmem_prev_time = update_from_shmem_current_time; update_index_from_shmem(); } @@ -123,7 +124,7 @@ int update_from_shmem(param_t param, union param_value_u *value) //else {PX4_INFO("no change to param %s", param_name(param));} PX4_DEBUG("%s %d bit on adsp index[%d]", - (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); + (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); return retval; } diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c index b4b24eab39..780a4cc7cf 100644 --- a/src/platforms/qurt/px4_layer/shmem_qurt.c +++ b/src/platforms/qurt/px4_layer/shmem_qurt.c @@ -75,8 +75,7 @@ static unsigned log2_for_int(unsigned v) return r; } -struct param_wbuf_s -{ +struct param_wbuf_s { param_t param; union param_value_u val; bool unsaved; @@ -86,17 +85,18 @@ extern struct param_wbuf_s *param_find_changed(param_t param); static void *map_memory(off_t target) { - return (void *) (target + LOCK_SIZE); + return (void *)(target + LOCK_SIZE); } int get_shmem_lock(const char *caller_file_name, int caller_line_number) { - unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET); + unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); unsigned int i = 0; #ifdef PARAM_LOCK_DEBUG - PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int*)0xfbfc000, strrchr(caller_file_name, '/'), caller_line_number); + PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int *)0xfbfc000, strrchr(caller_file_name, '/'), + caller_line_number); #endif while (!atomic_compare_and_set(lock, 1, 0)) { @@ -110,12 +110,12 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number) if (i > 100) { PX4_INFO("Could not get lock, file name: %s, line number: %d.\n", - strrchr(caller_file_name, '/'), caller_line_number); + strrchr(caller_file_name, '/'), caller_line_number); return -1; } else { PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", - caller_file_name, caller_line_number); + caller_file_name, caller_line_number); } return 0; //got the lock @@ -124,13 +124,13 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number) void release_shmem_lock(const char *caller_file_name, int caller_line_number) { - unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET); + unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); *lock = 1; #ifdef PARAM_LOCK_DEBUG PX4_INFO("release lock, file name: %s, line number: %d.\n", - strrchr(caller_file_name, '/'), caller_line_number); + strrchr(caller_file_name, '/'), caller_line_number); #endif return; @@ -141,14 +141,15 @@ void init_shared_memory(void) //PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000); int i; - if (shmem_info_p) + if (shmem_info_p) { return; + } virt_addr = map_memory(MAP_ADDRESS); shmem_info_p = (struct shmem_info *) virt_addr; //init lock as 1 - unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET); + unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET); *lock = 1; for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { @@ -230,12 +231,12 @@ void update_to_shmem(param_t param, union param_value_u value) 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); + bit_changed); } 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); + bit_changed); } #endif @@ -261,17 +262,19 @@ void update_index_from_shmem(void) // If a param has changed, we need to find out which one. // From the byte and bit that is different, we can resolve the param number. unsigned bit = log2_for_int( - krait_changed_index[i] - ^ shmem_info_p->krait_changed_index[i]); + krait_changed_index[i] + ^ shmem_info_p->krait_changed_index[i]); param_t param_to_get = i * 8 + bit; // Update our krait_changed_index as well. krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; params[i] = param_to_get; + } else { params[i] = 0xFFFF; } } + release_shmem_lock(__FILE__, __LINE__); // FIXME: this is a hack but it gets the param so that it gets added @@ -306,12 +309,12 @@ static void update_value_from_shmem(param_t param, union param_value_u *value) 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); + bit_changed); } 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); + bit_changed); } #endif @@ -329,7 +332,7 @@ 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 + > 1000000) { //update every 1 second update_from_shmem_prev_time = update_from_shmem_current_time; update_index_from_shmem(); } @@ -346,7 +349,7 @@ int update_from_shmem(param_t param, union param_value_u *value) //else {PX4_INFO("no change to param %s\n", param_name(param));} PX4_DEBUG("%s %d bit on krait changed index[%d]\n", - (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); + (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); return retval; }