//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; }