PX4 Firmware
PX4 Autopilot Software http://px4.io
px4muorb.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2015 Mark Charlebois. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include "px4muorb.hpp"
35 #include "uORBFastRpcChannel.hpp"
36 #include "uORBManager.hpp"
37 
38 #include <px4_platform_common/tasks.h>
39 #include <px4_platform_common/posix.h>
40 #include <dspal_platform.h>
42 #include "uORB.h"
43 #include <parameters/param.h>
44 #include <px4_platform_common/shmem.h>
45 #include <px4_platform_common/log.h>
46 
48 extern int dspal_main(int argc, char *argv[]);
50 
52 {
53  HAP_power_request(100, 100, 1000);
54  shmem_info_p = NULL;
55 
56  // The uORB Manager needs to be initialized first up, otherwise the instance is nullptr.
58  // Register the fastrpc muorb with uORBManager.
59  uORB::Manager::get_instance()->set_uorb_communicator(
61 
62  // Now continue with the usual dspal startup.
63  const char *argv[] = { "dspal", "start" };
64  int argc = 2;
65  int rc;
66  rc = dspal_main(argc, (char **) argv);
67 
68  return rc;
69 }
70 
71 int px4muorb_set_absolute_time_offset(int32_t time_diff_us)
72 {
73  return hrt_set_absolute_time_offset(time_diff_us);
74 }
75 
76 int px4muorb_get_absolute_time(uint64_t *time_us)
77 {
78  *time_us = hrt_absolute_time();
79  return 0;
80 }
81 
82 /*update value and param's change bit in shared memory*/
83 int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value,
84  int data_len_in_bytes)
85 {
86  unsigned int byte_changed, bit_changed;
87  union param_value_u *param_value = (union param_value_u *) value;
88 
89  if (!shmem_info_p) {
90  init_shared_memory();
91  }
92 
93  if (get_shmem_lock(__FILE__, __LINE__) != 0) {
94  PX4_INFO("Could not get shmem lock\n");
95  return -1;
96  }
97 
98  shmem_info_p->params_val[param] = *param_value;
99 
100  byte_changed = param / 8;
101  bit_changed = 1 << param % 8;
102  shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
103 
104  release_shmem_lock(__FILE__, __LINE__);
105 
106  return 0;
107 }
108 
109 int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes)
110 {
111  if (!shmem_info_p) {
112  return -1;
113  }
114 
115  if (get_shmem_lock(__FILE__, __LINE__) != 0) {
116  PX4_INFO("Could not get shmem lock\n");
117  return -1;
118  }
119 
120  for (int i = 0; i < data_len_in_bytes; i++) {
121  data[i] = shmem_info_p->adsp_changed_index[i];
122  }
123 
124  release_shmem_lock(__FILE__, __LINE__);
125 
126  return 0;
127 }
128 
129 int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value,
130  int data_len_in_bytes)
131 {
132  unsigned int byte_changed, bit_changed;
133  union param_value_u *param_value = (union param_value_u *) value;
134 
135  if (!shmem_info_p) {
136  return -1;
137  }
138 
139  if (get_shmem_lock(__FILE__, __LINE__) != 0) {
140  PX4_INFO("Could not get shmem lock\n");
141  return -1;
142  }
143 
144  *param_value = shmem_info_p->params_val[param];
145 
146  /*also clear the index since we are holding the lock*/
147  byte_changed = param / 8;
148  bit_changed = 1 << param % 8;
149  shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
150 
151  release_shmem_lock(__FILE__, __LINE__);
152 
153  return 0;
154 }
155 
156 int px4muorb_topic_advertised(const char *topic_name)
157 {
158  int rc = 0;
159  PX4_INFO("TEST px4muorb_topic_advertised of [%s] on remote side...", topic_name);
161  uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
162 
163  if (rxHandler != nullptr) {
164  rc = rxHandler->process_remote_topic(topic_name, 1);
165 
166  } else {
167  rc = -1;
168  }
169 
170  return rc;
171 }
172 
173 int px4muorb_topic_unadvertised(const char *topic_name)
174 {
175  int rc = 0;
176  PX4_INFO("TEST px4muorb_topic_unadvertised of [%s] on remote side...", topic_name);
178  uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
179 
180  if (rxHandler != nullptr) {
181  rc = rxHandler->process_remote_topic(topic_name, 0);
182 
183  } else {
184  rc = -1;
185  }
186 
187  return rc;
188 }
189 
191 {
192  int rc = 0;
194  channel->AddRemoteSubscriber(name);
195  uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
196 
197  if (rxHandler != nullptr) {
198  rc = rxHandler->process_add_subscription(name, 0);
199 
200  if (rc != OK) {
201  channel->RemoveRemoteSubscriber(name);
202  }
203 
204  } else {
205  rc = -1;
206  }
207 
208  return rc;
209 }
210 
212 {
213  int rc = 0;
215  channel->RemoveRemoteSubscriber(name);
216  uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
217 
218  if (rxHandler != nullptr) {
219  rc = rxHandler->process_remove_subscription(name);
220 
221  } else {
222  rc = -1;
223  }
224 
225  return rc;
226 }
227 
228 int px4muorb_send_topic_data(const char *name, const uint8_t *data,
229  int data_len_in_bytes)
230 {
231  int rc = 0;
233  uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
234 
235  if (rxHandler != nullptr) {
236  rc = rxHandler->process_received_message(name, data_len_in_bytes,
237  (uint8_t *) data);
238 
239  } else {
240  rc = -1;
241  }
242 
243  return rc;
244 }
245 
246 int px4muorb_is_subscriber_present(const char *topic_name, int *status)
247 {
248  int rc = 0;
249  int32_t local_status = 0;
251  rc = channel->is_subscriber_present(topic_name, &local_status);
252 
253  if (rc == 0) {
254  *status = (int) local_status;
255  }
256 
257  return rc;
258 }
259 
260 int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len,
261  uint8_t *data, int data_len_in_bytes, int *bytes_returned)
262 {
263  int rc = 0;
264  int32_t local_msg_type = 0;
265  int32_t local_bytes_returned = 0;
267  //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
268  rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data,
269  data_len_in_bytes, &local_bytes_returned);
270  *msg_type = (int) local_msg_type;
271  *bytes_returned = (int) local_bytes_returned;
272  return rc;
273 }
274 
275 int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer,
276  int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count)
277 {
278  int rc = 0;
279  int32_t local_bytes_returned = 0;
280  int32_t local_topic_count = 0;
282  //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
283  rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes,
284  &local_bytes_returned, &local_topic_count);
285  *returned_length_in_bytes = (int) local_bytes_returned;
286  *topic_count = (int) local_topic_count;
287  return rc;
288 }
289 
291 {
292  int rc = 0;
294  rc = channel->unblock_get_data_method();
295  return rc;
296 }
int32_t param_value
uORBCommunicator::IChannelRxHandler * GetRxHandler()
static uORB::Manager * get_instance()
Method to get the singleton instance for the uORB::Manager.
Definition: uORBManager.hpp:89
static struct vehicle_status_s status
Definition: Commander.cpp:138
int px4muorb_add_subscriber(const char *name)
Definition: px4muorb.cpp:190
#define __END_DECLS
Definition: visibility.h:59
API for the uORB lightweight object broker.
int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count)
Definition: px4muorb.cpp:275
int px4muorb_topic_advertised(const char *topic_name)
Definition: px4muorb.cpp:156
static bool initialize()
Initialize the singleton.
Definition: uORBManager.cpp:49
virtual int16_t process_remove_subscription(const char *messageName)=0
Interface to process a received control msg to remove subscription.
void AddRemoteSubscriber(const std::string &messageName)
static uORB::FastRpcChannel * GetInstance()
static method to get the IChannel Implementor.
int32_t i
Definition: param.h:418
virtual int16_t process_remote_topic(const char *topic_name, bool isAdvertisement)=0
Interface to process a received topic from remote.
Global flash based parameter store.
int px4muorb_is_subscriber_present(const char *topic_name, int *status)
Definition: px4muorb.cpp:246
virtual int16_t process_add_subscription(const char *messageName, int32_t msgRateInHz)=0
Interface to process a received AddSubscription from remote.
virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data)=0
Interface to process the received data message.
int px4muorb_get_absolute_time(uint64_t *time_us)
Definition: px4muorb.cpp:76
void RemoveRemoteSubscriber(const std::string &messageName)
int px4muorb_topic_unadvertised(const char *topic_name)
Definition: px4muorb.cpp:173
Parameter value union.
Definition: param.h:416
__END_DECLS int px4muorb_orb_initialize()
Definition: px4muorb.cpp:51
int px4muorb_unblock_recieve_msg(void)
Definition: px4muorb.cpp:290
uint8_t * data
Definition: dataman.cpp:149
#define __BEGIN_DECLS
Definition: visibility.h:58
int16_t is_subscriber_present(const char *messageName, int32_t *status)
int16_t get_bulk_data(uint8_t *buffer, int32_t max_size_in_bytes, int32_t *returned_bytes, int32_t *topic_count)
Class passed to the communication link implement to provide callback for received messages over a cha...
int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes)
Definition: px4muorb.cpp:228
int px4muorb_remove_subscriber(const char *name)
Definition: px4muorb.cpp:211
int px4muorb_set_absolute_time_offset(int32_t time_diff_us)
Definition: px4muorb.cpp:71
int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes)
Definition: px4muorb.cpp:109
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)
Definition: px4muorb.cpp:260
__BEGIN_DECLS int dspal_main(int argc, char *argv[])
const char * name
Definition: tests_main.c:58
#define OK
Definition: uavcan_main.cpp:71
int16_t get_data(int32_t *msg_type, char *topic_name, int32_t topic_name_len, uint8_t *data, int32_t data_len_in_bytes, int32_t *bytes_returned)
int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
Definition: px4muorb.cpp:83
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
Definition: px4muorb.cpp:129