struct conn* createConnection(const char* ip_address, int port, int protocol, int naggles)
{
struct conn* connection = malloc(sizeof(struct conn));
memset(connection, 0, sizeof(struct conn));
if(protocol == UDP_MODE) {
connection->sock = openUdpSocket(ip_address, port);
connection->protocol = UDP_MODE;
}
else {
connection->sock = openTcpSocket(ip_address, port);
connection->protocol = TCP_MODE;
if(!naggles) {
//Disable naggle's algorithm
int flag = 1;
int result = setsockopt(connection->sock,/* socket affected */
IPPROTO_TCP, /* set option at TCP level */
TCP_NODELAY, /* name of option */
(char *) &flag, /* the cast is historical cruft */
sizeof(int)); /* length of option value */
if (result < 0){
printf("couldn't set tcp_nodelay\n");
exit(-1);
}
}
}
static int uid_gen;
connection->uid = uid_gen;
uid_gen++;
if (verbose) {
printf("Created connection on fd %d, uid %d\n", connection->sock, connection->uid);
}
return connection;
}
void * receiveUdpMessage(void * temp){
do{
openUdpSocket();
sleep(1);
}while(sockfdU == 0);
char buff[BUFFER_SIZE];
bzero(buff,BUFFER_SIZE);
struct sockaddr_in cli_addr;
socklen_t clilen;
clilen = sizeof(cli_addr);
listen((int)sockfdU,5);
// unsigned short CRCRead = calculateCRC16((char*) 0xD0, 1);
// unsigned short CRCSend = calculateCRC16((char*) 0xD1, 1);
while(_continueUdp == 1){
// _isWorkingUdp = false;
int n = recvfrom(sockfdU,buff,sizeof(buff),0,(sockaddr *)&cli_addr,&clilen);
if(n == 3 ){
// _isWorkingUdp = true;
if( buff[0] == (char) 0xD0 /*&& buff[1] == CRCRead>>8 && buff[2]==CRCRead & 0xFF */ ){ // Auto detect frame
#ifdef DEBUG
printf("Auto detect frame\n");
fflush(stdout);
#endif
char answer[] = {(char) 0xD1, 0x00, 0x00}; // Remplace 0x22 et 0x33 par crc
int aa = sendto(sockfdU, answer, sizeof(answer), 0, (struct sockaddr*)&cli_addr, clilen); // Send the answer
printf("Send to: %i\n",aa);
fflush(stdout);
}
}
}
close(sockfdU);
return NULL;
}
int main() {
// --------------------- Opensim model.
OpenSim::Model model{};
model.setUseVisualizer(true);
auto slab = new OpenSim::Body{"slab",
1,
SimTK::Vec3{0},
SimTK::Inertia{0}};
auto balljoint = new OpenSim::BallJoint{"balljoint",
model.getGround(),
SimTK::Vec3{0, 1, 0},
SimTK::Vec3{0},
*slab,
SimTK::Vec3{0},
SimTK::Vec3{0}};
OpenSim::Brick brick{};
brick.setFrame(*slab);
brick.set_half_lengths(SimTK::Vec3{0.5, 0.05, 0.25});
slab->addComponent(brick.clone());
model.addBody(slab);
model.addJoint(balljoint);
auto& state = model.initSystem();
model.updMatterSubsystem().setShowDefaultGeometry(false);
SimTK::Visualizer& viz = model.updVisualizer().updSimbodyVisualizer();
viz.setBackgroundType(viz.GroundAndSky);
viz.setShowSimTime(true);
viz.drawFrameNow(state);
// -------------------------- UDP Socket.
constexpr unsigned BUFFSIZE{8192};
auto sock = openUdpSocket();
// ------------------------- Kalman filter.
Kalman kalman_roll{};
Kalman kalman_pitch{};
double oldtimestamp{};
bool firstrow{true};
// ------------------------ Streaming.
while(true) {
char buffer[BUFFSIZE];
auto bytes = recvfrom(sock, buffer, BUFFSIZE, 0, 0, 0);
if(bytes > 0) {
auto data = parseImuData(buffer, BUFFSIZE);
auto& timestamp = std::get<0>(data);
auto& gravity = std::get<1>(data);
auto& omega = std::get<2>(data);
// If omega is (0, 0, 0), skip over because there was no data.
// All three components are never equal except when they are 0.
if(omega[0] == omega[1] && omega[1] == omega[2])
continue;
// Compute change in time and record the timestamp.
auto deltat = timestamp - oldtimestamp;
oldtimestamp = timestamp;
if(firstrow) {
firstrow = false;
continue;
}
auto tilt = computeRollPitch(gravity);
auto roll = radToDeg(tilt.first);
auto pitch = radToDeg(tilt.second);
omega[0] = radToDeg(omega[0]);
omega[1] = radToDeg(omega[1]);
omega[2] = radToDeg(omega[2]);
// Angular velocity about axis y is roll.
// Angular velocity about axis x is pitch.
auto roll_hat = kalman_roll.getAngle( roll, omega[1], deltat);
auto pitch_hat = kalman_pitch.getAngle(pitch, omega[0], deltat);
// Multiplying -1 to roll just for display. This way visualizaiton moves
// like the physical phone.
model.getCoordinateSet()[0].setValue(state, -1 * degToRad( roll_hat));
model.getCoordinateSet()[2].setValue(state, degToRad(pitch_hat));
viz.drawFrameNow(state);
//std::cout << buffer << std::endl;
} else
std::cout << "skipping....." << std::endl;
}
return 0;
}
/**
* Function for retrieving the Multicast Group socket.
*/
int getMcGroupSock() {
if( ! mcGroupSock ) {
mcGroupSock = openUdpSocket( INADDR_ANY, 0 );;
}
return mcGroupSock;
}
