139 lines
2.9 KiB
C++
139 lines
2.9 KiB
C++
#include <Arduino.h>
|
|
#include <RHReliableDatagram.h>
|
|
#include <RH_NRF24.h>
|
|
#include <SPI.h>
|
|
#include "radio.h"
|
|
|
|
static unsigned long lastSerialTime;
|
|
|
|
RcData data;
|
|
|
|
RcData* getDataptr( void )
|
|
{
|
|
return &data;
|
|
}
|
|
|
|
void radio_c::begin( )
|
|
{
|
|
if (!RadioManager.init())
|
|
{
|
|
#ifdef DEBUG
|
|
Serial.println("init failed");
|
|
#endif
|
|
open = false;
|
|
}
|
|
else
|
|
{
|
|
open = true;
|
|
}
|
|
}
|
|
|
|
bool radio_c::transmitData( void )
|
|
{
|
|
RcData* data = getDataptr();
|
|
uint8_t* payload = data->getByteArray();
|
|
if (RadioManager.sendtoWait(payload, sizeof(payload), m_myaddress))
|
|
{
|
|
// Now wait for a reply from the server
|
|
uint8_t len = sizeof(buf);
|
|
uint8_t from;
|
|
if (RadioManager.recvfromAckTimeout(buf, &len, 2000, &from))
|
|
{
|
|
#ifdef DEBUG
|
|
Serial.print("got reply from : 0x");
|
|
Serial.print(from, HEX);
|
|
Serial.print(": ");
|
|
Serial.println((char*)buf);
|
|
#endif
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
#ifdef DEBUG
|
|
Serial.println("No reply, is nrf24_reliable_datagram_server running?");
|
|
#endif
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
#ifdef DEBUG
|
|
Serial.println("sending failed");
|
|
#endif
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool radio_c::receiveData( void )
|
|
{
|
|
RcData* data = getDataptr();
|
|
|
|
if (RadioManager.available())
|
|
{
|
|
// Wait for a message addressed to us from the client
|
|
uint8_t len = sizeof(buf);
|
|
uint8_t from;
|
|
if (RadioManager.recvfromAck(buf, &len, &from))
|
|
{
|
|
lastRecvTime = millis();
|
|
if(!data->setRcData(buf))
|
|
{
|
|
#ifdef DEBUG
|
|
Serial.println("package inclompete")
|
|
#endif
|
|
return false;
|
|
}
|
|
// Send a reply back to the originator client, check for error
|
|
if (!RadioManager.sendtoWait(data->getReturnData(), sizeof(data->getReturnData()), from))
|
|
{
|
|
#ifdef DEBUG
|
|
Serial.println("sendtoWait failed");
|
|
#endif
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// set all analog values to their middle position, if no RC signal is received during 1s!
|
|
if (millis() - lastRecvTime > 1000)
|
|
{
|
|
data->setfailsafe();
|
|
#ifdef DEBUG
|
|
Serial.println("No Radio Available - Check Transmitter!");
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
if (millis() - lastRecvTime > 2000) {
|
|
begin(); // re-initialize radio
|
|
lastRecvTime = millis();
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
void sendSerialCommands( void )
|
|
{
|
|
RcData* data = getDataptr();
|
|
|
|
if (millis() - lastSerialTime > 20) { // Send the data every 20ms
|
|
lastSerialTime = millis();
|
|
#ifdef DEBUG
|
|
Serial.print('<'); // Start marker
|
|
Serial.println(data->axis1);
|
|
Serial.println(data->axis2);
|
|
Serial.println(data->axis3);
|
|
Serial.println(data->axis4);
|
|
Serial.println(data->pot);
|
|
Serial.println(data->momentary1);
|
|
Serial.println(data->momentary2);
|
|
Serial.println(data->getfailsafe());
|
|
Serial.print('>'); // End marker
|
|
#endif
|
|
}
|
|
}
|
|
|
|
|
|
|