char canstdio_getcharacter(bool blocking)
{
while (blocking && nodo_attivo && !input_buffer_nonempty)
simulator_relax();
if (input_buffer_nonempty == false)
return 0;
int current_cpu_ipl;
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, 7); // disattiva interrupt
char res;
if (input_buffer_count == 0)
{
res = 0;
}
else
{
res = input_buffer[0];
memmove(input_buffer, input_buffer + 1, --input_buffer_count);
}
input_buffer_nonempty = (input_buffer_count != 0);
RESTORE_CPU_IPL(current_cpu_ipl); // riattiva interrupt
return res;
}
//************************************************************
// Registers callback in "OffsetMS" milliseconds
//************************************************************
void TMRCallBackAfter( ulong OffsetMS,
TMRCallBack callBack)
{
//======================================================
char saved_ipl;
SET_AND_SAVE_CPU_IPL(saved_ipl,_TMRIL);
//--------------------------------------------------
// Reset/Enable CTMU (callback) interrupt
//--------------------------------------------------
_RTCIF = 0; // Clear RTCC interrupt flag
_RTCIE = 1; // Enable RTCC interrupt
//--------------------------------------------------
// Set callback request
//--------------------------------------------------
_TMR_CallBack = callBack;
//======================================================
// Important NOTE: Timer counter updated every 125 usec!
//------------------------------------------------------
// Time offset need to be converted to timer units
//------------------------------------------------------
_TMR_CallBack_TriggerTime = _TMR_RT_ElapsedTime
+ (OffsetMS << 3);
//--------------------------------------------------
RESTORE_CPU_IPL(saved_ipl);
//---------------------------------------------------
return;
}
static void invoke_update_handler(t_object * obj)
{
t_object_buffer *src_buffer;
unsigned int data_length;
uint8_t buffer[8];
int current_cpu_ipl;
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, 7); // disattiva interrupt
if ((obj->flags & ECAN_RX_FLAG_FIFO))
{
src_buffer = &fifo_buffer[fifo_head++];
if (fifo_head == ECAN_FIFO_SIZE)
fifo_head = 0;
if (--fifo_count != 0)
obj->new_data_arrived = true;
else
obj->new_data_arrived = false;
}
else
{
src_buffer = &obj->buff;
obj->new_data_arrived = false;
}
data_length = src_buffer->data_length;
memcpy(buffer, src_buffer->data, data_length);
RESTORE_CPU_IPL(current_cpu_ipl); // ripristina interrupt
obj->update(buffer, data_length, obj->user_ptr);
}
void fct_can_cmd(unsigned long ID, T_TYPE_ID type_ID, const void * data_rx, char nbr_data)
{
char UART_can_receive[50]={0x00};
int old_ipl, ii, jj;
// Block interruptions
SET_AND_SAVE_CPU_IPL(old_ipl, 7);
datReceive_can_cmd.datReceive = (char *)data_rx;
datReceive_can_cmd.nb_data = (unsigned int)nbr_data;
RESTORE_CPU_IPL(old_ipl);
}
void fct_can_pitch(unsigned long ID, T_TYPE_ID type_ID, T_CAN_DATA* recopie, char nbr_data)
{
const char ubReceiveData[5] = {(recopie->data3 & 0x00FF),(recopie->data3 & 0xFF00)>>8,(recopie->data4 & 0x00FF),(recopie->data4 & 0xFF00)>>8,(recopie->data5 & 0x00FF)};
int old_ipl;
// Block interruptions
SET_AND_SAVE_CPU_IPL(old_ipl, 7);
off = 0;
chinookpack_unpack_next(&unpacker,ubReceiveData,5,&off);
off = 0;
RESTORE_CPU_IPL(old_ipl);
fCanPitchPercent = unpacker.data.via.dec;
}
void fct_can_gear(unsigned long ID, T_TYPE_ID type_ID, T_CAN_DATA* recopie, char nbr_data)
{
const char ubReceiveData[2] = {(recopie->data3 & 0x00FF),(recopie->data3 & 0xFF00)>>8};
int old_ipl;
// Block interruptions
SET_AND_SAVE_CPU_IPL(old_ipl, 7);
off = 0;
chinookpack_unpack_next(&unpacker,ubReceiveData,5,&off);
off = 0;
RESTORE_CPU_IPL(old_ipl);
ubGearCanReceive = (unsigned char)unpacker.data.via.u64;
}
//********************************************************************
// SR04Read function - returns calculated Altitude and Speed if there
// is at least one captured sample since last call to the function.
//--------------------------------------------------------------------
// Return value indicates whether initialization condition was met and,
// if so, how many samples were available for calculation of resulting
// values.
//--------------------------------------------------
// NOTE: Output data structure updated ONLY if a new
// sample is available!
//====================================================================
uint SR04Read(SR04Data* SR04Sample)
{
if (0 == _SR04Init)
return -1; // Component not initialized
if (0 == _SR04Ready)
return 0; // New Sample is not ready yet!
// SR04Sample structure left "as-is"
//----------------------------------------------------------------
byte saved_ipl;
//-----------------------
uint SR04ReadyCount;
ulong SR04TimeSum;
//-----------------------
ulong SR04RecentTS;
//================================================================
// Enter Critical Section: SR04 (Input Capture) interrupt is blocked
//================================================================
SET_AND_SAVE_CPU_IPL(saved_ipl,_SR04IL);
//================================================================
// Consume current sample
//----------------------------------------------------------------
SR04ReadyCount = _SR04Ready;
SR04TimeSum = _SR04Sum;
//------------------------------
SR04RecentTS = _SR04RecentTS;
//----------------------------------------------------------------
_SR04Ready = 0; // Sample is consumed...
_SR04Sum = 0;
_SR04RecentTS = 0;
//================================================================
// Leave Critical Section
//================================================================
RESTORE_CPU_IPL(saved_ipl);
//================================================================
//================================================================
// Calculate Altitude
//----------------------------------------------------------------
SR04Sample->Altitude = ((float)SR04TimeSum) * _SR04CF;
if (SR04ReadyCount > 1)
// Several samples were aggregated, let's get average
SR04Sample->Altitude /= (float)SR04ReadyCount;
//----------------------------------------------------------------
// Update output data structure
//----------------------------------------------------------------
SR04Sample->Altitude -= _SR04Offset;
SR04Sample->TS = SR04RecentTS;
//================================================================
return SR04ReadyCount; // SR04Sample updated with new sample
}
void fct_can_pitch_eeprom(unsigned long ID, T_TYPE_ID type_ID, T_CAN_DATA* recopie, char nbr_data)
{
//LED_STAT1 = 1;
const char ubReceiveData[2] = {(recopie->data3 & 0x00FF),(recopie->data3 & 0xFF00)>>8};
int old_ipl;
// Block interruptions
SET_AND_SAVE_CPU_IPL(old_ipl, 7);
off = 0;
chinookpack_unpack_next(&unpacker,ubReceiveData,2,&off);
off = 0;
RESTORE_CPU_IPL(old_ipl);
ubPitchEepromAnswerReceive = (unsigned char)unpacker.data.via.u64;
}
void fct_can_conf(unsigned long ID, T_TYPE_ID type_ID, T_CAN_DATA* recopie, char nbr_data)
{
const char ubReceiveData[1] = {(recopie->data3 & 0x00FF)};
int old_ipl;
// Block interruptions
SET_AND_SAVE_CPU_IPL(old_ipl, 7);
off = 0;
chinookpack_unpack_next(&unpacker,ubReceiveData,2,&off);
off = 0;
RESTORE_CPU_IPL(old_ipl);
datReceive_can_conf = (int)unpacker.data.via.i64;
}
//************************************************************
void UARTRXGetStat(pUARTRXStat RXStat)
{
uint current_cpu_ipl;
//---------------------------------
// Enter UART_RX CRITICAL SECTION
//---------------------------------
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _UART_IL_RX); /* disable UART_RX interrupts */
//--------------------------------------------------------
RXStat->RecordCount = _UART_RX_RecCnt;
RXStat->FrameOverrunCount = _UART_RX_OvrCnt;
RXStat->InvalidFrameCount = _UART_RX_InvCnt;
RXStat->ErrorCount = _UART_RX_ErrCnt;
//---------------------------------
// Leave UART_RX CRITICAL SECTION
//---------------------------------
RESTORE_CPU_IPL(current_cpu_ipl);
//--------------------------
}
//*************************************************************
uint _MPLAsyncRead(MPLData* pSample)
{
//----------------------------------------------
int current_cpu_ipl;
//----------------------------------------------
uint Ready_Cnt;
ulong RawTS;
long RawData;
//==============================================
// Enter MPL/I2C CRITICAL SECTION
//----------------------------------------------
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _MPL_IL); /* disable interrupts */
//-----------------------------------------------
Ready_Cnt = _MPL_Ready;
RawTS = _MPL_DataTS;
RawData = _MPL_Data;
//-----------------------------------------------
_MPL_Ready = 0; // Sample consumed...
_MPL_Data = 0; // Data cleared
//----------------------------------------------
RESTORE_CPU_IPL(current_cpu_ipl);
//----------------------------------------------
// Leave MPL/I2C CRITICAL SECTION
//==============================================
//==============================================
// Timestamp and Altitude
//-----------------------------------------------
pSample->TS = RawTS;
pSample->Alt = ((float)RawData)*0.0625;
//==============================================
if (Ready_Cnt > 1)
// Find average of samples if multiple
// samples summed together
pSample->Alt /= (float)Ready_Cnt;
//==============================================
// Adjust normalized (averaged) Altitude to
// account for Ground level
//-----------------------------------------------
pSample->Alt = pSample->Alt - _MPL_BaseAlt;
//-----------------------------------------------
return MPL_OK; // The return code was OK
}
void fct_can_cmd(unsigned long ID, T_TYPE_ID type_ID, T_CAN_DATA* recopie, char nbr_data)
{
const char ubReceiveData[2] = {(recopie->data3 & 0x00FF),(recopie->data3 & 0xFF00)>>8};
int old_ipl, tempo=0;
// Block interruptions
SET_AND_SAVE_CPU_IPL(old_ipl, 7);
off = 0;
chinookpack_unpack_next(&unpacker,ubReceiveData,2,&off);
off = 0;
RESTORE_CPU_IPL(old_ipl);
tempo = (int)unpacker.data.via.i64;
datReceive_can_cmd[0] = tempo & 0b0001;
datReceive_can_cmd[1] = (tempo & 0b0010)>>1;
datReceive_can_cmd[2] = (tempo & 0b0100)>>2;
datReceive_can_cmd[3] = (tempo & 0b1000)>>3;
}
//************************************************************
void TMRCallBackDiscard()
{
char saved_ipl;
SET_AND_SAVE_CPU_IPL(saved_ipl,_TMRIL);
//--------------------------------------------------
// Disable/Reset CTMU (callback) interrupt
//--------------------------------------------------
_RTCIE = 0; // Disable RTCC interrupt
_RTCIF = 0; // Clear RTCC interrupt flag
//--------------------------------------------------
// Reset callback request
//--------------------------------------------------
_TMR_CallBack_TriggerTime = -1; // Infinite...
_TMR_CallBack = 0; // Reset callback address
//--------------------------------------------------
RESTORE_CPU_IPL(saved_ipl);
//---------------------------------------------------
return;
}
void fct_can_EEPROM_CONFIG_ANSWER(unsigned long ID, T_TYPE_ID type_ID, T_CAN_DATA* recopie, char nbr_data)
{
const char ubReceiveData[5] = {(recopie->data3 & 0x00FF),(recopie->data3 & 0xFF00)>>8,(recopie->data4 & 0x00FF),(recopie->data4 & 0xFF00)>>8,(recopie->data5 & 0x00FF)};
const char ubReceiveData2[2] = {(recopie->data5 & 0xFF00)>>8,(recopie->data6 & 0x00FF)};
int old_ipl;
// Block interruptions
SET_AND_SAVE_CPU_IPL(old_ipl, 7);
off = 0;
chinookpack_unpack_next(&unpacker,ubReceiveData2,2,&off);
off=0;
chinookpack_unpack_next(&unpacker,ubReceiveData,5,&off);
off = 0;
RESTORE_CPU_IPL(old_ipl);
datReceive_can_EEPROM_CONFIG_ANSWER_gear = (char)unpacker.data.via.u64;
datReceive_can_EEPROM_CONFIG_ANSWER_mat = unpacker.data.via.dec;
}
/* main ***********************************************************************/
int main (void){
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
/* Initialize two CAN led diodes */
TRISAbits.TRISA0 = 0; LATAbits.LATA0 = 0;
TRISAbits.TRISA1 = 0; LATAbits.LATA1 = 1;
#define CAN_RUN_LED LATAbits.LATA0
#define CAN_ERROR_LED LATAbits.LATA1
/* Initialize other LED diodes for RPDO */
TRISAbits.TRISA2 = 0; LATAbits.LATA2 = 0;
TRISAbits.TRISA3 = 0; LATAbits.LATA3 = 0;
TRISAbits.TRISA4 = 0; LATAbits.LATA4 = 0;
TRISAbits.TRISA5 = 0; LATAbits.LATA5 = 0;
TRISAbits.TRISA6 = 0; LATAbits.LATA6 = 0;
TRISAbits.TRISA7 = 0; LATAbits.LATA7 = 0;
/* Configure Oscillator */
/* Fosc = Fin*M/(N1*N2), Fcy=Fosc/2 */
/* Fosc = 8M*24(2*2) = 48MHz -> Fcy = 24MHz */
PLLFBD=22; /* M=24 */
CLKDIVbits.PLLPOST=0; /* N1=2 */
CLKDIVbits.PLLPRE=0; /* N2=2 */
OSCTUN=0; /* Tune FRC oscillator, if FRC is used */
while(OSCCONbits.LOCK!=1) ClrWdt(); /* wait for PLL to lock */
/* Verify, if OD structures have proper alignment of initial values */
if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) while(1) ClrWdt();
if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) while(1) ClrWdt();
if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) while(1) ClrWdt();
/* initialize EEPROM */
/* (not implemented) */
/* increase variable each startup. Variable is stored in eeprom. */
OD_powerOnCounter++;
while(reset != CO_RESET_APP){
/* CANopen communication reset - initialize CANopen objects *******************/
static uint16_t timer1msPrevious;
CO_ReturnError_t err;
/* disable timer and CAN interrupts, turn on red LED */
CO_TMR_ISR_ENABLE = 0;
CO_CAN_ISR_ENABLE = 0;
CAN_RUN_LED = 0;
CAN_ERROR_LED = 1;
/* Initialize digital outputs */
TRISAbits.TRISA2 = 0; LATAbits.LATA2 = 0;
TRISAbits.TRISA3 = 0; LATAbits.LATA3 = 0;
TRISAbits.TRISA4 = 0; LATAbits.LATA4 = 0;
TRISAbits.TRISA5 = 0; LATAbits.LATA5 = 0;
TRISAbits.TRISA6 = 0; LATAbits.LATA6 = 0;
TRISAbits.TRISA7 = 0; LATAbits.LATA7 = 0;
OD_writeOutput8Bit[0] = 0;
OD_writeOutput8Bit[1] = 0;
/* initialize CANopen */
err = CO_init();
if(err != CO_ERROR_NO){
while(1) ClrWdt();
/* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
}
/* start CAN */
CO_CANsetNormalMode(ADDR_CAN1);
/* Configure Timer interrupt function for execution every 1 millisecond */
CO_TMR_CON = 0;
CO_TMR_TMR = 0;
CO_TMR_PR = CO_FCY - 1; /* Period register */
CO_TMR_CON = 0x8000; /* start timer (TON=1) */
CO_timer1ms = 0;
CO_TMR_ISR_FLAG = 0; /* clear interrupt flag */
CO_TMR_ISR_PRIORITY = 3; /* interrupt - set lower priority than CAN */
CO_TMR_ISR_ENABLE = 1; /* enable interrupt */
/* Configure CAN1 Interrupt (Combined) */
CO_CAN_ISR_FLAG = 0; /* CAN1 Interrupt - Clear flag */
CO_CAN_ISR_PRIORITY = 5; /* CAN1 Interrupt - Set higher priority than timer */
CO_CAN_ISR_ENABLE = 1; /* CAN1 Interrupt - Enable interrupt */
reset = CO_RESET_NOT;
timer1msPrevious = CO_timer1ms;
while(reset == CO_RESET_NOT){
/* loop for normal program execution ******************************************/
uint16_t timer1msCopy, timer1msDiff;
static uint16_t TMR_TMR_PREV = 0;
timer1msCopy = CO_timer1ms;
timer1msDiff = timer1msCopy - timer1msPrevious;
timer1msPrevious = timer1msCopy;
ClrWdt();
/* calculate cycle time for performance measurement */
uint16_t t0 = CO_TMR_TMR;
uint16_t t = t0;
if(t >= TMR_TMR_PREV){
t = t - TMR_TMR_PREV;
t = (timer1msDiff * 100) + (t / (CO_FCY / 100));
}
else if(timer1msDiff){
t = TMR_TMR_PREV - t;
t = (timer1msDiff * 100) - (t / (CO_FCY / 100));
}
else t = 0;
OD_performance[ODA_performance_mainCycleTime] = t;
if(t > OD_performance[ODA_performance_mainCycleMaxTime])
OD_performance[ODA_performance_mainCycleMaxTime] = t;
TMR_TMR_PREV = t0;
/* CANopen process */
reset = CO_process(CO, timer1msDiff);
CAN_RUN_LED = LED_GREEN_RUN(CO->NMT);
CAN_ERROR_LED = LED_RED_ERROR(CO->NMT);
ClrWdt();
/* (not implemented) eeprom_process(&eeprom); */
}
}
/* program exit ***************************************************************/
/* save variables to eeprom */
RESTORE_CPU_IPL(7); /* disable interrupts */
CAN_RUN_LED = 0;
/* CAN_ERROR_LED = 0; */
/* (not implemented) eeprom_saveAll(&eeprom); */
CAN_ERROR_LED = 1;
/* delete CANopen object from memory */
CO_delete();
/* reset */
return 0;
}
//*************************************************************
uint _MPUAsyncRead(MPU_CB* pCB, MPUData* pSample)
{
//----------------------------------------------
uint Ready_Cnt;
_MPURawData RawData;
//----------------------------------------------
int current_cpu_ipl;
//----------------------------------------------
//==============================================
// Enter MPU/I2C CRITICAL SECTION
//----------------------------------------------
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _MPU_IL); /* disable interrupts */
//---------------------------------
RawData.Temp = pCB->_MPU_Sample.Temp;
//---------------------------------
RawData.AX = pCB->_MPU_Sample.AX;
RawData.AY = pCB->_MPU_Sample.AY;
RawData.AZ = pCB->_MPU_Sample.AZ;
//---------------------------------
RawData.GX = pCB->_MPU_Sample.GX;
RawData.GY = pCB->_MPU_Sample.GY;
RawData.GZ = pCB->_MPU_Sample.GZ;
//-----------------------------------------------
Ready_Cnt = pCB->_MPU_Ready;
//-----------------------------------------------
pCB->_MPU_Ready = 0; // Sample consumed...
//----------------------------------------------
// Leave MPU/I2C CRITICAL SECTION
//==============================================
RESTORE_CPU_IPL(current_cpu_ipl);
//==============================================
// Set sample timestamp
//----------------------------------------------
pSample->TS = TMRGetTS();
//==============================================
// Adjust Sample Weight to account for multiple samples
//----------------------------------------------
float Weight;
if (Ready_Cnt > 1)
Weight = 1.0/(float)Ready_Cnt;
else
Weight = 1.0;
//----------------------------------------------
// Process collected sample
//----------------------------------------------
// Temperature (C)
//-----------------------------------------------
pSample->Temp = (Weight * RawData.Temp - pCB->_MPU_Temp_OffsetTo0)
* pCB->_MPU_Temp_Sensitivity;
//-----------------------------------------------
// Acceleration (G)
//-----------------------------------------------
VectorSet (
Weight * RawData.AX * pCB->_MPU_Accl_Sensitivity,
Weight * RawData.AY * pCB->_MPU_Accl_Sensitivity,
Weight * RawData.AZ * pCB->_MPU_Accl_Sensitivity,
//---------
&pSample->A
);
//-----------------------------------------------
// Gyroscopes (Rad/sec)
//-----------------------------------------------
VectorSet (
Weight * RawData.GX * pCB->_MPU_Gyro_Sensitivity,
Weight * RawData.GY * pCB->_MPU_Gyro_Sensitivity,
Weight * RawData.GZ * pCB->_MPU_Gyro_Sensitivity,
//---------
&pSample->G
);
//-----------------------------------------------
// Applying calibration and tuning parameters
//-----------------------------------------------
_MPU_ApplyCalibration(pCB, pSample);
//-----------------------------------------------
return MPU_OK; // The return code was OK
}
//************************************************************
// Synchronous READ - I2C API (visible externally) component
//************************************************************
uint I2CSyncRead( uint I2Cx,
byte DevAddr,
byte Register,
byte* Buffer,
uint BufLen )
{
if (!_I2C_Init) return I2CRC_NRDY;
//=========================================================
// Obtain references to proper Control Blocks and registers
//---------------------------------------------------------
_I2C_CB* pCB;
if ( NULL == (pCB = I2CpCB(I2Cx)) ) return I2CRC_NOTA;
//---------------------------------------------------------
I2C_CONBITS* pCON = I2CpCON(pCB);
I2C_STATBITS* pSTAT = I2CpSTAT(pCB);
//=========================================================
// Validate run-time conditions
//---------------------------------------------------------
uint RC;
BYTE CPU_IPL;
uint RetryCount = 0;
//---------------------------------------------------------
// Enter I2C (and related modules) CRITICAL SECTION
//---------------------------------------------------------
Retry:
RC = I2CRC_OK;
//------------------------------------
SET_AND_SAVE_CPU_IPL(CPU_IPL, _I2C_IL);
{
if (I2CRC_OK == (RC=I2CGetStatus(pCB, pCON, pSTAT)))
//---------------------------------------------------------
// Set Flag indicating Synchronous operation is in progress
//---------------------------------------------------------
pCB->_I2C_SBSY = 1; // Should be cleared at exit
}
//---------------------------------------------------------
// Leave I2C CRITICAL SECTION
//---------------------------------------------------------
RESTORE_CPU_IPL(CPU_IPL);
//=========================================================
switch (RC)
{
case I2CRC_OK:
break; // Run-time conditions are OK
//-------------------------------------------
case I2CRC_ABSY:
case I2CRC_SBSY:
case I2CRC_BUSY:
// Situation could be helped by delay/retry
if (RetryCount < I2C_BUSYRetry)
{
TMRDelayTicks(1); // Small delay ~125 usec
RetryCount++;
goto Retry; // Attempt retry
}
else; // Fall through to "default"
//-------------------------------------------
default:
return RC; // Run-time conditions are not met
}
//=========================================================
//---------------------------------------------------------
// Event sequence to accomplish simple READ from the target
// slave device in the MASTER mode:
// 1. Assert a Start condition on SDAx and SCLx.
// 2. Send the I2C device address byte to the slave with a
// read indication ("1" in the LSB of address)
// 3. Wait for and verify an Acknowledge from the slave.
// 4. Assert receiving state (RCEN = 1).
// 5. Receive byte and send ACK to the slave.
// 6. Repeat steps 4 and 5 for every byte in a message,
// except the last - after receiving last byte send NACK.
// 7. Generate a Stop condition on SDAx and SCLx.
//---------------------------------------------------------
// Prepare READ and WRITE forms of device address
byte i2cAddrW = DevAddr & 0xFE; // Set WRITE cond.
byte i2cAddrR = DevAddr | 0x01; // Set READ cond.
//---------------------------------------------------------
RC = I2CRC_OK;
//---------------------------------------------------------
I2CStart(pCB); // Signal START on SDA and SCL pins
//---------------------------------------------------------
// Send Device WRITE address
//---------------------------------------------------------
RC = I2CMasterWriteByte(pCB, i2cAddrW);
if (RC != I2CRC_OK) goto Finally;
//---------------------------------------------------------
// Send Register address (as data))
//---------------------------------------------------------
RC = I2CMasterWriteByte(pCB, Register);
if (RC != I2CRC_OK) goto Finally;
//---------------------------------------------------------
I2CReStart(pCB); // Signal Repeated-START on SDA and SCL pins
//---------------------------------------------------------
// Send Device READ address
//---------------------------------------------------------
RC = I2CMasterWriteByte(pCB, i2cAddrR);
if (RC != I2CRC_OK) goto Finally;
//---------------------------------------------------------
// Receive data
//---------------------------------------------------------
pSTAT->I2COV = 0; // Clear receive OVERFLOW bit, if any
//---------------------------------------------------------
uint BufPos;
uint BufCnt = BufLen;
//---------------------------------------------------------
for (BufPos = 0; BufPos < BufLen; BufPos++)
{
BufCnt--; // Used as a FLAG (BufCnt=0) to indicate last byte
//---------------------------------------------------------
RC = I2CMasterReadByte(pCB, Buffer, BufCnt);
if (RC != I2CRC_OK)
break; // Error...
Buffer++;
}
//---------------------------------------------------------
Finally:
I2CStop(pCB); // Signal STOP on SDA and SCL pins
//---------------------------------------------------------
pCB->_I2C_SBSY = 0; // Clear SYNC flag
return RC; // Return last Error Code...
}
int main(){
uint8_t start_r, old_IPL;
uint8_t hz50_scaler, hz5_scaler, hz1_scaler, sec;
uint32_t tick = 0;
hz50_scaler = hz5_scaler = hz1_scaler = sec = 0;
touch_init();
__delay_ms(200);
lcd_initialize(); // Initialize the LCD
motor_init();
lcd_clear();
lcd_locate(0,0);
lcd_printf("-- Ball position: --");
timers_initialize(); // Initialize timers
while (1) {
start_r = 0;
while(!start_r) {
// disable all maskable interrupts
SET_AND_SAVE_CPU_IPL(old_IPL, 7);
start_r = start;
// enable all maskable interrupts
RESTORE_CPU_IPL(old_IPL);
}
// Periodic real-time task code starts here = CENTER_X + RADIUS * cos(tick * SPEED);
// Ypos_set = CENT!!!
double pidX, pidY;
uint16_t duty_us_x, duty_us_y;
// 50Hz control task
if(hz50_scaler == 0) {
calcQEI(Xpos_set, Xpos, Ypos_set, Ypos);
// Xpos_set = CENTER_X;
// Xpos_set = CENTER_Y;
Xpos_set = CENTER_X + RADIUS * cos(tick * SPEED);
Ypos_set = CENTER_Y + RADIUS * sin(tick * SPEED);
tick++;
pidX = pidX_controller(Xpos);
pidY = pidY_controller(Ypos);
// TODO: Convert PID to motor duty cycle (900-2100 us)
// setMotorDuty is a wrapper function that calls your motor_set_duty
// implementation in flexmotor.c. The 2nd parameter expects a value
// between 900-2100 us
// duty_us_x = cap((pidX*1000.0), 2100, 900);
// duty_us_y = cap((pidY*1000.0), 2100, 900);
duty_us_x = cap((pidX + 1500), 2100, 900);
duty_us_y = cap((pidY + 1500), 2100, 900);
motor_set_duty(1, duty_us_x);
motor_set_duty(2, duty_us_y+100);
// setMotorDuty(MOTOR_X_CHAN, duty_us_x);
// setMotorDuty(MOTOR_Y_CHAN, duty_us_y);
}
// 5Hz display task
if(hz5_scaler == 0) {
// lcd_locate(0,1);
// lcd_printf("Xp=%.1f,Yp=%.1f", Xpos, Ypos);
// lcd_locate(0,2);
// lcd_printf("X*=%.1f, Y*=%.1f", Xpos_set, Ypos_set);
// lcd_locate(0,3);
// lcd_printf("pX=%.1f,pY=%.1f", pidX, pidY);
// lcd_locate(0,4);
// lcd_printf("dx=%u, dY=%u", duty_us_x, duty_us_y);
//
if(deadline_miss >= 1) {
lcd_locate(0,6);
lcd_printf("%4d d_misses!!!", deadline_miss);
}
}
// 1Hz seconds display task
if(hz1_scaler == 0) {
lcd_locate(0,7);
lcd_printf("QEI: %5u", getQEI());
sec++;
}
hz50_scaler = (hz50_scaler + 1) % 2;
hz5_scaler = (hz5_scaler + 1) % 20;
hz1_scaler = (hz1_scaler + 1) % 100;
start = 0;
}
return 0;
}
//============================================================
// Asynchronous I2C API (visible externally) component
//============================================================
// <editor-fold defaultstate="collapsed" desc="uint I2CAsyncStart(uint SubscrID)">
uint I2CAsyncStart(uint I2Cx, I2CAsyncRqst* Rqst)
{
if (!_I2C_Init) return I2CRC_NRDY;
//=========================================================
// Obtain references to proper Control Blocks and registers
//---------------------------------------------------------
_I2C_CB* pCB;
if ( NULL == (pCB = I2CpCB(I2Cx)) ) return I2CRC_NOTA;
//---------------------------------------------------------
I2C_CONBITS* pCON = I2CpCON(pCB);
I2C_STATBITS* pSTAT = I2CpSTAT(pCB);
//=========================================================
// Validate run-time conditions
//---------------------------------------------------------
int i;
//---------------------------------------------------------
uint RC = I2CRC_OK;
BYTE CPU_IPL;
//---------------------------------------------------------
// Enter I2C (and related modules) CRITICAL SECTION
//---------------------------------------------------------
SET_AND_SAVE_CPU_IPL(CPU_IPL, _I2C_IL);
{
//-----------------------------------------------------------
// New request...
//-----------------------------------------------------------
if (pCB->_I2C_CallBack)
// There is an active request being processed -
// several checks need to be implemented...
{
//------------------------------------------------------------
// First, check whether this request already active
//---------------------------------------------------------
if ( pCB->_I2C_CallBack == Rqst->CallBack
&& pCB->_I2C_CallBackArg == Rqst->CallBackArg )
{
// Duplicate request...
RC = I2CRC_RQSTA;
goto Finally;
}
//-------------------------------------------------------
// Second, check whether this request already queued
//-------------------------------------------------------
for (i = 0; i < I2CMaxAsyncRqst; i++)
{
if ( pCB->_I2CRqstQueue[i].CallBack == Rqst->CallBack
&&
pCB->_I2CRqstQueue[i].CallBackArg == Rqst->CallBackArg )
{
// Request is already in the queue...
RC = I2CRC_OK;
goto Finally;
}
}
//-------------------------------------------------------
// Third, try to add this request to the queue
//-------------------------------------------------------
for (i = 0; i < I2CMaxAsyncRqst; i++)
{
if ( NULL == pCB->_I2CRqstQueue[i].CallBack )
{
// Free slot found! Add request to the queue
pCB->_I2CRqstQueue[i].CallBack = Rqst->CallBack;
pCB->_I2CRqstQueue[i].CallBackArg = Rqst->CallBackArg;
//-------------------------------------------
RC = I2CRC_OK;
goto Finally;
}
}
// No free slots available in the queue...
RC = I2CRC_QFULL;
goto Finally;
}
else
// No Active Asynchronous requests now - let's try
// to innitiate a new Asynchronous request
{
// Frist, let's check bus condition
if (0 == pSTAT->P)
{
// Bus is not idle as the last status is not "Stop"
RC = I2CRC_BUSY; // // Bus is busy...
goto Finally;
}
//--------------------------------------------
// Bus condition validated, proceed with activating
// Async operation
//---------------------------------------------------------
// Store in the I2C Library control block address of the
// callback function of the current bus owner and
// respective callback parameter.
//---------------------------------------------------------
pCB->_I2C_CallBack = Rqst->CallBack;
// NOTE: non-NULL value of pCB->_I2C_CallBack field
// also serves as a FLAG indicating that bus is
// busy and tested for in I2CGetStatus(...) routine
pCB->_I2C_CallBackArg = Rqst->CallBackArg;
//--------------------------------------------------------
I2CSetIF(I2Cx, 0); // Clear I2C Master interrupt flag
I2CSetIE(I2Cx, 1); // Enable I2C Master interrupt
//--------------------------------------------------------
// Initiate Start on I2C bus
pCON->SEN = 1;
// NOTE: because I2C bus is being allocated to the client,
// from now until the asynchronous operation completes
// I2C interrupts will be routed to client's callback
// routine.
RC = I2CRC_OK;
goto Finally;
}
}
Finally:
//---------------------------------------------------------
// Leave I2C CRITICAL SECTION
//---------------------------------------------------------
RESTORE_CPU_IPL(CPU_IPL);
//=========================================================
return RC;
}
//*************************************************************
uint _MPUAsyncRead(MPUData* pSample)
{
//----------------------------------------------
uint Ready_Cnt;
_MPURawData RawData;
//----------------------------------------------
int current_cpu_ipl;
//----------------------------------------------
//==============================================
// Enter MPU/I2C CRITICAL SECTION
//----------------------------------------------
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _MPU_IL); /* disable interrupts */
//-----------------------------------------------
pSample->Count = _MPU_Count;
//---------------------------------
RawData.Temp = _MPU_Sample.Temp;
//---------------------------------
RawData.AX = _MPU_Sample.AX;
RawData.AY = _MPU_Sample.AY;
RawData.AZ = _MPU_Sample.AZ;
//---------------------------------
RawData.GX = _MPU_Sample.GX;
RawData.GY = _MPU_Sample.GY;
RawData.GZ = _MPU_Sample.GZ;
//-----------------------------------------------
Ready_Cnt = _MPU_Ready;
//-----------------------------------------------
_MPU_Ready = 0; // Sample consumed...
//----------------------------------------------
// Leave MPU/I2C CRITICAL SECTION
//==============================================
RESTORE_CPU_IPL(current_cpu_ipl);
//==============================================
// Set sample timestamp
//----------------------------------------------
pSample->TS = TMRGetTS();
//==============================================
float TDev;
//----------------------------------------------
// Adjust Sample Weight to account for multiple samples
//----------------------------------------------
float Weight;
if (Ready_Cnt > 1)
Weight = 1.0/(float)Ready_Cnt;
else
Weight = 1.0;
//----------------------------------------------
// Process collected sample
//----------------------------------------------
// Temperature (C) (will be used in subsequent
// temperature compensation calculation)
//-----------------------------------------------
pSample->Temp = (Weight * RawData.Temp - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity;
//-----------------------------------------------
// Acceleration (G)
//-----------------------------------------------
TDev = pSample->Temp - _MPU_Accl_BaseTemp;
//-----------------------------------------------
VectorSet (
(Weight * RawData.AX - (_MPU_Accl_XOffset + _MPU_Accl_XSlope*TDev)) * _MPU_Accl_Sensitivity,
(Weight * RawData.AY - (_MPU_Accl_YOffset + _MPU_Accl_YSlope*TDev)) * _MPU_Accl_Sensitivity,
(Weight * RawData.AZ - (_MPU_Accl_ZOffset + _MPU_Accl_ZSlope*TDev)) * _MPU_Accl_Sensitivity,
&pSample->A
);
//-----------------------------------------------
// Gyroscopes (Rad/sec)
//-----------------------------------------------
TDev = pSample->Temp - _MPU_Gyro_BaseTemp;
//-----------------------------------------------
VectorSet (
(Weight * RawData.GX - (_MPU_Gyro_XOffset + _MPU_Gyro_XSlope*TDev)) * _MPU_Gyro_Sensitivity,
(Weight * RawData.GY - (_MPU_Gyro_YOffset + _MPU_Gyro_YSlope*TDev)) * _MPU_Gyro_Sensitivity,
(Weight * RawData.GZ - (_MPU_Gyro_ZOffset + _MPU_Gyro_ZSlope*TDev)) * _MPU_Gyro_Sensitivity,
&pSample->G
);
//-----------------------------------------------
return MPU_OK; // The return code was OK
}
//=============================================================
uint _MPUCalibrateAsync()
{
if (!_MPU_Init)
return MPU_NOTINIT; // Not initialized...
//-----------------------
if (0 == _MPU_Async)
return MPU_NACT; // Asynchronous read not active...
//=========================================================
// Local Variables
//---------------------------------------------------------
ulong Alarm = TMRSetAlarm(2000); // Set Alarm time 2 sec
// into the future.
//---------------------------------------------------------
uint Ready_Cnt;
_MPURawData RawData;
//----------------------------------------------
int current_cpu_ipl;
//=========================================================
//=========================================================
// Reset Gyro offsets...
//---------------------------------------------------------
_MPU_Gyro_XOffset = 0;
_MPU_Gyro_YOffset = 0;
_MPU_Gyro_ZOffset = 0;
//---------------------------------------------------------
// To collect averages we would like to "watch" MPU for at
// least 2 seconds; number of samples will be variable
// depending on the value of RateDiv
//---------------------------------------------------------
TMRWaitAlarm(Alarm);
//---------------------------------------------------------
//=========================================================
// Enter MPU/I2C CRITICAL SECTION
//---------------------------------------------------------
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _MPU_IL); /* disable interrupts */
//---------------------------------------------------------
RawData.Temp = _MPU_Sample.Temp;
//---------------------------------
RawData.AX = _MPU_Sample.AX;
RawData.AY = _MPU_Sample.AY;
RawData.AZ = _MPU_Sample.AZ;
//---------------------------------
RawData.GX = _MPU_Sample.GX;
RawData.GY = _MPU_Sample.GY;
RawData.GZ = _MPU_Sample.GZ;
//---------------------------------------------------------
Ready_Cnt = _MPU_Ready;
//---------------------------------------------------------
_MPU_Ready = 0; // Sample consumed...
//---------------------------------------------------------
// Leave MPU/I2C CRITICAL SECTION
//=========================================================
RESTORE_CPU_IPL(current_cpu_ipl);
//=========================================================
//=========================================================
// Adjust Sample Weight to account for multiple samples
//---------------------------------------------------------
float Weight;
if (Ready_Cnt > 1)
Weight = 1.0/(float)Ready_Cnt;
else
Weight = 1.0;
//=========================================================
//=========================================================
// Process collected sample
//---------------------------------------------------------
// Gyro offset is calculated in LSB units
//---------------------------------------
_MPU_Gyro_XOffset = (float)RawData.GX * Weight;
_MPU_Gyro_YOffset = (float)RawData.GY * Weight;
_MPU_Gyro_ZOffset = (float)RawData.GZ * Weight;
//---------------------------------------
// Base temperature converted to degrees C
//---------------------------------------
_MPU_Gyro_BaseTemp = ((float)RawData.Temp * Weight - _MPU_Temp_OffsetTo0)
* _MPU_Temp_Sensitivity;
//*********************************************************
return MPU_OK;
}
//*************************************************************
uint MPUAsyncAdjustAccZBase(float Incl)
{
if (0 == _MPU_Async)
return MPU_NACT; // Asynchronous read not active...
//--------------------------------------------------
TMRDelay(2000); // Wait 2,000 msec (2 second) to
// accumulate in the buffer enough
// samples for for solid averaging
//----------------------------------------------
uint Ready_Cnt;
_MPURawData RawData;
//----------------------------------------------
int current_cpu_ipl;
//----------------------------------------------
//==============================================
// Enter MPU/I2C CRITICAL SECTION
//----------------------------------------------
SET_AND_SAVE_CPU_IPL(current_cpu_ipl, _MPU_IL); /* disable interrupts */
//-----------------------------------------------
RawData.Temp = _MPU_Sample.Temp;
//---------------------------------
RawData.AX = _MPU_Sample.AX;
RawData.AY = _MPU_Sample.AY;
RawData.AZ = _MPU_Sample.AZ;
//---------------------------------
RawData.GX = _MPU_Sample.GX;
RawData.GY = _MPU_Sample.GY;
RawData.GZ = _MPU_Sample.GZ;
//-----------------------------------------------
Ready_Cnt = _MPU_Ready;
//-----------------------------------------------
_MPU_Ready = 0; // Sample consumed...
//----------------------------------------------
// Leave MPU/I2C CRITICAL SECTION
//==============================================
RESTORE_CPU_IPL(current_cpu_ipl);
//==============================================
// Adjust Sample Weight to account for multiple samples
//----------------------------------------------
float Weight;
if (Ready_Cnt > 1)
Weight = 1.0/(float)Ready_Cnt;
else
Weight = 1.0;
//----------------------------------------------
// Process collected sample
//----------------------------------------------
// Temperature (C) (will be used in subsequent
// temperature compensation calculation)
//-----------------------------------------------
float Temp = (Weight * RawData.Temp - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity;
//-----------------------------------------------
// Acceleration (G)
//-----------------------------------------------
float TDev = Temp - _MPU_Accl_BaseTemp;
//-----------------------------------------------
// AccZBase = (Weight * RawData.AZ - _MPU_Accl_ZOffset - _MPU_Accl_ZSlope*TDev) * _MPU_Accl_Sensitivity;
// NOTE: Incl is the cosine of the angle between Earth gravity
// ( -1G given given Z-axis orientation to be "down"), thus
// Expected AccZ => -Incl; AccZ should appear on the right side
// with the "-" sign and as two "-" becomes "+" we can use Incl
// directly in the formula below
_MPU_Accl_ZOffset = Incl/_MPU_Accl_Sensitivity + Weight * RawData.AZ - _MPU_Accl_ZSlope*TDev;
//-----------------------------------------------
return MPU_OK; // The return code was OK
}
void ReadCTMU( void )
{
int current_ipl;
int i;
int j;
volatile unsigned int tempADch;
tempADch = AD1CHS0; //store the current A/D mux channel selected
AD1CON1 = 0x0000; // Unsigned integer format
AD1CSSL = 0x0000;
AD1CON3 = 0x0002;
AD1CON2 = 0x0000;
AD1CON1bits.ADON = 1; // Start A/D in continuous mode
for(i = 0; i < NUM_TOUCHPADS; i++)
{
// Get the raw sensor reading.
AD1CHS0 = STARTING_ADC_CHANNEL + buttonIndex; //select A/D channel
// Make sure touch circuit is completely discharged
IFS0bits.AD1IF = 0;
AD1CON1bits.DONE = 0;
AD1CON1bits.SAMP = 1; // Manually start the conversion
// Wait for the A/D converter to begin sampling
Nop(); Nop(); Nop(); Nop(); Nop(); Nop(); Nop(); Nop();
CTMUCONbits.IDISSEN = 1; // Drain any charge on the circuit
Nop(); Nop(); Nop(); Nop(); Nop();
CTMUCONbits.IDISSEN = 0;
Nop(); Nop(); Nop(); Nop(); Nop();
IFS0bits.AD1IF = 0;
AD1CON1bits.SAMP = 0;
while(!IFS0bits.AD1IF); // Wait for the A/D conversion to finish
// Note: This A/D conversion not used.
// A/D mux must connect to the channel
// for CTMU to drain charge
// Charge touch circuit
// Since the charge is time dependent, set the CPU priority such
// that we will not be interrupted during the read.
SET_AND_SAVE_CPU_IPL( current_ipl, 7 );
IFS0bits.AD1IF = 0;
AD1CON1bits.SAMP = 1; // Manually start the conversion
CTMUCONbits.EDG2STAT = 0; // Make sure edge2 is 0
CTMUCONbits.EDG1STAT = 1; // Set edge1 - Start Charge
for (j = 0; j < CHARGE_TIME_COUNT; j++); // Delay for CTMU charge time
CTMUCONbits.EDG1STAT = 0; // Clear edge1 - Stop Charge
// Re-enable interrupts.
RESTORE_CPU_IPL( current_ipl );
IFS0bits.AD1IF = 0;
AD1CON1bits.SAMP = 0;
while(!IFS0bits.AD1IF); // Wait for the A/D conversion to finish
value = ADC1BUF0; // Read the value from the A/D conversion
//Discharge the touch circuit
IFS0bits.AD1IF = 0;
AD1CON1bits.SAMP = 1; // Manually start the conversion
// Wait for A/D conversion to begin
Nop(); Nop(); Nop(); Nop(); Nop(); Nop(); Nop(); Nop();
CTMUCONbits.IDISSEN = 1; // Drain any charge on the circuit
Nop(); Nop(); Nop(); Nop(); Nop();
CTMUCONbits.IDISSEN = 0; // End charge drain
Nop(); Nop(); Nop(); Nop();
IFS0bits.AD1IF = 0;
AD1CON1bits.SAMP = 0; // Perform conversion
while(!IFS0bits.AD1IF); // Wait for the A/D conversion to finish
IFS0bits.AD1IF = 0;
AD1CON1bits.DONE = 0; // Note: The A/D conversion not used
// A/D mux must connect to the channel
// for CTMU to drain charge
//End of CTMU read
bigVal = value * 16; // bigVal is current measurement left shifted 4 bits
smallAvg = average[buttonIndex] / 16; // smallAvg is the current average right shifted 4 bits
rawCTMU[buttonIndex] = bigVal; // raw array holds the most recent bigVal values
// On power-up, reach steady-state readings
if (first > 0)
{
first--;
average[buttonIndex] = bigVal;
SetNextChannel();
break;
}
// Is a keypad button pressed or released?
if (bigVal < (average[buttonIndex] - trip[buttonIndex]))
{
// Pressed
switch(buttonIndex)
{
case TOUCHPAD1: buttons.BTN1 = 1; break;
case TOUCHPAD2: buttons.BTN2 = 1; break;
case TOUCHPAD3: buttons.BTN3 = 1; break;
case TOUCHPAD4: buttons.BTN4 = 1; break;
case TOUCHPAD5: buttons.BTN5 = 1; break;
}
}
else if (bigVal > (average[buttonIndex] - trip[buttonIndex] + hyst[buttonIndex]))
{
// Released
switch(buttonIndex)
{
case TOUCHPAD1: buttons.BTN1 = 0; break;
case TOUCHPAD2: buttons.BTN2 = 0; break;
case TOUCHPAD3: buttons.BTN3 = 0; break;
case TOUCHPAD4: buttons.BTN4 = 0; break;
case TOUCHPAD5: buttons.BTN5 = 0; break;
}
}
// Implement quick-release for a released button
if (bigVal > average[buttonIndex])
{
average[buttonIndex] = bigVal; // If raw is above Average, reset to high average.
}
// Average in the new value. Always Average (all buttons)
// Counting 0..8 has effect of every 9th count cycling the next button.
// Counting 0..4 will average faster and also can use 0..4*m, m=0,1,2,3..
if(i == 0)
{
if (AvgIndex < AVG_DELAY)
{
AvgIndex++;
}
else
{
AvgIndex = 0;
}
}
if (AvgIndex == AVG_DELAY)
{
// Average in raw value.
average[buttonIndex] = average[buttonIndex] + (value - smallAvg);
}
// Determine next sensor to test.
SetNextChannel();
}
if((screenState == SCREEN_GRAPH) || (screenState == SCREEN_CAPTURE))
{
// Read the potentiometer and store it for the demo.
GraphReadPotentiometer();
}
#ifdef USE_TOUCHPAD_STATE_MACHINE
GestureStateMachine();
#endif
AD1CHS0 = tempADch; //restore A/D channel select
}