lms400_cola.h
00001
00002
00003
00004
00005
00006
00007 #include <sys/types.h>
00008 #include <vector>
00009 #include <netinet/in.h>
00010 #include <libplayercore/player.h>
00011
00012 #define BUF_SIZE 1024
00013
00015 typedef struct
00016 {
00017 unsigned char* string;
00018 int length;
00019 } MeasurementQueueElement_t;
00020
00022 typedef struct
00023 {
00024 uint16_t Format;
00025 uint16_t DistanceScaling;
00026 int32_t StartingAngle;
00027 uint16_t AngularStepWidth;
00028 uint16_t NumberMeasuredValues;
00029 uint16_t ScanningFrequency;
00030 uint16_t RemissionScaling;
00031 uint16_t RemissionStartValue;
00032 uint16_t RemissionEndValue;
00033 } MeasurementHeader_t;
00034
00035
00037 class lms400_cola
00038 {
00039 public:
00040 lms400_cola (const char* host, int port, int debug_mode);
00041
00042
00043 int Connect ();
00044 int Disconnect ();
00045
00046
00047 int SetAngularResolution (const char* password, float ang_res, float angle_start, float angle_range);
00048 int SetScanningFrequency (const char* password, float freq, float angle_start, float angle_range);
00049 int SetResolutionAndFrequency (float freq, float ang_res, float angle_start, float angle_range);
00050
00051 int StartMeasurement (bool intensity = true);
00052 player_laser_data ReadMeasurement ();
00053 int StopMeasurement ();
00054
00055 int SetUserLevel (int8_t userlevel, const char* password);
00056 int GetMACAddress (char** macadress);
00057
00058 int SetIP (char* ip);
00059 int SetGateway (char* gw);
00060 int SetNetmask (char* mask);
00061 int SetPort (uint16_t port);
00062
00063 int ResetDevice ();
00064 int TerminateConfiguration ();
00065
00066 int SendCommand (const char* cmd);
00067 int ReadResult ();
00068
00069 int ReadAnswer ();
00070
00071 int ReadConfirmationAndAnswer ();
00072
00073 int EnableRIS (int onoff);
00074 player_laser_config GetConfiguration ();
00075 int SetMeanFilterParameters (int num_scans);
00076 int SetRangeFilterParameters (float *ranges);
00077 int EnableFilters (int filter_mask);
00078
00079
00080 unsigned char* ParseIP (char* ip);
00081
00082 private:
00083
00084 int assemblecommand (unsigned char* command, int len);
00085
00086 const char* hostname;
00087 int sockfd, portno, n;
00088 struct sockaddr_in serv_addr;
00089 struct hostent *server;
00090
00091
00092 int verbose;
00093 int ExtendedRIS;
00094 int MeanFilterNumScans;
00095 float RangeFilterTopLimit;
00096 float RangeFilterBottomLimit;
00097 int FilterMask;
00098 player_laser_config Configuration;
00099
00100
00101 unsigned char buffer[4096];
00102 unsigned int bufferlength;
00103
00104
00105 unsigned char command[BUF_SIZE];
00106 int commandlength;
00107 std::vector<MeasurementQueueElement_t>* MeasurementQueue;
00108 };