// Receive data from the brain
void brain_rx_thread(void) {
uint8_t input;
while(1) {
// wait for input
while(!rx_ready(BRAIN)) yeild();
input = rx_byte(BRAIN);
switch(input) {
case 'M':
// receive motor command
brain_pack.reset();
do {
while(!rx_ready(BRAIN)) yeild();
input = rx_byte(BRAIN);
brain_pack.input((char)input);
} while( input != '\r');
if( !bt_control ) {
target_speed = brain_pack.reads8();
steer = brain_pack.readu8();
input = steer + STEER_OFFSET; // steering zero set
servo_set(0, input);
}
break;
case 'G':
passthrough(BRAIN, BT, 'G');
break;
default:
passthrough(BRAIN, BT, input);
break;
}
}
}
/** \brief main compare functions
*/
int netif_stat_t::compare(const netif_stat_t &other) const throw()
{
// handle the case where at least one is null
if( is_null() && !other.is_null() ) return -1;
if( !is_null() && other.is_null() ) return +1;
if( is_null() && other.is_null() ) return 0;
// NOTE: here both are NOT null
// compare the name
if( name() < other.name() ) return -1;
if( name() > other.name() ) return +1;
// NOTE: here both name are equal
// compare the capture_date
if( capture_date() < other.capture_date() ) return -1;
if( capture_date() > other.capture_date() ) return +1;
// NOTE: here both capture_date are equal
// compare the rx_byte
if( rx_byte() < other.rx_byte() ) return -1;
if( rx_byte() > other.rx_byte() ) return +1;
// NOTE: here both rx_byte are equal
// compare the tx_byte
if( tx_byte() < other.tx_byte() ) return -1;
if( tx_byte() > other.tx_byte() ) return +1;
// NOTE: here both tx_byte are equal
// here both are considered equal
return 0;
}
/* GPS listen thread */
void gps_spinOnce(void) {
if(rx_ready(gps_port)) {
gps_input = rx_byte(gps_port);
if(gps.encode(gps_input)) {
gps.get_position(&lat, &lon);
if( gps_pub.reset() ) {
gps_pub.append(lat);
gps_pub.append(lon);
// TODO: fill in rest of GPS message
gps_pub.append(gps.altitude());
gps_pub.append(gps.hdop());
gps_pub.finish();
}
if( gps_pub2.reset() ) {
gps_pub2.append(lat);
gps_pub2.append(lon);
// TODO: fill in rest of GPS message
gps_pub2.finish();
}
}
}
}
void main (void) {
long command, left, right;
int rec, i;
P0_5=1;
while (1) {
printf("%ld\n", travelled);
rec = 1;
for (i = 0; i < 4; i++){
if (voltage(0) > MIN){
rec = 0;
}
}
if (!rec){
right = getDistance(2);
left = getDistance(1);
if (orientation == REVERSE){
long temp = left;
left = right;
right = temp;
}
if (autonomous){
if (left > right + 300){
motorRight.power = 0;
motorLeft.power = totalpower;
} else if (right > left + 300){
motorLeft.power = 0;
motorRight.power = totalpower;
} else if (left > distance * ERROR){
motorLeft.direction = FORWARD;
motorRight.direction = FORWARD;
motorLeft.power = totalpower;
motorRight.power = totalpower;
} else if (left < distance / ERROR){
motorLeft.direction = REVERSE;
motorRight.direction = REVERSE;
motorLeft.power = totalpower;
motorRight.power = totalpower;
} else {
motorLeft.power = 0;
motorRight.power = 0;
}
}
} else {
motorLeft.power = 0;
motorRight.power = 0;
waitms(10); //this makes sure that the power change takes effect before shutting off the interrupt
command = rx_byte();
implement_command(command);
waitms(200); //prevents receiving commands back to back
}
}
}
/** \brief convert the object to a string
*/
std::string netif_stat_t::to_string() const throw()
{
std::ostringstream oss;
// handle the null case
if( is_null() ) return "null";
// build the string to return
oss << "name=" << name();
oss << " " << "capture_date=" << capture_date();
oss << " " << "rx_byte=" << rx_byte();
oss << " " << "tx_byte=" << tx_byte();
// return the just built string
return oss.str();
}
IRAM int esp_uart_dispatch_rx_top(int uart_no) {
struct esp_uart_state *us = s_us[uart_no];
if (us == NULL || !us->rx_enabled) return 1;
uint32_t rxn = 0;
cs_rbuf_t *rxb = &us->rx_buf;
/* RX */
if (rxb->avail > 0 && rx_fifo_len(uart_no) > 0) {
int linger_counter = 0;
/* 32 here is a constant measured (using system_get_time) to provide
* linger time of rx_linger_micros. It basically means that one iteration
* of the loop takes 3.2 us.
*
* Note: lingering may starve TX FIFO if the flow is bidirectional.
* TODO(rojer): keep transmitting from tx_buf while lingering.
*/
int max_linger = us->cfg->rx_linger_micros / 10 * 32;
#ifdef MEASURE_LINGER_TIME
uint32_t st = system_get_time();
#endif
while (rxb->avail > 0 && linger_counter <= max_linger) {
int rx_len = rx_fifo_len(uart_no);
if (rx_len > 0) {
while (rx_len-- > 0 && rxb->avail > 0) {
cs_rbuf_append_one(rxb, rx_byte(uart_no));
rxn++;
}
if (linger_counter > 0) {
us->stats.rx_linger_conts++;
linger_counter = 0;
}
} else {
linger_counter++;
}
}
#ifdef MEASURE_LINGER_TIME
fprintf(stderr, "Time spent reading: %u us\n", system_get_time() - st);
#endif
us->stats.rx_bytes += rxn;
}
int rfl = rx_fifo_len(uart_no);
if (rfl < us->cfg->rx_fifo_full_thresh) {
CLEAR_PERI_REG_MASK(UART_INT_CLR(uart_no), UART_RX_INTS);
}
return rfl == 0;
}
void mgos_uart_hal_dispatch_rx_top(struct mgos_uart_state *us) {
int uart_no = us->uart_no;
struct mbuf *rxb = &us->rx_buf;
uint32_t rxn = 0;
/* RX */
if (mgos_uart_rxb_free(us) > 0 && esp_uart_rx_fifo_len(uart_no) > 0) {
int linger_counter = 0;
/* 32 here is a constant measured (using system_get_time) to provide
* linger time of rx_linger_micros. It basically means that one iteration
* of the loop takes 3.2 us.
*
* Note: lingering may starve TX FIFO if the flow is bidirectional.
* TODO(rojer): keep transmitting from tx_buf while lingering.
*/
int max_linger = us->cfg.rx_linger_micros / 10 * 32;
#ifdef MEASURE_LINGER_TIME
uint32_t st = system_get_time();
#endif
while (mgos_uart_rxb_free(us) > 0 && linger_counter <= max_linger) {
size_t rx_len = esp_uart_rx_fifo_len(uart_no);
if (rx_len > 0) {
rx_len = MIN(rx_len, mgos_uart_rxb_free(us));
if (rxb->size < rxb->len + rx_len) mbuf_resize(rxb, rxb->len + rx_len);
while (rx_len > 0) {
uint8_t b = rx_byte(uart_no);
mbuf_append(rxb, &b, 1);
rx_len--;
rxn++;
}
if (linger_counter > 0) {
us->stats.rx_linger_conts++;
linger_counter = 0;
}
} else {
linger_counter++;
}
}
#ifdef MEASURE_LINGER_TIME
fprintf(stderr, "Time spent reading: %u us\n", system_get_time() - st);
#endif
us->stats.rx_bytes += rxn;
}
CLEAR_PERI_REG_MASK(UART_INT_CLR(uart_no), UART_RX_INTS);
}
/************************************************************************
* void tftp()
* Simple application layer server to return data
* The only functionality provided is reading, and only one 'file'.
* this function doesn't need to know anything about buffer
* positions, it simply uses functions provided by the IP layer.
***********************************************************************/
void tftp()
{
WORD opcode;
/* read the operation code */
copy_rx_word(&opcode);
/* read request */
if (opcode == TFTP_READ) {
/* check the requested filename is 'stats' */
if (rx_byte() == 's' && rx_byte() == 't' && rx_byte() == 'a' &&
rx_byte() == 't' && rx_byte() == 's' && rx_byte() == 0x00)
{
tx_word(TFTP_DATA); /* op code */
tx_word(0x0001); /* block number */
q_string(" IP Statistics \n\n",21);
q_string("Received:\t",10);
q_num(ipstat.rxpackets);
q_string("\nTransmitted:\t",14);
q_num(ipstat.txpackets);
q_string("\nDropped:\t",10);
q_num(ipstat.dropped);
q_string("\n\n",2);
}
/* requested a different file, not found */
else {
tx_word(TFTP_ERR);
tx_word(0x0001);
q_string("File Not Found.",15);
tx_byte(0x00);
}
udp_send(txpos-IPSIZE-UDPHSIZE,1);
}
/* write request, send error code and message */
else if (opcode == TFTP_WRITE) {
tx_word(TFTP_ERR);
tx_word(0x0002);
q_string("Access Violation.",17);
tx_byte(0x00);
udp_send(txpos-IPSIZE-UDPHSIZE,1);
}
}
static void uart_irq_handler(NRF_UART_Type * p_uart,
uart_control_block_t * p_cb)
{
if (nrf_uart_int_enable_check(p_uart, NRF_UART_INT_MASK_ERROR) &&
nrf_uart_event_check(p_uart, NRF_UART_EVENT_ERROR))
{
nrfx_uart_event_t event;
nrf_uart_event_clear(p_uart, NRF_UART_EVENT_ERROR);
NRFX_LOG_DEBUG("Event: %s.", EVT_TO_STR(NRF_UART_EVENT_ERROR));
nrf_uart_int_disable(p_uart, NRF_UART_INT_MASK_RXDRDY |
NRF_UART_INT_MASK_ERROR);
if (!p_cb->rx_enabled)
{
nrf_uart_task_trigger(p_uart, NRF_UART_TASK_STOPRX);
}
event.type = NRFX_UART_EVT_ERROR;
event.data.error.error_mask = nrf_uart_errorsrc_get_and_clear(p_uart);
event.data.error.rxtx.bytes = p_cb->rx_buffer_length;
event.data.error.rxtx.p_data = p_cb->p_rx_buffer;
// Abort transfer.
p_cb->rx_buffer_length = 0;
p_cb->rx_secondary_buffer_length = 0;
p_cb->handler(&event,p_cb->p_context);
}
else if (nrf_uart_int_enable_check(p_uart, NRF_UART_INT_MASK_RXDRDY) &&
nrf_uart_event_check(p_uart, NRF_UART_EVENT_RXDRDY))
{
rx_byte(p_uart, p_cb);
if (p_cb->rx_buffer_length == p_cb->rx_counter)
{
if (p_cb->rx_secondary_buffer_length)
{
uint8_t * p_data = p_cb->p_rx_buffer;
size_t rx_counter = p_cb->rx_counter;
// Switch to secondary buffer.
p_cb->rx_buffer_length = p_cb->rx_secondary_buffer_length;
p_cb->p_rx_buffer = p_cb->p_rx_secondary_buffer;
p_cb->rx_secondary_buffer_length = 0;
p_cb->rx_counter = 0;
rx_done_event(p_cb, rx_counter, p_data);
}
else
{
if (!p_cb->rx_enabled)
{
nrf_uart_task_trigger(p_uart, NRF_UART_TASK_STOPRX);
}
nrf_uart_int_disable(p_uart, NRF_UART_INT_MASK_RXDRDY |
NRF_UART_INT_MASK_ERROR);
p_cb->rx_buffer_length = 0;
rx_done_event(p_cb, p_cb->rx_counter, p_cb->p_rx_buffer);
}
}
}
if (nrf_uart_event_check(p_uart, NRF_UART_EVENT_TXDRDY))
{
if (p_cb->tx_counter < p_cb->tx_buffer_length &&
!p_cb->tx_abort)
{
tx_byte(p_uart, p_cb);
}
else
{
nrf_uart_event_clear(p_uart, NRF_UART_EVENT_TXDRDY);
if (p_cb->tx_buffer_length)
{
tx_done_event(p_cb, p_cb->tx_buffer_length);
}
}
}
if (nrf_uart_event_check(p_uart, NRF_UART_EVENT_RXTO))
{
nrf_uart_event_clear(p_uart, NRF_UART_EVENT_RXTO);
// RXTO event may be triggered as a result of abort call. In th
if (p_cb->rx_enabled)
{
nrf_uart_task_trigger(p_uart, NRF_UART_TASK_STARTRX);
}
if (p_cb->rx_buffer_length)
{
p_cb->rx_buffer_length = 0;
rx_done_event(p_cb, p_cb->rx_counter, p_cb->p_rx_buffer);
}
}
}
nrfx_err_t nrfx_uart_rx(nrfx_uart_t const * p_instance,
uint8_t * p_data,
size_t length)
{
uart_control_block_t * p_cb = &m_cb[p_instance->drv_inst_idx];
NRFX_ASSERT(m_cb[p_instance->drv_inst_idx].state == NRFX_DRV_STATE_INITIALIZED);
NRFX_ASSERT(p_data);
NRFX_ASSERT(length > 0);
nrfx_err_t err_code;
bool second_buffer = false;
if (p_cb->handler)
{
nrf_uart_int_disable(p_instance->p_reg, NRF_UART_INT_MASK_RXDRDY |
NRF_UART_INT_MASK_ERROR);
}
if (p_cb->rx_buffer_length != 0)
{
if (p_cb->rx_secondary_buffer_length != 0)
{
if (p_cb->handler)
{
nrf_uart_int_enable(p_instance->p_reg, NRF_UART_INT_MASK_RXDRDY |
NRF_UART_INT_MASK_ERROR);
}
err_code = NRFX_ERROR_BUSY;
NRFX_LOG_WARNING("Function: %s, error code: %s.",
__func__,
NRFX_LOG_ERROR_STRING_GET(err_code));
return err_code;
}
second_buffer = true;
}
if (!second_buffer)
{
p_cb->rx_buffer_length = length;
p_cb->p_rx_buffer = p_data;
p_cb->rx_counter = 0;
p_cb->rx_secondary_buffer_length = 0;
}
else
{
p_cb->p_rx_secondary_buffer = p_data;
p_cb->rx_secondary_buffer_length = length;
}
NRFX_LOG_INFO("Transfer rx_len: %d.", length);
if ((!p_cb->rx_enabled) && (!second_buffer))
{
rx_enable(p_instance);
}
if (p_cb->handler == NULL)
{
nrf_uart_event_clear(p_instance->p_reg, NRF_UART_EVENT_RXTO);
bool rxrdy;
bool rxto;
bool error;
do
{
do
{
error = nrf_uart_event_check(p_instance->p_reg, NRF_UART_EVENT_ERROR);
rxrdy = nrf_uart_event_check(p_instance->p_reg, NRF_UART_EVENT_RXDRDY);
rxto = nrf_uart_event_check(p_instance->p_reg, NRF_UART_EVENT_RXTO);
} while ((!rxrdy) && (!rxto) && (!error));
if (error || rxto)
{
break;
}
rx_byte(p_instance->p_reg, p_cb);
} while (p_cb->rx_buffer_length > p_cb->rx_counter);
p_cb->rx_buffer_length = 0;
if (error)
{
err_code = NRFX_ERROR_INTERNAL;
NRFX_LOG_WARNING("Function: %s, error code: %s.",
__func__,
NRFX_LOG_ERROR_STRING_GET(err_code));
return err_code;
}
if (rxto)
{
err_code = NRFX_ERROR_FORBIDDEN;
NRFX_LOG_WARNING("Function: %s, error code: %s.",
__func__,
NRFX_LOG_ERROR_STRING_GET(err_code));
return err_code;
}
if (p_cb->rx_enabled)
{
nrf_uart_task_trigger(p_instance->p_reg, NRF_UART_TASK_STARTRX);
}
else
{
// Skip stopping RX if driver is forced to be enabled.
nrf_uart_task_trigger(p_instance->p_reg, NRF_UART_TASK_STOPRX);
}
}
else
{
nrf_uart_int_enable(p_instance->p_reg, NRF_UART_INT_MASK_RXDRDY |
NRF_UART_INT_MASK_ERROR);
}
err_code = NRFX_SUCCESS;
NRFX_LOG_INFO("Function: %s, error code: %s.", __func__, NRFX_LOG_ERROR_STRING_GET(err_code));
return err_code;
}
// eat the data on a uart until a carriage return is found
void finish(int src) {
do {
while(!rx_ready(src)) yeild();
} while( rx_byte(src) != '\r' );
}
// Receive data from bluetooth
void bt_rx_thread(void) {
uint8_t input;
uint16_t i;
volatile uint16_t sz = 0;
while(1) {
while(!rx_ready(BT)) yeild();
input = rx_byte(BT);
switch(input) {
case 'Z':
input = 1;
for( i=0; i<8; i++ ) {
while(!rx_ready(BT)) yeild();
if( rx_byte(BT) != 'Z' ) input = 0;
}
if( input ) {
//tx_bytes(BRAIN, (uint8_t*)"ZZZZZZZZ\r", 9);
while(sz != 0) yeild();
sz = 9;
brain_tx_buffer((uint8_t*)"ZZZZZZZZ\r", (uint16_t*)&sz);
shutdown_count = 4*60;
}
finish(BT);
break;
case 'M':
input = rx_byte(BT);
if( bt_control ) {
int8_t speed = (int8_t)input;
speed = speed>100?100:speed;
speed = speed<-100?-100:speed;
//motor_speed(speed);
target_speed = speed;
}
finish(BT);
break;
case 'S':
input = rx_byte(BT);
if( bt_control ) {
servo_set(0, input);
steer = input - STEER_OFFSET;
}
finish(BT);
break;
case 'C':
bt_control = rx_byte(BT);
finish(BT);
break;
case 'L':
// at 8 bytes/pair, we can hold about 127 points
buffer[0] = 'L';
for( i=1, input=rx_byte(BT);
i<1023 && input != '\r';
i++, input=rx_byte(BT) ) {
buffer[i] = input;
}
buffer[i] = '\r';
while( sz != 0 ) yeild();
sz = i+1;
brain_tx_buffer(buffer, (uint16_t*)&sz);
//passthrough(BT, BRAIN, 'L');
//finish(BT);
break;
}
}
}
