diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp index 8ce0a50d36..dad971ac7a 100644 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ b/src/modules/muorb/adsp/px4muorb.cpp @@ -49,10 +49,10 @@ __BEGIN_DECLS 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); +extern void release_shmem_lock(const char *caller_file_name, + int caller_line_number); extern void init_shared_memory(void); __END_DECLS - int px4muorb_orb_initialize() { HAP_power_request(100, 100, 1000); @@ -60,13 +60,14 @@ int px4muorb_orb_initialize() // The uORB Manager needs to be initialized first up, otherwise the instance is nullptr. uORB::Manager::initialize(); // Register the fastrpc muorb with uORBManager. - uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance()); + uORB::Manager::get_instance()->set_uorb_communicator( + uORB::FastRpcChannel::GetInstance()); // Now continue with the usual dspal startup. - const char *argv[] = {"dspal", "start"}; + const char *argv[] = { "dspal", "start" }; int argc = 2; int rc; - rc = dspal_main(argc, (char **)argv); + rc = dspal_main(argc, (char **) argv); return rc; } @@ -83,12 +84,13 @@ 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 px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, + int data_len_in_bytes) { unsigned int byte_changed, bit_changed; - union param_value_u *param_value = (union param_value_u *)value; + union param_value_u *param_value = (union param_value_u *) value; - if(!shmem_info_p) { + if (!shmem_info_p) { init_shared_memory(); } @@ -108,11 +110,12 @@ int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int dat return 0; } -int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) +int px4muorb_param_update_index_from_shmem(unsigned char *data, + int data_len_in_bytes) { unsigned int i; - if(!shmem_info_p) + if (!shmem_info_p) return -1; if (get_shmem_lock(__FILE__, __LINE__) != 0) { @@ -129,12 +132,13 @@ int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_ return 0; } -int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) +int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, + int data_len_in_bytes) { unsigned int byte_changed, bit_changed; - union param_value_u *param_value = (union param_value_u *)value; + 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) { @@ -192,14 +196,16 @@ int px4muorb_remove_subscriber(const char *name) return rc; } -int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +int px4muorb_send_topic_data(const char *name, const uint8_t *data, + int data_len_in_bytes) { int rc = 0; uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); if (rxHandler != nullptr) { - rc = rxHandler->process_received_message(name, data_len_in_bytes, (uint8_t *)data); + rc = rxHandler->process_received_message(name, data_len_in_bytes, + (uint8_t *) data); } else { rc = -1; @@ -216,37 +222,39 @@ int px4muorb_is_subscriber_present(const char *topic_name, int *status) rc = channel->is_subscriber_present(topic_name, &local_status); if (rc == 0) { - *status = (int)local_status; + *status = (int) local_status; } return rc; } -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) +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) { int rc = 0; int32_t local_msg_type = 0; int32_t local_bytes_returned = 0; 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); - *msg_type = (int)local_msg_type; - *bytes_returned = (int)local_bytes_returned; + rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, + 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 px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, + int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count) { int rc = 0; int32_t local_bytes_returned = 0; int32_t local_topic_count = 0; 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); - *returned_length_in_bytes = (int)local_bytes_returned; - *topic_count = (int)local_topic_count; + rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes, + &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 71a80e6969..8d7c2db5f2 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -52,7 +52,7 @@ using namespace px4muorb; static char *_TopicNameBuffer = 0; static const int32_t _MAX_TOPIC_NAME_BUFFER = 256; -static uint8_t *_DataBuffer = 0; +static uint8_t *_DataBuffer = 0; static const uint32_t _MAX_DATA_BUFFER_SIZE = 2048; static bool _Initialized = false; @@ -61,7 +61,8 @@ static bool _Initialized = false; // hence we are trying to allocation 64K of byte buffers. 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; +static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = + _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; static uint8_t *_BulkTransferBuffer = 0; unsigned char *adsp_changed_index = 0; @@ -102,12 +103,13 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) // Do this call right after reading to avoid latency here. timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); - uint64_t time_appsproc = ((uint64_t)ts.tv_sec) * 1000000llu + (ts.tv_nsec / 1000); + uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu + + (ts.tv_nsec / 1000); close(fd); uint64_t time_dsp; - int ret = sscanf(buffer, "%llx", &time_dsp); + int ret = sscanf(buffer, "%llx", &time_dsp); if (ret < 0) { PX4_ERR("Could not parse DSP timer."); @@ -119,8 +121,9 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) time_dsp /= 19.2; // 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); + uint64_t abs_diff = + (time_appsproc > time_dsp) ? + (time_appsproc - time_dsp) : (time_dsp - time_appsproc); if (abs_diff > INT32_MAX) { PX4_ERR("Timer difference too big"); @@ -129,13 +132,14 @@ 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); - PX4_DEBUG("found time_diff: %li us, %.6f s", *time_diff_us, ((double)*time_diff_us) / 1e6); + PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", + time_dsp, time_appsproc); + PX4_DEBUG("found time_diff: %li us, %.6f s", + *time_diff_us, ((double)*time_diff_us) / 1e6); return 0; } - px4muorb::KraitRpcWrapper::KraitRpcWrapper() { } @@ -153,21 +157,25 @@ 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)); + _BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + 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__); + PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", + __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); + 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); } _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; @@ -182,7 +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; @@ -199,7 +208,7 @@ 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; @@ -218,7 +227,8 @@ bool px4muorb::KraitRpcWrapper::Initialize() // call muorb initialize routine. if (px4muorb_orb_initialize() != 0) { - PX4_ERR("%s Error calling the uorb fastrpc initalize method..", __FUNCTION__); + PX4_ERR("%s Error calling the uorb fastrpc initalize method..", + __FUNCTION__); rc = false; return rc; } @@ -233,7 +243,8 @@ 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); + PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", + time_dsp, time_appsproc, diff); _Initialized = true; return rc; @@ -256,7 +267,7 @@ bool px4muorb::KraitRpcWrapper::Terminate() _DataBuffer = 0; } - if(adsp_changed_index != NULL) { + if (adsp_changed_index != NULL) { rpcmem_free(adsp_changed_index); adsp_changed_index = 0; } @@ -275,28 +286,32 @@ int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic) return (_Initialized ? px4muorb_remove_subscriber(topic) : -1); } -int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, int32_t *status) +int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, + int32_t *status) { return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1); } -int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) +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); + return (_Initialized ? + px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); } -int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, - uint8_t **data) +int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, + int32_t *length_in_bytes, uint8_t **data) { int32_t rc = -1; if (_Initialized) { - rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, - length_in_bytes); + rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, + _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, + length_in_bytes); if (rc == 0) { *topic = _TopicNameBuffer; - *data = _DataBuffer; + *data = _DataBuffer; } else { PX4_ERR("ERROR: Getting data from fastRPC link"); @@ -309,13 +324,15 @@ int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, return rc; } -int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) +int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, + int32_t *length_in_bytes, int32_t *topic_count) { int32_t rc = -1; 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); + rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, + _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 8fbaece56f..aa0ef2b959 100644 --- a/src/platforms/posix/px4_layer/shmem_posix.c +++ b/src/platforms/posix/px4_layer/shmem_posix.c @@ -60,33 +60,36 @@ 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; }; - /*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) { - if(!adsp_changed_index) { + if (!adsp_changed_index) { PX4_ERR("%s no param buffer", __FUNCTION__); return; } - px4muorb_param_update_index_from_shmem(adsp_changed_index, PARAM_BUFFER_SIZE); + px4muorb_param_update_index_from_shmem(adsp_changed_index, + 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__); } @@ -95,7 +98,7 @@ int update_from_shmem(param_t param, union param_value_u *value) unsigned int byte_changed, bit_changed; unsigned int retval = 0; - if(!adsp_changed_index) { + if (!adsp_changed_index) { PX4_ERR("%s no param buffer", __FUNCTION__); return 0; } @@ -103,7 +106,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(); } @@ -119,7 +122,8 @@ 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); + PX4_DEBUG("%s %d bit on adsp index[%d]", + (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 5aeabf520b..b4b24eab39 100644 --- a/src/platforms/qurt/px4_layer/shmem_qurt.c +++ b/src/platforms/qurt/px4_layer/shmem_qurt.c @@ -1,4 +1,3 @@ - /**************************************************************************** * * Copyright (c) 2015 Vijay Venkatraman. All rights reserved. @@ -76,23 +75,24 @@ static unsigned log2_for_int(unsigned v) return r; } -struct param_wbuf_s { - param_t param; - union param_value_u val; - bool unsaved; +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); 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 @@ -103,16 +103,19 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number) i++; usleep(1000); - if (i > 100) { break; } + if (i > 100) { + break; + } } 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); + PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", + caller_file_name, caller_line_number); } return 0; //got the lock @@ -121,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; @@ -138,14 +141,14 @@ 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; + 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++) { @@ -157,7 +160,7 @@ void init_shared_memory(void) 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(__FILE__, __LINE__) != 0) { @@ -171,9 +174,13 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) //{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_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; } + 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 @@ -197,7 +204,6 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) } - /*update value and param's change bit in shared memory*/ void update_to_shmem(param_t param, union param_value_u value) { @@ -224,12 +230,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 @@ -254,7 +260,9 @@ 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]); + unsigned bit = log2_for_int( + 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. @@ -269,14 +277,13 @@ void update_index_from_shmem(void) // FIXME: this is a hack but it gets the param so that it gets added // to the local list param_values in param_shmem.c. for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - if(params[i] != 0xFFFF){ + if (params[i] != 0xFFFF) { int32_t dummy; param_get(params[i], &dummy); } } } - static void update_value_from_shmem(param_t param, union param_value_u *value) { unsigned int byte_changed, bit_changed; @@ -299,12 +306,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 @@ -321,7 +328,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(); } @@ -337,9 +345,9 @@ 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); + PX4_DEBUG("%s %d bit on krait changed index[%d]\n", + (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); return retval; } -