RaspberryPi[26] プロセス間通信（shared memory）
mack0113複数のセンサーを読み込む場合、センサーの取り込み周期が異なる場合があります。そんな場合、一つのプロセス（プログラム）で作ると全体の速度に依存することがあります。そんな場合は、プロセスを分けるとプログラム自体が楽になり、処理速度も速くできます。今回、初めて共有メモリを介してデータを読むプログラムを書いてみました。
構成はこんな感じです。
GPSのNMEAから緯度経度、速度を算出、風速計から風速と風向を、IMUセンサーから姿勢と3次元加速度を得ます。風速は10Hz、IMUは50Hz、GPSは１Hzなのでそれぞれ別々にプロセスを作り、一番遅いGPSのタイミングでUDPで送信します。IMUを遅延なしで回すと50Hzと早いので50msecでIMUサンプリングをします。それらを移動平均計算し、GPSプロセスに共有メモリを介して姿勢と3次元加速度を渡します。このセンサーHATは他に温度センサーと色センサーも載っていますが、今回は使いませでした。風速計は10Hzなので、その周期で供給メモリに書き込みます。GPSプロセスが各データを受け取るために、それぞれのセンサー用に共有メモリを二つ用意します。一つは風速計用、もう一つはIMUセンサ用です。共有メモリのID（アドレス）はGPSプロセスが作成し、それをファイルとして保存します。風速とIMUはそのファイルを読み込み、そのIDが示す共有メモリにそれぞれのデータを書き込みます。そのため、起動順番はGPSプロセスを一番先に起動させます。また、GPS、IMUセンサーと風速センサーのデータはUSBにすべて記録します。
GPSプロセスは集めたデータを解析したのち、UDPを用いてGPSの受信周期でPCに送ります。PCではVB.NETで作成した表示プログラムで受信して表示させます。
GPSプロセスのコードです。
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <time.h>
#include <sys/ipc.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <sys/socket.h> //socket()
#include <sys/ioctl.h> //ioctl()
#include <arpa/inet.h> //htons(), inet_addr()
#define SERIAL_PORT "/dev/ttyACM0"
#define SERIAL_PORT2 "/dev/ttyUSB0"
#define TRUE 1
#define FALSE 0
#define DATA_SIZE_WIND 256 // shared memory size for wind
#define DATA_SIZE_IMU 512 // shared memory size for imu
#define DATA_SIZE_UDP 512 // data size for udp
#define DATA_SIZE_CMN 255
const int PortNumber = 60000;
const char *IPaddress = "192.168.100.200";
// const char *IPaddress = "192.168.100.165";
// const char *IPaddress = "192.168.1.5";
int gettimeofday();
float chg_deg(float lat)
{
float deg;
int deg_int;
float minutes;
float deg_min;
deg_int = (int)lat/100;
minutes = lat - (deg_int*100 );
deg_min = deg_int + minutes/60.0;
// printf("di:%d mi:%f dm:%f\n",deg_int,minutes,deg_min);
return deg_min;
}
void arg_error()
{
printf("Argment error!! type as below\n");
printf("sudo ./hanki -d 0/1 -ip \"192.168.100.y -id 1\"\n");
printf("-d 1 :show data -d 0: no show data\n");
printf("-ip \"192.168.100.y\": client PC ip address\n");
printf("-id #: uniq RasPi ID <100\n");
exit(0);
}
int main(int argc, char *argv[])
{
unsigned char msg[] = "serial port open...\n";
unsigned char cfn[DATA_SIZE_CMN]; // current file name based on size
unsigned char buf[DATA_SIZE_CMN] = {0};
unsigned char buf_w[DATA_SIZE_CMN] = {0};
unsigned char buf2[128] = {0};
unsigned char ip_addr[32];
int rd;
int fd;
struct termios tio;
int baudRate = B9600;
int datasave;
double d_sec;
int i;
int j;
int k;
int l;
int c;
int tk;
int item;
int content;
int cnt;
int cng_file;
int len;
int len_w;
int ret;
int size;
int disp;
int store_hour;
char header[16];
char host_ip[32];
float gpstime;
float latitude;
float latitude_deg;
int latitude_deg_int;
char nswe;
float longitude;
float longitude_deg;
int longitude_deg_int;
char eorw;
int quality;
int numgps;
float lowrate;
float avobesealevel;
float hightg;
int dgps;
int base;
int csum;
int unit1, unit2, unit3, unit4, unit5, unit6;
float true_c;
float magnetic_c;
float knot_sp;
float kiro_sp;
int mode;
int msec;
int sec;
int nsec;
int ps_msec;
int d_sec_int;
int display;
int send_flag;
int id;
int msg_cnt = 0;
time_t t;
FILE *file;
FILE *file_shm;
struct tm *local;
struct timespec start_time, end_time;
struct tm *ptm;
struct timeval tvToday; //for msec
float temp;
int id_wind;
int id_imu ;
char *shmData_imu;
char *shmData_wind;
char data_wind[DATA_SIZE_WIND];
char data_imu[DATA_SIZE_IMU];
char data_udp[DATA_SIZE_UDP];
struct sockaddr_in addr;
int sock_df;
int shmData_imu_flag = TRUE;
int shmData_wind_flag = TRUE;
int arg_err_flg = FALSE;
char b_unit1[8];
char b_unit2[8];
char b_unit3[8];
char b_unit4[8];
if(argc == 7)
{
if(strcmp(argv[1],"-d") == 0)
{
if(strcmp(argv[2],"1") == 0) display = TRUE;
else display = FALSE;
}
else
{
printf("sudo ./hanki -d 0/1 -ip \"192.168.x.y\" -id #\n");
exit(0);
}
strcpy(host_ip,argv[5]);
printf("%s:%s:%s:%s:%s:%s:%s\n",argv[0],argv[1],argv[2],argv[3],argv[4],argv[5],argv[6]);
// sscanf(argv[7],"%d",&id);
}
else
{
arg_error();
}
// Check argv
arg_err_flg = FALSE;
for(i=0;i<argc;i++)
{
if (strcmp(argv[i],"-d")==0)
{
sscanf(argv[i+1],"%d",&display);
if(display >1) arg_err_flg = TRUE;
}
else if(strcmp(argv[i],"-ip")==0)
{
strcpy(host_ip,argv[i+1]);
}
else if(strcmp(argv[i],"-id")==0)
{
sscanf(argv[i+1],"%d",&id);
if(id>99) arg_err_flg = TRUE;
}
if(arg_err_flg == TRUE) arg_error();
}
printf("display:%d host_ip:%s id:%d\n",display,host_ip,id);
/*
if(argc >1) display = TRUE;
else display = FALSE;
*/
sock_df = socket(AF_INET, SOCK_DGRAM, 0);
if(sock_df < 0)
{
perror("Couldn't make a socket");
return -1;
}
// 通信の設宁E
addr.sin_family = AF_INET; //IPv4を指宁E
addr.sin_port = htons(PortNumber);
//ポぅト番号。ここでは60000を指宁E
addr.sin_addr.s_addr = inet_addr(host_ip); //サーバぅ側のアドレス
ssize_t send_status;
// Shared memory create a new with IPC_CREATE
if((id_wind = shmget(IPC_PRIVATE, DATA_SIZE_WIND, IPC_CREAT|0666)) == -1)
{
perror("shmget()");
return -1;
}
// Shared memory create a new with IPC_CREATE
if((id_imu = shmget(IPC_PRIVATE, DATA_SIZE_IMU, IPC_CREAT|0666)) == -1)
{
perror("shmget()");
return -1;
}
// Attach shared memory for IMU
if((shmData_imu = (char *)shmat(id_imu, NULL, 0)) == (void *)-1)
{
printf(" imu shard memory error\n");
perror("shmat()");
}
// Attach shared memory for WIND
if((shmData_wind = (char *)shmat(id_wind, NULL, 0)) == (void *)-1)
{
printf(" wind shard memory error\n");
perror("shmat()");
}
if(display == TRUE) printf("Shm ID_WIND:%d ID_IMU:%d\n",id_wind,id_imu);
file_shm = fopen("shared_id.txt","w");
fprintf(file_shm,"%d,%d",id_wind,id_imu);
fclose(file_shm);
// while(1);
fd = open(SERIAL_PORT, O_RDWR);
if (fd < 0)
{
printf("serial 1 open error\n");
return -1;
}
tio.c_cflag += CREAD;
tio.c_cflag += CLOCAL;
tio.c_cflag += CS8;
tio.c_cflag += 0;
tio.c_cflag += 0;
cfsetispeed( &tio, baudRate );
cfsetospeed( &tio, baudRate );
cfmakeraw(&tio);
tcsetattr( fd, TCSANOW, &tio );
tcsetattr( rd, TCSANOW, &tio );
ioctl(fd, TCSETS, &tio);
ioctl(rd, TCSETS, &tio);
cnt = 0;
cng_file = 1;
shmData_imu_flag = TRUE;
shmData_wind_flag = TRUE;
clock_gettime(CLOCK_MONOTONIC, &start_time);
while(1)
{
clock_gettime(CLOCK_MONOTONIC, &start_time);
if(cng_file == 1) // generate file name
{
t = time(NULL);
local = localtime(&t);
sprintf(cfn,"/media/pi/NTSEL/HANKI/GPS/gps%02d%02d%02d%02d.txt",local->tm_mon+1,local->tm_mday,local->tm_hour,local->tm_min);
file = fopen(cfn,"a");
cng_file = 0;
store_hour = local->tm_hour;
// printf("File create %s\n",cfn);
}
strcpy(buf,"");
len = read(fd, buf, sizeof(buf));
// printf("buf:%s\n",buf);
//check header
sscanf(buf,"%[^,]",header);
if(strcmp("$GPGGA",header) == 0)
{
// printf("\rbuf1:%s\n",buf);
// printf("header:%s\n",header);
sscanf(buf,"%[^,],%f,%f,%c,%f,%c,%d,%d,%f,%f,%d,%f,%d,%d,%d,%d",
header,&gpstime,&latitude,&nswe,&longitude,&eorw,&quality,&numgps,&lowrate,&avobesealevel,&unit1,&hightg,&unit2,&dgps,&base,&csum);
// printf("header:%s gpstime:%f lat:%f lng:%f asl:%f\n",header,gpstime,latitude,longitude,avobesealevel);
latitude_deg = chg_deg(latitude);
longitude_deg = chg_deg(longitude);
latitude_deg_int = (int)latitude_deg;
longitude_deg_int = (int)longitude_deg;
if((latitude_deg_int > 37) || (latitude_deg_int < 32)) latitude_deg = 0;
if((longitude_deg_int > 140) || (longitude_deg_int < 134)) longitude_deg = 0;
send_flag = TRUE;
}
if(strcmp("$GPVTG",header) == 0)
{
// printf("\rbuf_VTG:%s\n",buf);
// printf("\rheader:%s\n",header);
c=0;
for(i=0;i<50;i++)
{
buf2[c] = buf[i];
if(buf[i]==',')
{
if(buf[i+1] == ',')
{
buf2[c+1] = 0x30;
c++;
}
}
if(buf[i] =='*') break;
c++;
}
buf2[c] = '\0';
// printf("\r\nbus2:%s\n",buf2);
sscanf(buf2,"%[^,],%f,%[^,],%f,%[^,],%f,%[^,],%f",header,&true_c,b_unit1,&magnetic_c,b_unit2,&knot_sp,b_unit3,&kiro_sp);
// printf("h:%s,t:%f,u:%s,m:%f,u:%s,k:%f,u%s,s:%f\n",header,true_c,b_unit1,magnetic_c,b_unit2,knot_sp,b_unit3,kiro_sp);
/*
sscanf(buf,"%[^,],%f,%c,%f,%c,%f,%c,%f,%c,%c,%x",
header,&true_c,&unit1,&magnetic_c,&unit2,&knot_sp,&unit3,&kiro_sp,&unit3,&mode,&csum);
if((true_c > 360) || (true_c < 0))true_c = 999.9;
printf("\rbuf_VTG:%s\n",buf);
printf("CHK_BUF: %s,%f,%c,%f,%c,%f,%c,%f,%c,%c,%x",
header,true_c,unit1,magnetic_c,unit2,knot_sp,unit3,&kiro_sp,unit3,mode,csum);
printf("\rVTG ->header:%s knot:%f speed:%f %x true_c:%f\n",header,knot_sp,kiro_sp,unit6,true_c);
*/
}
cnt++;
t = time(NULL);
local = localtime(&t);
ret = gettimeofday(&tvToday,NULL); //Today
ptm = localtime(&tvToday.tv_sec);
// printf("%02d/%02d %02d:%02d:%02d:%03d\n",ptm->tm_mon+1,ptm->tm_mday,ptm->tm_hour,ptm->tm_min,ptm->tm_sec,tvToday.tv_usec/1000);
// printf("%02d/%02d %02d:%02d:%02d\n",local->tm_mon+1,local->tm_mday,local->tm_hour,local->tm_min,local->tm_sec);
strcpy(buf_w,"");
sprintf(buf_w,"ID:%02d,MCT:%06d,TIME:%02d/%02d,%02d:%02d:%02d:%03d LAT:%02.6f LNG:%02.6f QAL:%1d ASL:%05.1f SPD:%05.1f TRC:%05.1f",
id,msg_cnt,
ptm->tm_mon+1,ptm->tm_mday,ptm->tm_hour,ptm->tm_min,ptm->tm_sec,tvToday.tv_usec/1000,latitude_deg,longitude_deg,quality,avobesealevel,kiro_sp,true_c);
if(display == TRUE) printf("\n\rbuf_w > %s",buf_w);
if(send_flag == TRUE)
{
fprintf(file,"%s",buf_w);
send_flag = FALSE;
}
if(store_hour != local->tm_hour)
{
fclose(file);
cng_file = 1;
cnt = 0;
}
//check timer
clock_gettime(CLOCK_MONOTONIC, &end_time);
sec = end_time.tv_sec - start_time.tv_sec;
nsec = end_time.tv_nsec - start_time.tv_nsec;
if(nsec < 0) nsec = 1000000-nsec;
d_sec = (double)sec + (double)nsec / (1000 * 1000 * 1000);
msec = (int)(d_sec*1000);
// printf("\n 1 d_sec:%f sec:%d msec:%d nsec:%d\n",d_sec,sec,msec,nsec);
clock_gettime(CLOCK_MONOTONIC, &start_time);
// Shared memory convert char address for IMU
// update data_imu if IMU shared memory updated.
if (strcmp(shmData_imu, data_imu) != 0)
{
if (strcmp(shmData_imu, data_imu) != 0)
{
// printf("%s\n\r", shmData_imu);
strcpy(data_imu, shmData_imu);
}
}
// Shared memory convert char address for WIND
// update data_imu if WIND shared memory updated.
if (strcmp(shmData_wind, data_wind) != 0)
{
if (strcmp(shmData_wind, data_wind) != 0)
{
// printf("%s\n\r", shmData_wind);
strcpy(data_wind, shmData_wind);
}
}
//Transmite data to UDP server.
// buf_w,data_wind,data_imu
sprintf(data_udp,"%s %s %s",buf_w,data_wind,data_imu);
if(display == TRUE) printf("\n\rdata_udp:%s\n",data_udp);
// 送信
send_status = sendto(sock_df, &data_udp , sizeof(data_udp) , 0,
(struct sockaddr *)&addr, sizeof(addr) );
msg_cnt++;
// 送信失敁E
if(send_status < 0)
{
perror("send error");
return -1;
}
/*
memset(data_wind,0,DATA_SIZE_WIND*sizeof(data_wind[0]));
memset(data_imu,0,DATA_SIZE_IMU*sizeof(data_imu[0]));
memset(shmData_wind,0,DATA_SIZE_WIND*sizeof(shmData_wind[0]));
memset(shmData_imu,0,DATA_SIZE_IMU*sizeof(shmData_imu[0]));
*/
}
fclose(file);
exit(0);
}
IMUのセンサーのプログラムです。IMUセンサーはメーカのドライバーやライブラリをインストールしてください。基本サンプルプログラムを使用しています。
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <time.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <unistd.h>
#include "../Sense_HAT_C_Pi/RaspberryPi/IMU/bcm2835/lib/IMU.h"
#define SERIAL_PORT "/dev/ttyACM0"
#define SERIAL_PORT2 "/dev/ttyUSB0"
#define TRUE 1
#define FALSE 0
#define NUM_MV_ROLL 10
#define NUM_MV_PITCH 10
#define NUM_MV_YAW 10
#define NUM_MV_ACEL_X 10
#define NUM_MV_ACEL_Y 10
#define NUM_MV_ACEL_Z 10
#define NUM_MV_CMMON 40
#define SAVE_INTERVAL 100 //msec
#define ACCEL_COVERT 16384.0
IMU_ST_ANGLES_DATA stAngles;
IMU_ST_SENSOR_DATA stGyroRawData;
IMU_ST_SENSOR_DATA stAccelRawData;
IMU_ST_SENSOR_DATA stMagnRawData;
int gettimeofday();
int main(int argc, char *argv[])
{
unsigned char msg[] = "serial port open...\n";
unsigned char cfn[255]; // current file name based on size
unsigned char buf[255] = {0};
unsigned char buf_w[255] = {0};
int fd;
int rd;
struct termios tio;
int baudRate = B9600;
int datasave;
double d_sec;
int i;
int j;
int k;
int l;
int tk;
int item;
int content;
int cnt;
int cng_file;
int len;
int len_w;
int ret;
int size;
int disp;
int store_hour;
int cnt_mv_roll, cnt_mv_pitch, cnt_mv_yaw, cnt_mv_acel_x, cnt_mv_acel_y, cnt_mv_acel_z;
float buf_roll[NUM_MV_CMMON], buf_pitch[NUM_MV_CMMON], buf_yaw[NUM_MV_CMMON];
int buf_acel_x[NUM_MV_CMMON], buf_acel_y[NUM_MV_CMMON], buf_acel_z[NUM_MV_CMMON];
float sum_roll, sum_pitch, sum_yaw;
int sum_acel_x, sum_acel_y, sum_acel_z;
float ave_roll, ave_pitch, ave_yaw;
int ave_acel_x, ave_acel_y, ave_acel_z;
int mode;
int msec;
int sec;
int nsec;
int ps_msec;
int d_sec_int;
int display;
int id_wind;
int id_imu;
time_t t;
FILE *file;
FILE *file_shm;
struct tm *ptm;
struct timeval tvToday; //for msec
struct tm *local;
struct timespec start_time, end_time;
float temp;
IMU_ST_ANGLES_DATA stAngles;
IMU_ST_SENSOR_DATA stGyroRawData;
IMU_ST_SENSOR_DATA stAccelRawData;
IMU_ST_SENSOR_DATA stMagnRawData;
int id;
char *shmData;
// GPSプロセスが書いた共有メモリのIDが書かれたファイルを読む。
file_shm = fopen("shared_id.txt","r");
if(file_shm == 0){
printf("Can't open shared_id.txt\n");
exit(0);
}
fscanf(file_shm,"%d,%d",&id_wind,&id_imu);
// IMUの初期化
imuInit();
cnt = 0;
cng_file = 1;
cnt_mv_roll = 0;
cnt_mv_pitch = 0;
cnt_mv_yaw = 0;
cnt_mv_acel_x = 0;
cnt_mv_acel_y = 0;
cnt_mv_acel_z = 0;
// デバッグ用
if(argc > 1) display = 1;
else display = 0;
// 共有メモリにアタッチ
if((shmData = (char *)shmat(id_imu, 0, 0)) == (void *)-1) {
perror("shmat()");
exit(EXIT_FAILURE);
}
while(1) {
// 時間計測用の時計スタート
clock_gettime(CLOCK_MONOTONIC, &start_time);
// USBにデータを書き込むためのファイル名の生成 一時間ごとに新しいファイルを作る。
if(cng_file == 1){ // generate file name
t = time(NULL);
local = localtime(&t);
sprintf(cfn,"/media/pi/USBNAME/imu%02d%02d%02d%02d.txt",local->tm_mon+1,local->tm_mday,local->tm_hour,local->tm_min);
file = fopen(cfn,"a");
cng_file = 0;
store_hour = local->tm_hour;
}
// IMU のデータを取得
imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
usleep(50000); // sleep for 50msec 50msec 周期で共有メモリにデータを書くため
// Check Timer 経過時間を知るために
clock_gettime(CLOCK_MONOTONIC, &end_time);
sec = end_time.tv_sec - start_time.tv_sec;
nsec = end_time.tv_nsec - start_time.tv_nsec;
if(nsec < 0) nsec = 1000000-nsec;
d_sec = (double)sec + (double)nsec / (1000 * 1000 * 1000);
msec = (int)(d_sec*1000);
// printf("\n 1 d_sec:%f sec:%d msec:%d nsec:%d\n",d_sec,sec,msec,nsec);
clock_gettime(CLOCK_MONOTONIC, &start_time);
// 移動平均の計算
for(i=0;i<NUM_MV_CMMON-1;i++){
sum_roll += buf_roll[i] = buf_roll[i+1];
sum_pitch += buf_pitch[i] = buf_pitch[i+1];
sum_yaw += buf_yaw[i] = buf_yaw[i+1];
sum_acel_x += buf_acel_x[i] = buf_acel_x[i+1];
sum_acel_y += buf_acel_y[i] = buf_acel_y[i+1];
sum_acel_z += buf_acel_z[i] = buf_acel_z[i+1];
}
sum_roll += buf_roll[NUM_MV_CMMON-1] = stAngles.fRoll;
sum_pitch += buf_pitch[NUM_MV_CMMON-1] = stAngles.fPitch;
sum_yaw += buf_yaw[NUM_MV_CMMON-1] = stAngles.fYaw;
sum_acel_x += buf_acel_x[NUM_MV_CMMON-1] = stAccelRawData.s16X;
sum_acel_y += buf_acel_y[NUM_MV_CMMON-1] = stAccelRawData.s16Y;
sum_acel_z += buf_acel_z[NUM_MV_CMMON-1] = stAccelRawData.s16Z;
ave_roll = sum_roll/NUM_MV_CMMON;
ave_pitch = sum_pitch/NUM_MV_CMMON;
ave_yaw = sum_yaw/NUM_MV_CMMON;
ave_acel_x = sum_acel_x/NUM_MV_CMMON;
ave_acel_y = sum_acel_y/NUM_MV_CMMON;
ave_acel_z = sum_acel_z/NUM_MV_CMMON;
// Check Timer
// clock_gettime(CLOCK_MONOTONIC, &start_time);
clock_gettime(CLOCK_MONOTONIC, &end_time);
sec = end_time.tv_sec - start_time.tv_sec;
nsec = end_time.tv_nsec - start_time.tv_nsec;
if(nsec < 0) nsec = 1000000-nsec;
d_sec = (double)sec + (double)nsec / (1000 * 1000 * 1000);
msec = (int)(d_sec*1000);
// printf("\n 2 i:%d d_sec:%f sec:%d msec:%d nsec:%d\n",i,d_sec,sec,msec,nsec);
clock_gettime(CLOCK_MONOTONIC, &start_time);
// msec を計算するために
ret = gettimeofday(&tvToday,NULL); //Today
ptm = localtime(&tvToday.tv_sec);
sum_roll = 0;
sum_pitch = 0;
sum_yaw = 0;
sum_acel_x = 0;
sum_acel_y = 0;
sum_acel_z = 0;
// 共有メモリに書き込むデータの成型
sprintf(buf,"AVR:%06.2f,AVP:%06.2f,AVW:%07.2f,AVX:%05.2f,AVY:%05.2f,AVZ:%05.2f,RWR:%06.2f,RWP:%06.2f,RWW:%07.2f,RWX:%05.2f,RWY:%05.2f,RWZ:%05.2f",
ave_roll,ave_pitch,ave_yaw,ave_acel_x/ACCEL_COVERT, ave_acel_y/ACCEL_COVERT, ave_acel_z/ACCEL_COVERT,stAngles.fRoll, stAngles.fPitch, stAngles.fYaw,
stAccelRawData.s16X/ACCEL_COVERT, stAccelRawData.s16Y/ACCEL_COVERT, stAccelRawData.s16Z/ACCEL_COVERT);
// USBのデータには時刻を付加
sprintf(buf_w,"TIME:%02d/%02d-%02d:%02d:%02d:%03d %s\n",ptm->tm_mon+1,ptm->tm_mday,ptm->tm_hour,ptm->tm_min,ptm->tm_sec,tvToday.tv_usec/1000,buf);
if(display == TRUE) printf("%s",buf_w);
fprintf(file,"%s",buf_w);
cnt++;
t = time(NULL);
local = localtime(&t);
// 時間が一時間たったらファイルを更新
if(store_hour != local->tm_hour) {
fclose(file);
cng_file = 1;
cnt = 0;
}
// 共有メモリにコピー
strcpy(shmData, buf);
if(display == TRUE) printf("IMU:%s\n", shmData);
cnt++;
}
fclose(file);
exit(0);
}共有メモリのアタッチのループの中で何回も行うとメモリ食ってプロセスが死にます。なので、ここでは、一回だけアタッチするようにしてみました。毎回、デッタチすればループの中でも動くとは思います。
風速計です。RS422でNMEAでシリアルデータが来ます。今回はRS422－USB変換器を使用してUSBで取り込んでいます。直接RS422で取り込んだほうが安いかも。
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <time.h>
#include <stdlib.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <unistd.h>
#define SERIAL_PORT "/dev/ttyACM0"
#define SERIAL_PORT2 "/dev/ttyUSB0"
#define TRUE 1
#define FALSE 0
#define SAVE_INTERVAL 100 //msec
int gettimeofday();
int main(int argc, char *argv[])
{
unsigned char msg[] = "serial port open...\n";
unsigned char cfn[255]; // current file name based on size
unsigned char buf[255] = { 0 };
unsigned char buf_w[255] = { 0 };
unsigned char buf_shm[255] = { 0 };
// char lbuf[255] = {0};
unsigned char DATA[20][11] = { 0, 0 };
int fd;
int rd;
int len;
struct termios tio;
int baudRate = B38400;
int datasave;
double d_sec;
int i;
int cnt;
int cnt1;
int cng_file;
int msec;
int sec;
int nsec;
int ps_msec;
int d_sec_int;
int store_hour;
int ret;
int display = 1;
int id_wind;
int id_imu;
time_t t;
struct tm *ptm;
struct timeval tvToday; //for msec
FILE *file;
FILE *file_shm;
struct tm *local;
char header[8];
int deg;
char ref;
char unit;
char status;
float wind_speed;
char csum[8];
struct timespec start_time, end_time;
float temp;
char *shmData;
rd = open(SERIAL_PORT2, O_RDWR);
if (rd < 0)
{
printf("serial 2 open error\n");
return -1;
}
tio.c_cflag += CREAD;
tio.c_cflag += CLOCAL;
tio.c_cflag += CS8;
tio.c_cflag += 0;
tio.c_cflag += 0;
cfsetispeed(&tio, baudRate);
cfsetospeed(&tio, baudRate);
cfmakeraw(&tio);
tcsetattr(fd, TCSANOW, &tio);
tcsetattr(rd, TCSANOW, &tio);
ioctl(fd, TCSETS, &tio);
ioctl(rd, TCSETS, &tio);
cnt = 0;
cnt1 = 0;
cng_file = 1;
// clock_gettime(CLOCK_MONOTONIC, &start_time);
if (argc > 1) display = 1;
else display = 0;
file_shm = fopen("shared_id.txt", "r");
if (file_shm == 0)
{
printf("Can't open shared_id.txt\n");
exit(0);
}
fscanf(file_shm, "%d,%d", &id_wind, &id_imu);
if (display == TRUE) printf("Shm ID_WIND:%d ID_IMU:%d\n", id_wind, id_imu);
if ((shmData = (char *)shmat(id_wind, 0, 0)) == (void *)-1)
{
perror("shmat()");
exit(EXIT_FAILURE);
}
while (1)
{
clock_gettime(CLOCK_MONOTONIC, &start_time);
if (cng_file == 1) // generate file name
{
t = time(NULL);
local = localtime(&t);
sprintf(cfn, "/media/pi/USBNAME/wind%02d%02d%02d%02d.txt", local->tm_mon+1, local->tm_mday, local->tm_hour, local->tm_min);
file = fopen(cfn, "a");
cng_file = 0;
store_hour = local->tm_hour;
// printf("File create %s\n",cfn);
}
strcpy(buf, "");
len = read(rd, buf, sizeof(buf));
sscanf(buf, "%[^,],%d,%c,%f,%c,%d,%s", header, °, &ref, &wind_speed, &unit, &csum);
// printf("%s,%d,%c,%2.1f,%c\n",header,deg,ref,wind_speed,unit);
sprintf(buf_shm, "WDG:%03d,WRF:%c,WSP:%04.1f", deg, ref, wind_speed);
local = localtime(&t);
ret = gettimeofday(&tvToday, NULL); //Today
ptm = localtime(&tvToday.tv_sec);
// printf("%02d/%02d %02d:%02d:%02d:%03d\n",ptm->tm_mon+1,ptm->tm_mday,ptm->tm_hour,ptm->tm_min,ptm->tm_sec,tvToday.tv_usec/1000);
// printf("%02d/%02d %02d:%02d:%02d\n",local->tm_mon+1,local->tm_mday,local->tm_hour,local->tm_min,local->tm_sec);
sprintf(buf_w, "%02d/%02d %02d:%02d:%02d:%03d %s", ptm->tm_mon+1, ptm->tm_mday, ptm->tm_hour, ptm->tm_min, ptm->tm_sec, tvToday.tv_usec/1000, buf);
if (display == TRUE) printf("%s", buf_w);
fprintf(file, "%s", buf_w);
cnt++;
t = time(NULL);
local = localtime(&t);
if (store_hour != local->tm_hour)
{
fclose(file);
cng_file = 1;
cnt = 0;
}
strcpy(shmData, buf_shm);
if (display == TRUE) printf("WIND:%s\n", shmData);
}
fclose (file);
exit(0);
}