//the init procedure is executed once when the REXLANG function block initializes
long init(void)
{
i2c_bus_handle = Open(I2CDEV_FNAME); // open I2C bus
i2c_chip_address = 0x20; // 7-bit address of the I2C device
// !!!!!!!!!!!!!
// By default, IOCON.BANK=0, therefore see Table 1-6 in the datasheet for register mapping
//
// Table 1-5, which confusingly appears sooner in the datasheet, displays register addresses
// for IOCON.BANK=1, which are significantly different
// !!!!!!!!!!!!!
//Setting PORTA to output
i2c_bufTx[0] = 0x00; //register no. (IODIRA)
i2c_bufTx[1] = 0x00; //IODIRA data, bitmask, 0=output, 1=input
i2c_write_count = 2;
i2c_read_count = 0;
i2c_ret_fun = I2C(i2c_bus_handle, i2c_chip_address, i2c_bufTx, i2c_write_count, i2c_bufRx, i2c_read_count);
//Setting PORTB to input
i2c_bufTx[0] = 0x01; //register no. (IODIRB)
i2c_bufTx[1] = 0xFF; //IODIRB data, bitmask, 0=output, 1=input
i2c_write_count = 2;
i2c_read_count = 0;
i2c_ret_fun = I2C(i2c_bus_handle, i2c_chip_address, i2c_bufTx, i2c_write_count, i2c_bufRx, i2c_read_count);
//Enabling pull-up resistors on PORTB
i2c_bufTx[0] = 0x0D; //register no. (GPPUB)
i2c_bufTx[1] = 0xFF; //GPPUB data, bitmask, 0=no pull-up, 1=pull-up enabled
i2c_write_count = 2;
i2c_read_count = 0;
i2c_ret_fun = I2C(i2c_bus_handle, i2c_chip_address, i2c_bufTx, i2c_write_count, i2c_bufRx, i2c_read_count);
return 0;
}
//the main procedure is executed once in each sampling period
long main(void)
{
//Controlling outputs
i2c_bufTx[0] = 0x12; //register no. (GPIOA)
i2c_bufTx[1] = digital_out & 0xFF; //masking the data to control the outputs
i2c_write_count = 2;
i2c_read_count = 0;
i2c_ret_fun = I2C(i2c_bus_handle, i2c_chip_address, i2c_bufTx, i2c_write_count, i2c_bufRx, i2c_read_count);
//Reading inputs
i2c_bufTx[0] = 0x13; //register no. (GPIOB)
i2c_write_count = 1;
i2c_read_count = 1;
i2c_ret_fun = I2C(i2c_bus_handle, i2c_chip_address, i2c_bufTx, i2c_write_count, i2c_bufRx, i2c_read_count);
digital_in = i2c_bufRx[0]; //publishing the received data
return 0;
}
Drone::Drone(int txPin, int rxPin) {
_serialIO = SerialIO(txPin, rxPin);
_callback = Callback();
_incomingPacketReader = IncomingPacketReader(&_serialIO);
_vitals = Vitals(&_serialIO, &_incomingPacketReader, &_callback);
_responseHandler = ResponseHandler(&_serialIO, &_incomingPacketReader, &_callback, &_vitals);
_rc = RC(&_serialIO, &_callback, &_incomingPacketReader);
_gpio = GPIO(&_serialIO, &_callback, &_incomingPacketReader);
_i2c = I2C(&_serialIO, &_callback, &_incomingPacketReader);
_pose = Pose(&_serialIO, &_callback, &_incomingPacketReader);
_autopilot = Autopilot(&_serialIO, &_callback, &_incomingPacketReader);
_transmitterSupport = TransmitterSupport(&_serialIO, &_callback, &_incomingPacketReader);
}
long getTdacCalibrationInfo(HANDLE hDevice, ue9TdacCalibrationInfo *caliInfo, uint8 DIOAPinNum)
{
int err;
uint8 options, speedAdjust, sdaPinNum, sclPinNum;
uint8 address, numByteToSend, numBytesToReceive, errorcode;
uint8 bytesCommand[1];
uint8 bytesResponse[32];
uint8 ackArray[4];
err = 0;
//Setting up I2C command for LJTDAC
options = 0; //I2COptions : 0
speedAdjust = 0; //SpeedAdjust : 0 (for max communication speed of about 130 kHz)
sdaPinNum = DIOAPinNum+1; //SDAPinNum : FIO channel connected to pin DIOB
sclPinNum = DIOAPinNum; //SCLPinNum : FIO channel connected to pin DIOA
address = (uint8)(0xA0); //Address : h0xA0 is the address for EEPROM
numByteToSend = 1; //NumI2CByteToSend : 1 byte for the EEPROM address
numBytesToReceive = 32; //NumI2CBytesToReceive : getting 32 bytes starting at EEPROM address specified in I2CByte0
bytesCommand[0] = 64; //I2CByte0 : Memory Address (starting at address 64 (DACA Slope)
//Performing I2C low-level call
err = I2C(hDevice, options, speedAdjust, sdaPinNum, sclPinNum, address, numByteToSend, numBytesToReceive, bytesCommand, &errorcode, ackArray, bytesResponse);
if( errorcode != 0 )
{
printf("Getting LJTDAC calibration info error : received errorcode %d in response\n", errorcode);
err = -1;
}
if( err == -1 )
return err;
caliInfo->ccConstants[0] = FPuint8ArrayToFPDouble(bytesResponse, 0);
caliInfo->ccConstants[1] = FPuint8ArrayToFPDouble(bytesResponse, 8);
caliInfo->ccConstants[2] = FPuint8ArrayToFPDouble(bytesResponse, 16);
caliInfo->ccConstants[3] = FPuint8ArrayToFPDouble(bytesResponse, 24);
caliInfo->prodID = 9;
return err;
}
};
static u32 mux_pllm_pllc2_c_c3_pllp_plla_pllc4_idx[] = {
[0] = 0, [1] = 1, [2] = 2, [3] = 3, [4] = 4, [5] = 6, [6] = 7,
};
static const char *mux_clkm_plldp_sor0lvds[] = {
"clk_m", "pll_dp", "sor0_lvds",
};
#define mux_clkm_plldp_sor0lvds_idx NULL
static struct tegra_periph_init_data periph_clks[] = {
AUDIO("d_audio", CLK_SOURCE_D_AUDIO, 106, TEGRA_PERIPH_ON_APB, tegra_clk_d_audio),
AUDIO("dam0", CLK_SOURCE_DAM0, 108, TEGRA_PERIPH_ON_APB, tegra_clk_dam0),
AUDIO("dam1", CLK_SOURCE_DAM1, 109, TEGRA_PERIPH_ON_APB, tegra_clk_dam1),
AUDIO("dam2", CLK_SOURCE_DAM2, 110, TEGRA_PERIPH_ON_APB, tegra_clk_dam2),
I2C("i2c1", mux_pllp_clkm, CLK_SOURCE_I2C1, 12, tegra_clk_i2c1),
I2C("i2c2", mux_pllp_clkm, CLK_SOURCE_I2C2, 54, tegra_clk_i2c2),
I2C("i2c3", mux_pllp_clkm, CLK_SOURCE_I2C3, 67, tegra_clk_i2c3),
I2C("i2c4", mux_pllp_clkm, CLK_SOURCE_I2C4, 103, tegra_clk_i2c4),
I2C("i2c5", mux_pllp_clkm, CLK_SOURCE_I2C5, 47, tegra_clk_i2c5),
INT("vde", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_VDE, 61, 0, tegra_clk_vde),
INT("vi", mux_pllm_pllc_pllp_plla, CLK_SOURCE_VI, 20, 0, tegra_clk_vi),
INT("epp", mux_pllm_pllc_pllp_plla, CLK_SOURCE_EPP, 19, 0, tegra_clk_epp),
INT("host1x", mux_pllm_pllc_pllp_plla, CLK_SOURCE_HOST1X, 28, 0, tegra_clk_host1x),
INT("mpe", mux_pllm_pllc_pllp_plla, CLK_SOURCE_MPE, 60, 0, tegra_clk_mpe),
INT("2d", mux_pllm_pllc_pllp_plla, CLK_SOURCE_2D, 21, 0, tegra_clk_gr2d),
INT("3d", mux_pllm_pllc_pllp_plla, CLK_SOURCE_3D, 24, 0, tegra_clk_gr3d),
INT8("vde", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_VDE, 61, 0, tegra_clk_vde_8),
INT8("vi", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_VI, 20, 0, tegra_clk_vi_8),
INT8("vi", mux_pllm_pllc2_c_c3_pllp_plla_pllc4, CLK_SOURCE_VI, 20, 0, tegra_clk_vi_9),
INT8("epp", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_EPP, 19, 0, tegra_clk_epp_8),
*target = TWDR;
// I suppose that if we get this far we're ok
return 0;
}
uint8_t I2C::stop()
{
unsigned long startingTime = millis();
TWCR = (1<<TWINT)|(1<<TWEN)| (1<<TWSTO);
while ((TWCR & (1<<TWSTO)))
{
if(!timeOutDelay){continue;}
if((millis() - startingTime) >= timeOutDelay)
{
lockUp();
return(1);
}
}
return(0);
}
void I2C::lockUp()
{
TWCR = 0; //releases SDA and SCL lines to high impedance
TWCR = _BV(TWEN) | _BV(TWEA); //reinitialize TWI
}
I2C I2c = I2C();
void PCMLoop(pcm_soft* p,bool_t Speed)
{
bool_t SrcPacked = !(p->Src.Flags & PCM_PLANES) && p->Src.Channels==2;
dyninst* Loop;
reg Left;
reg Right;
reg Shift;
if (!p->ActualDither && p->Shift)
I1C(MOVI,R13,p->Shift);
if (p->ActualDither)
if (Speed)
I2C(MOVL_STOFS,R7,SP,OFS(stack,Step)); // save backup
else
I1C(MOVI,R7,-p->Shift);
Loop = Label(1);
Left = R2;
if (p->UseLeft)
Right = R3;
else
Right = R2;
if (Speed)
{
I2(MOV,R8,R0);
IShift(R0,NONE,-8);
IShift(R0,NONE,p->SrcShift);
switch (p->Src.Bits)
{
case 8:
if (p->UseLeft)
I2(MOVB_LDR0,R9,Left);
if (p->UseRight)
I2(MOVB_LDR0,R10,Right);
if (p->SrcUnsigned)
{
I2(EXTUB,R2,R2);
if (p->SrcChannels>1)
I2(EXTUB,R3,R3);
}
break;
case 16:
if (p->UseLeft)
I2(MOVW_LDR0,R9,Left);
if (p->UseRight)
I2(MOVW_LDR0,R10,Right);
if (p->SrcUnsigned)
{
I2(EXTUW,R2,R2);
if (p->SrcChannels>1)
I2(EXTUW,R3,R3);
}
break;
case 32:
if (p->UseLeft)
I2(MOVL_LDR0,R9,Left);
if (p->UseRight)
I2(MOVL_LDR0,R10,Right);
break;
}
I2(ADD,R7,R8);
}
else
{
switch (p->Src.Bits)
{
case 8:
if (p->UseLeft)
{
I2(MOVB_LDADD,R9,Left);
if (SrcPacked && !p->UseRight) I1C(ADDI,R9,1);
}
if (p->UseRight)
if (SrcPacked)
{
if (!p->UseLeft) I1C(ADDI,R9,1);
I2(MOVB_LDADD,R9,Right);
}
else
I2(MOVB_LDADD,R10,Right);
if (p->SrcUnsigned)
{
I2(EXTUB,R2,R2);
if (p->SrcChannels>1)
I2(EXTUB,R3,R3);
}
break;
case 16:
if (p->UseLeft)
{
I2(MOVW_LDADD,R9,Left);
if (SrcPacked && !p->UseRight) I1C(ADDI,R9,2);
}
if (p->UseRight)
if (SrcPacked)
{
if (!p->UseLeft) I1C(ADDI,R9,2);
I2(MOVW_LDADD,R9,Right);
}
else
I2(MOVW_LDADD,R10,Right);
if (p->SrcUnsigned)
{
I2(EXTUW,R2,R2);
if (p->SrcChannels>1)
I2(EXTUW,R3,R3);
}
break;
case 32:
if (p->UseLeft)
{
I2(MOVL_LDADD,R9,Left);
if (SrcPacked && !p->UseRight) I1C(ADDI,R9,4);
}
if (p->UseRight)
if (SrcPacked)
{
if (!p->UseLeft) I1C(ADDI,R9,4);
I2(MOVL_LDADD,R9,Right);
}
else
I2(MOVL_LDADD,R10,Right);
break;
}
}
if (p->InstSrcUnsigned)
{
MB(); I1P(MOVL_PC,R0,p->InstSrcUnsigned,0);
I2(SUB,R0,R2);
if (p->SrcChannels>1)
I2(SUB,R0,R3);
}
else
if (p->SrcUnsigned == 128)
{
I1C(ADDI,R2,-128);
if (p->SrcChannels>1)
I1C(ADDI,R3,-128);
}
if (p->Stereo)
{
if ((p->Src.Flags ^ p->Dst.Flags) & PCM_SWAPPEDSTEREO)
{
Left = R3;
Right = R2;
}
else
{
Left = R2;
Right = R3;
}
}
else
{
if (p->Join)
I2(ADD,R3,R2);
Right = Left = R2;
}
if (p->ActualDither)
{
I2(ADD,R13,R2);
I2(MOV,R2,R13);
if (p->Stereo)
{
I2(ADD,R1,R3);
I2(MOV,R3,R1);
}
}
if (p->Clip>0)
{
dyninst* ClipMin = Label(0);
dyninst* ClipMax = Label(0);
I2(CMPGT,R2,R4);
I0P(BFS,ClipMin);
I2(CMPGT,R5,R2); // delay slot
I2(MOV,R4,R2);
InstPost(ClipMin);
I0P(BFS,ClipMax);
I1C(ADDI,R11,1<<p->DstShift); // delay slot
I2(MOV,R5,R2);
InstPost(ClipMax);
if (p->Stereo)
{
ClipMin = Label(0);
ClipMax = Label(0);
I2(CMPGT,R3,R4);
I0P(BFS,ClipMin);
I2(CMPGT,R5,R3); // delay slot
I2(MOV,R4,R3);
InstPost(ClipMin);
I0P(BFS,ClipMax);
I1C(ADDI,R12,1<<p->DstShift); // delay slot
I2(MOV,R5,R3);
InstPost(ClipMax);
}
else
if (p->Dst.Channels>1)
I1C(ADDI,R12,1<<p->DstShift); // delay slot
}
else
{
I1C(ADDI,R11,1<<p->DstShift);
if (p->Dst.Channels>1)
I1C(ADDI,R12,1<<p->DstShift);
}
I2(CMPEQ,R11,R14);
if (p->Shift)
{
if (p->ActualDither)
{
I1C(MOVI,R0,p->Shift);
Shift = R0;
}
else
Shift = R13;
I2(SHAD,Shift,R2);
if (p->Stereo) I2(SHAD,Shift,R3);
}
if (p->ActualDither)
{
if (Speed)
I1C(MOVI,R7,-p->Shift);
I2(MOV,R2,R0);
I2(SHAD,R7,R0);
I2(SUB,R0,R13);
if (p->Stereo)
{
I2(MOV,R3,R0);
I2(SHAD,R7,R0);
I2(SUB,R0,R1);
}
if (Speed)
I2C(MOVL_LDOFS,SP,R7,OFS(stack,Step));
}
if (p->InstDstUnsigned)
{
MB(); I1P(MOVL_PC,R0,p->InstDstUnsigned,0);
I1C(ADD,R0,Left);
if (Left != Right)
I1C(ADD,R0,Right);
}
else
if (p->DstUnsigned == 128)
{
// p->Dst.Bits=8 so doesn't matter if it's add or sub
I1C(ADDI,Left,-128);
if (Left != Right)
I1C(ADDI,Right,-128);
}
switch (p->Dst.Bits)
{
case 8:
I2(MOVB_ST,Left,R11);
if (p->Dst.Channels>1)
I2(MOVB_ST,Right,R12);
break;
case 16:
I2(MOVW_ST,Left,R11);
if (p->Dst.Channels>1)
I2(MOVW_ST,Right,R12);
break;
case 32:
I2(MOVL_ST,Left,R11);
if (p->Dst.Channels>1)
I2(MOVL_ST,Right,R12);
break;
}
DS(); I0P(BFS,Loop);
if (p->ActualDither)
{
I2(MOV,SP,R0);
I1C(ADDI,R0,OFS(stack,State));
I2(MOVL_LD,R0,R4);
I2C(MOVL_STOFS,R13,R4,OFS(pcmstate,Dither[0]));
I2C(MOVL_STOFS,R1,R4,OFS(pcmstate,Dither[1]));
}
CodeEnd(7,OFS(stack,StackFrame));
}
void PCMCompile(pcm_soft* p)
{
dyninst* Speed;
dyninst* Min = NULL;
dyninst* Max = NULL;
CodeBegin(7,OFS(stack,StackFrame));
p->InstSrcUnsigned = NULL;
p->InstDstUnsigned = NULL;
if (p->SrcUnsigned && p->SrcUnsigned != 128)
p->InstSrcUnsigned = InstCreate32(p->SrcUnsigned,NONE,NONE,NONE,0,0);
if (p->DstUnsigned && p->DstUnsigned != 128)
p->InstDstUnsigned = InstCreate32(p->DstUnsigned,NONE,NONE,NONE,0,0);
// dst pointers
I2C(MOVL_LDOFS,R5,R11,0);
I1C(ADDI,R11,-(1<<p->DstShift)); // back one step
if (p->Dst.Channels > 1)
{
if (p->Dst.Flags & PCM_PLANES)
{
I2C(MOVL_LDOFS,R5,R12,4);
I1C(ADDI,R12,-(1<<p->DstShift)); // back one step
}
else
{
I2(MOV,R11,R12);
I1C(ADDI,R12,1<<(p->DstShift-1));
}
}
I2(MOV,R7,R14);
I2(ADD,R11,R14); // dstend
// src pointers
I2C(MOVL_LDOFS,R6,R9,0);
if (p->Src.Channels > 1)
{
if (p->Src.Flags & PCM_PLANES)
I2C(MOVL_LDOFS,R6,R10,4);
else
{
I2(MOV,R9,R10);
I1C(ADDI,R10,p->Src.Bits>>3);
}
}
I2(MOV,SP,R0);
I1C(ADDI,R0,OFS(stack,State));
I2(MOVL_LD,R0,R4);
I2C(MOVL_LDOFS,R4,R7,OFS(pcmstate,Step));
I2C(MOVL_LDOFS,R4,R8,OFS(pcmstate,Pos));
/*
if (p->UseVolume)
{
I1C(ADDI,R0,OFS(stack,Volume)-OFS(stack,State));
I2(MOVL_LD,R0,R1);
}
*/
if (p->ActualDither)
{
I2C(MOVL_LDOFS,R4,R13,OFS(pcmstate,Dither[0]));
I2C(MOVL_LDOFS,R4,R1,OFS(pcmstate,Dither[1]));
}
if (p->Clip>0)
{
Min = InstCreate32(p->MinLimit,NONE,NONE,NONE,0,0);
Max = InstCreate32(p->MaxLimit,NONE,NONE,NONE,0,0);
I1P(MOVL_PC,R4,Min,0);
I1P(MOVL_PC,R5,Max,0);
}
Speed = Label(0);
I1C(MOVI,R2,64);
I1(SHLL2,R2);
I2(CMPEQ,R2,R7);
I0P(BF,Speed);
PCMLoop(p,0);
InstPost(Speed);
PCMLoop(p,1);
Align(4);
if (Min) InstPost(Min);
if (Max) InstPost(Max);
if (p->InstSrcUnsigned) InstPost(p->InstSrcUnsigned);
if (p->InstDstUnsigned) InstPost(p->InstDstUnsigned);
}
Serial.println(DataRegister.read(reg));
}
DataRegister.close(reg);
}
}
void requestHandler(){
//intentionally left blank. might implement a default behavior later
}
I2C::I2C(){
_resetI2C();
}
void I2C::_resetI2C(){
_i2c = DataRegister.open(SET_I2C_ADDRESS, REG_MASTER_MODE);
_i2c_add = DataRegister.read(_i2c);
Wire.begin(_i2c_add);
Wire.onReceive(receiveHandler);
Wire.onRequest(requestHandler);
_is_wire_active = true;
}
I2C DT_Wire = I2C();
static u32 mux_pllp_pllre_clkm_idx[] = {
[0] = 0, [1] = 2, [2] = 3,
};
static const char *mux_clkm_plldp_sor0lvds[] = {
"clk_m", "pll_dp", "sor0_lvds",
};
#define mux_clkm_plldp_sor0lvds_idx NULL
static struct tegra_periph_init_data periph_clks[] = {
AUDIO("d_audio", CLK_SOURCE_D_AUDIO, 106, TEGRA_PERIPH_ON_APB, tegra_clk_d_audio),
AUDIO("dam0", CLK_SOURCE_DAM0, 108, TEGRA_PERIPH_ON_APB, tegra_clk_dam0),
AUDIO("dam1", CLK_SOURCE_DAM1, 109, TEGRA_PERIPH_ON_APB, tegra_clk_dam1),
AUDIO("dam2", CLK_SOURCE_DAM2, 110, TEGRA_PERIPH_ON_APB, tegra_clk_dam2),
I2C("i2c1", mux_pllp_clkm, CLK_SOURCE_I2C1, 12, tegra_clk_i2c1),
I2C("i2c2", mux_pllp_clkm, CLK_SOURCE_I2C2, 54, tegra_clk_i2c2),
I2C("i2c3", mux_pllp_clkm, CLK_SOURCE_I2C3, 67, tegra_clk_i2c3),
I2C("i2c4", mux_pllp_clkm, CLK_SOURCE_I2C4, 103, tegra_clk_i2c4),
I2C("i2c5", mux_pllp_clkm, CLK_SOURCE_I2C5, 47, tegra_clk_i2c5),
I2C("i2c6", mux_pllp_clkm, CLK_SOURCE_I2C6, 166, tegra_clk_i2c6),
INT("vde", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_VDE, 61, 0, tegra_clk_vde),
INT("vi", mux_pllm_pllc_pllp_plla, CLK_SOURCE_VI, 20, 0, tegra_clk_vi),
INT("epp", mux_pllm_pllc_pllp_plla, CLK_SOURCE_EPP, 19, 0, tegra_clk_epp),
INT("host1x", mux_pllm_pllc_pllp_plla, CLK_SOURCE_HOST1X, 28, 0, tegra_clk_host1x),
INT("mpe", mux_pllm_pllc_pllp_plla, CLK_SOURCE_MPE, 60, 0, tegra_clk_mpe),
INT("2d", mux_pllm_pllc_pllp_plla, CLK_SOURCE_2D, 21, 0, tegra_clk_gr2d),
INT("3d", mux_pllm_pllc_pllp_plla, CLK_SOURCE_3D, 24, 0, tegra_clk_gr3d),
INT8("vde", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_VDE, 61, 0, tegra_clk_vde_8),
INT8("vi", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_VI, 20, 0, tegra_clk_vi_8),
INT8("vi", mux_pllm_pllc2_c_c3_pllp_plla_pllc4, CLK_SOURCE_VI, 20, 0, tegra_clk_vi_9),
IMUTester::IMUTester(void)
{
i2c = I2C();
return;
}
