int main()
{
struct RingBuffer *h = NULL;
int x;
assert(ringbuffer_init(&h, 3, sizeof(x)) == 0);
assert(ringbuffer_top(h, (unsigned char *)&x) == RBE_NO_SPACE_OR_DATA);
x = 10;
assert(ringbuffer_push(h, (unsigned char *)&x) == RBE_SUCCESS);
x = -1;
assert(ringbuffer_top(h, (unsigned char *)&x) == RBE_SUCCESS);
assert(x == 10);
x = 666;
assert(ringbuffer_push(h, (unsigned char *)&x) == RBE_SUCCESS);
x = -1;
assert(ringbuffer_top(h, (unsigned char *)&x) == RBE_SUCCESS);
assert(x == 10);
assert(ringbuffer_push(h, (unsigned char *)&x) == RBE_NO_SPACE_OR_DATA);
ringbuffer_destroy(&h);
assert(ringbuffer_init(&h, 3, sizeof(x)) == 0);
x = INT_MIN;
assert(ringbuffer_push(h, (unsigned char *)&x) == RBE_SUCCESS);
x = -1;
assert(ringbuffer_top(h, (unsigned char *)&x) == RBE_SUCCESS);
assert(x == INT_MIN);
x = INT_MAX;
assert(ringbuffer_push(h, (unsigned char *)&x) == RBE_SUCCESS);
x = -1;
assert(ringbuffer_top(h, (unsigned char *)&x) == RBE_SUCCESS);
assert(x == INT_MIN);
ringbuffer_destroy(&h);
return 0;
}
static int wait_fill_event(struct tb_event *event, struct timeval *timeout)
{
/* ;-) */
#define ENOUGH_DATA_FOR_INPUT_PARSING 128
int result;
char buf[ENOUGH_DATA_FOR_INPUT_PARSING];
fd_set events;
memset(event, 0, sizeof(struct tb_event));
/* try to extract event from input buffer, return on success */
event->type = TB_EVENT_KEY;
if (extract_event(event, &inbuf, inputmode) == 0)
return TB_EVENT_KEY;
/* it looks like input buffer is incomplete, let's try the short path */
size_t r = fread(buf, 1, ENOUGH_DATA_FOR_INPUT_PARSING, in);
if (r > 0) {
if (ringbuffer_free_space(&inbuf) < r)
return -1;
ringbuffer_push(&inbuf, buf, r);
if (extract_event(event, &inbuf, inputmode) == 0)
return TB_EVENT_KEY;
}
/* no stuff in FILE's internal buffer, block in select */
while (1) {
FD_ZERO(&events);
FD_SET(in_fileno, &events);
FD_SET(winch_fds[0], &events);
int maxfd = (winch_fds[0] > in_fileno) ? winch_fds[0] : in_fileno;
result = select(maxfd+1, &events, 0, 0, timeout);
if (!result)
return 0;
if (FD_ISSET(in_fileno, &events)) {
event->type = TB_EVENT_KEY;
size_t r = fread(buf, 1, ENOUGH_DATA_FOR_INPUT_PARSING, in);
if (r == 0)
continue;
/* if there is no free space in input buffer, return error */
if (ringbuffer_free_space(&inbuf) < r)
return -1;
/* fill buffer */
ringbuffer_push(&inbuf, buf, r);
if (extract_event(event, &inbuf, inputmode) == 0)
return TB_EVENT_KEY;
}
if (FD_ISSET(winch_fds[0], &events)) {
event->type = TB_EVENT_RESIZE;
int zzz = 0;
read(winch_fds[0], &zzz, sizeof(int));
buffer_size_change_request = 1;
get_term_size(&event->w, &event->h);
return TB_EVENT_RESIZE;
}
}
}
void USART1_IRQHandler(void)
{
if (USART_GetITStatus(CEREAL_USARTx, USART_IT_RXNE) != RESET)
{
uint8_t c = cereal_rx_raw();
if (!ringbuffer_isfull(&cereal_incoming))
{
ringbuffer_push(&cereal_incoming, c);
if (c == 0 || c == '\r' || c == '\n') {
cereal_incoming.flag = 1;
}
}
else
{
cereal_incoming.flag = 1;
}
}
if (USART_GetITStatus(CEREAL_USARTx, USART_IT_TXE) != RESET )
{
#ifdef ENABLE_CEREAL_BUFFERED_TX
if (!ringbuffer_isempty(&cereal_outgoing)) {
cereal_tx_raw(ringbuffer_pop(&cereal_outgoing));
}
else {
cereal_outgoing.flag = 0;
USART_ITConfig(CEREAL_USARTx, USART_IT_TXE, DISABLE);
}
#endif
}
}
/**
* Adapter from http://simreal.com/content/Odometry
*/
void callback_encoders(const ras_arduino_msgs::EncodersConstPtr& encoders)
{
static tf::TransformBroadcaster pub_tf;
if (!_mute) {
double c_l = 1.0;//0.98416;
double c_r = 1.0;//1.0538;
double dist_l = c_l * (2.0*M_PI*robot::dim::wheel_radius) * (-encoders->delta_encoder1 / robot::prop::ticks_per_rev);
double dist_r = c_r * (2.0*M_PI*robot::dim::wheel_radius) * (-encoders->delta_encoder2 / robot::prop::ticks_per_rev);
_theta += (dist_r - dist_l) / robot::dim::wheel_distance;
double dist = (dist_r + dist_l) / 2.0;
_x += dist * cos(_theta);
_y += dist * sin(_theta);
ras_arduino_msgs::Encoders fixed = *encoders;
fixed.timestamp = (int)(ros::Time::now().toNSec()/1000l);
ringbuffer_push(fixed);
}
pack_pose(_q, _odom);
_pub_odom.publish(_odom);
tf::Transform transform;
transform.setOrigin(tf::Vector3(_x, _y, 0));
transform.setRotation(_q);
pub_tf.sendTransform(tf::StampedTransform(transform, _odom.header.stamp, "map", "robot"));
send_marker(transform);
}
void* producerH(void* arg) {
printf("%s\n", __FUNCTION__);
int i = 0;
for (;;) {
ringbuffer_push(queue, 8, __FUNCTION__, strlen(__FUNCTION__));
printf("after produce: ");
ringbuffer_dump(queue);
sleep(1);
}
}
int taskloop_add(task_callback task)
{
void *object = task;
if (ringbuffer_full(&taskqueue.tasks))
return 0;
ringbuffer_push(&taskqueue.tasks, &object);
return 1;
}
int main()
{
struct RingBuffer *h = NULL;
int x = 10, y = INT_MAX, z = 2863311530, m = -3333;
int offset_remove = 2;
int offset_insert = 3;
int offset_data = 4;
assert(ringbuffer_init(&h, 3, sizeof(x)) == 0);
assert(ringbuffer_push(h, (unsigned char *)&x) == RBE_SUCCESS);
assert(ringbuffer_push(h, (unsigned char *)&y) == RBE_SUCCESS);
assert(ringbuffer_full(h) != 0);
assert(ringbuffer_pop(h) == RBE_SUCCESS);
assert(ringbuffer_push(h, (unsigned char *)&z) == RBE_SUCCESS);
assert(ringbuffer_full(h) != 0);
assert(ringbuffer_resize(&h, 5) == RBE_SUCCESS);
assert(ringbuffer_full(h) == 0);
assert(ringbuffer_empty(h) == 0);
assert(ringbuffer_push(h, (unsigned char *)&m) == RBE_SUCCESS);
assert(ringbuffer_push(h, (unsigned char *)&x) == RBE_SUCCESS);
assert(ringbuffer_full(h) != 0);
{ /* checking the data: x y z m x */
unsigned *off = (unsigned *) h;
int *values = (int *) off + offset_data;
assert(*(off + offset_remove) == 1 * sizeof(x));
assert(*(off + offset_insert) == 0);
assert(values[0] == x);
assert(values[1] == y);
assert(values[2] == z);
assert(values[3] == m);
assert(values[4] == x);
}
ringbuffer_destroy(&h);
return 0;
}
void cereal_tx(uint8_t x)
{
#ifdef ENABLE_CEREAL_BUFFERED_TX
if (!ringbuffer_isfull(&cereal_outgoing))
{
ringbuffer_push(&cereal_outgoing, x);
if (cereal_outgoing.flag == 0) {
cereal_tx_raw(ringbuffer_pop(&cereal_outgoing));
cereal_outgoing.flag = 1;
USART_ITConfig(CEREAL_USARTx, USART_IT_TXE, ENABLE);
}
}
#else
cereal_tx_raw(x);
while (USART_GetFlagStatus(CEREAL_USARTx, USART_FLAG_TXE) == RESET) ;
#endif
}
/**
* @brief USBD_CDC_DataOut
* Data received on non-control Out endpoint
* @param pdev: device instance
* @param epnum: endpoint number
* @retval status
*/
static uint8_t USBD_CDC_DataOut (void *pdev, uint8_t epnum)
{
uint16_t USB_Rx_Cnt;
/* Get the received data buffer and update the counter */
USB_Rx_Cnt = ((USB_OTG_CORE_HANDLE*)pdev)->dev.out_ep[epnum].xfer_count;
for (uint16_t i = 0; i < USB_Rx_Cnt; i++) {
uint8_t c = USBD_CDC_H2D_Buff[i];
ringbuffer_push(&USBD_CDC_H2D_FIFO, c);
}
/* Prepare Out endpoint to receive next packet */
DCD_EP_PrepareRx(pdev,
USBD_Dev_CDC_H2D_EP,
(uint8_t*)(USBD_CDC_H2D_Buff),
CDC_DATA_OUT_PACKET_SIZE);
return USBD_OK;
}
/****************************************************************************
*
* NAME: SPM_u32PullData
*
* DESCRIPTION:
* SPM pull some data into data pool
*
*
* RETURNS:
* available data size of SPM
*
****************************************************************************/
uint32 SPM_u32PullData(void *data, int len)
{
uint32 free_cnt = 0;
uint32 min_cnt = 0;
uint32 avlb_cnt = 0;
/* Cal how much free space do SPM have */
OS_eEnterCriticalSection(mutexRxRb);
free_cnt = ringbuffer_free_space(&rb_rx_spm);
OS_eExitCriticalSection(mutexRxRb);
min_cnt = MIN(free_cnt, len);
DBG_vPrintf(TRACE_NODE, "rev_cnt: %u, free_cnt: %u \r\n", len, free_cnt);
if(min_cnt > 0)
{
OS_eEnterCriticalSection(mutexRxRb);
ringbuffer_push(&rb_rx_spm, data, min_cnt);
avlb_cnt = ringbuffer_data_size(&rb_rx_spm);
OS_eExitCriticalSection(mutexRxRb);
}
return avlb_cnt;
}
/** push a copy of a (temporary) memory buffer.
This is a helper function that will first do a `malloc(size)` and then copy
the `buffer` contents. You may use this if you wish to ringbuffer_push() some
"volatile" data, where the buffer pointer gets invalidated afterwards.
*/
void ringbuffer_push_copy(ringbuffer_t *rb, void *buffer, size_t size) {
if (size == 0 || !buffer) return;
void *copy = malloc(size);
if (copy)
ringbuffer_push(rb, memcpy(copy, buffer, size));
}
