Unfortunately, lot's of whitespace changes, required to satisfy unspecified code style format errors.

This commit is contained in:
jwilson
2016-06-20 19:36:21 -07:00
committed by Julian Oes
parent 1091319274
commit acc1f04b67
4 changed files with 131 additions and 94 deletions
+34 -26
View File
@@ -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;
}
@@ -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;
+13 -9
View File
@@ -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;
}
+37 -29
View File
@@ -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;
}