void OLED::setChargePump(unsigned char d)
{
writeCmd(0x8D); // Set Charge Pump
writeCmd(0x10|d); // Default => 0x10
// 0x10 (0x00) => Disable Charge Pump
// 0x14 (0x04) => Enable Charge Pump
}
void OLED::setPrechargePeriod(unsigned char d)
{
writeCmd(0xD9); // Set Pre-Charge Period
writeCmd(d); // Default => 0x22 (2 Display Clocks [Phase 2] / 2 Display Clocks [Phase 1])
// D[3:0] => Phase 1 Period in 1~15 Display Clocks
// D[7:4] => Phase 2 Period in 1~15 Display Clocks
}
void OLED::setDisplayClock(unsigned char d)
{
writeCmd(0xD5); // Set Display Clock Divide Ratio / Oscillator Frequency
writeCmd(d); // Default => 0x80
// D[3:0] => Display Clock Divider
// D[7:4] => Oscillator Frequency
}
void OLED::setCommonConfig(unsigned char d)
{
writeCmd(0xDA); // Set COM Pins Hardware Configuration
writeCmd(0x02|d); // Default => 0x12 (0x10)
// Alternative COM Pin Configuration
// Disable COM Left/Right Re-Map
}
static asynStatus gpibPortAddressedCmd(void *pdrvPvt,asynUser *pasynUser,
const char *data, int length)
{
niport *pniport = (niport *)pdrvPvt;
double timeout = pasynUser->timeout;
int addr = 0;
int actual;
asynStatus status;
char cmdbuf[2] = {IBUNT,IBUNL};
status = pasynManager->getAddr(pasynUser,&addr);
if(status!=asynSuccess) return status;
asynPrint(pasynUser,ASYN_TRACE_FLOW,
"%s addr %d gpibPortAddressedCmd nchar %d\n",
pniport->portName,addr,length);
pniport->errorMessage[0] = 0;
status = writeAddr(pniport,0,addr,timeout,transferStateIdle);
if(status==asynSuccess) {
status=writeCmd(pniport,data,length,timeout,transferStateIdle);
}
if(status!=asynSuccess) {
epicsSnprintf(pasynUser->errorMessage,pasynUser->errorMessageSize,
"%s writeGpib failed %s",pniport->portName,pniport->errorMessage);
}
actual = length - pniport->bytesRemainingCmd;
asynPrintIO(pasynUser,ASYN_TRACEIO_DRIVER,
data,actual,"%s gpibPortAddressedCmd\n",pniport->portName);
if(status!=asynSuccess) return status;
writeCmd(pniport,cmdbuf,2,timeout,transferStateIdle);
return status;
}
void OLED::setStartColumn(unsigned char d)
{
writeCmd(0x00+d%16); // Set Lower Column Start Address for Page Addressing Mode
// Default => 0x00
writeCmd(0x10+d/16); // Set Higher Column Start Address for Page Addressing Mode
// Default => 0x10
}
void OLED::setAddressingMode(unsigned char d)
{
writeCmd(0x20); // Set Memory Addressing Mode
writeCmd(d); // Default => 0x02
// 0x00 => Horizontal Addressing Mode
// 0x01 => Vertical Addressing Mode
// 0x02 => Page Addressing Mode
}
void RfidInput::init(void)
{
static const byte nonzero = 1;
getPort()->begin(RFID_BAUD_RATE);
delay(20);
while (-1 != read()); // flush out the receive FIFO
// The following are silly placeholders for initialization commands:
writeCmd(0, RFID_CMD_Firmware, 0);
writeCmd(1, RFID_CMD_Antenna_Power, &nonzero);
writeCmd(0, RFID_CMD_Seek_for_Tag, 0);
}
void OLED::fill(unsigned char data)
{
unsigned char x,y;
for(y=0;y<8;y++)
{
writeCmd(0xb0+y);
writeCmd(0x00);
writeCmd(0x10);
for(x=0;x<128;x++)
writeData(data);
}
}
void OLED::setDisplayOnOff(unsigned char d)
{
writeCmd(0xAE|d); // Set Display On/Off
// Default => 0xAE
// 0xAE (0x00) => Display Off
// 0xAF (0x01) => Display On
}
asynStatus readGpib(niport *pniport,char *buf, int cnt, int *actual,
int addr, double timeout,int *eomReason)
{
char cmdbuf[2] = {IBUNT,IBUNL};
asynStatus status;
epicsUInt8 isr1 = pniport->isr1;
*actual=0; *buf=0;
pniport->bytesRemainingRead = cnt;
pniport->nextByteRead = buf;
pniport->eomReason = 0;
if(isr1&DI) {
pniport->bytesRemainingRead--;
pniport->nextByteRead++;
buf[0] = readRegister(pniport,DIR);
}
writeRegister(pniport,AUXMR,AUXFH);
pniport->status = asynSuccess;
status = writeAddr(pniport,addr,0,timeout,transferStateRead);
if(status!=asynSuccess) return status;
*actual = cnt - pniport->bytesRemainingRead;
if(eomReason) *eomReason = pniport->eomReason;
writeCmd(pniport,cmdbuf,2,timeout,transferStateIdle);
return status;
}
void OLED::setInverseDisplay(unsigned char d)
{
writeCmd(0xA6|d); // Set Inverse Display On/Off
// Default => 0xA6
// 0xA6 (0x00) => Normal Display
// 0xA7 (0x01) => Inverse Display On
}
void OLED::setCommonRemap(unsigned char d)
{
writeCmd(0xC0|d); // Set COM Output Scan Direction
// Default => 0xC0
// 0xC0 (0x00) => Scan from COM0 to 63
// 0xC8 (0x08) => Scan from COM63 to 0
}
void OLED::setEntireDisplay(unsigned char d)
{
writeCmd(0xA4|d); // Set Entire Display On / Off
// Default => 0xA4
// 0xA4 (0x00) => Normal Display
// 0xA5 (0x01) => Entire Display On
}
void OLED::setSegmentRemap(unsigned char d)
{
writeCmd(0xA0|d); // Set Segment Re-Map
// Default => 0xA0
// 0xA0 (0x00) => Column Address 0 Mapped to SEG0
// 0xA1 (0x01) => Column Address 0 Mapped to SEG127
}
static asynStatus writeAddr(niport *pniport,int talk, int listen,
double timeout,transferState_t nextState)
{
char cmdbuf[4];
int lenCmd = 0;
int primary,secondary;
if(talk<0) {
; /*do nothing*/
} else if(talk<100) {
cmdbuf[lenCmd++] = talk + TADBASE;
} else {
primary = talk / 100; secondary = talk % 100;
cmdbuf[lenCmd++] = primary + TADBASE;
cmdbuf[lenCmd++] = secondary + SADBASE;
}
if(listen<0) {
; /*do nothing*/
} else if(listen<100) {
cmdbuf[lenCmd++] = listen + LADBASE;
} else {
primary = listen / 100; secondary = listen % 100;
cmdbuf[lenCmd++] = primary + LADBASE;
cmdbuf[lenCmd++] = secondary + SADBASE;
}
return writeCmd(pniport,cmdbuf,lenCmd,timeout,nextState);
}
void Lcd::setCursor(int line, int pos) {
pos |= 0b10000000;
if (line == 1) {
pos += 0x40;
}
writeCmd(pos);
}
void DataTerminal::fail()
{
mByteCount = 0;
mState = STATE_WAITING;
GPIO_ResetBits(GPIOB, GPIO_Pin_14|GPIO_Pin_15);
FLASH_Lock();
trace_printf("Resetting\n");
writeCmd(NACK);
}
// Clears the entire LCD screen
// Inputs: color of background
void clearLCD(unsigned short rgb)
{
unsigned int i;
setWindow(0,0,239,319);
writeCmd(0x0022);
for(i = 0; i < (MAX_X*MAX_Y); i++)
{
writeData(rgb);
}
}
bool ntfs::writeLabel(Report& report, const QString& deviceNode, const QString& newLabel)
{
ExternalCommand writeCmd(report, QStringLiteral("ntfslabel"), { QStringLiteral("--force"), deviceNode, newLabel });
writeCmd.setProcessChannelMode(QProcess::SeparateChannels);
if (!writeCmd.run(-1))
return false;
return true;
}
static asynStatus gpibPortSerialPollEnd(void *pdrvPvt)
{
niport *pniport = (niport *)pdrvPvt;
double timeout = 1.0;
asynStatus status;
char cmd[2];
cmd[0] = IBSPD; cmd[1] = IBUNT;
status = writeCmd(pniport,cmd,2,timeout,transferStateIdle);
return status;
}
static asynStatus gpibPortSerialPollBegin(void *pdrvPvt)
{
niport *pniport = (niport *)pdrvPvt;
double timeout = 1.0;
asynStatus status;
char cmd[3];
cmd[0] = IBUNL; cmd[1] = LADBASE; cmd[2] = IBSPE;
status = writeCmd(pniport,cmd,3,timeout,transferStateIdle);
return status;
}
void main(void)
{
writeCmd(0x01);
writeCMD(0xF);
writeCMD(0x06);
WriteData('A');
WriteData('a');
WriteData('r');
WriteData('o');
WriteData('n');
}
// Clears a certain area on the LCD screen
// Inputs: x1,y1,x2,y2 coordinates and color of area to be cleared
void clearArea(unsigned short x1, unsigned short y1,unsigned short x2,unsigned short y2, unsigned short rgb)
{
unsigned int i;
unsigned short dx, dy;
setWindow(x1,y1,x2,y2);
dx = x2-x1;
dy = y2-y1;
writeCmd(0x0022);
for(i = 0; i < (dx*dy); i++)
{
writeData(rgb);
}
}
void sendMovement(int seqNum, int flag, float frontBackTilt, float leftRightTilt, float verticalSpeed, float angularSpeed)
{
int seqSize = 0;
char seqStr[64];
numberToString(seqNum,seqStr,&seqSize);
int cflagSize = 0;
char cflagStr[64];
numberToString(flag,cflagStr,&cflagSize);
int lrTilt_int = *(int*)&leftRightTilt;
int fbTilt_int = *(int*)&frontBackTilt;
int vSpeed_int = *(int*)&verticalSpeed;
int aSpeed_int = *(int*)&angularSpeed;
int lrTiltSize = 0;
char lrTiltStr[64];
numberToString(lrTilt_int,lrTiltStr,&lrTiltSize);
int fbTiltSize = 0;
char fbTiltStr[64];
numberToString(fbTilt_int,fbTiltStr,&fbTiltSize);
int vSpeedSize = 0;
char vSpeedStr[64];
numberToString(vSpeed_int,vSpeedStr,&vSpeedSize);
int aSpeedSize = 0;
char aSpeedStr[64];
numberToString(aSpeed_int,aSpeedStr,&aSpeedSize);
char * move = (char *)malloc((14+seqSize+cflagSize+lrTiltSize+fbTiltSize+vSpeedSize+aSpeedSize)*sizeof(char));
memcpy(move,"AT*PCMD=",8);
memcpy(&move[8],seqStr,seqSize);
memcpy(&move[8+seqSize],",",1);
memcpy(&move[9+seqSize],cflagStr,cflagSize);
memcpy(&move[9+seqSize+cflagSize],",",1);
memcpy(&move[10+seqSize+cflagSize],lrTiltStr,lrTiltSize);
memcpy(&move[10+seqSize+cflagSize+lrTiltSize],",",1);
memcpy(&move[11+seqSize+cflagSize+lrTiltSize],fbTiltStr,fbTiltSize);
memcpy(&move[11+seqSize+cflagSize+lrTiltSize+fbTiltSize],",",1);
memcpy(&move[12+seqSize+cflagSize+lrTiltSize+fbTiltSize],vSpeedStr,vSpeedSize);
memcpy(&move[12+seqSize+cflagSize+lrTiltSize+fbTiltSize+vSpeedSize],",",1);
memcpy(&move[13+seqSize+cflagSize+lrTiltSize+fbTiltSize+vSpeedSize],aSpeedStr,aSpeedSize);
memcpy(&move[13+seqSize+cflagSize+lrTiltSize+fbTiltSize+vSpeedSize+aSpeedSize],"\r",1);
writeCmd(move,14+seqSize+cflagSize+lrTiltSize+fbTiltSize+vSpeedSize+aSpeedSize);
free(move);
}
bool ACC_GYRO_GY80::init() {
// Wire.begin();
// L3G4200D
writeCmd(L3G4_ADDRESS, CTRL_REG1, L3G4_BW_200_50);
writeCmd(L3G4_ADDRESS, CTRL_REG2, L3G4_HPF1);
writeCmd(L3G4_ADDRESS, CTRL_REG3, L3G4_INTERRUPT);
writeCmd(L3G4_ADDRESS, CTRL_REG4, MODE_2000);
writeCmd(L3G4_ADDRESS, CTRL_REG5, L3G4_LPF2);
// ADXL345
writeCmd(ADXL_ADDRESS, REG_DATA_FORMAT, RANGE_8);
writeCmd(ADXL_ADDRESS, REG_PWR_CTL, MEASURE_MODE);
writeCmd(ADXL_ADDRESS, REG_BW_RATE, MODE_200_100);
computeGyroOffset();
//computeAccDynamicOffset();
return true;
}
void sendNavDataInit(int seqNum)
{
int seqSize = 0;
char seqStr[64];
numberToString(seqNum,seqStr,&seqSize);
char * cfg = (char *)malloc((41+seqSize)*sizeof(char));
memcpy(cfg,"AT*CONFIG=",10);
memcpy(&cfg[10],seqStr,seqSize);
memcpy(&cfg[10+seqSize],",\"general:navdata_demo\",\"TRUE\"\r",31);
writeCmd(cfg,41+seqSize);
free(cfg);
}
void sendCalibMagn(int seqNum)
{
int seqSize = 0;
char seqStr[64];
numberToString(seqNum,seqStr,&seqSize);
char * calib = (char *)malloc((12+seqSize)*sizeof(char));
memcpy(calib,"AT*CALIB=",9);
memcpy(&calib[9],seqStr,seqSize);
memcpy(&calib[9+seqSize],",0\r",3);
writeCmd(calib,12+seqSize);
free(calib);
}
void sendCalibHPlan(int seqNum)
{
int seqSize = 0;
char seqStr[64];
numberToString(seqNum,seqStr,&seqSize);
char * calib = (char *)malloc((10+seqSize)*sizeof(char));
memcpy(calib,"AT*FTRIM=",9);
memcpy(&calib[9],seqStr,seqSize);
memcpy(&calib[9+seqSize],"\r",1);
writeCmd(calib,10+seqSize);
free(calib);
}
void sendEmergency(int seqNum)
{
int seqSize = 0;
char seqStr[64];
numberToString(seqNum,seqStr,&seqSize);
char * reset = (char *)malloc((18+seqSize)*sizeof(char));
memcpy(reset,"AT*REF=",7);
memcpy(&reset[7],seqStr,seqSize);
memcpy(&reset[7+seqSize],",290717952\r",11);
writeCmd(reset,18+seqSize);
free(reset);
}
