00001
00027
00028
00029 #ifndef DSA_h_
00030 #define DSA_h_
00031
00032 #include "sdhlibrary_settings.h"
00033
00034
00035
00036
00037
00038 #include <assert.h>
00039
00040
00041
00042
00043
00044 #include "sdhexception.h"
00045
00046 #include "dbg.h"
00047 #if SDH_USE_VCC
00048 # include "rs232-vcc.h"
00049 #else
00050 # include "rs232-cygwin.h"
00051 #endif
00052 #include "simpletime.h"
00053 #include "crc.h"
00054
00055
00056
00057
00058
00059 NAMESPACE_SDH_START
00060
00061 #define DSA_MAX_PREAMBLE_SEARCH (2*3*(6*(14+13)) + 16)
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00088 class cDSAException: public cSDHLibraryException
00089 {
00090 public:
00091 cDSAException( cMsg const & _msg )
00092 : cSDHLibraryException( "cDSAException", _msg )
00093 {}
00094 };
00095
00096
00098
00109 class cDSA
00110 {
00111 public:
00113 typedef UInt16 tTexel;
00114
00116 enum eDSAErrorCode
00117 {
00118 E_SUCCESS,
00119 E_NOT_AVAILABLE,
00120 E_NO_SENSOR,
00121 E_NOT_INITIALIZED,
00122 E_ALREADY_RUNNING,
00123 E_FEATURE_NOT_SUPPORTED,
00124 E_INCONSISTENT_DATA,
00125 E_TIMEOUT,
00126 E_READ_ERROR,
00127 E_WRITE_ERROR,
00128 E_INSUFFICIENT_RESOURCES,
00129 E_CHECKSUM_ERROR,
00130 E_CMD_NOT_ENOUGH_PARAMS,
00131 E_CMD_UNKNOWN,
00132 E_CMD_FORMAT_ERROR,
00133 E_ACCESS_DENIED,
00134 E_ALREADY_OPEN,
00135 E_CMD_FAILED,
00136 E_CMD_ABORTED,
00137 E_INVALID_HANDLE,
00138 E_DEVICE_NOT_FOUND,
00139 E_DEVICE_NOT_OPENED,
00140 E_IO_ERROR,
00141 E_INVALID_PARAMETER,
00142 E_INDEX_OUT_OF_BOUNDS,
00143 E_CMD_PENDING,
00144 E_OVERRUN,
00145 E_RANGE_ERROR
00146 };
00147
00148
00150 #if SDH_USE_VCC
00151 #pragma pack(push,1) // for VCC (MS Visual Studio) we have to set the necessary 1 byte packing with this pragma
00152 #endif
00153 struct sControllerInfo
00154 {
00155 UInt16 error_code;
00156 UInt32 serial_no;
00157 UInt8 hw_version;
00158 UInt16 sw_version;
00159 UInt8 status_flags;
00160 UInt8 feature_flags;
00161 UInt8 senscon_type;
00162 UInt8 active_interface;
00163 UInt32 can_baudrate;
00164 UInt16 can_id;
00165 } SDH__attribute__((__packed__));
00166
00168 struct sSensorInfo
00169 {
00170 UInt16 error_code;
00171 UInt16 nb_matrices;
00172 UInt16 generated_by;
00173 UInt8 hw_revision;
00174 UInt32 serial_no;
00175 UInt8 feature_flags;
00176 } SDH__attribute__((__packed__));
00177
00179 struct sMatrixInfo
00180 {
00181 UInt16 error_code;
00182 float texel_width;
00183 float texel_height;
00184 UInt16 cells_x;
00185 UInt16 cells_y;
00186 UInt8 uid[6];
00187 UInt8 reserved[2];
00188 UInt8 hw_revision;
00189
00190 float matrix_center_x;
00191 float matrix_center_y;
00192 float matrix_center_z;
00193
00194 float matrix_theta_x;
00195 float matrix_theta_y;
00196 float matrix_theta_z;
00197 float fullscale;
00198 UInt8 feature_flags;
00199 } SDH__attribute__((__packed__));
00200
00202 struct sSensitivityInfo
00203 {
00204 UInt16 error_code;
00205 UInt8 adj_flags;
00206
00207
00208
00209 float cur_sens;
00210
00211 float fact_sens;
00212
00213
00214 } SDH__attribute__((__packed__));
00215
00216 #if SDH_USE_VCC
00217 #pragma pack(pop) // for VCC (MS Visual Studio) restore normal packing
00218 #endif
00219
00220
00233 struct sTactileSensorFrame
00234 {
00235 UInt32 timestamp;
00236 UInt8 flags;
00237 tTexel* texel;
00238
00240 sTactileSensorFrame( void )
00241 {
00242 texel = NULL;
00243 }
00244 };
00245
00247 struct sContactInfo
00248 {
00249 double force;
00250 double area;
00251 double cog_x;
00252 double cog_y;
00253 };
00254
00255
00256 protected:
00257
00259 #if SDH_USE_VCC
00260 #pragma pack(push,1) // for VCC (MS Visual Studio) we have to set the necessary 1 byte packing with this pragma
00261 #endif
00262 struct sResponse {
00263 UInt8 packet_id;
00264 UInt16 size;
00265 UInt8* payload;
00266 Int32 max_payload_size;
00267
00269 sResponse( UInt8* _payload, int _max_payload_size )
00270 {
00271 payload = _payload;
00272 max_payload_size = _max_payload_size;
00273 }
00274 } SDH__attribute__((__packed__));
00275 #if SDH_USE_VCC
00276 #pragma pack(pop) // for VCC (MS Visual Studio) restore normal packing
00277 #endif
00278
00279 friend std::ostream &operator<<(std::ostream &stream, cDSA::sResponse const &response );
00280
00282 cDBG dbg;
00283
00285 cRS232 comm_interface;
00286
00288 bool do_RLE;
00289
00291 sControllerInfo controller_info;
00292
00294 sSensorInfo sensor_info;
00295
00297 sMatrixInfo* matrix_info;
00298
00300 sTactileSensorFrame frame;
00301
00303 int nb_cells;
00304
00306 int* texel_offset;
00307
00309 long read_timeout_us;
00310
00311 cSimpleTime start_pc;
00312 UInt32 start_dsa;
00313
00315 tTexel contact_area_cell_threshold;
00316
00318 tTexel contact_force_cell_threshold;
00319
00321 double force_factor;
00322
00323
00327 double calib_pressure;
00329 double calib_voltage;
00330
00331
00332 void WriteCommandWithPayload( UInt8 command, UInt8* payload, UInt16 payload_len );
00333
00334 inline void WriteCommand( UInt8 command )
00335 {
00336 WriteCommandWithPayload( command, NULL, 0 );
00337 }
00338
00339
00350 void ReadResponse( sResponse* response );
00351
00352
00356 void ReadControllerInfo( sControllerInfo* _controller_info );
00357
00358
00362 void ReadSensorInfo( sSensorInfo* _sensor_info );
00363
00364
00368 void ReadMatrixInfo( sMatrixInfo* _matrix_info );
00369
00370
00374 void ReadFrame( sTactileSensorFrame* frame_p );
00375
00376
00381 void QueryControllerInfo( sControllerInfo* _controller_info );
00382
00383
00388 void QuerySensorInfo( sSensorInfo* _sensor_info );
00389
00390
00395 void QueryMatrixInfo( sMatrixInfo* _matrix_info, int matrix_no );
00396
00397
00401 void QueryMatrixInfos( void );
00402
00403
00407 void ParseFrame( sResponse* response, sTactileSensorFrame* frame_p );
00408
00409
00410 public:
00430 cDSA( int debug_level=0, int port=1, char const* device_format_string="/dev/ttyS%d" );
00431
00432
00434 ~cDSA();
00435
00436
00438 inline sControllerInfo const & GetControllerInfo( void ) const
00439 {
00440 return controller_info;
00441 }
00442
00443
00445 inline sSensorInfo const & GetSensorInfo( void ) const
00446 {
00447 return sensor_info;
00448 }
00449
00450
00452 inline sMatrixInfo const & GetMatrixInfo( int m ) const
00453 {
00454 assert( 0 <= m && m <= (int ) sensor_info.nb_matrices );
00455 return matrix_info[m];
00456 }
00457
00458
00460 inline sTactileSensorFrame const & GetFrame() const
00461 {
00462 return frame;
00463 }
00464
00465
00467 inline sTactileSensorFrame const & UpdateFrame()
00468 {
00469 ReadFrame( &frame );
00470 return frame;
00471 }
00472
00473
00475 void Open(void);
00476
00478 void Close(void);
00479
00480
00491 void SetFramerate( UInt16 framerate, bool do_RLE=true, bool do_data_acquisition=true );
00492
00505 void SetFramerateRetries( UInt16 framerate, bool do_RLE=true, bool do_data_acquisition=true, unsigned int retries=0, bool ignore_exceptions=false )
00506 throw (cDSAException*);
00507
00508
00523 sSensitivityInfo GetMatrixSensitivity( int matrix_no )
00524 throw (cDSAException*);
00525
00526
00543 void SetMatrixSensitivity( int matrix_no,
00544 double sensitivity,
00545 bool do_all_matrices=false,
00546 bool do_reset=false,
00547 bool do_persistent=false )
00548 throw (cDSAException*);
00549
00550
00567 void SetMatrixThreshold( int matrix_no,
00568 UInt16 threshold,
00569 bool do_all_matrices=false,
00570 bool do_reset=false,
00571 bool do_persistent=false )
00572 throw (cDSAException*);
00573
00583 UInt16 GetMatrixThreshold( int matrix_no )
00584 throw (cDSAException*);
00585
00589 tTexel GetTexel( int m, int x, int y ) const;
00590
00591
00592
00596 inline int GetMatrixIndex( int fi, int part )
00597 {
00598 return fi * 2 + part;
00599 }
00600
00601
00605 inline unsigned long GetAgeOfFrame( sTactileSensorFrame* frame_p )
00606 {
00607 return ((unsigned long) (start_pc.Elapsed()*1000.0)) - (frame_p->timestamp - start_dsa);
00608 }
00609
00610 double GetContactArea( int m );
00611
00612 private:
00613 double VoltageToPressure( double voltage );
00614
00615 void ReadAndCheckErrorResponse( char const* msg );
00616
00617
00618 public:
00619 sContactInfo GetContactInfo( int m );
00620
00621
00628 static char const* ErrorCodeToString( eDSAErrorCode error_code );
00629 static char const* ErrorCodeToString( UInt16 error_code )
00630 {
00631 return ErrorCodeToString( (eDSAErrorCode) error_code );
00632 }
00633
00634
00635 };
00636
00637
00638
00639
00640 std::ostream &operator<<( std::ostream &stream, cDSA::sControllerInfo const &controller_info );
00641
00642
00643 std::ostream &operator<<( std::ostream &stream, cDSA::sSensorInfo const &sensor_info );
00644
00645
00646 std::ostream &operator<<( std::ostream &stream, cDSA::sMatrixInfo const &matrix_info );
00647
00648
00649 std::ostream &operator<<( std::ostream &stream, cDSA::sResponse const &response );
00650
00651
00652 std::ostream &operator<<( std::ostream &stream, cDSA const &dsa );
00653
00654
00655 NAMESPACE_SDH_END
00656
00657 #endif
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672