double launch_bench(uint32_t const* idxes, const size_t nint, const size_t n, const int nthreads)
{
double time_start, time_end;
#pragma omp parallel num_threads(nthreads)
{
uint32_t* buf = (uint32_t*) malloc(sizeof(uint32_t)*nint);
memset(buf, 0, sizeof(uint32_t)*nint);
#pragma omp barrier
#pragma omp master
{
time_start = get_current_timestamp();
}
for (size_t i = 0; i < n; i++) {
buf[idxes[i]] = 0;
}
#pragma omp barrier
#pragma omp master
{
time_end = get_current_timestamp();
}
free(buf);
}
return ((double)(nthreads*sizeof(uint32_t)*n)/(time_end-time_start))/(1024.0*1024.0);
}
int main() {
FILE *ptr = fopen("/dev/ttyACM0","r");
char data[100];
int node_online = 1;
pthread_t inc_x_thread;
/* create a second thread which executes inc_x(&x) */
if(pthread_create(&inc_x_thread, NULL, pthread_server, NULL)) {
fprintf(stderr, "Error creating thread\n");
return 1;
}
while(fgets(data,100,ptr) != NULL)
{
if(data[0] == 'P' && data[1] == 'a' && data[2] == 'k') {
read_time_stamp = get_current_timestamp();
}
//printf("%s",data);
//printf("node online\n");
}
printf("Clossing the program\n");
return 0;
}
void rtos_system_test(void) {
print("\n\n==============RTOS System Test==============\n");
#ifndef RTOS_SYSTEM_TEST_SUMMARY
print_cur_data_buf(_get_cur_data_buf());
print_equistacks();
int max_num_errors = -1;
#else
int max_num_errors = 10;
#endif
print("\n\n==============System Info==============\n");
print("timestamp: \t%ds \t(%dmin)\n", get_current_timestamp(), get_current_timestamp()/60);
print("ticks: \t%d\n", xTaskGetTickCount());
print("sat state: \t%s\n", get_sat_state_str(get_sat_state()));
print("reboot #: \t%d\n", cache_get_reboot_count());
print("num errors:\t%d\n", error_equistack.cur_size);
print("most recent errs: ");
print_errors(max_num_errors);
print_task_info();
print("====================End=====================\n\n");
}
void SAS::associate_trails(TrailId trail_a,
TrailId trail_b,
Marker::Scope scope)
{
std::string trail_assoc_msg;
write_hdr(trail_assoc_msg, 29, SAS_MSG_TRAIL_ASSOC, get_current_timestamp());
write_trail(trail_assoc_msg, trail_a);
write_trail(trail_assoc_msg, trail_b);
write_int8(trail_assoc_msg, (uint8_t)scope);
if (_connection)
{
_connection->send_msg(trail_assoc_msg);
}
}
loop_timer_t* loop_runevery(loop_t* loop, unsigned interval, onexpire_f expirecb, void *userdata)
{
unsigned long long timestamp;
if (NULL == loop || NULL == expirecb || 0 == interval)
{
log_error("loop_runevery: bad loop(%p) or bad expirecb(%p) or bad interval(%u)", loop, expirecb, interval);
return NULL;
}
get_current_timestamp(×tamp);
timestamp += interval;
return timer_queue_add(loop->timer_queue, timestamp, interval, expirecb, userdata);
}
void ssp_batch_report(struct ssp_data *data)
{
u8 sensor_type = 0;
struct sensor_value sensor_data;
int idx_data = 0;
int count = 0;
u64 timestamp = get_current_timestamp();
ssp_dbg("[SSP_BAT] LENGTH = %d, start index = %d ts %lld\n", data->batch_event.batch_length, idx_data, timestamp);
while (idx_data < data->batch_event.batch_length)
{
//ssp_dbg("[SSP_BAT] bcnt %d\n", count);
sensor_type = data->batch_event.batch_data[idx_data++];
if(sensor_type == META_SENSOR) {
sensor_data.meta_data.sensor = data->batch_event.batch_data[idx_data++];
report_meta_data(data, &sensor_data);
count++;
continue;
}
if ((sensor_type != ACCELEROMETER_SENSOR) &&
(sensor_type != GEOMAGNETIC_UNCALIB_SENSOR) &&
(sensor_type != PRESSURE_SENSOR) &&
(sensor_type != GAME_ROTATION_VECTOR) &&
(sensor_type != PROXIMITY_SENSOR)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d, idx_data %d\n", __func__,
sensor_type, idx_data - 1);
return ;
}
if(count%80 == 0)
usleep_range(1000,1000);
//ssp_dbg("[SSP_BAT] cnt %d\n", count);
data->get_sensor_data[sensor_type](data->batch_event.batch_data, &idx_data, &sensor_data);
data->skipEventReport = false;
get_timestamp(data, data->batch_event.batch_data, &idx_data, &sensor_data, BATCH_MODE_RUN, sensor_type);
if (data->skipEventReport == false) {
data->report_sensor_data[sensor_type](data, &sensor_data);
}
data->reportedData[sensor_type] = true;
count++;
}
ssp_dbg("[SSP_BAT] max cnt %d\n", count);
}
int main(int, char**)
{
HWND previous_foreground_window = nullptr;
for (;;)
{
HWND foreground_window = GetForegroundWindow();
if (foreground_window != nullptr && foreground_window != previous_foreground_window)
{
//Foreground window changed.
std::wstring filename(get_process_filename(foreground_window));
std::wstring window_title(get_window_title(foreground_window));
std::wstring timestamp(get_current_timestamp());
// Format: [HH:MM:SS] name.exe | Window Title
wprintf(L"%s %s | %s\n", timestamp.c_str(), filename.c_str(), window_title.c_str());
previous_foreground_window = foreground_window;
}
Sleep(100);
}
}
bool SAS::Connection::connect_init()
{
if (_socket_callback)
{
_sock = _socket_callback(_sas_address.c_str(), SAS_PORT);
}
else
{
_sock = get_local_sock(_sas_address.c_str(), SAS_PORT);
}
if (_sock < 0)
{
return false;
}
SAS_LOG_DEBUG("Connected SAS socket to %s:%s", _sas_address.c_str(), SAS_PORT);
set_send_timeout(_sock, SEND_TIMEOUT);
// Send an init message to SAS.
std::string init;
std::string version("v0.1");
// The resource version is part of the binary protocol but is not currently
// exposed over the C++ API.
std::string resource_version("");
int init_len = INIT_HDR_SIZE +
sizeof(uint8_t) + _system_name.length() +
sizeof(uint32_t) +
sizeof(uint8_t) + version.length() +
sizeof(uint8_t) + _system_type.length() +
sizeof(uint8_t) + _resource_identifier.length() +
sizeof(uint8_t) + resource_version.length();
init.reserve(init_len);
write_hdr(init, init_len, SAS_MSG_INIT, get_current_timestamp());
write_int8(init, (uint8_t)_system_name.length());
write_data(init, _system_name.length(), _system_name.data());
int endianness = 1;
init.append((char*)&endianness, sizeof(int)); // Endianness must be written in machine order.
write_int8(init, version.length());
write_data(init, version.length(), version.data());
write_int8(init, (uint8_t)_system_type.length());
write_data(init, _system_type.length(), _system_type.data());
write_int8(init, (uint8_t)_resource_identifier.length());
write_data(init, _resource_identifier.length(), _resource_identifier.data());
write_int8(init, (uint8_t)resource_version.length());
write_data(init, resource_version.length(), resource_version.data());
SAS_LOG_DEBUG("Sending SAS INIT message");
int rc = ::send(_sock, init.data(), init.length(), 0);
if (rc < 0)
{
SAS_LOG_ERROR("SAS connection to %s:%s failed: %d %s", _sas_address.c_str(), SAS_PORT, errno, ::strerror(errno));
::close(_sock);
_sock = -1;
return false;
}
SAS_LOG_STATUS("Connected to SAS %s:%s", _sas_address.c_str(), SAS_PORT);
return true;
}
static void get_timestamp(struct ssp_data *data, char *pchRcvDataFrame,
int *iDataIdx, struct sensor_value *sensorsdata,
u16 batch_mode, int sensor_type)
{
unsigned int idxTimestamp = 0;
unsigned int time_delta_us = 0;
u64 time_delta_ns = 0;
memset(&time_delta_us, 0, 4);
memcpy(&time_delta_us, pchRcvDataFrame + *iDataIdx, 4);
memset(&idxTimestamp, 0, 4);
memcpy(&idxTimestamp, pchRcvDataFrame + *iDataIdx + 4, 4);
//ssp_dbg("[SSP_IDX] [%3d] TS %d idx %d]\n", sensor_type, time_delta_us, idxTimestamp);
if (time_delta_us > MS_IDENTIFIER) {
//We condsider, unit is ms (MS->NS)
time_delta_ns = ((u64) (time_delta_us % MS_IDENTIFIER)) * U64_MS2NS;
} else {
time_delta_ns = (((u64) time_delta_us) * U64_US2NS);//US->NS
}
// TODO: Reverse calculation of timestamp when non wake up batching.
if (batch_mode == BATCH_MODE_RUN) {
// BATCHING MODE
//ssp_dbg("[SSP_TIM] BATCH [%3d] TS %lld %lld\n", sensor_type, data->lastTimestamp[sensor_type],time_delta_ns);
data->lastTimestamp[sensor_type] += time_delta_ns;
} else {
// NORMAL MODE
unsigned int tmpIndex = 0;
unsigned int tmpPrevIndex = 0;
u64 tmp_ts_delta = 0ULL;
u64 tmp_ts_avg = 0ULL;
switch (sensor_type) {
case ACCELEROMETER_SENSOR:
case GYROSCOPE_SENSOR:
case GYRO_UNCALIB_SENSOR:
case GEOMAGNETIC_SENSOR:
case GEOMAGNETIC_UNCALIB_SENSOR:
case GEOMAGNETIC_RAW:
case ROTATION_VECTOR:
case GAME_ROTATION_VECTOR:
case PRESSURE_SENSOR:
case LIGHT_SENSOR:
case PROXIMITY_RAW:
if (data->ts_stacked_cnt < idxTimestamp) {
int i = 0;
unsigned int offset = 0;
offset = idxTimestamp - data->ts_stacked_cnt;
//ssp_dbg("[SSP_TIM] %20s ERROR : TS_STACK_OVERFLOW [MCU %5u AP %5u OFFSET %5u]\n",
//__func__, idxTimestamp, data->ts_stacked_cnt, offset);
// INSERT DUMMY INDEXING & SKIP 1 EVENT.
for (i = 1; i <= offset; i++) {
// TODO: Test if this can causing duplicated timestamp.
data->ts_index_buffer[(data->ts_stacked_cnt + i) % SIZE_TIMESTAMP_BUFFER]
= data->ts_index_buffer[data->ts_stacked_cnt % SIZE_TIMESTAMP_BUFFER];
}
data->ts_stacked_cnt += offset;
data->skipEventReport = true;// SKIP OVERFLOWED INDEX
tmpIndex = data->ts_stacked_cnt % SIZE_TIMESTAMP_BUFFER;
} else {
tmpIndex = idxTimestamp % SIZE_TIMESTAMP_BUFFER;
tmpPrevIndex = data->ts_prev_index[sensor_type] % SIZE_TIMESTAMP_BUFFER;
if (data->ts_avg_skip_cnt[sensor_type] > 0) { // Skip 2 events for avg.
//pr_err("[SSP_TIM] skip cnt %d-%d\n", sensor_type, data->ts_avg_skip_cnt[sensor_type]);
data->ts_avg_skip_cnt[sensor_type]--;
if(data->lastTimestamp[sensor_type] > data->ts_index_buffer[tmpIndex])
{
data->lastTimestamp[sensor_type] = data->lastTimestamp[sensor_type] + time_delta_ns;
//pr_err("[SSP_TIM] %d +DT %lld\n", sensor_type, time_delta_ns);
}
else
{
data->lastTimestamp[sensor_type] = data->ts_index_buffer[tmpIndex];
//pr_err("[SSP_TIM] %d lst=buffts %lld\n",sensor_type,data->ts_index_buffer[tmpIndex]);
}
//pr_err("[SSP_TIM] %d lts %lld", sensor_type, data->lastTimestamp[sensor_type]);
data->ts_prev_index[sensor_type] = idxTimestamp;// KEEP LAST INDEX;
} else {
tmp_ts_delta = data->ts_index_buffer[tmpIndex] - data->ts_index_buffer[tmpPrevIndex];
/*
pr_err("[SSP_TIM] ts_delta %lld c %lld Pr %lld cIdx %d Pre %d\n",tmp_ts_delta,
data->ts_index_buffer[tmpIndex], data->ts_index_buffer[tmpPrevIndex],
tmpIndex, tmpPrevIndex);
*/
if((sensor_type == GYROSCOPE_SENSOR) && (data->cameraGyroSyncMode == true))
tmp_ts_avg = 0;
else
tmp_ts_avg = get_ts_average(data, sensor_type, tmp_ts_delta);
if (tmp_ts_avg == 0) {
if(data->lastTimestamp[sensor_type] > data->ts_index_buffer[tmpIndex])
{
//pr_err("[SSP_TIM] %d after avg + DT %lld", sensor_type, time_delta_ns);
data->lastTimestamp[sensor_type] = data->lastTimestamp[sensor_type] + time_delta_ns;
}
else
{
//pr_err("[SSP_TIM] %d lst=buffts %lld\n",sensor_type,data->ts_index_buffer[tmpIndex]);
data->lastTimestamp[sensor_type] = data->ts_index_buffer[tmpIndex];
}
} else {
//pr_err("[SSP_TIM] %d +avg %lld\n",sensor_type, tmp_ts_avg);
data->lastTimestamp[sensor_type] = data->lastTimestamp[sensor_type] + tmp_ts_avg;
}
data->ts_prev_index[sensor_type] = idxTimestamp;// KEEP LAST INDEX;
}
}
break;
// None Indexing
default:
data->lastTimestamp[sensor_type] = data->timestamp;
break;
}
//ssp_dbg("[SSP_TIM] NORMA [%3d] TS %lld\n",
//sensor_type, data->lastTimestamp[sensor_type]);
}
sensorsdata->timestamp = data->lastTimestamp[sensor_type];
//sensorsdata->timestamp = data->lastTimestamp[sensor_type] - (6ULL * U64_MS2NS);
#if 0// TEST To correcting timestamp sync... (why 6ms? we needs PROOF)
if(sensor_type == GYROSCOPE_SENSOR){
u64 cTS = get_current_timestamp();
pr_err("[SSP_TIM]gyroTS %lld DT %lld[MCU %5u AP %5u]curTS %lld\n",
sensorsdata->timestamp, time_delta_ns,idxTimestamp, data->ts_stacked_cnt, cTS);
}
ssp_dbg("[SSP_TIM] ----- [%3d] TS %lld DT %lld[MCU %5u AP %5u]\n",
sensor_type,
sensorsdata->timestamp, time_delta_ns,idxTimestamp, data->ts_stacked_cnt);
#endif// TEST
*iDataIdx += 8;
}
int delivery(struct db_context_t *dbc, struct delivery_t *data, char ** vals, int nvals)
{
/* Input variables. */
int w_id = data->w_id;
int o_carrier_id = data->o_carrier_id;
struct sql_result_t result;
char query[256];
int d_id;
int NO_O_ID=0;
int O_C_ID=1;
int OL_AMOUNT=2;
for (d_id = 1; d_id <= 10; d_id++)
{
sprintf(query, DELIVERY_1, w_id, d_id);
#ifdef DEBUG_QUERY
LOG_ERROR_MESSAGE("DELIVERY_1: %s\n",query);
#endif
if (dbt2_sql_execute(dbc, query, &result, "DELIVERY_1") && result.result_set)
{
dbt2_sql_fetchrow(dbc, &result);
vals[NO_O_ID]= (char *)dbt2_sql_getvalue(dbc, &result, 0); //NO_O_ID
dbt2_sql_close_cursor(dbc, &result);
if (!vals[NO_O_ID])
{
LOG_ERROR_MESSAGE("ERROR: NO_O_ID=NULL for query DELIVERY_1:\n%s\n", query);
//return -1;
}
}
else
{
/* Nothing to delivery for this district, try next. */
continue;
}
if (vals[NO_O_ID] && atoi(vals[NO_O_ID])>0)
{
sprintf(query, DELIVERY_2, vals[NO_O_ID], w_id, d_id);
#ifdef DEBUG_QUERY
LOG_ERROR_MESSAGE("DELIVERY_2: %s\n",query);
#endif
if (!dbt2_sql_execute(dbc, query, &result, "DELIVERY_2"))
{
return -1;
}
sprintf(query, DELIVERY_3, vals[NO_O_ID], w_id, d_id);
#ifdef DEBUG_QUERY
LOG_ERROR_MESSAGE("DELIVERY_3: %s\n",query);
#endif
if (dbt2_sql_execute(dbc, query, &result, "DELIVERY_3") && result.result_set)
{
dbt2_sql_fetchrow(dbc, &result);
vals[O_C_ID]= (char *)dbt2_sql_getvalue(dbc, &result, 0); //O_C_ID
dbt2_sql_close_cursor(dbc, &result);
if (!vals[O_C_ID])
{
LOG_ERROR_MESSAGE("DELIVERY_3:query %s\nO_C_ID= NULL", query);
//return -1;
}
}
else //error
{
return -1;
}
sprintf(query, DELIVERY_4, o_carrier_id, vals[NO_O_ID], w_id, d_id);
#ifdef DEBUG_QUERY
LOG_ERROR_MESSAGE("DELIVERY_4: query %s\n", query);
#endif
if (!dbt2_sql_execute(dbc, query, &result, "DELIVERY_4"))
{
return -1;
}
char current_timestamp[80];
get_current_timestamp(current_timestamp);
sprintf(query, DELIVERY_5, current_timestamp, vals[NO_O_ID], w_id, d_id);
#ifdef DEBUG_QUERY
LOG_ERROR_MESSAGE("DELIVERY_5: query %s\n", query);
#endif
if (!dbt2_sql_execute(dbc, query, &result, "DELIVERY_5"))
{
return -1;
}
sprintf(query, DELIVERY_6, vals[NO_O_ID], w_id, d_id);
#ifdef DEBUG_QUERY
LOG_ERROR_MESSAGE("DELIVERY_6: query %s\n", query);
#endif
if (dbt2_sql_execute(dbc, query, &result, "DELIVERY_6") && result.result_set)
{
dbt2_sql_fetchrow(dbc, &result);
vals[OL_AMOUNT]= (char *)dbt2_sql_getvalue(dbc, &result, 0); //OL_AMOUNT
dbt2_sql_close_cursor(dbc, &result);
if (!vals[OL_AMOUNT])
{
return -1;
}
}
else //error
{
return -1;
}
snprintf(query, 250, DELIVERY_7, vals[OL_AMOUNT], vals[O_C_ID], w_id, d_id);
#ifdef DEBUG_QUERY
LOG_ERROR_MESSAGE("DELIVERY_7: query %s LEN %d\n", query, strlen(query));
#endif
if (!dbt2_sql_execute(dbc, query, &result, "DELIVERY_7"))
{
LOG_ERROR_MESSAGE("DELIVERY_7: OL_AMOUNT: |%s| O_C_ID: |%s| query %s", vals[OL_AMOUNT],
vals[O_C_ID], query);
return -1;
}
}
dbt2_free_values(vals, nvals);
}
return 1;
}
/* this function is run by the second thread */
void *pthread_server(void *x_void_ptr)
{
int socket_desc , client_sock , c , read_size;
struct sockaddr_in server , client;
char client_message[2000];
//Create socket
socket_desc = socket(AF_INET , SOCK_STREAM , 0);
if (socket_desc == -1)
{
printf("Could not create socket");
}
puts("Socket created");
//Prepare the sockaddr_in structure
server.sin_family = AF_INET;
server.sin_addr.s_addr = INADDR_ANY;
server.sin_port = htons( 8888 );
//Bind
if( bind(socket_desc,(struct sockaddr *)&server , sizeof(server)) < 0)
{
//print the error message
perror("bind failed. Error");
return NULL;
}
puts("bind done");
//Listen
listen(socket_desc , 3);
//Accept and incoming connection
puts("Waiting for incoming connections...");
c = sizeof(struct sockaddr_in);
//accept connection from an incoming client
client_sock = accept(socket_desc, (struct sockaddr *)&client, (socklen_t*)&c);
if (client_sock < 0)
{
perror("accept failed");
return NULL;
}
puts("Connection accepted");
//Receive a message from client
//while( (read_size = recv(client_sock , client_message , 2000 , 0)) > 0 )
while(1)
{
sleep(1);
current_timestamp = get_current_timestamp();
if(current_timestamp - read_time_stamp < delta)
{
client_message[0] = '1';
} else {
client_message[0] = '0';
}
//printf("%lli\n\r",current_timestamp - read_time_stamp);
client_message[1] = '\n';
write(client_sock , client_message , 2);
}
if(read_size == 0)
{
puts("Client disconnected");
fflush(stdout);
}
else if(read_size == -1)
{
perror("recv failed");
}
/* the function must return something - NULL will do */
return NULL;
}
void ssp_batch_resume_check(struct ssp_data *data)
{
u64 acc_offset = 0, uncal_mag_offset = 0, press_offset = 0, grv_offset = 0, proxi_offset = 0;
//if suspend -> wakeup case. calc. FIFO last timestamp
if(data->bIsResumed)
{
u8 sensor_type = 0;
struct sensor_value sensor_data;
unsigned int delta_time_us = 0;
unsigned int idx_timestamp = 0;
int idx_data = 0;
u64 timestamp = get_current_timestamp();
//ssp_dbg("[SSP_BAT] LENGTH = %d, start index = %d ts %lld resume %lld\n", data->batch_event.batch_length, idx_data, timestamp, data->resumeTimestamp);
timestamp = data->resumeTimestamp = data->timestamp;
while (idx_data < data->batch_event.batch_length)
{
sensor_type = data->batch_event.batch_data[idx_data++];
if(sensor_type == META_SENSOR) {
sensor_data.meta_data.sensor = data->batch_event.batch_data[idx_data++];
continue;
}
if ((sensor_type != ACCELEROMETER_SENSOR) &&
(sensor_type != GEOMAGNETIC_UNCALIB_SENSOR) &&
(sensor_type != PRESSURE_SENSOR) &&
(sensor_type != GAME_ROTATION_VECTOR) &&
(sensor_type != PROXIMITY_SENSOR)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d, idx_data %d\n", __func__,
sensor_type, idx_data - 1);
data->bIsResumed = false;
data->resumeTimestamp = 0ULL;
return ;
}
data->get_sensor_data[sensor_type](data->batch_event.batch_data, &idx_data, &sensor_data);
memset(&delta_time_us, 0, 4);
memcpy(&delta_time_us, data->batch_event.batch_data + idx_data, 4);
memset(&idx_timestamp, 0, 4);
memcpy(&idx_timestamp, data->batch_event.batch_data + idx_data + 4, 4);
if (delta_time_us > MS_IDENTIFIER) {
//We condsider, unit is ms (MS->NS)
delta_time_us = ((u64) (delta_time_us % MS_IDENTIFIER)) * U64_MS2NS;
} else {
delta_time_us = (((u64) delta_time_us) * U64_US2NS);//US->NS
}
switch(sensor_type)
{
case ACCELEROMETER_SENSOR:
acc_offset += delta_time_us;
break;
case GEOMAGNETIC_UNCALIB_SENSOR:
uncal_mag_offset += delta_time_us;
break;
case GAME_ROTATION_VECTOR:
grv_offset += delta_time_us;
break;
case PRESSURE_SENSOR:
press_offset += delta_time_us;
break;
case PROXIMITY_SENSOR:
proxi_offset += delta_time_us;
break;
default:
break;
}
idx_data += 8;
}
if(acc_offset > 0)
data->lastTimestamp[ACCELEROMETER_SENSOR] = timestamp - acc_offset;
if(uncal_mag_offset > 0)
data->lastTimestamp[GEOMAGNETIC_UNCALIB_SENSOR] = timestamp - uncal_mag_offset;
if(press_offset > 0)
data->lastTimestamp[PRESSURE_SENSOR] = timestamp - press_offset;
if(grv_offset > 0)
data->lastTimestamp[GAME_ROTATION_VECTOR] = timestamp - grv_offset;
if(proxi_offset > 0)
data->lastTimestamp[PROXIMITY_SENSOR] = timestamp - proxi_offset;
ssp_dbg("[SSP_BAT] resume calc. acc %lld. uncalmag %lld. pressure %lld. GRV %lld proxi %lld\n", acc_offset, uncal_mag_offset, press_offset, grv_offset, proxi_offset);
}
data->bIsResumed = false;
data->resumeTimestamp = 0ULL;
}
bool ssp_check_buffer(struct ssp_data *data)
{
int idx_data = 0;
u8 sensor_type = 0;
bool res = true;
u64 ts = get_current_timestamp();
pr_err("[SSP_BUF] start check %lld\n", ts);
do{
sensor_type = data->batch_event.batch_data[idx_data++];
if ((sensor_type != ACCELEROMETER_SENSOR) &&
(sensor_type != GEOMAGNETIC_UNCALIB_SENSOR) &&
(sensor_type != PRESSURE_SENSOR) &&
(sensor_type != GAME_ROTATION_VECTOR) &&
(sensor_type != PROXIMITY_SENSOR) &&
(sensor_type != META_SENSOR)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d, idx_data %d\n", __func__,
sensor_type, idx_data - 1);
res = false;
break;
}
switch(sensor_type)
{
case ACCELEROMETER_SENSOR:
idx_data += 14;
break;
case GEOMAGNETIC_UNCALIB_SENSOR:
idx_data += 20;
break;
case PRESSURE_SENSOR:
idx_data += 14;
break;
case GAME_ROTATION_VECTOR:
idx_data += 25;
break;
case PROXIMITY_SENSOR:
idx_data += 11;
break;
case META_SENSOR:
idx_data += 1;
break;
}
if(idx_data > data->batch_event.batch_length){
//stop index over max length
pr_info("[SSP_CHK] invalid data1\n");
res = false;
break;
}
// run until max length
if(idx_data == data->batch_event.batch_length){
//pr_info("[SSP_CHK] valid data\n");
break;
}
else if(idx_data + 1 == data->batch_event.batch_length){
//stop if only sensor type exist
pr_info("[SSP_CHK] invalid data2\n");
res = false;
break;
}
}while(true);
ts = get_current_timestamp();
pr_err("[SSP_BUF] finish check %lld\n", ts);
return res;
}
void get_datetime(str *dest)
{
timestamp2isodt(dest, get_current_timestamp());
}
int main(int argc, char *argv[])
{
int status;
char hostname[MAXHOSTNAMELEN];
char *domain = DEFAULT_SERVICE; /// on Linux sets DB q-file directory
db_clt_typ *pclt; /// data server client pointer
int xport = COMM_OS_XPORT; /// OS-agnostic designation
int db_num = 0; /// must be set to non-zero for DB update.
int verbose = 0; /// if 1, print extra info for debugging
int do_timing = 0; /// if 1, print users and sys time at end
struct avcs_timing timing;
int udp_port = 7015; /// port for receiving heartbeat
int option;
path_gps_point_t hb; /// fill in from GPS messages received
int sd_in; /// socket descriptor for UDP receive
int bytes_rcvd; /// returned from recvfrom
struct sockaddr_in src_addr; /// used in recvfrom call
unsigned int socklen;
/* Read and interpret any user switches. */
while ((option = getopt(argc, argv, "n:vu:")) != EOF) {
switch(option) {
case 'n':
db_num = atoi(optarg);
break;
case 'v':
verbose = 1;
break;
case 'u':
udp_port = atoi(optarg);
break;
default:
fprintf(stderr, "Usage %s: ", argv[0]);
fprintf(stderr, " -u (UDP port number for input) ");
fprintf(stderr, " -n DB variable number ");
fprintf(stderr, " -v (verbose, info to stdout) ");
fprintf(stderr, "\n");
exit(EXIT_FAILURE);
}
}
fprintf(stderr, "Receiving GPS point for DB variable %d\n", db_num);
fprintf(stderr, "writing output to stdout\n");
fflush(stderr);
sd_in = udp_allow_all(udp_port);
if (sd_in < 0) {
printf("failure opening socket on %d\n", udp_port);
exit(EXIT_FAILURE);
}
if (db_num == 0) {
fprintf(stderr, "Must specify DB variable number to use\n");
fprintf(stderr, "Create DB variable in another process\n");
fflush(stderr);
exit(EXIT_FAILURE);
}
get_local_name(hostname, MAXHOSTNAMELEN);
if ((pclt = clt_login(argv[0], hostname, domain, xport)) == NULL ) {
printf("%s: Database initialization error\n", argv[0]);
exit(EXIT_FAILURE);
}
if (setjmp(env) != 0) {
if(pclt)
db_list_done(pclt, NULL, 0, NULL, 0);
exit(EXIT_SUCCESS);
} else
sig_ign(sig_list, sig_hand);
socklen = sizeof(src_addr);
memset(&src_addr, 0, socklen);
memset(&hb, 0, sizeof(hb));
while (1) {
timestamp_t ts;
bytes_rcvd = recvfrom(sd_in, &hb, sizeof(hb), 0,
(struct sockaddr *) &src_addr, &socklen);
if (bytes_rcvd < 0) {
perror("recvfrom failed\n");
continue;
}
get_current_timestamp(&ts);
path_gps_update_object_id(&src_addr.sin_addr.s_addr, &hb, 4);
db_clt_write(pclt, db_num, sizeof(path_gps_point_t), &hb);
if (verbose) {
print_timestamp(stdout, &ts);
fprintf(stdout, " 0x%08x ", src_addr.sin_addr.s_addr);
path_gps_print_point(stdout, &hb);
fprintf(stdout, "\n");
fflush(stdout);
}
}
}
void eRTSPStreamClient::http_response(int sock, int rc, const std::string &ah, const std::string &desc, int cseq, int lr)
{
std::stringstream ss;
if (!lr)
lr = desc.size();
ss << "RTSP"
<< "/1.0"
<< " ";
if (rc == 200)
ss << rc << " "
<< "OK";
else if (rc == 400)
ss << rc << " "
<< "Bad Request";
else if (rc == 403)
ss << rc << " "
<< "Forbidden";
else if (rc == 404)
ss << rc << " "
<< "Not Found";
else if (rc == 500)
ss << rc << " "
<< "Internal Server Error";
else if (rc == 501)
ss << rc << " "
<< "Not Implemented";
else if (rc == 405)
ss << rc << " "
<< "Method Not Allowed";
else if (rc == 454)
ss << rc << " "
<< "Session Not Found";
else
{
rc = 503;
ss << rc << " "
<< "Service Unavailable";
}
ss << "\r\n";
ss << "Date: " << get_current_timestamp() << "\r\n";
if (session_id && ah.find("Session") == std::string::npos && rc != 454)
ss << "Session: " << std::setfill('0') << std::setw(10) << session_id << "\r\n";
if (cseq > 0)
ss << "Cseq: " << cseq << "\r\n";
ss << ah << "\r\n";
ss << "Server: " << app_name << "/" << version << "\r\n";
if (lr > 0)
ss << "Content-Length: " << lr << "\r\n\r\n"
<< desc;
else
ss << "\r\n";
char *resp = strdup(ss.str().c_str());
int len = strlen(resp);
eDebug("reply to %d, mr %p, len %d: %s", sock, mr, len, resp);
if (mr)
{
mr->pushReply((void *)resp, len);
}
else
{
struct timespec tv, rem;
tv.tv_sec = 0;
tv.tv_nsec = 5000000;
int times = 20;
int pos = 0;
int rb = 0;
while (pos < len)
{
rb = send(sock, resp + pos, len - pos, MSG_NOSIGNAL);
if (rb > 0)
pos += rb;
if (pos == len)
break;
if (rb == -1 && (errno != EAGAIN && errno != EWOULDBLOCK))
break;
if (rb == 0)
break;
eDebug("partial write %d out of %d for socket %d", pos, len, sock);
nanosleep(&tv, &rem);
if (times-- < 0)
break;
}
if (pos < len)
eDebug("error writing %d out of %d to socket %d, errno: %d", pos, len, sock, errno);
eDebug("wrote successfully %d", len);
}
free(resp);
}
// Control batched data with long term
// Ref ssp_read_big_library_task
void ssp_batch_data_read_task(struct work_struct *work)
{
struct ssp_big *big = container_of(work, struct ssp_big, work);
struct ssp_data *data = big->data;
struct ssp_msg *msg;
int buf_len, residue, ret = 0, index = 0, pos = 0;
u64 ts = 0;
mutex_lock(&data->batch_events_lock);
wake_lock(&data->ssp_wake_lock);
residue = big->length;
data->batch_event.batch_length = big->length;
data->batch_event.batch_data = vmalloc(big->length);
if (data->batch_event.batch_data == NULL)
{
ssp_dbg("[SSP_BAT] batch data alloc fail \n");
kfree(big);
wake_unlock(&data->ssp_wake_lock);
mutex_unlock(&data->batch_events_lock);
return;
}
//ssp_dbg("[SSP_BAT] IN : LENGTH = %d \n", big->length);
while (residue > 0) {
buf_len = residue > DATA_PACKET_SIZE
? DATA_PACKET_SIZE : residue;
msg = kzalloc(sizeof(*msg),GFP_ATOMIC);
msg->cmd = MSG2SSP_AP_GET_BIG_DATA;
msg->length = buf_len;
msg->options = AP2HUB_READ | (index++ << SSP_INDEX);
msg->data = big->addr;
msg->buffer = data->batch_event.batch_data + pos;
msg->free_buffer = 0;
ret = ssp_spi_sync(big->data, msg, 1000);
if (ret != SUCCESS) {
pr_err("[SSP_BAT] read batch data err(%d) ignor\n", ret);
vfree(data->batch_event.batch_data);
data->batch_event.batch_data = NULL;
data->batch_event.batch_length = 0;
kfree(big);
wake_unlock(&data->ssp_wake_lock);
mutex_unlock(&data->batch_events_lock);
return;
}
pos += buf_len;
residue -= buf_len;
pr_info("[SSP_BAT] read batch data (%5d / %5d)\n", pos, big->length);
}
// TODO: Do not parse, jut put in to FIFO, and wake_up thread.
// READ DATA FROM MCU COMPLETED
//Wake up check
if(ssp_check_buffer(data)){
ssp_batch_resume_check(data);
// PARSE DATA FRAMES, Should run loop
ts = get_current_timestamp();
pr_info("[SSP] report start %lld\n", ts);
ssp_batch_report(data);
ts = get_current_timestamp();
pr_info("[SSP] report finish %lld\n", ts);
}
vfree(data->batch_event.batch_data);
data->batch_event.batch_data = NULL;
data->batch_event.batch_length = 0;
kfree(big);
wake_unlock(&data->ssp_wake_lock);
mutex_unlock(&data->batch_events_lock);
}
int parse_dataframe(struct ssp_data *data, char *pchRcvDataFrame, int iLength)
{
int iDataIdx;
int sensor_type;
u16 length = 0;
struct sensor_value sensorsdata;
s16 caldata[3] = { 0, };
#ifdef CONFIG_SENSORS_SSP_HIFI_BATCHING // HIFI batch
u16 batch_event_count;
u16 batch_mode;
u64 ts = 0;
#else
struct ssp_time_diff sensortime;
sensortime.time_diff = 0;
#endif
data->uIrqCnt++;
for (iDataIdx = 0; iDataIdx < iLength;) {
switch (pchRcvDataFrame[iDataIdx++]) {
#ifdef CONFIG_SENSORS_SSP_HIFI_BATCHING // HIFI batch
case MSG2AP_INST_BYPASS_DATA:
sensor_type = pchRcvDataFrame[iDataIdx++];
if ((sensor_type < 0) || (sensor_type >= SENSOR_MAX)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d\n", __func__,
sensor_type);
return ERROR;
}
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 2;
batch_event_count = length;
batch_mode = length > 1 ? BATCH_MODE_RUN : BATCH_MODE_NONE;
//pr_err("[SSP]: %s batch count (%d)\n", __func__, batch_event_count);
// TODO: When batch_event_count = 0, we should not run.
do {
data->get_sensor_data[sensor_type](pchRcvDataFrame, &iDataIdx, &sensorsdata);
// TODO: Integrate get_sensor_data function.
// TODO: get_sensor_data(pchRcvDataFrame, &iDataIdx, &sensorsdata, data->sensor_data_size[sensor_type]);
// TODO: Divide control data batch and non batch.
data->skipEventReport = false;
//if(sensor_type == GYROSCOPE_SENSOR) //check packet recieve time
// pr_err("[SSP_PARSE] bbd event time %lld\n", data->timestamp);
get_timestamp(data, pchRcvDataFrame, &iDataIdx, &sensorsdata, batch_mode, sensor_type);
if (data->skipEventReport == false)
data->report_sensor_data[sensor_type](data, &sensorsdata);
batch_event_count--;
//pr_err("[SSP]: %s batch count (%d)\n", __func__, batch_event_count);
} while ((batch_event_count > 0) && (iDataIdx < iLength));
if (batch_event_count > 0)
pr_err("[SSP]: %s batch count error (%d)\n", __func__, batch_event_count);
data->reportedData[sensor_type] = true;
//pr_err("[SSP]: (%d / %d)\n", iDataIdx, iLength);
ts = get_current_timestamp();
if(ts > 10000000000ULL + data->lastTimestamp[sensor_type]) {
data->lastTimestamp[sensor_type] = ts;
pr_err("[SSP_PARSE] %d late 10 sec sync to %lld\n",sensor_type, ts);
}
break;
case MSG2AP_INST_DEBUG_DATA:
sensor_type = print_mcu_debug(pchRcvDataFrame, &iDataIdx, iLength);
if (sensor_type) {
pr_err("[SSP]: %s - Mcu data frame3 error %d\n", __func__,
sensor_type);
return ERROR;
}
break;
#else
case MSG2AP_INST_BYPASS_DATA:
sensor_type = pchRcvDataFrame[iDataIdx++];
if ((sensor_type < 0) || (sensor_type >= SENSOR_MAX)) {
pr_err("[SSP]: %s - Mcu data frame1 error %d\n", __func__,
sensor_type);
return ERROR;
}
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 2;
sensortime.batch_count = sensortime.batch_count_fixed = length;
sensortime.batch_mode = length > 1 ? BATCH_MODE_RUN : BATCH_MODE_NONE;
sensortime.irq_diff = data->timestamp - data->lastTimestamp[sensor_type];
if (sensortime.batch_mode == BATCH_MODE_RUN) {
if (data->reportedData[sensor_type] == true) {
u64 time;
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[sensor_type]), (s64)length);
if (length > 8)
time = data->adDelayBuf[sensor_type] * 18;
else if (length > 4)
time = data->adDelayBuf[sensor_type] * 25;
else if (length > 2)
time = data->adDelayBuf[sensor_type] * 50;
else
time = data->adDelayBuf[sensor_type] * 130;
if ((sensortime.time_diff * 10) > time) {
data->lastTimestamp[sensor_type] = data->timestamp - (data->adDelayBuf[sensor_type] * length);
sensortime.time_diff = data->adDelayBuf[sensor_type];
} else {
time = data->adDelayBuf[sensor_type] * 11;
if ((sensortime.time_diff * 10) > time)
sensortime.time_diff = data->adDelayBuf[sensor_type];
}
} else {
if (data->lastTimestamp[sensor_type] < (data->timestamp - (data->adDelayBuf[sensor_type] * length))) {
data->lastTimestamp[sensor_type] = data->timestamp - (data->adDelayBuf[sensor_type] * length);
sensortime.time_diff = data->adDelayBuf[sensor_type];
} else
sensortime.time_diff = div64_long((s64)(data->timestamp - data->lastTimestamp[sensor_type]), (s64)length);
}
} else {
if (data->reportedData[sensor_type] == false)
sensortime.irq_diff = data->adDelayBuf[sensor_type];
}
do {
data->get_sensor_data[sensor_type](pchRcvDataFrame, &iDataIdx, &sensorsdata);
get_timestamp(data, pchRcvDataFrame, &iDataIdx, &sensorsdata, &sensortime, sensor_type);
if (sensortime.irq_diff > 1000000)
data->report_sensor_data[sensor_type](data, &sensorsdata);
else if ((sensor_type == PROXIMITY_SENSOR) || (sensor_type == PROXIMITY_RAW)
|| (sensor_type == STEP_COUNTER) || (sensor_type == STEP_DETECTOR)
|| (sensor_type == GESTURE_SENSOR) || (sensor_type == SIG_MOTION_SENSOR))
data->report_sensor_data[sensor_type](data, &sensorsdata);
else
pr_err("[SSP]: %s irq_diff is under 1msec (%d)\n", __func__, sensor_type);
sensortime.batch_count--;
} while ((sensortime.batch_count > 0) && (iDataIdx < iLength));
if (sensortime.batch_count > 0)
pr_err("[SSP]: %s batch count error (%d)\n", __func__, sensortime.batch_count);
data->lastTimestamp[sensor_type] = data->timestamp;
data->reportedData[sensor_type] = true;
break;
case MSG2AP_INST_DEBUG_DATA:
sensor_type = print_mcu_debug(pchRcvDataFrame, &iDataIdx, iLength);
if (sensor_type) {
pr_err("[SSP]: %s - Mcu data frame3 error %d\n", __func__,
sensor_type);
return ERROR;
}
break;
#endif
case MSG2AP_INST_LIBRARY_DATA:
memcpy(&length, pchRcvDataFrame + iDataIdx, 2);
iDataIdx += 2;
ssp_sensorhub_handle_data(data, pchRcvDataFrame, iDataIdx,
iDataIdx + length);
iDataIdx += length;
break;
case MSG2AP_INST_BIG_DATA:
handle_big_data(data, pchRcvDataFrame, &iDataIdx);
break;
case MSG2AP_INST_META_DATA:
sensorsdata.meta_data.what = pchRcvDataFrame[iDataIdx++];
sensorsdata.meta_data.sensor = pchRcvDataFrame[iDataIdx++];
report_meta_data(data, &sensorsdata);
break;
case MSG2AP_INST_TIME_SYNC:
data->bTimeSyncing = true;
break;
case MSG2AP_INST_GYRO_CAL:
pr_info("[SSP]: %s - Gyro caldata received from MCU\n", __func__);
memcpy(caldata, pchRcvDataFrame + iDataIdx, sizeof(caldata));
wake_lock(&data->ssp_wake_lock);
save_gyro_caldata(data, caldata);
wake_unlock(&data->ssp_wake_lock);
iDataIdx += sizeof(caldata);
break;
case SH_MSG2AP_GYRO_CALIBRATION_EVENT_OCCUR:
data->gyro_lib_state = GYRO_CALIBRATION_STATE_EVENT_OCCUR;
break;
}
}
return SUCCESS;
}
int main(int argc, char **argv)
{
int size;
int fd;
unsigned long id;
unsigned char data[8];
char *port = "/dev/can1"; /// use "1" to "4" for steinhoff
int opt;
int status;
int read_err = 0;
int msg_count = 0;
timestamp_t start_ts, end_ts, diff_ts; /// report start and end times
can_err_count_t errs;
while ((opt = getopt(argc, argv, "p:")) != -1) {
switch (opt) {
case 'p':
port = strdup(optarg);
break;
default:
printf("Usage: %s -p <port>\n", argv[0]);
exit(1);
}
}
fd = can_open(port, O_RDONLY);
if (fd == -1)
exit(EXIT_FAILURE); // error message printed by can_open
// Save starting time
get_current_timestamp(&start_ts);
if(setjmp(exit_env) != 0) {
unsigned int i;
get_current_timestamp(&end_ts);
print_timestamp(stdout, &start_ts);
printf(" ");
print_timestamp(stdout, &end_ts);
printf(" ");
decrement_timestamp(&end_ts, &start_ts, &diff_ts);
print_timestamp(stdout, &diff_ts);
printf("\n");
errs = can_get_errs(fd);
if (read_err > 0)
printf(" %d client read errors\n", read_err);
printf("Error counts at finish:\n");
printf("intr_in_handler_count %u\n", errs.intr_in_handler_count);
printf("rx_interrupt_count %u\n", errs.rx_interrupt_count);
printf("rx_message_lost_count %u\n", errs.rx_message_lost_count);
printf("tx_interrupt_count %u\n", errs.tx_interrupt_count);
printf("shadow_buffer_count %u\n", errs.shadow_buffer_count);
if (fd != -1)
status = can_close(&fd);
for (i = 0; i < msg_count; i++) {
save_data_t *p = &saved_data[i];
printf("%u ", i);
printf("%hhu ", p->can_id);
printf("%hd ", p->longitudinal);
printf("%hd ", p->vertical);
printf("%hd ", p->lateral);
printf("%hhu ", p->thread_counter);
printf("%hhu ", p->sensor_group_counter);
printf("\n");
}
if (status != -1)
exit(EXIT_SUCCESS);
else
exit(EXIT_FAILURE); // can_close prints error info
} else
sig_ign(sig_list, sig_hand);
/// Clear error counts in driver
errs = can_clear_errs(fd);
printf("Error counts at start:\n");
printf("intr_in_handler_count %u\n", errs.intr_in_handler_count);
printf("rx_interrupt_count %u\n", errs.rx_interrupt_count);
printf("rx_message_lost_count %u\n", errs.rx_message_lost_count);
printf("tx_interrupt_count %u\n", errs.tx_interrupt_count);
printf("shadow_buffer_count %u\n", errs.shadow_buffer_count);
for(;;) {
id = 0;
size = can_read(fd,&id,(char *)NULL,data,8);
if (size < 0)
read_err++;
else {
saved_data[msg_count].can_id = id;
saved_data[msg_count].longitudinal =
(*(short *) &data[2]);
saved_data[msg_count].vertical =
(*(short *) &data[0]);
saved_data[msg_count].lateral =
(*(short *) &data[6]);
saved_data[msg_count].sensor_group_counter =
data[4];
saved_data[msg_count].thread_counter =
data[5];
}
msg_count++;
if (msg_count >= max_count)
longjmp(exit_env, 1);
}
}
