COSMOS core  1.0.2 (beta)
Comprehensive Open-architecture Solution for Mission Operations Systems
Functions | Variables
razor_imu.cpp File Reference
#include "rw_lib.h"
#include "cssl_lib.cpp"
#include "agentlib.h"
#include "satlib.h"
#include <stdio.h>
#include <unistd.h>
#include <string.h>
Include dependency graph for razor_imu.cpp:

Functions

void setup ()
 
int process ()
 
char * request_send_command (char *request, char *output)
 
long int doChecksum (char *string)
 
int main (int argc, char *argv[])
 
int process (char *pbuf)
 

Variables

int count
 
int i
 
int iretn
 
FILE * fp
 
char cmds [15][50]
 
char buf [128]
 
char ibuf [1000]
 
char arg1str [100] = ""
 
char arg2str [100] = ""
 
int arg2int = 0
 
jstring mainjstring ={0,0,0}
 
cosmosstruc cosmos_data
 

Function Documentation

void setup ( )
120  {
121  i = rw_connect((char *)"/dev/tty.usbmodem1d11");
122 
123  if (i != 0)
124  {
125  printf("Could not connect to reaction wheel in port (%s)", RW_DEVICE);
126  printf("\n");
127  exit(1);
128  } else {
129  printf("Sucessfully connected to %s\n",RW_DEVICE);
130  }
131 
132  strcpy(cmds[0], "$IMU,GET,ALL*34A\n");
133  strcpy(cmds[1], "$IMU,GET,YAW*362\n");
134  strcpy(cmds[2], "$IMU,STREAM,START*4EB\n");
135  strcpy(cmds[3], "$IMU,STREAM,STOP*4A3\n");
136  strcpy(cmds[4], "$IMU,GET,ALL*34A\n");
137  strcpy(cmds[5], "$IMU,GET,YAW*362\n");
138  strcpy(cmds[6], "$IMU,STREAM,START*4EB\n");
139  strcpy(cmds[7], "$SET,RW,SPD,-17.0*441\n");
140  strcpy(cmds[8], "$SET,RW,SPD,-17.0*441\n");
141  strcpy(cmds[9], "$SET,RW,ACC,10.0*3ED\n");
142  strcpy(cmds[10], "$SET,RW,ACC,10.0*3ED\n");
143  strcpy(cmds[11], "$SET,RW,SPD,0.0*3DC\n");
144  strcpy(cmds[12], "$SET,RW,SPD,-17.0*441\n");
145  strcpy(cmds[13], "$SET,RW,SPD,-17.0*441\n");
146  strcpy(cmds[14], "$SET,RW,ACC,10.0*3ED\n");
147  strcpy(cmds[15], "$IMU,STREAM,START*4EB\n");
148  strcpy(cmds[16], "$SET,RW,ACC,10.0*3ED\n");
149  strcpy(cmds[17], "$SET,RW,ACC,10.0*3ED\n");
150  strcpy(cmds[18], "$SET,RW,SPD,0.0*3DC\n");
151  strcpy(cmds[19], "$SET,RW,SPD,-17.0*441\n");
152  strcpy(cmds[20], "$SET,RW,SPD,-17.0*441\n");
153  strcpy(cmds[21], "$SET,RW,ACC,10.0*3ED\n");
154  strcpy(cmds[22], "$SET,RW,ACC,10.0*3ED\n");
155  strcpy(cmds[23], "$SET,RW,ACC,10.0*3ED\n");
156  strcpy(cmds[24], "$SET,RW,ACC,10.0*3ED\n");
157  strcpy(cmds[25], "$SET,RW,ACC,10,SPD,16*535\n");
158  strcpy(cmds[26], "$SET,RW,ACC,-10,SPD,0*52B\n");
159  strcpy(cmds[27], "$SET,RW,SPD,0.0*3DC\n");
160  strcpy(cmds[28], "$IMU,STREAM,START*4EB\n");
161  strcpy(cmds[29], "$SET,RW,ACC,10.0*3ED\n");
162  strcpy(cmds[30], "$SET,RW,ACC,10.0*3ED\n");
163  strcpy(cmds[31], "$SET,RW,SPD,0.0*3DC\n");
164  strcpy(cmds[32], "$IMU,STREAM,STOP*4A3\n");
165 
166  printf("Table of commands: set\n");
167 }
cmds
char cmds[15][50]
Definition: razor_imu.cpp:41
i
int i
Definition: razor_imu.cpp:39
int process ( )
char * request_send_command ( char *  request,
char *  output 
)
116 {
117 
118 }
long int doChecksum ( char *  string)

doChecksum(char * string, int stringSize, char * chksum) Calculates the checksum.

Returns
the checksum
59  {
60  int i;
61 
62  long int stringVal = 0;
63  int stringSize = strlen(string);
64  int asteriskPos = 0;
65  for(i = 0; i < stringSize; i++) {
66  if(string[i] == '*')
67  asteriskPos = i;
68  }
69  for(i = 0; i < asteriskPos + 1; i++) {
70  stringVal += string[i];
71  }
72  return stringVal;
73 }
i
int i
Definition: razor_imu.cpp:39
int main ( int  argc,
char *  argv[] 
)
80  {
81 double mjd, fmjd;
82 rvector axis;
83  //fp = fopen("output.txt", "a+");
84 
85 // setup();
86 
87  json_map_cosmosstruc () ;
88  get_resdir();
89  node_init((char *)"./");
90  iretn = agent_setup_server(AGENT_TYPE_BROADCAST,(char *)"engine",1000,0,1000);
91  iretn = agent_add_request((char *)"send_command",request_send_command);
92 
93 fmjd = currentmjd();
94 while (agent_running())
95  {
96  mjd = currentmjd();
97  cosmos_data.dyn.node.loc.pos.geod.s.lat = RADOF(21.29747);
98  cosmos_data.dyn.node.loc.pos.geod.s.lon = RADOF(-157.815967);
99  cosmos_data.dyn.node.loc.pos.geod.s.h = 0.;
100  cosmos_data.dyn.node.loc.pos.geod.utc = mjd;
101  pos_geod(&cosmos_data.dyn.node.loc);
102  axis = rv_unitz();
103  cosmos_data.dyn.node.rw[0].omg = (int)(86400.*(mjd-fmjd)) % 100;
104  axis = rv_smult(cosmos_data.dyn.node.rw[0].omg,axis);
105  cosmos_data.dyn.node.loc.att.topo.s = q_axis2quaternion_rv(axis);
106  cosmos_data.dyn.node.loc.att.topo.utc = mjd;
107  att_topo(&cosmos_data.dyn.node.loc);
108  agent_post(AGENT_MESSAGE_SOH,json_soh(&mainjstring));
109  usleep(999999);
110  } ;
111 
112  return 0;
113 }
geoidpos::utc
double utc
Definition: convertdef.h:261
request_send_command
char * request_send_command(char *request, char *output)
Definition: razor_imu.cpp:115
rvector
3 element generic row vector
Definition: vector.h:53
q_axis2quaternion_rv
quaternion q_axis2quaternion_rv(rvector v)
Row vector axis and angle to Quaternion.
Definition: mathlib.cpp:260
rv_smult
rvector rv_smult(double a, rvector b)
Multiply row vector by scalar.
Definition: vector.cpp:266
cosmosstruc::node
nodestruc node
Structure for summary information in node.
Definition: jsondef.h:4220
node_init
int32_t node_init(string node, cosmosstruc *cinfo)
Initialize Node configuration.
Definition: jsonlib.cpp:10832
qatt::utc
double utc
Definition: convertdef.h:477
locstruc::att
attstruc att
attstruc for this time.
Definition: convertdef.h:883
iretn
int iretn
Definition: razor_imu.cpp:39
nodestruc::loc
locstruc loc
Location structure.
Definition: jsondef.h:3596
gvector::h
double h
Height in meters.
Definition: vector.h:229
rv_unitz
rvector rv_unitz(double scale)
Scaled z row vector.
Definition: vector.cpp:140
mjd
double mjd
Definition: udp_send.cpp:41
gvector::lon
double lon
Longitude in radians.
Definition: vector.h:227
pos_geod
int32_t pos_geod(locstruc *loc)
Set Geodetic position.
Definition: convertlib.cpp:576
currentmjd
double currentmjd(double offset)
Current UTC in Modified Julian Days.
Definition: timelib.cpp:65
locstruc::pos
posstruc pos
posstruc for this time.
Definition: convertdef.h:881
geoidpos::s
gvector s
Position vector.
Definition: convertdef.h:263
gvector::lat
double lat
Latitude in radians.
Definition: vector.h:225
attstruc::topo
qatt topo
Definition: convertdef.h:826
qatt::s
quaternion s
0th derivative: Quaternion
Definition: convertdef.h:479
posstruc::geod
geoidpos geod
Definition: convertdef.h:741
att_topo
int32_t att_topo(locstruc *loc)
Definition: convertlib.cpp:2252
mainjstring
jstring mainjstring
Definition: razor_imu.cpp:75
cosmos_data
cosmosstruc cosmos_data
Definition: testengine.cpp:45
RADOF
#define RADOF(deg)
Radians of a Degree value.
Definition: math/constants.h:29
int process ( char *  pbuf)
169  {
170 
171  char cmdString[100] = "";
172  uint32_t chksum = 0;
173 
174  sscanf(pbuf,"%s %d",arg1str,&arg2int);
175  if (strcmp (arg1str,"SETSPD") == 0) {
176 
177  sprintf(cmdString,"$SET,RW,SPD,%s*",arg2str);
178 
179  chksum = doChecksum(cmdString);
180  printf("Checksum: %x\n",chksum); // return 441
181 
182  sprintf(cmdString,"$SET,RW,SPD,%s*%X\n",arg2str,chksum);
183  printf("Setting speed of RW to %d\n",arg2int);
184 
185  //min -18.3 max 18.3
186 
187  rw_send(cmdString,1);
188  printf("Command sucessfully sent: %s\n",cmdString);
189  }
190 
191  if(strcmp(arg1str,"GET")==0) {
192 
193  sprintf(cmdString,"$IMU,GET,%s*",arg2str);
194 
195  chksum = doChecksum(cmdString);
196  printf("Checksum: %x\n",chksum);
197 
198  sprintf(cmdString,"$IMU,GET,%s*%X\n",arg2str,chksum);
199  printf("Getting %s from IMU\n",arg2str);
200 
201  rw_send(cmdString,1);
202  printf("%s: ", arg2str);
203  if(strcmp(arg2str,"ALL")==0)
204  {
205  rw_getdata(buf,128);
206  printf("%s\n",buf);
207  }
208  if(strcmp(arg2str,"YAW")==0)
209  {
210  rw_getdata(buf,128);
211  printf("%s\n",buf);
212  for(int i = 0; i < 128; i++)
213  {
214  buf[i] = '\0';
215  }
216  }
217  }
218 
219  if(strcmp(arg1str,"STREAM")==0) {
220 
221  sprintf(cmdString,"IMU,STREAM,%s*",arg2str);
222 
223  chksum = doChecksum(cmdString);
224  printf("Checksum: %x\n",chksum);
225 
226  sprintf(cmdString,"$IMU,STREAM,%s*%X\n",arg2str,chksum);
227  printf("%sing data from IMU\n",arg2str);
228 
229  rw_send(cmdString,1);
230  if(strcmp(arg2str,"START")==0)
231  {
232  while(1)
233  {
234  rw_getdata(buf,128);
235  printf("%s\n",buf);
236  for(int i = 0; i < 128; i++)
237  {
238  buf[i] = '\0';
239  }
240  }
241  }
242  if(strcmp(arg2str,"STOP")==0)
243  {
244  rw_send(cmdString,1);
245  printf("Command sucessfully sent: %s\n", cmdString);
246  }
247  }
248 
249  /*
250  while(count < 28) {
251  for(int i = 0; i < 128; i++) {
252  buf[i] = '\0';
253  }
254  rw_send(cmds[count], 1);
255  rw_getdata(buf, 128);
256  printf("Issuing command: ");
257  printf(cmds[count]);
258  printf("\n");
259  printf("Looking for data...");
260  iretn = rw_getdata(buf, 128);
261  if(iretn > 0) {
262  printf("Got output: ");
263  printf(buf);
264  }
265  if(iretn == RW_ERROR_FAILED)
266  printf("Did not get anything within a second.");
267  count++;
268  sleep(3);
269  }
270  */
271  return(0);
272 }
arg2int
int arg2int
Definition: razor_imu.cpp:50
arg1str
char arg1str[100]
Definition: razor_imu.cpp:48
i
int i
Definition: razor_imu.cpp:39
doChecksum
long int doChecksum(char *string)
Definition: razor_imu.cpp:59
buf
char buf[128]
Definition: razor_imu.cpp:42
arg2str
char arg2str[100]
Definition: razor_imu.cpp:49

Variable Documentation

int count
int i
int iretn
FILE* fp
char cmds[15][50]
char buf[128]
char ibuf[1000]
char arg1str[100] = ""
char arg2str[100] = ""
int arg2int = 0
jstring mainjstring ={0,0,0}
cosmosstruc cosmos_data