static uint8_t ReadSUMD(void) {
int res;
uint8_t resRx;
int i;
uint8_t rssi=0; /* Received Signal Strength Indicator, not used as SUMD does not provide this */
uint8_t rx_count=0; /* counter of received packets, will be incremented for each received packet */
uint16_t channel_count; /* number of received channels in data packet */
uint16_t channels[SUMD_MAX_CHANNELS]; /* here the channel data get stored */
uint8_t ch;
res = 1; /* preset to accumulating */
while(res==1){
resRx=SUMDRx_RecvChar(&ch);
if (resRx==ERR_RXEMPTY) {
break;
}
if (resRx!=ERR_OK) { /* general error */
SUMDRx_ClearRxBuf(); /* clear RX buffer */
break;
}
res = sumd_decode(ch, &rssi, &rx_count, &channel_count, &channels[0], sizeof(channels)/sizeof(channels[0]));
/* @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */
}
if (res!=0) {
return ERR_FAILED;
}
/* copy data */
FRTOS1_taskENTER_CRITICAL();
memcpy(REMOTE_channels, channels, sizeof(REMOTE_channels));
FRTOS1_taskEXIT_CRITICAL();
return ERR_OK;
}
RefStateType REF_GetCalibData(void){
FRTOS1_taskENTER_CRITICAL();
SensorCalibMinMax = *(SensorCalibT*)NVMC_GetReflectanceData();
if ((void*)&SensorCalibMinMax == NULL){
SHELL_SendString("No calibration data found");
FRTOS1_taskEXIT_CRITICAL();
return (RefStateType) REF_STATE_NOT_CALIBRATED;
}
else{
SHELL_SendString("Calibration data loaded");
FRTOS1_taskEXIT_CRITICAL();
return (RefStateType) REF_STATE_READY;
}
return (RefStateType) REF_STATE_INIT;
}
/*
* read a byte of data from the ps2 device. Ignores parity.
*/
unsigned char PS2_read(unsigned char * data)
{
*data = 0x00;//En esta variable se van a guardar los datos
unsigned char data_paridad;
unsigned char i;
unsigned char bit = 0x01;
unsigned char parity_recvd;
unsigned char parity_calc = 1;
FRTOS1_taskENTER_CRITICAL();
//Estado idle
//PS2_PIN_clock debe estar continuamente en alto por al menos 50 microsegundos antes de
//que el mouse pueda comenzar a transmitir.
BitClock_SetDir((bool)INPUT);
BitData_SetDir((bool)INPUT);
WAIT1_Waitus(50);
//El mouse debe escribir los datos mientras la línea PS2_PIN_clock está en alto y el host
//los leerá mientras PS2_PIN_clock está en bajo.
while (BitClock_GetVal() == 1U);
WAIT1_Waitus(5);
while (BitClock_GetVal() == 0U); // Se come el bit de start
for (i=0; i < 8; i++)
{
//Se espera a que el clk este en bajo para poder leer el dato
//Se pasan los datos recibidos al buffer
while (BitClock_GetVal() == 1U);
if (BitData_GetVal() == 1U)
{
*data = *data | bit;
}
while (BitClock_GetVal() == 0U);
bit = bit << 1;
}
// Se recibe el bit de paridad
while (BitClock_GetVal() == 1U);
parity_recvd = BitData_GetVal();
while (BitClock_GetVal() == 0U);
// Se recibe el bit de stop
while (BitClock_GetVal() == 1U);
while (BitClock_GetVal() == 0U);
BitClock_SetDir((bool)OUTPUT);
BitClock_ClrVal(); //Poner en espera los datos entrantes
FRTOS1_taskEXIT_CRITICAL();
data_paridad = *data;
//Calcula la paridad
for (i=0; i < 8; i++)
{
parity_recvd = parity_recvd ^ (data_paridad & 0x01);
data_paridad = data_paridad >> 1;
}
if (parity_recvd != parity_calc){
//Si se recibe un valor con la paridad incorrecta
return 0; //se devuelve 0
} else {
return 1;
}
}
void REF_SaveCalibData(void){
FRTOS1_taskENTER_CRITICAL();
if(NVMC_SaveReflectanceData(&SensorCalibMinMax, sizeof(SensorCalibMinMax)) == ERR_OK){
SHELL_SendString("Calib ok");
}else{
SHELL_SendString("An error occurred");
}
FRTOS1_taskEXIT_CRITICAL();
}
/*!
* \brief Measures the time until the sensor discharges
* \param raw Array to store the raw values.
* \return ERR_OVERFLOW if there is a timeout, ERR_OK otherwise
*/
static void REF_MeasureRaw(SensorTimeType raw[REF_NOF_SENSORS]) {
uint8_t cnt; /* number of sensor */
uint8_t i;
RefCnt_TValueType timerVal;
LED_IR_On(); /* IR LED's on */
WAIT1_Waitus(200);
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorFctArray[i].SetOutput(); /* turn I/O line as output */
SensorFctArray[i].SetVal(); /* put high */
raw[i] = MAX_SENSOR_VALUE;
}
WAIT1_Waitus(50); /* give at least 10 us to discharge the capacitor */
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorFctArray[i].SetInput(); /* turn I/O line as input */
}
FRTOS1_taskENTER_CRITICAL();
(void)RefCnt_ResetCounter(timerHandle); /* reset timer counter */
do {
cnt = 0;
timerVal = RefCnt_GetCounterValue(timerHandle);
for(i=0;i<REF_NOF_SENSORS;i++) {
if (raw[i]==MAX_SENSOR_VALUE) { /* not measured yet? */
if ((SensorFctArray[i].GetVal()==0)){// || (timerVal > 0x2000)) {
raw[i] = timerVal;
}
}
else { /* have value */
cnt++;
}
}
if (timerVal > 0x3000){ //9375) {/*5ms*/
for(i=0;i<REF_NOF_SENSORS;i++) {
if (raw[i]==MAX_SENSOR_VALUE) { /* not measured yet? */
raw[i] = timerVal;
}
}
break;
}
} while(cnt!=REF_NOF_SENSORS);
FRTOS1_taskEXIT_CRITICAL();
LED_IR_Off(); /* IR LED's off */
}
static void ReadCalibrated(SensorTimeType calib[REF_NOF_SENSORS], SensorTimeType raw[REF_NOF_SENSORS]) {
int i;
int32_t x, denominator;
REF_MeasureRaw(raw);
for(i=0;i<REF_NOF_SENSORS;i++) {
x = 0;
denominator = SensorCalibMinMax.maxVal[i]-SensorCalibMinMax.minVal[i];
if (denominator!=0) {
x = (((int32_t)raw[i]-SensorCalibMinMax.minVal[i])*1000)/denominator;
}
if (x<0) {
x = 0;
} else if (x>1000) {
x = 1000;
}
FRTOS1_taskENTER_CRITICAL();
calib[i] = x;
FRTOS1_taskEXIT_CRITICAL();
}
}
static void REF_MeasureRaw(SensorTimeType raw[REF_NOF_SENSORS]) {
uint8_t cnt; /* number of sensor */
uint8_t i;
LED_IR_On(); /* IR LED's on */
WAIT1_Waitus(200); /*! \todo adjust time as needed */
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorFctArray[i].SetOutput(); /* turn I/O line as output */
SensorFctArray[i].SetVal(); /* put high */
raw[i] = MAX_SENSOR_VALUE;
}
WAIT1_Waitus(50); /* give some time to charge the capacitor */
(void)RefCnt_ResetCounter(timerHandle); /* reset timer counter */
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorFctArray[i].SetInput(); /* turn I/O line as input */
}
int cntTimeOUt = 0;
do {
cnt = 0;
for(i=0;i<REF_NOF_SENSORS;i++) {
if (raw[i]==MAX_SENSOR_VALUE) { /* not measured yet? */
if (SensorFctArray[i].GetVal()==0) {
FRTOS1_taskENTER_CRITICAL();
raw[i] = RefCnt_GetCounterValue(timerHandle);
FRTOS1_taskEXIT_CRITICAL();
}
cntTimeOUt++;
if(cntTimeOUt >= 50*REF_NOF_SENSORS){
return;
}
} else { /* have value */
cnt++;
}
}
} while(cnt!=REF_NOF_SENSORS);
LED_IR_Off();
}
static uint8_t GetCmd(void) {
DRV_Command cmd;
portBASE_TYPE res;
res = FRTOS1_xQueueReceive(DRV_Queue, &cmd, 0);
if (res==errQUEUE_EMPTY) {
return ERR_RXEMPTY; /* no command */
}
/* process command */
FRTOS1_taskENTER_CRITICAL();
if (cmd.cmd==DRV_SET_MODE) {
PID_Start(); /* reset PID, especially integral counters */
DRV_Status.mode = cmd.u.mode;
} else if (cmd.cmd==DRV_SET_SPEED) {
DRV_Status.speed.left = cmd.u.speed.left;
DRV_Status.speed.right = cmd.u.speed.right;
} else if (cmd.cmd==DRV_SET_POS) {
DRV_Status.pos.left = cmd.u.pos.left;
DRV_Status.pos.right = cmd.u.pos.right;
}
FRTOS1_taskEXIT_CRITICAL();
return ERR_OK;
}
/* write a byte to the PS2 device */
void PS2_write(unsigned char data)
{
unsigned char i;
unsigned char parity = 1;
FRTOS1_taskENTER_CRITICAL();
BitClock_SetDir((bool)INPUT);
BitData_SetDir((bool)INPUT);
WAIT1_Waitus(100);
// Poner el bus en el modo Request to Send (petición de envío), para ello primero debe
//inhibir la comunicación llevando
//la línea de PS2_PIN_clock a “0” por al menos 100 microsegundos
//El protocolo dice por lo menos 100 ms
//PS2_PIN_data = high, PS2_PIN_clock = low: Communication Inhibited.
BitClock_SetDir((bool)OUTPUT);
BitClock_ClrVal();
WAIT1_Waitus(100);
//Se lleva al bus al estado de Request to Send poniendo la línea PS2_PIN_data en “bajo”
//y liberando la línea PS2_PIN_clock (estado “alto”)
//PS2_PIN_data = low, PS2_PIN_clock = high: Host Request-to-Send
BitData_SetDir((bool)OUTPUT);
BitData_ClrVal();
WAIT1_Waitus(10);
//Se libera la linea PS2_PIN_clock
BitClock_SetDir((bool)INPUT);
//Se espera a que el mouse tome el control y nos de el tren de pulsos
//El Mouse va a leer cuando el PS2_PIN_clock este en alto
//Cuando el mouse detecte este estado comenzará a enviar pulsos de reloj
while (BitClock_GetVal() == 1U);
//El host cambia los datos cuando la línea PS2_PIN_clock esta en “bajo”
//y el mouse los lee cuando PS2_PIN_clock está en “alto”
for (i=0; i < 8; i++)
{
if (data & 0x01)
{
BitData_SetDir((bool)INPUT); //Si el bit es un 1 se pone un 1 en el bus de datos
} else {
BitData_SetDir((bool)OUTPUT);
BitData_ClrVal(); //Si el bit es un 0 se pone un 0 en el bus de datos
}
//Hay que esperar que el clk se ponga en alto que es cuando
//el reloj va a leer el dato
while (BitClock_GetVal() == 0U);
//Hay que esperar que esperar que el clk se vuelva a poner en bajo
//para enviar
while (BitClock_GetVal() == 1U);
parity = parity ^ (data & 0x01);
data = data >> 1;
}
//Luego de mandar los datos, se envia el bit de paridad
if (parity)
{
BitData_SetDir((bool)INPUT);
} else {
BitData_SetDir((bool)OUTPUT);
BitData_ClrVal();
}
//Se espera a que el ckl lea el bit de paridad
while (BitClock_GetVal() == 0U);
//Se espera a que baje el pulso
while (BitClock_GetVal() == 1U);
//Se libera el bus de datos
BitData_SetDir((bool)INPUT);
WAIT1_Waitus(50);
//Esperar a que se ponga en bajo el ckl
while (BitClock_GetVal() == 1U);
//Esperar a que se liberen el bus de datos y clk
while ((BitClock_GetVal() == 0U) || (BitData_GetVal() == 0U));
//Poner en espera los datos entrantes
BitClock_SetDir((bool)OUTPUT);
BitClock_ClrVal();
FRTOS1_taskEXIT_CRITICAL();
}
