uint32_t convert_data(struct INPUT* inputModel) {
uint32_t data = level_convert(inputModel->roll) << 8;
data = (data | level_convert(inputModel->pitch)) << 8;
data = (data | level_convert(inputModel->yaw)) << 8;
data = data | level_convert(inputModel->lift);
return data;
}
void loop() // run over and over
{
message = readSerial();
int length = message.length();
if(length > 0){
Serial.println("CMD>\"" + message + "\"");
// AF_GAIN
if(message == "AF_GAIN"){
memcpy(cmd_bytes, AF_GAIN,7);
}
if(message == "SET_MODE_LSB"){
memcpy(cmd_bytes, SET_MODE_LSB,7);
}
if(message == "SET_MODE_USB"){
memcpy(cmd_bytes, SET_MODE_USB,7);
}
if(message == "SET_MODE_AM"){
memcpy(cmd_bytes, SET_MODE_AM,7);
}
if(message == "SET_MODE_RTTY"){
memcpy(cmd_bytes, SET_MODE_RTTY,7);
}
if(message == "SET_MODE_RTTY_REV"){
memcpy(cmd_bytes, SET_MODE_RTTY_REV,7);
}
if(message == "SET_MODE_CW"){
memcpy(cmd_bytes, SET_MODE_CW,7);
}
if(message == "SET_MODE_CW_REV"){
memcpy(cmd_bytes, SET_MODE_CW_REV,7);
}
if(message == "NR_LEVEL"){
memcpy(cmd_bytes, NR_LEVEL,7);
}
if(message.substring(0,8) == "AF_GAIN "){
byte one = 0x00; // First Byte
byte two = 0x00; // Second Byte
level_convert(message.substring(8,13),one,two);
byte AF_GAIN_SET[] = {0xFE,0xFE,0x5E,0x0E,0x14,0x01,one,two,0xFD};
memcpy(cmd_bytes, AF_GAIN_SET, sizeof(AF_GAIN_SET));
}
if(message == "RF_GAIN"){
byte RF_GAIN[] = {0xFE,0xFE,0x5E,0x0E,0x14,0x02,0xFD};
memcpy(cmd_bytes, RF_GAIN, sizeof(RF_GAIN));
}
if(message.substring(0,8) == "RF_GAIN "){
byte one = 0x00; // First Byte
byte two = 0x00; // Second Byte
level_convert(message.substring(8,13),one,two);
byte RF_GAIN_SET[] = {0xFE,0xFE,0x5E,0x0E,0x14,0x02,one,two,0xFD};
memcpy(cmd_bytes, RF_GAIN_SET, sizeof(RF_GAIN_SET));
}
if(message.substring(0,9) == "RF_POWER "){
byte one = 0x00; // First Byte
byte two = 0x00; // Second Byte
level_convert(message.substring(9,13),one,two);
byte RF_POWER_SET[] = {0xFE,0xFE,0x5E,0x0E,0x14,0x0A,one,two,0xFD};
memcpy(cmd_bytes, RF_POWER_SET, sizeof(RF_POWER_SET));
}
if(message.substring(0,9) == "MIC_GAIN "){
byte one = 0x00; // First Byte
byte two = 0x00; // Second Byte
level_convert(message.substring(9,13),one,two);
byte MIC_GAIN_SET[] = {0xFE,0xFE,0x5E,0x0E,0x14,0x0B,one,two,0xFD};
memcpy(cmd_bytes, MIC_GAIN_SET, sizeof(MIC_GAIN_SET));
}
if(message.substring(0,9) == "NR_LEVEL "){
String level_str = message.substring(9,11);
level_str.trim();
int level_int;
level_int = level_str.toInt();
if(level_int > 0 and level_int < 16){
memcpy(cmd_bytes, NR_LEVELS[level_int], sizeof(NR_LEVELS[level_int]));
}
}
if(message == "MIC_GAIN"){
memcpy(cmd_bytes, MIC_GAIN,7);
}
if(message == "RF_LEVEL"){
memcpy(cmd_bytes, RF_LEVEL,7);
}
if(message == "RF_LEVEL"){
memcpy(cmd_bytes, RF_LEVEL,7);
}
if(message == "RF_LEVEL_HI"){
memcpy(cmd_bytes, RF_LEVEL_HI,sizeof(RF_LEVEL_HI));
}
if(message == "RF_LEVEL_LOW"){
memcpy(cmd_bytes, RF_LEVEL_LOW,sizeof(RF_LEVEL_LOW));
}
if(message == "SET_VFO_A"){
memcpy(cmd_bytes, SET_VFO_A,7);
}
if(message == "SET_VFO_B"){
memcpy(cmd_bytes, SET_VFO_B,7);
}
if(message == "VFO_A_eq_B"){
memcpy(cmd_bytes, VFO_A_eq_B,7);
}
if(message == "VFO_A_Sw_B"){
memcpy(cmd_bytes, VFO_A_Sw_B,7);
}
if(message == "SPLIT_ON"){
memcpy(cmd_bytes, SPLIT_ON,7);
}
if(message == "SPLIT_OFF"){
memcpy(cmd_bytes, SPLIT_OFF,7);
}
if(message == "SET_TS_ON"){
memcpy(cmd_bytes, SET_TS_ON,7);
}
if(message == "SET_TS_OFF"){
memcpy(cmd_bytes, SET_TS_OFF,7);
}
if(message == "ATT_ON"){
memcpy(cmd_bytes, ATT_ON,7);
}
if(message == "ATT_OFF"){
memcpy(cmd_bytes, ATT_OFF,7);
}
if(message == "NB_OFF" ){
memcpy(cmd_bytes,NB_OFF,8);
}
if(message == "NB_ON"){
memcpy(cmd_bytes,NB_ON,8);
}
if(message == "PRE_AMP_ON"){
memcpy(cmd_bytes, PRE_AMP_ON,8);
}
if(message == "PRE_AMP_OFF"){
memcpy(cmd_bytes, PRE_AMP_OFF,8);
}
if(message == "NR_ON"){
memcpy(cmd_bytes, NR_ON,8);
}
if(message == "NR_OFF"){
memcpy(cmd_bytes, NR_OFF,8);
}
if(message == "COMP_ON"){
memcpy(cmd_bytes, COMP_ON,8);
}
if(message == "COMP_OFF"){
memcpy(cmd_bytes, COMP_OFF,8);
}
if(message == "VOX_ON"){
memcpy(cmd_bytes, VOX_ON,8);
}
if(message == "VOX_OFF"){
memcpy(cmd_bytes, VOX_OFF,8);
}
if(message == "BK_IN_ON"){
memcpy(cmd_bytes, BK_IN_ON,8);
}
if(message == "BK_IN_OFF"){
memcpy(cmd_bytes, BK_IN_OFF,8);
}
if(message == "READ_OPERATING_MODE"){
memcpy(cmd_bytes, READ_OPERATING_MODE,6);
}
if(message == "READ_FREQUENCYS"){
memcpy(cmd_bytes, READ_FREQUENCYS,6);
}
if(message == "READ_UPPER_LOWER_FREQUENCIES"){
memcpy(cmd_bytes, READ_UPPER_LOWER_FREQUENCIES,6);
}
if(message == "READ_SMETER"){
memcpy(cmd_bytes, READ_SMETER,7);
}
if(message == "SET_OPERATING_FRQ"){
memcpy(cmd_bytes, SET_OPERATING_FRQ,sizeof(SET_OPERATING_FRQ));
}
/*
if(message.substring(0,4) == "CMD "){
memcpy(cmd_bytes,genericCommand(message.substring(4,message.length())),8);
}
*/
//Issue Command
for(int i = 0; i < sizeof(cmd_bytes); i = i + 1){
Serial.print(cmd_bytes[i],HEX);
Serial.print(" ");
}
Serial.println();
mySerial.write(cmd_bytes,sizeof(cmd_bytes));
//Clear the memory
memcpy(cmd_bytes,CLEAR,8);
}
byte read_byte = mySerial.read();
byte read_bytes[11];
int read_byte_index = 0;
if(read_byte == 0xFE){
Serial.print("RX -->:");
Serial.print(read_byte,HEX);
Serial.print(" ");
read_bytes[read_byte_index] = read_byte;
read_byte_index++;
if(read_byte != 0xFF){
while(1 == 1){
read_byte = mySerial.read();
Serial.print(read_byte,HEX);
Serial.print(" ");
read_bytes[read_byte_index] = read_byte;
read_byte_index++;
if(read_byte == 0xFD){
Serial.println(" <--");
break;
}
}
Serial.print("RX:");
for(int i = 0; i < sizeof(read_bytes); i = i + 1){
Serial.print(i);
Serial.print("=>");
Serial.print(read_bytes[i],HEX);
Serial.print(" ");
}
Serial.println();
if(read_bytes[4] == 0x00 or read_bytes[4] == 0x03){
Serial.println("ParseFreq:" + parseFreq(read_bytes));
}
if(read_bytes[4] == 0x01){
Serial.println("ParseMode:" + parseMode(read_bytes));
}
if(read_bytes[4] == 0x04){
Serial.println("ReadOperatingMode:" + parseMode(read_bytes));
}
}
}
}
