int stm32CmdGet()
{
int ret = STM_ERR;
if ( sendCommand(0x00)==STM_OK ) {
int i;
unsigned char num;
serialRead(board.serial_fd, &num, 1); // number of bytes
serialRead(board.serial_fd, &board.version, 1);
serialRead(board.serial_fd, board.cmd, num);
if ( waitAck()==STM_OK ) {
printf("Bootloader version: %x.%x\nCommand set-[", board.version >> 4, board.version & 0x0F);
for ( i=0; i < num; i++ )
printf(" %02X", board.cmd[i]);
printf("]\n");
ret = STM_OK;
}
void serialCom(void)
{
uint8_t c;
int i;
for (i = 0; i < numTelemetryPorts; i++) {
currentPortState = &ports[i];
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
if (pendReboot)
systemReset(false); // noreturn
while (serialTotalBytesWaiting(currentPortState->port)) {
c = serialRead(currentPortState->port);
if (currentPortState->c_state == IDLE) {
currentPortState->c_state = (c == '$') ? HEADER_START : IDLE;
if (currentPortState->c_state == IDLE && !f.ARMED)
evaluateOtherData(c); // if not armed evaluate all other incoming serial data
} else if (currentPortState->c_state == HEADER_START) {
currentPortState->c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (currentPortState->c_state == HEADER_M) {
currentPortState->c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (currentPortState->c_state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
currentPortState->c_state = IDLE;
continue;
}
currentPortState->dataSize = c;
currentPortState->offset = 0;
currentPortState->checksum = 0;
currentPortState->indRX = 0;
currentPortState->checksum ^= c;
currentPortState->c_state = HEADER_SIZE; // the command is to follow
} else if (currentPortState->c_state == HEADER_SIZE) {
currentPortState->cmdMSP = c;
currentPortState->checksum ^= c;
currentPortState->c_state = HEADER_CMD;
} else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset < currentPortState->dataSize) {
currentPortState->checksum ^= c;
currentPortState->inBuf[currentPortState->offset++] = c;
} else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset >= currentPortState->dataSize) {
if (currentPortState->checksum == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
currentPortState->c_state = IDLE;
}
}
}
}
//Get power of a specific channel
unsigned short Parallax::getPower(unsigned char channel) {
if(channel > PARALLAX_NUM_MOTORS) {
Log::logf(Log::WARN, "Invalid PWM channel!\n");
return 0;
}
unsigned char command[8] = {'!', 'S', 'C', 'R', 'S', 'P', channel, '\r'};
//3 preamble bytes, 3 command bytes, channel byte, command terminator
if(serialWrite(&command, sizeof(command)) != sizeof(command)) {
Log::logf(Log::ERR, "Error writing to Parallax servo controller!\n");
}
serialRead(&command, sizeof(command)); //Remove remote echo from input buffer, ignore any errors
unsigned char data[3] = {0};
if(serialRead(&data, sizeof(data)) == sizeof(data)) {
return (data[1] << 8) | data[2];
} else {
return 0;
}
}
//!Wait the response of the module
void WaspRFID::waitResponse(void)
{
int val = 0xFF;
long cont = 0;
while((val != PREAMBLE) && (cont < timeOut)) {
val = serialRead(_uart);
delay(5);
cont ++;
}
}
int8_t gpsSetPassthrough(void)
{
if (gpsData.state != GPS_RECEIVINGDATA)
return -1;
LED0_OFF;
LED1_OFF;
while(1) {
if (serialTotalBytesWaiting(core.gpsport)) {
LED0_ON;
serialWrite(core.mainport, serialRead(core.gpsport));
LED0_OFF;
}
if (serialTotalBytesWaiting(core.mainport)) {
LED1_ON;
serialWrite(core.gpsport, serialRead(core.mainport));
LED1_OFF;
}
}
}
void initlbuf(void) {
lbufptr = lbuf;
#if defined(SERIAL_OVERRIDE) && 0
// don't do the prompt in serialIsOverridden mode
if (serialIsOverridden()) return;
#endif
prompt();
// flush any pending serial input
while (serialAvailable()) serialRead();
}
//Set power of a specific channel
void Parallax::setPower(unsigned char channel, unsigned short power) {
if(channel > PARALLAX_NUM_MOTORS) {
Log::logf(Log::WARN, "Invalid PWM channel!\n");
return;
}
unsigned char command[8] = {'!', 'S', 'C', channel, '\0', (unsigned char)(power & 0xFF), (unsigned char)((power >> 8) & 0xFF), '\r'};
//3 preamble byte, channel byte, ramp byte, power low byte, power high byte, command terminator
if(serialWrite(&command, sizeof(command)) != sizeof(command)) {
Log::logf(Log::ERR, "Error writing to Parallax servo controller!\n");
}
serialRead(&command, sizeof(command)); //Remove remote echo from input buffer, ignore any errors
}
//Functions
//Increase device-side connection speed from 2400 baud to 38400 baud
void Parallax::setBaudRate(bool increase) {
unsigned char command[8] = {'!','S', 'C', 'S', 'B', 'R', increase, '\r'};
//3 preamble bytes, 3 command bytes, baud rate byte, command terminator
if(serialWrite(&command, sizeof(command)) != sizeof(command)) {
Log::logf(Log::ERR, "Error writing to Parallax servo controller!\n");
}
//Remove remote echo from input buffer, ignore any errors
serialRead(&command, sizeof(command));
//Set local baud rate
if(increase) {
setBaudRate(38400);
} else {
setBaudRate(2400);
}
//Check if a response was received
if(!serialSelect(100000)) { //1 decisecond
//No response received; wrong baud rate?
Log::logf(Log::ERR, "Error communicating with Parallax servo controller!\n");
throw std::runtime_error("parallax speed increase failed");
}
//Verify new baud rate
unsigned char data[3] = {0};
//Response is "BR#", where # is the device's current baud rate setting (0x00 or 0x01)
//FIXME: First character of response is sometimes dropped on baud rate increase, so check the second byte if the third is missing
if((unsigned int)serialRead(&data, sizeof(data)) <= (ssize_t)sizeof(data) - 1) {
Log::logf(Log::ERR, "Error reading from Parallax servo controller!\n");
}
if(data[2] == increase || data[1] == increase) {
Log::logf(Log::INFO, "Parallax serial connection baud rate changed successfully!\n");
} else {
Log::logf(Log::ERR, "Error setting Parallax servo controller baud rate!\n");
throw std::runtime_error("parallax speed increase failed");
}
}
/*
A high-level serial passthrough implementation. Used by cli to start an
arbitrary serial passthrough "proxy". Optional callbacks can be given to allow
for specialized data processing.
*/
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
*leftC, serialConsumer *rightC)
{
waitForSerialPortToFinishTransmitting(left);
waitForSerialPortToFinishTransmitting(right);
if (!leftC)
leftC = &nopConsumer;
if (!rightC)
rightC = &nopConsumer;
LED0_OFF;
LED1_OFF;
// Either port might be open in a mode other than MODE_RXTX. We rely on
// serialRxBytesWaiting() to do the right thing for a TX only port. No
// special handling is necessary OR performed.
while(1) {
// TODO: maintain a timestamp of last data received. Use this to
// implement a guard interval and check for `+++` as an escape sequence
// to return to CLI command mode.
// https://en.wikipedia.org/wiki/Escape_sequence#Modem_control
if (serialRxBytesWaiting(left)) {
LED0_ON;
uint8_t c = serialRead(left);
serialWrite(right, c);
leftC(c);
LED0_OFF;
}
if (serialRxBytesWaiting(right)) {
LED0_ON;
uint8_t c = serialRead(right);
serialWrite(left, c);
rightC(c);
LED0_OFF;
}
}
}
void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
{
waitForSerialPortToFinishTransmitting(gpsPort);
waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
if(!(gpsPort->mode & MODE_TX))
serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
LED0_OFF;
LED1_OFF;
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayShowFixedPage(PAGE_GPS);
}
#endif
char c;
while(1) {
if (serialRxBytesWaiting(gpsPort)) {
LED0_ON;
c = serialRead(gpsPort);
gpsNewData(c);
serialWrite(gpsPassthroughPort, c);
LED0_OFF;
}
if (serialRxBytesWaiting(gpsPassthroughPort)) {
LED1_ON;
c = serialRead(gpsPassthroughPort);
serialWrite(gpsPort, c);
LED1_OFF;
}
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
updateDisplay();
}
#endif
}
}
/* getCellInfo() - gets the information from the cell where the module is connected
*
* This function gets the information from the cell where the module is connected
*
* It stores in 'RSSI' and 'cellID' variables the information from the cell
*
* It modifies 'flag' if expected answer is not received after sending a command to GPRS module
*
* Returns '1' on success and '0' if error
*/
uint8_t WaspGPRS::getCellInfo()
{
char command[30];
uint8_t byteIN[200];
long previous=millis();
uint8_t counter=0;
uint8_t a,b,c=0;
serialFlush(PORT_USED);
sprintf(command,"AT%s\r\n",AT_GPRS_CELLID);
printString(command,PORT_USED);
while( (!serialAvailable(PORT_USED)) && ((millis()-previous)<3000) );
previous=millis();
a=0;
while( (millis()-previous) < 2000 )
{
while( serialAvailable(PORT_USED) && (millis()-previous) < 2000 && (a<200) )
{
byteIN[a]=serialRead(PORT_USED);
a++;
}
}
a=0;
while( counter < 5 )
{
while( (byteIN[a]!=',') && (a<200) )
{
a++;
}
a++;
counter++;
}
if(a>=200) return 0;
counter=0;
while( (byteIN[a]!=',') && (a<200) )
{
cellID[c]=byteIN[a];
a++;
c++;
}
a++;
while( (byteIN[a]!=',') && (a<200) )
{
RSSI[b]=byteIN[a];
delay(10);
b++;
a++;
}
return 1;
}
uint16_t Arduino::command(uint8_t nid, uint8_t cmd, uint8_t pin, uint8_t val) {
// Send.
// printf("Sending command\n");
serialWrite(MESSAGE_HEADER);
serialWrite(nid);
serialWrite(cmd);
serialWrite(pin);
serialWrite(val);
usleep(100); // let it time to reply
// Receive.
char rd = serialRead();
if (rd != MESSAGE_HEADER) {
printf("Error: wrong message header received from node %d: '%c'\n", nid, rd);
exit(-1);
}
uint8_t h = serialRead();
uint8_t l = serialRead();
// Return received value.
return (uint16_t) ( (h << 8) | l );
}
static void hottCheckSerialData(uint32_t currentMicros)
{
static bool lookingForRequest = true;
uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort);
if (bytesWaiting <= 1) {
return;
}
if (bytesWaiting != 2) {
flushHottRxBuffer();
lookingForRequest = true;
return;
}
if (lookingForRequest) {
lastHoTTRequestCheckAt = currentMicros;
lookingForRequest = false;
return;
} else {
bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE;
if (!enoughTimePassed) {
return;
}
lookingForRequest = true;
}
uint8_t requestId = serialRead(hottPort);
uint8_t address = serialRead(hottPort);
if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) {
processBinaryModeRequest(address);
}
}
/**
* Send a file over the serial port.
* @param file file to send
*/
int Xmodem::send(File* file) {
_file = file;
_packetNumber = 1;
beginLed();
while (true) {
int c = serialRead();
if (c != -1) {
resetTimer();
break;
}
delay(100);
}
int status = 0;
while (true) {
size_t bytesRead = readPacket();
resetTimer();
if (bytesRead > 0) {
status = sendPacket();
if (_packetNumber % 20 == 0) {
toggleLed();
}
if (status < 0) {
break;
}
}
if (bytesRead < PACKET_SIZE) {
break;
}
}
serialWrite(EOT);
while (serialRead() == -1);
endLed();
return status;
}
void sp_process()
{
char c;
while((c = serialRead()) != -1)
{
if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute!
line[char_counter] = 0; // treminate string
status_message(gc_execute_line(line));
char_counter = 0; // reset line buffer index
} else if (c <= ' ') { // Throw away whitepace and control characters
} else if (c >= 'a' && c <= 'z') { // Upcase lowercase
line[char_counter++] = c-'a'+'A';
} else {
line[char_counter++] = c;
}
}
}
static bool gpsReceiveData(void)
{
bool hasNewData = false;
if (gpsState.gpsPort) {
while (serialRxBytesWaiting(gpsState.gpsPort)) {
uint8_t newChar = serialRead(gpsState.gpsPort);
if (gpsNewFrameNMEA(newChar)) {
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
gpsSol.flags.validVelNE = 0;
gpsSol.flags.validVelD = 0;
hasNewData = true;
}
}
}
return hasNewData;
}
/* getIMSI() - gets the IMSI from the SIM card
*
* This function gets the IMSI from the SIM card. It stores the IMSI into 'IMSI' variable.
*
* Returns '1' on success and '0' if error
*/
uint8_t WaspGPRS::getIMSI()
{
char command[15];
uint8_t byteIN[20];
long previous=millis();
uint8_t a,b=0;
serialFlush(PORT_USED);
sprintf(command,"%s\r\n",AT_GPRS_IMSI);
printString(command,PORT_USED);
while( (!serialAvailable(PORT_USED)) && ((millis()-previous)<3000) );
previous=millis();
a=0;
while( (millis()-previous) < 2000 )
{
while( serialAvailable(PORT_USED) && (millis()-previous) < 2000 && (a<20) )
{
byteIN[a]=serialRead(PORT_USED);
a++;
}
}
a=0;
while( (byteIN[a]!='\r') && (byteIN[a]!='\n') && (a<20) )
{
a++;
}
if(a>=20) return 0;
a++;
if(a>=20) return 0;
b=0;
while( (byteIN[a]!='\r') && (a<20) )
{
IMSI[b]=byteIN[a];
a++;
b++;
}
IMSI[b]='\0';
if(b<=10) return 0;
return 1;
}
void gpsThread(void)
{
// read out available GPS bytes
if (gpsPort) {
while (serialRxBytesWaiting(gpsPort))
gpsNewData(serialRead(gpsPort));
}
switch (gpsData.state) {
case GPS_UNKNOWN:
break;
case GPS_INITIALIZING:
case GPS_CHANGE_BAUD:
case GPS_CONFIGURE:
gpsInitHardware();
break;
case GPS_LOST_COMMUNICATION:
gpsData.timeouts++;
if (gpsConfig()->autoBaud) {
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
}
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
DISABLE_STATE(GPS_FIX);
gpsSetState(GPS_INITIALIZING);
break;
case GPS_RECEIVING_DATA:
// check for no data/gps timeout/cable disconnection etc
if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability
sensorsClear(SENSOR_GPS);
gpsSetState(GPS_LOST_COMMUNICATION);
}
break;
}
}
//Get power of a specific channel
unsigned short Pololu::getPower(unsigned char channel) {
if(channel > POLOLU_NUM_MOTORS) {
Log::logf(Log::WARN, "Invalid PWM channel!\n");
return 0;
}
unsigned char command[2] = {0x90, channel};
//Command byte, channel byte
if(serialWrite(&command, sizeof(command)) != sizeof(command)) {
Log::logf(Log::ERR, "Error writing to Pololu Maestro servo controller!\n");
}
unsigned char data[2] = {0};
if(serialRead(&data, sizeof(data)) == sizeof(data)) {
return (data[0] << 8) | data[1];
} else {
Log::logf(Log::ERR, "Error reading from Pololu Maestro servo controller!\n");
return 0;
}
}
void loop(){
seq.rotation.update(getAnalogValue(SEQUENCER_ROTATE_CONTROL));
seq.step.update(getAnalogValue(SEQUENCER_STEP_CONTROL));
seq.fill.update(getAnalogValue(SEQUENCER_FILL_CONTROL));
seq.update();
#ifdef SERIAL_DEBUG
if(serialAvailable() > 0){
serialRead();
printString("a: [");
seq.dump();
printString("] ");
seq.print();
if(clockIsHigh())
printString(" clock high");
if(resetIsHigh())
printString(" reset high");
printNewline();
}
#endif
}
void handleIbusTelemetry(void)
{
if (!ibusTelemetryEnabled) {
return;
}
while (serialRxBytesWaiting(ibusSerialPort) > 0) {
uint8_t c = serialRead(ibusSerialPort);
if (outboundBytesToIgnoreOnRxCount) {
outboundBytesToIgnoreOnRxCount--;
continue;
}
pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c);
if (isChecksumOkIa6b(ibusReceiveBuffer, IBUS_RX_BUF_LEN)) {
outboundBytesToIgnoreOnRxCount += respondToIbusRequest(ibusReceiveBuffer);
}
}
}
int stm32Sync()
{
int ret = STM_ERR;
static unsigned char STM32_INIT = 0x7F;
unsigned char r;
int rr;
int i;
printf("Sync");
fflush(stdout);
for ( i=0; i<MAXTRY; i++ ) {
printf(".");
fflush(stdout);
rr = serialWrite(board.serial_fd, &STM32_INIT, 1);
if ( rr == SER_OK ) {
serialFlush(board.serial_fd);
rr = serialRead(board.serial_fd, &r, sizeof(r));
if ( rr == SER_OK ) {
if ( r==STM32_ACK || r==STM32_NACK) {
ret = STM_OK;
break;
}
}
}
}
printf("\n");
if ( ret == STM_OK ) {
printf("Connected to board.\n");
} else {
printf("Can not connect to board, ");
if ( rr == SER_ERR ) {
printf(" failed.\n");
} else {
printf(" timed-out.\n");
}
}
return ret;
}
void gpsThread(void)
{
// read out available GPS bytes
if (core.gpsport) {
while (serialTotalBytesWaiting(core.gpsport))
gpsNewData(serialRead(core.gpsport));
}
switch (gpsData.state) {
case GPS_UNKNOWN:
break;
case GPS_INITIALIZING:
case GPS_INITDONE:
gpsInitHardware();
break;
case GPS_LOSTCOMMS:
gpsData.errors++;
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
f.GPS_FIX = 0;
gpsSetState(GPS_INITIALIZING);
break;
case GPS_RECEIVINGDATA:
// check for no data/gps timeout/cable disconnection etc
if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability
sensorsClear(SENSOR_GPS);
gpsSetState(GPS_LOSTCOMMS);
}
break;
}
}
//Read one byte from serial
unsigned char nextChar
(unsigned char * byte
,GioEndpoint *gioEndpoint){
unsigned char get = 1;
#ifdef ARDUINO
unsigned char timeoutCounter = 0;
while(1){
if(serialAvailable(gioEndpoint))break;
if(timeoutCounter++>TIMEOUT_AFTER){
get = 0;
break;
}
delay(1);
}
#endif
if(get){
serialRead(byte,gioEndpoint);
return get;
}else{
return 0;
}
}
int serialReadLine(PORT *port, char *buf, int *lineLen, int term, long ms)
{
int bytesRead;
int err;
*lineLen = 0;
//printf("term:%d\n",term);
while(1) {
err = serialRead(port, buf, 1, &bytesRead);
//printf("bytesRead:%d\n", bytesRead);
*lineLen += bytesRead;
buf[bytesRead] = '\0'; // Null terminate
//printf("serialRead:%s\n", buf);
if(*buf == term) // If termination character is found
return(0);
//printf(".");
buf += bytesRead;
usleep(20000); // Sleep for 20ms
ms -= 20;
if(ms < 0)
return(1);
}
}
/* sendCommand(ATcommand, ATcommand_R) - sends any command to GPRS module
*
* This function sends any command to GPRS module
*
* It stores in 'answer_command' variable the answer returned by the GPRS module
*
* Returns '1' on success and '0' if error
*/
uint8_t WaspGPRS::sendCommand(char* ATcommand)
{
char command[30];
uint8_t timeout=0;
uint8_t i=0;
answer_command[0]='\0';
sprintf(command, "AT%s%c%c", ATcommand,'\r','\n');
serialFlush(PORT_USED);
USB.print('d');
while(!serialAvailable(PORT_USED)) {
printString(command,PORT_USED);
delay(DELAY_ON_SEND);
USB.print('e');
}
USB.print('a');
while( timeout < 5 )
{
while(!serialAvailable(PORT_USED) && timeout < 5) {
timeout++;
delay(1000);
}
USB.print('b');
while(serialAvailable(PORT_USED) && timeout < 5){
answer_command[i] = serialRead(PORT_USED);
delay(20);
i++;
timeout=0;
if(i>=199) timeout=5;
}
USB.print('c');
}
answer_command[i]='\0';
if( i<5 ) return 0;
return 1;
}
int check_for_bootloader(int fd, int timeout)
{
uint8_t cmdbuf[2];
uint8_t inbuf[2];
cmdbuf[0] = 0x55; // autobauding
cmdbuf[1] = 0x55;
if (2 != serialWrite(fd, cmdbuf, 2))
return 1;
if (2 != serialRead(fd, inbuf, 2))
return 1;
if (inbuf[0] != 0x00 || inbuf[1] != 0xCC)
return 1;
cmdbuf[0] = COMMAND_PING;
if (0 != run_command(fd, cmdbuf, 1))
{
printf("ping failed\n");
return 1;
}
printf("ping ok\n");
return 0;
}
int main(void) {
//spiInit(SPIDEV_1);
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
SetSysClock();
systemInit();
timerInit(); // timer must be initialized before any channel is allocated
serial0 = serial0_open();
dmaInit();
setup();
while (true) {
#ifndef EXTERNAL_DEBUG
static uint32_t dbg_start_msec;
// support reboot from host computer
if (millis()-dbg_start_msec > 100) {
dbg_start_msec = millis();
while (serialRxBytesWaiting(serial0)) {
uint8_t c = serialRead(serial0);
if (c == 'R')
systemResetToBootloader();
}
}
#endif
loop();
}
} // main
/*
Function: Reads internal temperature sensor of bluetooth module.
Returns: Temperature
Parameters:
Values:
*/
long WaspBT_Pro::getTemp(){
long a;
char dummy[3];
char temperature[4];
for (uint8_t i = 0; i < COMMAND_SIZE; i++) theCommand[i] = ' '; // Clear variable
for (uint8_t i = 0; i < 3; i++) temperature[i] = ' ';
sendCommand("temp");
delay(100);
while(serialAvailable(1)){
dummy[0]=serialRead(1);
if (dummy[0]=='M'){
dummy[1]=serialRead(1);
if (dummy[1]=='P'){
dummy[0]=serialRead(1);
temperature[0]=serialRead(1);
temperature[1]=serialRead(1);
temperature[2]=serialRead(1);
a = Utils.array2long(temperature);
}
}
}
return a;
}
void handleSmartPortTelemetry(void)
{
uint32_t smartPortLastServiceTime = millis();
if (!smartPortTelemetryEnabled) {
return;
}
if (!canSendSmartPortTelemetry()) {
return;
}
while (serialRxBytesWaiting(smartPortSerialPort) > 0) {
uint8_t c = serialRead(smartPortSerialPort);
smartPortDataReceive(c);
}
uint32_t now = millis();
// if timed out, reconfigure the UART back to normal so the GUI or CLI works
if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) {
smartPortState = SPSTATE_TIMEDOUT;
return;
}
while (smartPortHasRequest) {
// Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
smartPortHasRequest = 0;
return;
}
// we can send back any data we want, our table keeps track of the order and frequency of each data type we send
uint16_t id = frSkyDataIdTable[smartPortIdCnt];
if (id == 0) { // end of table reached, loop back
smartPortIdCnt = 0;
id = frSkyDataIdTable[smartPortIdCnt];
}
smartPortIdCnt++;
int32_t tmpi;
static uint8_t t1Cnt = 0;
switch(id) {
#ifdef GPS
case FSSP_DATAID_SPEED :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100;
smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
smartPortHasRequest = 0;
}
break;
#endif
case FSSP_DATAID_VFAS :
if (feature(FEATURE_VBAT)) {
uint16_t vfasVoltage;
if (telemetryConfig->frsky_vfas_cell_voltage) {
vfasVoltage = vbat / batteryCellCount;
} else {
vfasVoltage = vbat;
}
smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_CURRENT :
if (feature(FEATURE_CURRENT_METER)) {
smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
smartPortHasRequest = 0;
}
break;
//case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_FUEL :
if (feature(FEATURE_CURRENT_METER)) {
smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
smartPortHasRequest = 0;
}
break;
//case FSSP_DATAID_ADC1 :
//case FSSP_DATAID_ADC2 :
#ifdef GPS
case FSSP_DATAID_LATLONG :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
uint32_t tmpui = 0;
// the same ID is sent twice, one for longitude, one for latitude
// the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track
if (smartPortIdCnt & 1) {
tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
}
else {
tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
}
smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0;
}
break;
#endif
//case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_HEADING :
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCX :
smartPortSendPackage(id, accSmooth[X] / 44);
// unknown input and unknown output unit
// we can only show 00.00 format, another digit won't display right on Taranis
// dividing by roughly 44 will give acceleration in G units
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCY :
smartPortSendPackage(id, accSmooth[Y] / 44);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, accSmooth[Z] / 44);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_T1 :
// we send all the flags as decimal digits for easy reading
// the t1Cnt simply allows the telemetry view to show at least some changes
t1Cnt++;
if (t1Cnt >= 4) {
t1Cnt = 1;
}
tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
// the Taranis seems to be able to fit 5 digits on the screen
// the Taranis seems to consider this number a signed 16 bit integer
if (ARMING_FLAG(OK_TO_ARM))
tmpi += 1;
if (ARMING_FLAG(PREVENT_ARMING))
tmpi += 2;
if (ARMING_FLAG(ARMED))
tmpi += 4;
if (FLIGHT_MODE(ANGLE_MODE))
tmpi += 10;
if (FLIGHT_MODE(HORIZON_MODE))
tmpi += 20;
if (FLIGHT_MODE(UNUSED_MODE))
tmpi += 40;
if (FLIGHT_MODE(PASSTHRU_MODE))
tmpi += 40;
if (FLIGHT_MODE(MAG_MODE))
tmpi += 100;
if (FLIGHT_MODE(BARO_MODE))
tmpi += 200;
if (FLIGHT_MODE(SONAR_MODE))
tmpi += 400;
if (FLIGHT_MODE(GPS_HOLD_MODE))
tmpi += 1000;
if (FLIGHT_MODE(GPS_HOME_MODE))
tmpi += 2000;
if (FLIGHT_MODE(HEADFREE_MODE))
tmpi += 4000;
smartPortSendPackage(id, (uint32_t)tmpi);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_T2 :
if (sensors(SENSOR_GPS)) {
#ifdef GPS
// provide GPS lock status
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
smartPortHasRequest = 0;
#endif
}
else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0);
smartPortHasRequest = 0;
}
break;
#ifdef GPS
case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
smartPortHasRequest = 0;
}
break;
#endif
case FSSP_DATAID_A4 :
if (feature(FEATURE_VBAT)) {
smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break;
default:
break;
// if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
}
}
}
