gpsEnablePassthroughResult_e gpsEnablePassthrough(void)
{
if (gpsData.state != GPS_RECEIVING_DATA)
return GPS_PASSTHROUGH_NO_GPS;
serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH);
if (gpsPassthroughPort) {
waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
serialSetBaudRate(gpsPassthroughPort, serialConfig->gps_passthrough_baudrate);
} else {
gpsPassthroughPort = openSerialPort(FUNCTION_GPS_PASSTHROUGH, NULL, serialConfig->gps_passthrough_baudrate, MODE_RXTX, SERIAL_NOT_INVERTED);
if (!gpsPassthroughPort) {
return GPS_PASSTHROUGH_NO_SERIAL_PORT;
}
}
LED0_OFF;
LED1_OFF;
while(1) {
if (serialTotalBytesWaiting(gpsPort)) {
LED0_ON;
serialWrite(gpsPassthroughPort, serialRead(gpsPort));
LED0_OFF;
}
if (serialTotalBytesWaiting(gpsPassthroughPort)) {
LED1_ON;
serialWrite(gpsPort, serialRead(gpsPassthroughPort));
LED1_OFF;
}
}
return GPS_PASSTHROUGH_ENABLED;
}
int run_command(int fd, uint8_t *data, uint8_t len)
{
uint8_t inbuf[2];
uint8_t sum = 0;
uint8_t *sump = data;
uint8_t i = len;
uint8_t packet_len = len+2;
while(i--)
sum += *sump++;
if (1 != serialWrite(fd, &packet_len, 1))
return 1;
if (1 != serialWrite(fd, &sum, 1))
return 1;
if (len != serialWrite(fd, data, len))
return 1;
if (2 != serialRead(fd, inbuf, 2))
return 1;
if (0x00 != inbuf[0] || 0xCC != inbuf[1])
{
fprintf(stderr, "NAK\n");
return 1;
}
return 0;
}
static void ltm_initialise_packet(uint8_t ltm_id)
{
ltm_crc = 0;
serialWrite(ltmPort, '$');
serialWrite(ltmPort, 'T');
serialWrite(ltmPort, ltm_id);
}
void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
{
waitForSerialPortToFinishTransmitting(gpsState.gpsPort);
waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
if(!(gpsState.gpsPort->mode & MODE_TX))
serialSetMode(gpsState.gpsPort, gpsState.gpsPort->mode | MODE_TX);
LED0_OFF;
LED1_OFF;
char c;
while(1) {
if (serialRxBytesWaiting(gpsState.gpsPort)) {
LED0_ON;
c = serialRead(gpsState.gpsPort);
serialWrite(gpsPassthroughPort, c);
LED0_OFF;
}
if (serialRxBytesWaiting(gpsPassthroughPort)) {
LED1_ON;
c = serialRead(gpsPassthroughPort);
serialWrite(gpsState.gpsPort, c);
LED1_OFF;
}
}
}
void stmWriteCommand(serialStruct_t *s, char *msb, char *lsb, char *len, char *data) {
char startAddress[9];
char lenPlusData[128];
char c;
strncpy(startAddress, msb, sizeof(startAddress));
strcat(startAddress, lsb);
sprintf(lenPlusData, "%02x%s", stmHexToChar(len) - 1, data);
write:
// send WRITE MEMORY command
do {
c = getResults[5];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// send address
if (!stmWrite(s, startAddress)) {
putchar('A');
goto write;
}
// send len + data
if (!stmWrite(s, lenPlusData)) {
putchar('D');
goto write;
}
putchar('='); fflush(stdout);
}
int DLS::setOutputFilter (int nsamples, int nspikes, int nerrors)
{
// Request Current Config
char write_data[] = "s0fi\r\n";
serialWrite(write_data, sizeof(write_data)/sizeof(write_data[0]));
// Read Current Config
char read_data[] = "g0fi+aa+bb+cc\r\n";
serialRead(read_data);
//
// Check for error indicator (@) and parse error value.
if (read_data[2]=='@') {
char errcode_str [] = "000";
sprintf (errcode_str, "%*s", 3, read_data+4);
return(-1 * atoi (errcode_str));
}
char aa_str [] = "aa";
aa_str[0] = read_data [5];
aa_str[1] = read_data [6];
char bb_str [] = "bb";
bb_str[0] = read_data [8];
bb_str[1] = read_data [9];
char cc_str [] = "cc";
cc_str[0] = read_data [11];
cc_str[1] = read_data [12];
if ((nsamples < 0) || (nsamples > 32))
nsamples = atoi (aa_str);
if (nspikes < 0)
nspikes = atoi (bb_str);
if (nerrors < 0)
nerrors = atoi (cc_str);
debug_print("samples %i\n", nsamples);
debug_print("spikes %i\n", nspikes);
debug_print("errors %i\n", nerrors);
if (2*nspikes+nerrors > 0.4 * nsamples) {
fprintf(stderr, "ERROR: Make sure that (2*nspikes + nerrors) <= nsamples\n");
nspikes = atoi (bb_str);
nerrors = atoi (cc_str);
nsamples = atoi (aa_str);
}
char write_data2[] = "s0fi+aa+bb+cc\r\n";
sprintf(write_data2, "s0fi+%02i+%02i+%02i\r\n",nsamples,nspikes,nerrors);
serialWrite(write_data2, sizeof(write_data2)/sizeof(write_data[0]));
int status = rxData();
if (status < 0)
printErrorMsg(status);
return(status);
}
int main(int argc, char *argv[]) {
SerialPort SP;
char buf[1025];
int nr;
if (argc<1) {
printf("Usage: setupbluesmirf <device>\n");
printf(" <device> must be your serial port, e.g, COM3: on Windows, or /dev/ttyS0 on Linux\n");
return 0;
}
if (!serialOpenByName(&SP, argv[1])) {
fprintf(stderr, "Could not open serial port %s\n", argv[1]);
return 1;
}
// last parameter is timeout in 1/10 of a second
if (!serialSetParameters(&SP, 57600, 8, 0, 1, 1)) {
fprintf(stderr, "Could not modify serial port parameters\n");
return 1;
}
nr = serialRead(&SP, 1024, buf);
serialWrite(&SP, 3, "$$$");
nr = serialRead(&SP, 1024, buf);
if (nr > 0) {
buf[nr]=0;
printf("Received after $$$: %s", buf);
} else {
goto cleanup;
}
serialWrite(&SP, 2, "D\r");
nr = serialRead(&SP, 1024, buf);
if (nr > 0) {
buf[nr]=0;
printf("Received after D<cr>: %s", buf);
} else {
goto cleanup;
}
serialWrite(&SP, 9, "U,576K,N\r");
nr = serialRead(&SP, 1024, buf);
if (nr > 0) {
buf[nr]=0;
printf("Received after U,576K,N<cr>: %s", buf);
} else {
goto cleanup;
}
cleanup:
serialClose(&SP);
return 0;
}
static uint8_t transmitIbusPacket(uint8_t *ibusPacket, size_t payloadLength)
{
uint16_t checksum = calculateChecksum(ibusPacket, payloadLength + IBUS_CHECKSUM_SIZE);
for (size_t i = 0; i < payloadLength; i++) {
serialWrite(ibusSerialPort, ibusPacket[i]);
}
serialWrite(ibusSerialPort, checksum & 0xFF);
serialWrite(ibusSerialPort, checksum >> 8);
return payloadLength + IBUS_CHECKSUM_SIZE;
}
void serialize16(int16_t a)
{
static uint8_t t;
t = a;
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 8 & 0xff;
serialWrite(core.mainport, t);
checksum ^= t;
}
// Get a 16 bit value off the serial port by doing two successive reads
// Assumes that data is being transmitted high byte first
int16_t get16bitValue(int serialIndex){
int16_t returnValue = 0;
serialWrite(serialIndex);
serialIndex++;
returnValue = serialRead(25);
serialWrite(serialIndex);
serialIndex++;
returnValue += serialRead(25) << 8;
return returnValue;
}
void loop() {
for(i = 0; i < 6; ++i) {
int old = accum[i];
v = analogRead(i);
v = smooth(v, &accum[i], 100);
if(v != old) {
serialWrite(0x80 | i);
serialWrite(v >> 7);
serialWrite(v & 0x7F);
x = 5;
}
}
static void serializeFrsky(uint8_t data)
{
// take care of byte stuffing
if (data == 0x5e) {
serialWrite(frskyPort, 0x5d);
serialWrite(frskyPort, 0x3e);
} else if (data == 0x5d) {
serialWrite(frskyPort, 0x5d);
serialWrite(frskyPort, 0x3d);
} else
serialWrite(frskyPort, data);
}
int stmWriteLen(serialStruct_t *s, char *data, int len, unsigned char ck) {
unsigned char c;
int i;
for (i = 0; i < len; i++) {
c = data[i];
ck ^= c;
serialWrite(s, (char *)&c, 1);
}
serialWrite(s, (char *)&ck, 1);
return stmWaitAck(s, STM_RETRIES_LONG);
}
/**
*void setServo(int servo, int pos)
*@param servo - int, what servo channel are we talking about?
*@param pos - int, the position or the speed of the servo being requested
*@param hf - int, whether or not the servo is in half or full duplex. 0 = half, 1 is full.
*@return void
*
*Sets a motor as half duplex or full duplex. All default as
*/
void setServo(int servo, int pos, int hf){
int startBit;
//If the servo is in full duplex, set it to such.
if(hf == FULL) servo += 8;
startBit = STARTBIT;
serialWrite(4, startBit);
serialWrite(4, servo);
serialWrite(4, pos);
}
void writeEscapedByte(
unsigned char byte,
GioEndpoint *gioEndpoint){
unsigned char escapeChar = ESCAPE_CHAR;
switch(byte){
case START_FLAG:
case END_FLAG:
case ESCAPE_CHAR:
serialWrite(gioEndpoint, escapeChar);
default:
break;
}
serialWrite(gioEndpoint, byte);
}
void slaveLoop()
{
watchdogReset();
uint32_t now = micros();
bool needHop=false;
switch (state) {
case 0: // waiting for packet
if (RF_Mode == Received) {
Green_LED_ON;
Red_LED_OFF;
// got packet
lastReceived = now;
lostpkts=0;
linkQuality = (linkQuality << 1) | 1;
RF_Mode = Receive;
spiSendAddress(0x7f); // Send the package read command
for (int16_t i = 0; i < bind_data.packetSize; i++) {
rx_buf[i] = spiReadData();
}
uint8_t payloadBytes = (rx_buf[0] & 0x3f);
if (payloadBytes > (bind_data.packetSize - 1)) {
// INVALID DATA
payloadBytes = 0;
}
// Check if this is a new packet from master and not a resent one
if ((rx_buf[0] ^ tx_buf[0]) & MASTER_SEQ) {
tx_buf[0] ^= MASTER_SEQ;
if (payloadBytes) {
// DATA FRAME
if (bind_data.flags & PACKET_MODE) {
serialWrite(0xf0);
serialWrite(payloadBytes);
CRCtx = 0;
}
for (uint8_t i=0; i < payloadBytes; i++) {
serialWrite(rx_buf[1 + i]);
if ((bind_data.flags & PACKET_MODE) && (bind_data.flags & PACKETCRC_MODE)) {
CRC16_add(&CRCtx, rx_buf[1 + i]);
}
}
if ((bind_data.flags & PACKET_MODE) && (bind_data.flags & PACKETCRC_MODE)) {
serialWrite(CRCtx >> 8);
serialWrite(CRCtx & 0xff);
}
}
}
__interrupt void ReceiveInterrupt(void)
{
char rec = UCA0RXBUF;
if(rec == '1'){
serialWrite('0');
}else if(rec == '2'){
serialWrite('9');
}else if(rec == '3'){
serialWrite('0');
}else if(rec == '4'){
serialWrite('1');
}else if(rec == 'a' && alarm_state ==3){
TA0CCR1 = period>>2;
alarm_state = 4;
}
void serialPrint(serialPort_t *instance, const char *str)
{
uint8_t ch;
while ((ch = *(str++)) != 0) {
serialWrite(instance, ch);
}
}
void sendPacket(TPacket *packet)
{
char buffer[PACKET_SIZE];
int len = serialize(buffer, packet, sizeof(TPacket));
serialWrite(buffer, len);
}
static void _putc(void *p, char c)
{
(void)p; // avoid compiler warning about unused variable
serialWrite(Serial1, c);
while (!isSerialTransmitBufferEmpty(Serial1));
}
void cliUsage(cliCommand_t *cmd) {
serialPrint("usage: ");
serialPrint(cmd->name);
serialWrite(' ');
serialPrint(cmd->params);
serialPrint("\r\n");
}
void Camera::serialWrite(char buffer[]){
clFlushPort(serialRefPtr);
unsigned int bufferlen= 20;
unsigned int serialTimeout = 500;
try{
checkSerialCom(clSerialWrite(serialRefPtr, buffer, &bufferlen, serialTimeout));
}catch(string const& error){
cout << "Write ERROR:" << error << endl;
serialWrite(buffer);
}
char *mybuff= NULL;
unsigned int numBytes=256;
mybuff = (char *) malloc(numBytes);
try{
checkSerialCom(clSerialRead(serialRefPtr, mybuff, &numBytes, serialTimeout));
}catch(string const& error){
free(mybuff);
cout << "Read ERROR:" << error << endl;
// serialWrite(buffer);
return;
}
char temp[8];
strncpy(temp, mybuff, 8);
cout << "Camera response:" << temp << endl;
free(mybuff);
}
void serialize32(uint32_t a)
{
static uint8_t t;
t = a;
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 8;
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 16;
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 24;
serialWrite(core.mainport, t);
checksum ^= t;
}
// keil/armcc version
int fputc(int c, FILE *f)
{
// let DMA catch up a bit when using set or dump, we're too fast.
while (!isSerialTransmitBufferEmpty(core.mainport));
serialWrite(core.mainport, c);
return c;
}
// Print the null-terminated string 's' to the blackbox device and return the number of bytes written
int blackboxPrint(const char *s)
{
int length;
const uint8_t *pos;
switch (masterConfig.blackbox_device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
length = strlen(s);
flashfsWrite((const uint8_t*) s, length, false); // Write asynchronously
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
pos = (uint8_t*) s;
while (*pos) {
serialWrite(blackboxPort, *pos);
pos++;
}
length = pos - (uint8_t*) s;
break;
}
return length;
}
// implement for mavlink convenience functions
inline void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{
if (chan == MAVLINK_COMM_0)
{
serialWrite(Serial1, ch);
}
}
static void _debugPrint(uint8_t scheduledTask, int32_t elapsedTime) {
static uint16_t len;
/* Print info about which task is currently running */
len = sprintf(_debugMsgBuf, _debugMsgFmt, scheduledTask, elapsedTime);
serialWrite(_debugMsgBuf, len);
}
// Print the null-terminated string 's' to the blackbox device and return the number of bytes written
int blackboxPrint(const char *s)
{
int length;
const uint8_t *pos;
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
length = strlen(s);
flashfsWrite((const uint8_t*) s, length, false); // Write asynchronously
break;
#endif
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
length = strlen(s);
afatfs_fwrite(blackboxSDCard.logFile, (const uint8_t*) s, length); // Ignore failures due to buffers filling up
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
pos = (uint8_t*) s;
while (*pos) {
serialWrite(blackboxPort, *pos);
pos++;
}
length = pos - (uint8_t*) s;
break;
}
return length;
}
// Process incoming commands. Call it as frequently as possible, to prevent overrun of serial input buffer.
void simplebgc_process_inqueue() {
int i;
while(sbgc_parser.read_cmd()) {
SerialCommand &cmd = sbgc_parser.in_cmd;
uint8_t error = 0;
char tmp[12];
switch(cmd.id) {
case SBGC_CMD_GET_ANGLES:
WOFL_get_angles_t t;
memcpy(&t, cmd.data, cmd.len);//asume little endin
WOFL_get_angles_send(t, wofl_parser);
break;
// Receive realtime data
case SBGC_CMD_REALTIME_DATA_3:
case SBGC_CMD_REALTIME_DATA_4:
error = SBGC_cmd_realtime_data_unpack(rt_data, cmd);
if(!error) {
// Extract some usefull data
/*
rt_data.imu_angle[ROLL];
rt_data.target_angle[ROLL];
rt_data.imu_angle[PITCH];
rt_data.target_angle[PITCH];
rt_data.imu_angle[YAW];
rt_data.target_angle[YAW];
*/
memcpy(tmp,rt_data.rc_raw_data,12);
serialXPrint(2,":");
for(i=0;i<12;i++)
serialXWrite(2,tmp[i]);
serialXWrite(2,'\n');
} else {
sbgc_parser.onParseError(error);
}
break;
default:
serialPrint("unparsed command:");
serialWrite(cmd.id);
for(i=0;i<cmd.len;i++)
serialWrite(cmd.data[i]);
serialWrite('\n');
}
}
}
int serialWriteSlow(SerialPort *SP, int size, void *buffer) {
int i, retval = 0;
for (i=0; i<size; i++) {
retval += serialWrite(SP, 1, buffer+i);
usleep(100000);
}
return retval;
}
