//handle ACDS specific commands
int SUB_parseCmd(unsigned char src,unsigned char cmd,unsigned char *dat,unsigned short len){
int i;
unsigned long block_id;
unsigned long sector;
switch(cmd){
case CMD_MAG_DATA:
//check packet length
if(len!=sizeof(magData)){
//length incorrect, report error and exit
report_error(ERR_LEV_ERROR,ACDS_ERR_SRC_SENSORS,ACDS_ERR_SEN_BAD_PACKET_LENGTH,len);
return ERR_PK_LEN;
}
memcpy(&magData,dat,sizeof(magData));
//sensor data recieved set event
ctl_events_set_clear(&ACDS_evt,ACDS_EVT_DAT_REC,0);
return RET_SUCCESS;
case CMD_SPI_DATA_ACTION:
if(len==0){
return ERR_PK_LEN;
}
switch(dat[0]){
case SPI_DAT_ACTION_SD_WRITE:
if(len!=5){
return ERR_PK_LEN;
}
sector=((unsigned long)dat[1]<<24)|((unsigned long)dat[2]<<16)|((unsigned long)dat[3]<<8)|((unsigned long)dat[4]);
//get lock on action
if(!ctl_mutex_lock(&spi_action.lock,CTL_TIMEOUT_DELAY,10)){
return ERR_SPI_BUSY;
}
spi_action.action=dat[0];
spi_action.parm.sector=sector;
ctl_mutex_unlock(&spi_action.lock);
break;
default:
return ERR_UNKNOWN_CMD;
}
return RET_SUCCESS;
case CMD_ACDS_READ_BLOCK:
if(len!=3){
return ERR_PK_LEN;
}
block_id =((unsigned long)dat[0])<<16;
block_id|=((unsigned long)dat[1])<<8;
block_id|=((unsigned long)dat[2]);
//check range
if(block_id>LOG_IDX_MAX){
//index is out of range
return ERR_PK_BAD_PARM;
}
//set SD address
SD_read_addr=LOG_ADDR_START+block_id;
//trigger event
ctl_events_set_clear(&ACDS_evt,ACDS_EVT_SEND_DAT,0);
}
//Return Error
return ERR_UNKNOWN_CMD;
}
bool get_real_tow(double& tow)
{
ctl_mutex_lock(&mutex, CTL_TIMEOUT_INFINITE, 0);
if (!real_time_valid)
{
ctl_mutex_unlock(&mutex);
return real_time_valid;
}
u64 cur_time = get_hw_clock().get_system_time();
cur_time -= sys_reference;
float sec_elapsed = get_hw_clock().system_to_sec(cur_time);
tow = real_tow;
tow += sec_elapsed;
while (tow >= 604800.0) tow -= 604800.0;
ctl_mutex_unlock(&mutex);
return true;
}
bool get_real_time(timedate& time)
{
ctl_mutex_lock(&mutex, CTL_TIMEOUT_INFINITE, 0);
if (!real_time_valid)
{
ctl_mutex_unlock(&mutex);
return real_time_valid;
}
u64 cur_time = get_hw_clock().get_system_time();
cur_time -= sys_reference;
float sec_elapsed = get_hw_clock().system_to_sec(cur_time);
time = real_time;
time.sec += sec_elapsed;
tsecoffset(&time);
ctl_mutex_unlock(&mutex);
return true;
}
void set_real_time(const timedate& time, const double& tow)
{
ctl_mutex_lock(&mutex, CTL_TIMEOUT_INFINITE, 0);
u64 cur_time = get_hw_clock().get_system_time();
u64 snapshot_time = sys_snapshot;
if (cur_time <= snapshot_time || snapshot_time == 0)
{
ctl_mutex_unlock(&mutex);
return; // no good data yet
}
cur_time -= snapshot_time;
u32 ms_elapsed = get_hw_clock().system_to_millisec(cur_time);
if (ms_elapsed >= 800)
{
ctl_mutex_unlock(&mutex);
return; // more than 800ms since the timepulse, cannot really trust this data, the time should never come in more than a second after the pulse
}
double before = 0, after = 0;
us correct_1 = get_hw_clock().get_microsec_time();
get_real_tow(before);
// correct time for our timezone
real_time = time;
real_time.sec = roundf(real_time.sec); // timepulses occur on the real boundaries between seconds
// but reported time differs from round seconds by a tiny amount. correct this.
real_time.sec += (FS_TIME_ZONE * 60. * 60.);
tsecoffset(&real_time);
real_tow = tow;
sys_reference = snapshot_time;
real_time_valid = true;
us correct_2 = get_hw_clock().get_microsec_time();
get_real_tow(after);
correct_2 -= correct_1; // time it took for the actual processing
correction_stat = static_cast<float>(after - before - (static_cast<double>(correct_2) / 1000000.));
ctl_mutex_unlock(&mutex);
}
//read write cmd for CLYDE
int clyde_take_data(int *array) {
int res,i,found=0;
unsigned char tx[2],rx[2];
unsigned short rez;
//prevent other tasks from sending commands to the EPS
if(!ctl_mutex_lock(&EPS_mutex,CTL_TIMEOUT_DELAY,200)) {
return -1;
}
//Set
//printf("sent cmd \r\n");
//send cmd\
//take first 21 adc measurements for beacon
for(i=0; i<20; i++) {
tx[0]=EPS_ADC_COMMAND;
tx[1]= name_addrs_clyde[i];
res=i2c_tx(EPS_I2C_ADDRESS,tx,2);
//error msg or success
if(res<0) {
//printf("tx returned = %s\r\n",I2C_error_str(res));
//set invalid value
array[i]=0xF800;
//skip to the next channel
continue;
}
//wait 1.2 ms (300)
ctl_timeout_wait(ctl_get_current_time()+5);
//read cmd
res=i2c_rx(EPS_I2C_ADDRESS,rx,2);
//error msg or success
if (res<0) {
//printf("rx returned = %s\r\n",I2C_error_str(res));
//set invalid value
array[i]=0xF800;
//skip to the next channel
continue;
}
rez=rx[1];
rez|=rx[0]<<8;
rez&=0x3FF;
//printf("rez = %i\r\n",rez);
array[i]=(int)rez;
//printf("EPS data = %i, array # = %i\r\n",array[i],i);
}
//take status packet for beacon
tx[0]=EPS_STATUS_COMMAND;
tx[1]=0;//THIS DOESNT MATTER WHAT IT IS
res=i2c_tx(EPS_I2C_ADDRESS,tx,2);
//error msg or success
//printf("%s\r\n",I2C_error_str(res));
//wait 1.2 ms (300)
ctl_timeout_wait(ctl_get_current_time()+5);
//read cmd
res=i2c_rx(EPS_I2C_ADDRESS,rx,2);
//error msg or success
if(res!=0) {
//printf("%s\r\n",I2C_error_str(res));
//invalid value
array[i]=0xFFFF;
} else {
rez=rx[1];
rez|=rx[0]<<8;
printf("rez = 0x%04X\r\n",rez);
array[i]=(int)rez;
}
//unlock the EPS
ctl_mutex_unlock(&EPS_mutex);
//return success
return RET_SUCCESS;
}
