mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-06 16:49:51 +08:00
Unfortunately, lot's of whitespace changes, required to satisfy unspecified code style format errors.
This commit is contained in:
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user