Unverified Commit 32d42740 authored by Aditya Kamath's avatar Aditya Kamath Committed by GitHub
Browse files

Add files via upload

parent 2b71882a
File added
File added
all: marvelmind_example
marvelmind_example:
gcc -g -pthread -c marvelmind.c -o marvelmind.o
gcc -g -pthread -c example.c -o example.o
gcc -o marvelmind_example example.o marvelmind.o -pthread
clean:
rm -rf *.o
This diff is collapsed.
This diff is collapsed.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#ifdef WIN32
#include <windows.h>
#else
#include <unistd.h>
#include <signal.h>
#endif // WIN32
#include "marvelmind.h"
#include <time.h>
#include <math.h>
#include<string.h>
#include<sys/socket.h>
#include<sys/types.h>
#include<netinet/in.h>
#include<arpa/inet.h>
bool terminateProgram=false;
int udp_port=3500;
#ifdef WIN32
BOOL CtrlHandler( DWORD fdwCtrlType )
{
if ((fdwCtrlType==CTRL_C_EVENT ||
fdwCtrlType==CTRL_BREAK_EVENT ||
fdwCtrlType==CTRL_CLOSE_EVENT ||
fdwCtrlType==CTRL_LOGOFF_EVENT ||
fdwCtrlType==CTRL_SHUTDOWN_EVENT) &&
(terminateProgram==false))
{
terminateProgram=true;
return true;
}
else return false;
}
#else
void CtrlHandler(int signum)
{
terminateProgram=true;
}
#endif
#ifdef WIN32
void sleep(unsigned int seconds)
{
Sleep (seconds*1000);
}
#endif
int main (int argc, char *argv[])
{
// get port name from command line arguments (if specified)
const char * ttyFileName;
if (argc==2) ttyFileName=argv[1];
else ttyFileName=DEFAULT_TTY_FILENAME;
//UDP parameters
//int port = atoi(udp_port);
int sockfd;
struct hedgePacket{
double x17;
double y17;
double z17;
double x18;
double y18;
double z18;
//double yaw_normal;
} drone_position;
struct sockaddr_in serverAddr;
char buffer[1024];
socklen_t addr_size;
sockfd=socket(PF_INET, SOCK_DGRAM, 0);
memset(&serverAddr, '\0', sizeof(serverAddr));
serverAddr.sin_family=AF_INET;
serverAddr.sin_port=htons(udp_port);
serverAddr.sin_addr.s_addr=inet_addr("127.0.0.1");
// Init
struct MarvelmindHedge * hedge=createMarvelmindHedge ();
if (hedge==NULL)
{
puts ("Error: Unable to create MarvelmindHedge");
return -1;
}
hedge->ttyFileName=ttyFileName;
hedge->verbose=true; // show errors and warnings
startMarvelmindHedge (hedge);
// Set Ctrl-C handler
#ifdef WIN32
SetConsoleCtrlHandler( (PHANDLER_ROUTINE) CtrlHandler, TRUE );
#else
signal (SIGINT, CtrlHandler);
signal (SIGQUIT, CtrlHandler);
#endif
clock_t t1, t2;
float diff;
double curtime;
FILE *fp17, *fp18;
fp17=fopen("Ultrasound_data_beacon17.txt", "w");
fp18=fopen("Ultrasound_data_beacon18.txt", "w");
// Main loop
while ((!terminateProgram) && (!hedge->terminationRequired))
{
t1 = clock();
//sleep (3); //this
//printPositionFromMarvelmindHedge (hedge, true);//this
struct PositionValue position17, position18;
getPositionFromMarvelmindHedgeByAddress(hedge, &position17, 17);
getPositionFromMarvelmindHedgeByAddress(hedge, &position18, 18);
t2 = clock();
diff = ((float) t2 - t1)/CLOCKS_PER_SEC * 1000;
//printf("Operation took %.3f milliseconds. Now sleeping for %.3d milliseconds.\n", diff, 200-(diff)); //this
//sleep(1); //this
//Sleep(200-diff); //this
struct timeval t;
gettimeofday(&t,NULL);
curtime = (double)t.tv_sec+t.tv_usec/1000000.0;
if (fabs(((double) position17.y)/1000.0)>0.000001)
{
//printf ("Address: %d, X: %.3f, Y: %.3f, Z: %.3f at time T: %u\n", position17.address, ((double) position17.x)/1000.0,
//((double) position17.y)/1000.0, ((double) position17.z)/1000.0, position17.timestamp);
fprintf(fp17, "%.4f \t %.4f \t %.4f \t %.4f \n", ((double) position17.x)/1000.0, ((double) position17.y)/1000.0, ((double) position17.z)/1000.0, curtime);
printf("Written to files\n");
drone_position.x17 = ((double) position17.x)/1000.0;
drone_position.y17 = ((double) position17.y)/1000.0;
drone_position.z17 = ((double) position17.z)/1000.0;
memcpy(buffer, &drone_position, sizeof(drone_position));
sendto(sockfd, buffer, sizeof(drone_position), MSG_DONTWAIT, (struct sockaddr*)&serverAddr, sizeof(serverAddr));
printf("[%d]Data sent: x:%lf, y:%lf, z:%lf \n", position17.address, drone_position.x17, drone_position.y17, drone_position.z17);
//printf("[+]Data sent: x:%lf, y:%lf, z:%lf \n", drone_position.x18, drone_position.y18, drone_position.z18);
}
if (fabs(((double) position18.y)/1000.0)>0.000001)
{
printf ("Address: %d, X: %.3f, Y: %.3f, Z: %.3f at time T: %u\n", position18.address, ((double) position18.x)/1000.0,
((double) position18.y)/1000.0, ((double) position18.z)/1000.0, position18.timestamp);
fprintf(fp18, "%.4f \t %.4f \t %.4f \t %.4f \n", ((double) position18.x)/1000.0, ((double) position18.y)/1000.0, ((double) position18.z)/1000.0, curtime);
printf("Written to files\n");
//UDP BROADCAST
drone_position.x18 = ((double) position18.x)/1000.0;
drone_position.y18 = ((double) position18.y)/1000.0;
drone_position.z18 = ((double) position18.z)/1000.0;
memcpy(buffer, &drone_position, sizeof(drone_position));
sendto(sockfd, buffer, sizeof(drone_position), MSG_DONTWAIT, (struct sockaddr*)&serverAddr, sizeof(serverAddr));
//printf("[+]Data sent: x:%lf, y:%lf, z:%lf \n", drone_position.x17, drone_position.y17, drone_position.z17);
printf("[%d]Data sent: x:%lf, y:%lf, z:%lf \n", position18.address, drone_position.x18, drone_position.y18, drone_position.z18);
}
//drone_position.yaw_normal = atan2((drone_position.y17 - drone_position.y18),(drone_position.x17 - drone_position.x18));
//printf("[+] YAW = %lf \n", drone_position.yaw_normal);
//getPositionFromMarvelmindHedgeCustom(hedge, true, &xm17, &ym17, &zm17);
//sleep(1.0);
//printf("X17: %.3f\n", xm17);
//sleep(0.1);
//printStationaryBeaconsPositionsFromMarvelmindHedge (hedge, true);
}
// Exit
fclose(fp17);
fclose(fp18);
stopMarvelmindHedge (hedge);
destroyMarvelmindHedge (hedge);
return 0;
}
This diff is collapsed.
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
struct PositionValue
{
uint8_t address;
uint32_t timestamp;
int32_t x, y, z;// coordinates in millimeters
bool highResolution;
bool ready;
bool processed;
};
struct StationaryBeaconPosition
{
uint8_t address;
int32_t x, y, z;// coordinates in millimeters
bool highResolution;
};
#define MAX_STATIONARY_BEACONS 30
struct StationaryBeaconsPositions
{
uint8_t numBeacons;
struct StationaryBeaconPosition beacons[MAX_STATIONARY_BEACONS];
bool updated;
};
struct MarvelmindHedge
{
// serial port device name (physical or USB/virtual). It should be provided as
// an argument:
// /dev/ttyACM0 - typical for Linux / Raspberry Pi
// /dev/tty.usbmodem1451 - typical for Mac OS X
const char * ttyFileName;
// Baud rate. Should be match to baudrate of hedgehog-beacon
// default: 9600
uint32_t baudRate;
// maximum count of measurements of coordinates stored in buffer
// default: 3
uint8_t maxBufferedPositions;
// buffer of measurements
struct PositionValue * positionBuffer;
struct StationaryBeaconsPositions positionsBeacons;
// verbose flag which activate console output
// default: False
bool verbose;
// pause flag. If True, class would not read serial data
bool pause;
// If True, thread would exit from main loop and stop
bool terminationRequired;
// receiveDataCallback is callback function to recieve data
void (*receiveDataCallback)(struct PositionValue position);
// private variables
uint8_t lastValuesCount_;
uint8_t lastValues_next;
bool haveNewValues_;
#ifdef WIN32
HANDLE thread_;
CRITICAL_SECTION lock_;
#else
pthread_t thread_;
pthread_mutex_t lock_;
#endif
};
#define POSITION_DATAGRAM_ID 0x0001
#define BEACONS_POSITIONS_DATAGRAM_ID 0x0002
#define POSITION_DATAGRAM_HIGHRES_ID 0x0011
#define BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012
struct MarvelmindHedge * createMarvelmindHedge ();
void destroyMarvelmindHedge (struct MarvelmindHedge * hedge);
void startMarvelmindHedge (struct MarvelmindHedge * hedge);
//void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge,
// bool onlyNew);
bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge,
struct PositionValue * position);
void getPositionFromMarvelmindHedgeCustom (struct MarvelmindHedge * hedge,
bool onlyNew, double *xm, double *ym, double *zm);
bool getPositionFromMarvelmindHedgeByAddress (struct MarvelmindHedge * hedge,
struct PositionValue * position, uint8_t address);
bool getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge,
struct StationaryBeaconsPositions * positions);
void printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge,
bool onlyNew);
void stopMarvelmindHedge (struct MarvelmindHedge * hedge);
#ifdef WIN32
#define DEFAULT_TTY_FILENAME "\\\\.\\COM3"
#else
#define DEFAULT_TTY_FILENAME "/dev/ttyACM0"
#endif // WIN32
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<CodeBlocks_project_file>
<FileVersion major="1" minor="6" />
<Project>
<Option title="marvelmind_c" />
<Option pch_mode="2" />
<Option compiler="gcc" />
<Build>
<Target title="Debug">
<Option output="bin/Debug/marvelmind_c" prefix_auto="1" extension_auto="1" />
<Option object_output="obj/Debug/" />
<Option type="1" />
<Option compiler="gcc" />
<Option parameters="COM3" />
<Compiler>
<Add option="-g" />
</Compiler>
</Target>
<Target title="Release">
<Option output="bin/Release/marvelmind_c" prefix_auto="1" extension_auto="1" />
<Option object_output="obj/Release/" />
<Option type="1" />
<Option compiler="gcc" />
<Compiler>
<Add option="-O2" />
</Compiler>
<Linker>
<Add option="-s" />
<Add option="-static" />
</Linker>
</Target>
</Build>
<Compiler>
<Add option="-Wall" />
<Add option="-pthread" />
</Compiler>
<Unit filename="example.c">
<Option compilerVar="CC" />
</Unit>
<Unit filename="marvelmind.c">
<Option compilerVar="CC" />
</Unit>
<Unit filename="marvelmind.h" />
<Extensions>
<code_completion />
<envvars />
<debugger />
<lib_finder disable_auto="1" />
</Extensions>
</Project>
</CodeBlocks_project_file>
# depslib dependency file v1.0
1520264917 source:c:\users\s158719\downloads\2017_02_08_c_example\2017_02_08_c_example\source\example.c
<stdio.h>
<stdlib.h>
<stdbool.h>
<windows.h>
<unistd.h>
<signal.h>
"marvelmind.h"
<time.h>
1520263145
1520263145 c:\users\s158719\downloads\2017_02_08_c_example\2017_02_08_c_example\source\marvelmind.h
<stdint.h>
<stdbool.h>
<stddef.h>
1520263194 source:c:\users\s158719\downloads\2017_02_08_c_example\2017_02_08_c_example\source\marvelmind.c
<stdlib.h>
<stdio.h>
<string.h>
<windows.h>
<process.h>
<unistd.h>
<termios.h>
<fcntl.h>
<pthread.h>
"marvelmind.h"
1520263194 d.h"
1520263194 source:/home/robocup/Documents/C/Marvelmind/2017_02_08_C_example/2017_02_08_C_example/source/marvelmind.c
<stdlib.h>
<stdio.h>
<string.h>
<windows.h>
<process.h>
<unistd.h>
<termios.h>
<fcntl.h>
<pthread.h>
"marvelmind.h"
1520263145 /home/robocup/Documents/C/Marvelmind/2017_02_08_C_example/2017_02_08_C_example/source/marvelmind.h
<stdint.h>
<stdbool.h>
<stddef.h>
1520265004 source:/home/robocup/Documents/C/Marvelmind/2017_02_08_C_example/2017_02_08_C_example/source/example.c
<stdio.h>
<stdlib.h>
<stdbool.h>
<windows.h>
<unistd.h>
<signal.h>
"marvelmind.h"
<time.h>
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<CodeBlocks_layout_file>
<ActiveTarget name="Debug" />
<File name="example.c" open="0" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="649" topLine="28" />
</Cursor>
</File>
<File name="marvelmind.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="2600" topLine="61" />
</Cursor>
</File>
<File name="marvelmind.c" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="20199" topLine="591" />
</Cursor>
</File>
</CodeBlocks_layout_file>
HEADERS=rtdb/rtdb_user.h rtdb/rtdb_api.h rtdb/rtdbdefs.h rtdb/rtdb_sem.h
LIBS=lib/librtdb.so
BINS=bin/comm bin/listen bin/scomm
INSTALL=/usr
all: libs config/rtdb.ini
libs:
make -C rtdb all
make -C comm all
config/rtdb.ini: worldmodel/*.h config/rtdb.conf
cd bin; ./xrtdb
clean:
make -C rtdb clean
make -C comm clean
install: all
mkdir -p ${INSTALL}/include/rtdb
mkdir -p ${INSTALL}/config
cp config/rtdb.ini ${INSTALL}/config
cp config/network.conf ${INSTALL}/config
cp ${HEADERS} ${INSTALL}/include/rtdb
cp ${LIBS} ${INSTALL}/lib
cp ${BINS} ${INSTALL}/bin
uninstall:
rm -fv ${INSTALL}/include/${HEADERS}
rm -fv ${INSTALL}/lib/${LIBS}
rm -fv ${INSTALL}/bin/${BINS}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment