ProgrammingWighSSM - PC-Yamabico Wiki - PC-Yamabico

This is a sample program demonstrating how to use SSM library.

---

Software structure of programs using SSM library

Programs which utilize SSM library always consist at least two parts: the sensor driver side which continuously registers data from sensor to SSM; the user program side which read data from SSM at any timing.

Must-Define Parameters

If you want to introduce a new sensor, the sensor must have the corresponding driver to use. When you create a new sensor driver, you must determine the following points.

• The name of the sensor
• The data type of the sensor
• The method to obtain timestamp
• The data type of the sensor properties (If needed)

The name of the sensor is required when calling the sensor from user program. And because it is necessary for the sensor driver and program that uses the sensor’s data to share the same data type, it is required to provide a header file.

The basic starting sequence

As a step in the user program:

• Use initSSM() to initialize,
• Use openSSM(sensor name, ID, 0) to open utilizing sensor,
• Use readSSM or readSSM_time to read the sensor’s data.

As a specific concept of SSM, there are the sensor ID(SSM_sid type) and time ID(int type). Sensor ID is like a file pointer that is required when accessing the sensor. Sensor ID can be obtained by openSSM or createSSM. The time ID is the serial number of the sensor data that is incremented every time the data is input.

The following is the available functions as well as samples of driver and user program.

---

Functions

Initialization:

• int initSSM(void)

Register & Open:

• SSM_sid createSSM(char* name, int ID, int of data, double period, double period of record) return value: sensor ID
• SSM_sid openSSM(char* name, int ID, 0) return value: sensor ID

Read & write (shared memory access only):

• int writeSSM(SSM_sid SID, char* of data to write, double time);
• int readSSM(SSM_sid SID, char* of data to read, double* of measure time variable, int time ID) return value: time ID
• int readSSM_time(SSM_sid SID, char* of data to read, double time, double* of measure time variable) return value: time ID

Read & write the sensor properties:

• set_propertySSM(char* name, int ID, char* of data to register, int of data to register);
• get_propertySSM(char* name, int ID, char* of data to acquire);

---

Sample of writing data (the sensor driver)

Here we will illustrate the example to write data into SSM. The following is a sample that applies SSM to target sensor, SensorA.

First, the register sensor name of SensorA is defined as:

sensor_A.

Although the name is at will, it is better to name it long and clearly because it will also be used such as the logging procedure.

Next, suppose that you define the following data type as sensor_A.h:

 Sensor_A.h
 
 #ifndef __SENSOR_A__
 #define __SENSOR_A__
 
 typedef struct{
  double a;
  double b;
  int    c;
 }SensorA;
 
 #endif

In this case, to read the data of SensorA and write it into SSM, the program is as follows.

 SensorA_driver.c
 
 #include<ssm_common.h>
 #include<Sensor_A.h>
 #include<unistd.h>
 
 //instead of read data from real sensor, we generate some appropriate data
 void receive_sensorA(SensorA *data, double *time){
  static double a,b;
  static int c;
  a+=0.1;
  b+=0.01;
  c++;
  data->a = a;
  data->b = b;
  data->c = c;
  *time = gettimeSSM();
  usleep(100000);
 }
 
 int main(int argc, char *argv[]){
  SensorA sens_data;
  double measured_time;
  SSM_sid sensA_sid;  
 
  initSSM();
  //sensor_A as sensor name, 0 as the ID, the data that comes in every 0.1 second will be stored for 10 seconds. 
  sensA_sid = createSSM_time("sensor_A", 0, sizeof(SensorA), 10, 0.1);

  while(1){
       //The function that uses some kind of method (for example, serial communication) to get data from SensorA, and that estimate the time period of data observation. 
   receive_sensorA(&sens_data, &measured_time);
       //Write the data into SSM.
   writeSSM(sensA_sid, (char*)&sens_data, measured_time);
  }
 }  

---

Sample of reading data (the user program)

Here is a sample program as a user program that uses sensor data.

You can use a variety of methods to access the sensor information in the SSM. The sample program will illustrate these four kinds:

• The access to latest data
• The access to entire data
• The access to past data
• Synchronization of multiple sensors

The access to latest data It is available to access the most recent data by placing a -1 in the time ID, the last argument of readSSM. To read the latest data of SensorA every second is as follows.

 SensorA_print.c 

 #include<ssm_common.h>
 #include<Sensor_A.h>
 #include<unistd.h>
 #include<stdio.h>
 
 int main(int argc, char *argv[]){
  SensorA sens_data;
  double measured_time;
  SSM_sid sensA_sid; 

  initSSM();
  sensA_sid = openSSM("sensor_A", 0, 0);
 
  while(1){
   readSSM(sensA_sid, (char*)&sens_data, &measured_time, -1);
   printf("time=%f a=%f b=%f c=%d\n",measured_time, sens_data.a, sens_data.b, sens_data.c);
   sleep(1);
  }
 }

In this case, it is not necessary to use all the data at the earliest period of the sensor update cycle.

Reading the data sequentially

If you want to calculate, such as odometry, on your own, you will need to use the entire data which are obtained by sensor. Following is an example illustrating that use the time ID to load data obtained by SensorA sequentially.

 SensorA_print2.c 

 #include<ssm_common.h>
 #include<Sensor_A.h>
 #include<unistd.h>
 #include<stdio.h>
 
 int main(int argc, char *argv[]){
  SensorA sens_data;
  double measured_time;
  SSM_sid sensA_sid;
  int tid;
 
  initSSM();
  sensA_sid = openSSM("sensor_A", 0, 0);
     //get the TID of latest data
  tid = readSSM(sensA_sid, (char*)&sens_data, &measured_time, -1);
  while(1){
   while(readSSM(sensA_sid, (char*)&sens_data, &measured_time, tid) <= 0){
     usleep(20000);  //Wait to see if the data is coming or not
   }
   printf("time=%f tid=%d a=%f b=%f c=%d\n",measured_time, tid, sens_data.a, sens_data.b, sens_data.c);
   tid++;
  }
 }

readSSM returns value below 0 if the sensor data for the specified TID is not exist.

The access to historical data By specifying the time ID or time, you can read the data in the past.

 SensorA_print3.c 

 #include<ssm_common.h>
 #include<Sensor_A.h>
 #include<unistd.h>
 #include<stdio.h>
 
 int main(int argc, char *argv[]){
  SensorA sens_data;
  double measured_time;
  SSM_sid sensA_sid;
  int tid;
  double now_time;
 
  initSSM();
  sensA_sid = openSSM("sensor_A", 0, 0);
 
  while(1){
   now_time = gettimeSSM();  //get current time

       //Obtain data for one second before current time
   tid = readSSM_time(sensA_sid, (char*)&sens_data, now_time-1, &measured_time);
   printf("time=%f tid=%d a=%f b=%f c=%d\n",measured_time, tid, sens_data.a, sens_data.b, sens_data.c);
       //obtain one of the data after that
   tid++;
   readSSM(sensA_sid, (char*)&sens_data, &measured_time, tid);
   printf("time=%f tid=%d a=%f b=%f c=%d\n\n",measured_time, tid, sens_data.a,  sens_data.b, sens_data.c);
   
   sleep(1); 
  }
 }

Retrieving same time data from different sensors By specifying the time, you can read the data obtained at the same time by different sensors.

 SensorAB_print.c 

 #include<ssm_common.h>
 #include<unistd.h>
 #include<stdio.h>
 #include<Sensor_A.h>
 #include<Sensor_B.h> 
 
 int main(int argc, char *argv[]){
  SensorA sens_dataA;
  SensorB sens_dataB;
  double measured_time;
  SSM_sid sensA_sid, sensB_sid;
  int tid;
  double time;

  initSSM();
 
  sensA_sid = openSSM("sensor_A", 0, 0);
  sensB_sid = openSSM("sensor_B", 0, 0);
  
  while(1){
       //obtain the latest data of SensorA
   tid = readSSM(sensA_sid, (char*)&sens_dataA, &measured_time, -1);

       //Obtain the same time data (closest, in the past)
   readSSM_time(sensB_sid, (char*)&sens_dataB, measured_time, &time);
   printf("timeA=%f timeB=%f A.a=%f B.a=%f",measured_time,time,sens_dataA.a, sens_dataB.a);   
   sleep(1); 
  }
 }

Register & read the static information (properties) of sensor

Sensor’s static information recording function had been added at 6/30. It is available to record the specific information of the sensor, such as position, step angle, Wheel diameter and etc.

Function:

• set_propertySSM(char* name, int ID, char* of data to register, int of data to register);
• get_propertySSM(char* name, int ID, char* of data to obtain);

Record the sensor properties

First, determine the data type. For example:

 typedef struct{ 
  double a;
  char name[10];
 }SensorA_Property;

Etc. Then, register:

 SensorA_Property sensA_property;

  sensA_property.a = 100;
  sprintf(sensA_property.name, "SensorA");
  set_propertySSM("sensor_A", 0, &sensA_property, sizeof(SensorA_Property));

Obtain the sensor properties

 SensorA_Property sensA_property;

 get_propertySSM("sensor_A", 0, &sensA_property);
 printf("%s %f\n",sensA_property.name, sensA_property.a)