diff --git a/Findmodel_car_drivers.cmake b/Findmodel_car_drivers.cmake
index 8bd60abf40d594009f9972e93fbfcf4bc8258c6f..0be3376852a9038a8cdb1f501c1573ed1becec8f 100644
--- a/Findmodel_car_drivers.cmake
+++ b/Findmodel_car_drivers.cmake
@@ -1,6 +1,15 @@
#edit the following line to add the librarie's header files
SET(name model_car_drivers)
-FIND_PATH(${name}_INCLUDE_DIRS ${name}.h /usr/local/include/iri/${name} /usr/include/iri/${name})
+
+SET(header_files
+ model_car_actuators.h
+ model_car_egomotion.h
+ model_car_ultrasounds.h
+ model_car_batteries.h
+ model_car_driver_base.h
+ model_car_protocol.h)
+
+FIND_PATH(${name}_INCLUDE_DIRS ${header_files} /usr/local/include/iri/${name} /usr/include/iri/${name})
FIND_LIBRARY(${name}_LIBRARIES
NAMES ${name}
@@ -21,4 +30,4 @@ ELSE (${name}_FOUND)
IF (${name}_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find ${name}")
ENDIF (${name}_FIND_REQUIRED)
-ENDIF (${name}_FOUND)
\ No newline at end of file
+ENDIF (${name}_FOUND)
diff --git a/ReadMe.md b/ReadMe.md
index bfbd3253543efa55f8668967dbe61bba335a1565..ac809c61deee0853d0d111679cec9210b5133368 100644
--- a/ReadMe.md
+++ b/ReadMe.md
@@ -3,26 +3,35 @@ model_car_drivers {#mainpage}
## Description
-Drivers for model car platform
+This C++ library includes the Linux drivers for the four Arduino based control boards present in the model car:
-## Installation
+* [Actuators](doc/actuators.md): To control the speed and steering angle of the model car.
+* [Egomotion](doc/egomotion.md): To get the wheel encoder the IMU sensor data.
+* [Ultrasounds](doc/ultrasounds.md): To get the distance measured by the ultrasound sensors.
+* [Batteries](doc/batteries.md): To get the current state of both batteries.
-* Add the labrobotica repository if it is not already added: run the commands on _add repository_ and _add key_ from [labrobotica_how_to installation](https://gitlab.iri.upc.edu/labrobotica/labrobotica_how_to/-/blob/master/README.md#installation)
-* Install the package:
+All these drivers inherit from a common base class as shown in the next image.
- sudo apt update && sudo apt install iri-model-car-drivers-dev
+<img align="center" src="doc/images/model_car_drivers.png" alt="Class hierarchy of the model car drivers" width="977" height="388">
-## Disclaimer
+This base class implements the following features, and the inherited classes implement the specific functionality required. See the description of each driver for more detailed description.
-Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-Mantainer IRI labrobotica (labrobotica@iri.upc.edu)
+* Handles the serial port.
+* Handles the communication protocol for transmission and reception.
+* Provides asynchronous notifications when new data is available.
+* Provides a virtual function to process the received data in the inherited classes.
+* Provides the timestamp of the most recent data received.
-This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.
+The four on-board control boards can be accessed through a ttyACM* Linux device. However, it is not possible to differentiate them before reading the unique ID by software. For this reason, the drivers loop through all available devices looking for the one matching the desired ID. It a driver loops through all available devices without finding the desired one, it will report a failure. In general this type of errors are caused by the concurrent access of multiple drivers to the same physical device, and can be solved by repeating the process.
-In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
+## Installation
-You should have received a copy of the GNU Lesser General Public License
-along with this program. If not, see <http://www.gnu.org/licenses/>
+* Add the labrobotica repository if it is not already added: run the commands on _add repository_ and _add key_ from [labrobotica_how_to installation](https://gitlab.iri.upc.edu/labrobotica/labrobotica_how_to/-/blob/master/README.md#installation)
+* Install the package:
+
+```
+sudo apt update && sudo apt install iri-model-car-drivers-dev
+```
## For developers
@@ -41,27 +50,38 @@ Under linux all of these utilities are available in ready-to-use packages.
Under MacOS most of the packages are available via [fink](http://www.finkproject.org/ "Fink's Homepage")
-This package also requires of the following dependencies:
-- comm
+This package also requires of the following IRI dependencies:
+
+* [iriutils](https://gitlab.iri.upc.edu/labrobotica/algorithms/iriutils "iriutils gitlab page"), a set of basic tools.
+* [comm](https://gitlab.iri.upc.edu/labrobotica/drivers/comm "comm gitlab page"), a set of basic communication drivers.
## Compilation and installation from source
Clone this repository and create a build folder inside:
- mkdir build
+```
+mkdir build
+```
Inside the build folder execute the following commands:
- cmake ..
+```
+cmake ..
+```
The default build mode is DEBUG. That is, objects and executables include debug information.
The RELEASE build mode optimizes for speed. To build in this mode execute instead
- cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+
+```
+cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+```
The release mode will be kept until next time cmake is executed.
- make -j $(nproc)
+```
+make -j $(nproc)
+```
In case no errors are reported, the generated libraries (if any) will be located at the
_lib_ folder and the executables (if any) will be located at the _bin_ folder.
@@ -69,7 +89,9 @@ _lib_ folder and the executables (if any) will be located at the _bin_ folder.
In order to be able to use the library, it it necessary to copy it into the system.
To do that, execute
- make install
+```
+make install
+```
as root and the shared libraries will be copied to */usr/local/lib/iri/model_car_drivers* directory
and the header files will be copied to */usr/local/include/iri/model_car_drivers* directory. At
@@ -77,31 +99,49 @@ this point, the library may be used by any user.
To remove the library from the system, exceute
- make uninstall
+```
+make uninstall
+```
as root, and all the associated files will be removed from the system.
To generate the documentation execute the following command:
- make doc
+```
+make doc
+```
## How to use it
To use this library in another library or application, in the CMakeLists.txt file, first it is necessary to locate if the library has been installed or not using the following command
- FIND_PACKAGE(model_car_drivers)
+```
+FIND_PACKAGE(model_car_drivers)
+```
In the case that the package is present, it is necessary to add the header files directory to the include directory path by using
- INCLUDE_DIRECTORIES(${model_car_drivers_INCLUDE_DIRS})
+```
+INCLUDE_DIRECTORIES(${model_car_drivers_INCLUDE_DIRS})
+```
and it is also necessary to link with the desired libraries by using the following command
- TARGET_LINK_LIBRARIES(<executable name> ${model_car_drivers_LIBRARIES})
+```
+TARGET_LINK_LIBRARIES(<executable name> ${model_car_drivers_LIBRARIES})
+```
-## Examples
+</p>
+</details>
-There are examples that show how to use the different classes.
+## Disclaimer
-</p>
-</details>
\ No newline at end of file
+Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+Mantainer IRI labrobotica (labrobotica@iri.upc.edu)
+
+This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.
+
+In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>
\ No newline at end of file
diff --git a/doc/actuators.md b/doc/actuators.md
new file mode 100644
index 0000000000000000000000000000000000000000..ddd342c6d4f6bb90be668602eb3cd381bda5e3df
--- /dev/null
+++ b/doc/actuators.md
@@ -0,0 +1,58 @@
+# Actuators driver
+
+This driver allows to send motion commands to the model car platform. The motions commands are:
+
+* **speed**: this is desired speed of the car. This value represents a percentage of the maximum speed and the valid range is from -100.0 to 100.0.
+* **steering**: This is the desired angle of the servo that controls the steering mechanism of the model car. The actual steering angle of the model car is not directly proportional to this value.
+
+Check section 1,5 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) for further details on the traction motor, its speed controller and the steering servo used in the model car.
+
+The main features of this driver are:
+
+* Send speed and steering commands.
+* A watchdog that forces the motion command to be periodically sent, or otherwise the model car will stop after a while.
+* An emergency stop that, when activated, disables both the traction motor and the steering servo, and prevents the car from moving again. The only way to re-enable them is to reset the associated Arduino board.
+
+## Calibration
+
+The [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) of the model car, in chapter 3, presents the procedure to calibrate and adjust both speed and steering controllers. The factory calibration should be enough, and, in general, it is only necessary to adjust the zero of both the speed and steering controllers using the two potentiometers located on the PCB.
+
+## Example
+
+This driver provides a simple example that sends both speed and steering commands following a sinusoidal function. To test it, make sure the wheels of the model car are not in contact with the floor and execute the following command:
+
+```
+./bin/model_car_actuators_test
+```
+
+## Troubleshooting
+
+If the driver can not connect to the Arduino board:
+
+* Check that at least there exist 4 **ttyACM\*** devices in the */dev* folder. If any **ttyACM\*** device is missing:
+ * check that the USB cable is attached to the min-USB connector. Connect it if necessary.
+ * check that the green LED in the Arduino board is blinking. If the LED is not blinking, check the power supply and contact the technical support.
+* Check that the user belongs to the *dialout* group. To do so, execute the following command on a terminal. An example output is shown below:
+```
+groups
+car adm **dialout** cdrom sudo dip plugdev lpadmin sambashare
+```
+* Contact the technical support.
+
+If the traction motor and/or the steering servo do not move:
+
+* Make sure both the power and monitor connectors of the motors battery are connected. The battery display monitor should display the current voltage of each cell. Connect them if necessary. See the next image.
+
+<img align="center" src="images/motor_battery_troubleshooting.png" alt="Correct connection of the motor battery" width="921" height="371">
+
+* Make sure the batteries are charged. Check the value shown in the battery display monitor and charge them if necessary.
+* Make sure the button on the right side of the model car, labeled Motor, is pressed. Press it if necessary.
+
+<img align="center" src="images/motor_button.png" alt="Correct position of the motor button on the right side of the model car" width="496" height="372">
+
+* Make sure the control board is not configured for RF control (see section 2.5 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r)).
+* Reset the Arduino actuator controller. The actuator Arduino is the one besides the motor battery display monitor.
+* Contact the technical support.
+
+
+
diff --git a/doc/batteries.md b/doc/batteries.md
new file mode 100644
index 0000000000000000000000000000000000000000..d4c1f0a9acba8459d8d346416e1acf8efaa74b19
--- /dev/null
+++ b/doc/batteries.md
@@ -0,0 +1,33 @@
+# Batteries
+
+This driver provides information on the state of the two batteries of the model car:
+
+* Motors battery: powers the traction motor, the speed controller and the steering servo.
+* Payload battery: powers the on-board computer and GPU, Arduino board controllers, sensors, USB hubs and other on-board devices.
+
+The information provided for each battery is the individual cell voltages and the whole battery voltage. It does not provide any fuel-gauging capabilities that estimates the actual state of charge of the batteries.
+
+In addition of the software monitoring of the batteries provided by this driver, it is very important to connect both batteries to the onboard display monitoring modules which will provide an acoustic signal when it is necessary to charge the batteries.
+
+# Example
+
+This driver provides a simple example that shows the measured data of the sensors on screen. To test it execute the following command:
+
+```
+./bin/model_car_batteries_test
+```
+
+This is an example output of the example program:
+
+```
+ motors battery voltage: 8.221
+ motors battery cell1 voltage: 4.051
+ motors battery cell2 voltage: 4.17
+ payload battery voltage: 23.837
+ payload battery cell1 voltage: 3.963
+ payload battery cell2 voltage: 3.992
+ payload battery cell3 voltage: 4.052
+ payload battery cell4 voltage: 4.081
+ payload battery cell5 voltage: 3.963
+ payload battery cell6 voltage: 4.111
+```
\ No newline at end of file
diff --git a/doc/egomotion.md b/doc/egomotion.md
new file mode 100644
index 0000000000000000000000000000000000000000..9479591d25112ee5b5e7e2fd4ee6d559cbf63363
--- /dev/null
+++ b/doc/egomotion.md
@@ -0,0 +1,59 @@
+# Egomotion
+
+This driver provides access to the following sensor information:
+
+* The left and right wheel encoder total number of counts.
+* The left and right wheel direction.
+* The linear acceleration, angular velocity and magnetic heading from the IMU.
+
+Each measurement has a timestamp associated to it. This timestamp starts when the car is first powered up or when it is reset and has a resolution of microseconds. Although it can not be used as an absolute time reference, it can be used to know the interval between consecutive measurements.
+
+Notice that there is no feedback for the actual steering servo angle.
+
+## Wheel encoders
+
+The number of counts for the wheel encoder data represents the total displacement in either direction, and the direction flag must be used two know if the count increment between the current and the last measurement is positive or negative.
+
+The wheel encoders have a relatively low resolution (60 counts per revolution) which makes it difficult to have a good speed control at low speeds. Check section 1.4.4 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) for further details.
+
+## IMU
+
+The IMU used in the car is the [MPU9250](https://invensense.tdk.com/products/motion-tracking/9-axis/mpu-9250/). Check its documentation for further details and section 1.4.3 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) fir further details.
+
+A set of functions is provided to handle the zero calibration process of the gyroscope sensor. These feature allows the user to:
+
+* Start the calibration process specifying the number of samples to average. The default value is 100.
+* Stop the calibration process at any time. In this case, the calibration data up to the stopping point will be used.
+* Check the current status of the calibration process.
+
+# Example
+
+This driver provides a simple example that shows the measured data of the sensors on screen. To test it execute the following command:
+
+```
+./bin/model_car_egomotion_test
+```
+
+This is an example output of the example program:
+
+```
+ Left Wheel:
+ tachometer: 64
+ direction: 0
+ timestamp: 1124172648
+ Right Wheel:
+ tachometer: 57
+ direction: 0
+ timestamp: 1124172804
+ IMU:
+ ax: -0.0724493
+ ay: -0.760419
+ az: 9.76869
+ gx: 0.00119842
+ gy: -0.00266316
+ gz: -0.00186421
+ mx: -1.31566e-05
+ my: -3.18546e-05
+ mz: 1.53447e-05
+ timestamp: 1124200160
+```
diff --git a/doc/images/drivers.odp b/doc/images/drivers.odp
new file mode 100644
index 0000000000000000000000000000000000000000..6e2ca7c8d9334165a1605f3afb52e876c9890c15
Binary files /dev/null and b/doc/images/drivers.odp differ
diff --git a/doc/images/model_car_drivers.png b/doc/images/model_car_drivers.png
new file mode 100644
index 0000000000000000000000000000000000000000..11d992ec6f988d0bae90734e9ea52d5a582d4282
Binary files /dev/null and b/doc/images/model_car_drivers.png differ
diff --git a/doc/images/motor_battery_troubleshooting.png b/doc/images/motor_battery_troubleshooting.png
new file mode 100644
index 0000000000000000000000000000000000000000..ebc3ce4567c637b29c576eadb6a65aa4956fef34
Binary files /dev/null and b/doc/images/motor_battery_troubleshooting.png differ
diff --git a/doc/images/motor_button.png b/doc/images/motor_button.png
new file mode 100644
index 0000000000000000000000000000000000000000..b75deacd09240e651b7569ed99cff6a2b6eb0597
Binary files /dev/null and b/doc/images/motor_button.png differ
diff --git a/doc/ultrasounds.md b/doc/ultrasounds.md
new file mode 100644
index 0000000000000000000000000000000000000000..19f2fae3b52a654b944e619cadc7e8ad28e2a7b6
--- /dev/null
+++ b/doc/ultrasounds.md
@@ -0,0 +1,29 @@
+# Ultrasounds
+
+This driver provides the distance measured by each of the ultrasound sensors available on the car:
+
+* Left side
+* Right side
+* Rear left side
+* Rear
+* Rear right side
+
+The distance provided by this driver is in meters. Check section 1.4.2 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) for more details on the ultrasound sensors used on the model car
+
+# Example
+
+This driver provides a simple example that shows the measured data of the sensors on screen. To test it execute the following command:
+
+```
+./bin/model_car_ultrasounds_test
+```
+
+This is an example output of the example program:
+
+```
+ Side right ultrasound distance: 0.18
+ Side left ultrasound distance: 1.17
+ Rear right ultrasound distance: 1.2
+ Rear center ultrasound distance: 1.17
+ Rear left ultrasound distance: 1.18
+```
diff --git a/include/model_car_actuators.h b/include/model_car_actuators.h
new file mode 100644
index 0000000000000000000000000000000000000000..47378aceb5ddefcf54b9ec2861104e6bd2c3a70c
--- /dev/null
+++ b/include/model_car_actuators.h
@@ -0,0 +1,29 @@
+#ifndef _MODEL_CAR_ACTUATORS_H
+#define _MODEL_CAR_ACTUATORS_H
+
+#include "model_car_driver_base.h"
+
+/**
+ * @class CModelCarActuators
+ * @brief CModelCarActuators class brief description text
+ * @details CModelCarActuators class detailed description text
+ *
+ */
+class CModelCarActuators: public CModelCarDriverBase
+{
+ private:
+ void process_data_frame(uint8_t id, TDataUnion data_union);
+
+ protected:
+ void send_watchdog(void);
+
+ public:
+ CModelCarActuators(std::string name);
+ void send_command(double speed,double steering);
+ void emergency_stop(void);
+ void enable_ultrasounds(void);
+ void disable_ultrasounds(void);
+ ~CModelCarActuators(void);
+};
+
+#endif
diff --git a/include/model_car_batteries.h b/include/model_car_batteries.h
new file mode 100644
index 0000000000000000000000000000000000000000..4b2ae9957e23a84bad26862d5558542cb2cd6c7e
--- /dev/null
+++ b/include/model_car_batteries.h
@@ -0,0 +1,36 @@
+#ifndef _MODEL_CAR_BATTERIES_H
+#define _MODEL_CAR_BATTERIES_H
+
+#include "model_car_driver_base.h"
+#include <map>
+
+typedef std::map<const SENSOR_ID, double> voltages_map_t;
+
+/**
+ * @class CModelCarBatteries
+ * @brief CModelCarBatteries class brief description text
+ * @details CModelCarBatteries class detailed description text
+ *
+ */
+class CModelCarBatteries: public CModelCarDriverBase
+{
+ private:
+ void process_data_frame(uint8_t id, TDataUnion data_union);
+ voltages_map_t battery_values;
+
+ public:
+ CModelCarBatteries(std::string name);
+ double get_motors_battery_voltage(void);
+ double get_payload_battery_voltage(void);
+ double get_motors_cell1_voltage(void);
+ double get_motors_cell2_voltage(void);
+ double get_payload_cell1_voltage(void);
+ double get_payload_cell2_voltage(void);
+ double get_payload_cell3_voltage(void);
+ double get_payload_cell4_voltage(void);
+ double get_payload_cell5_voltage(void);
+ double get_payload_cell6_voltage(void);
+ ~CModelCarBatteries(void);
+};
+
+#endif
diff --git a/include/model_car_driver_base.h b/include/model_car_driver_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..31fd8a94b93bd31f3976255781d7277e0193f7c5
--- /dev/null
+++ b/include/model_car_driver_base.h
@@ -0,0 +1,82 @@
+#ifndef _MODEL_CAR_DRIVER_BASE_H
+#define _MODEL_CAR_DRIVER_BASE_H
+
+#include "rs232.h"
+#include "commexceptions.h"
+#include "threadserver.h"
+#include "eventserver.h"
+#include "eventexceptions.h"
+#include "model_car_protocol.h"
+
+#define DEFAULT_TIMEOUT 200
+#define NO_RX_TIMEOUT -1.0
+
+enum sm_state { WAIT_START, READ_ID, READ_DATA_LENGTH, READ_TIMESTAMP, READ_DATA, READ_CRC, READ_END};
+
+union TTimeStampUnion
+{
+ uint8_t raw[4];
+ uint32_t time_stamp;
+};
+
+union TCRCUnion
+{
+ uint8_t raw[2];
+ uint16_t crc;
+};
+
+/**
+ * @class CModelCarDriverBase
+ * @brief CModelCarDriverBase class brief description text
+ * @details CModelCarDriverBase class detailed description text
+ *
+ */
+class CModelCarDriverBase
+{
+ private:
+ std::string name;
+ std::string device;
+ CRS232 *serial_port;
+ CMutex port_mutex;
+
+ uint8_t arduino_id;
+ uint8_t detected_id;
+ uint16_t sw_version;
+ uint32_t rx_timeout;
+ uint32_t timestamp_lsb;
+ uint32_t timestamp_msb;
+
+ /* event attributes */
+ std::string finish_thread_event_id;
+ std::string new_info_event_id;
+
+ /* thread attributes */
+ CThreadServer *thread_server;
+ std::string data_thread_id;
+
+ protected:
+ CMutex data_mutex;
+ CEventServer *event_server;
+ std::string new_data_event_id;
+
+ bool open_dev(std::string &serial_dev);
+ static void *data_thread(void *param);
+ void send_request(uint8_t id, uint8_t data_length, uint8_t *data);
+ void stuff_frame(uint8_t *frame_in, uint8_t size_in, uint8_t **frame_out, uint8_t &size_out);
+ bool process_byte(THeader &header, TDataUnion &data_union, TCRCUnion &crc_union, uint8_t byte);
+ void process_data(THeader & header, TDataUnion & data_union, TCRCUnion & crc_union);
+ virtual void process_data_frame(uint8_t id,TDataUnion data_union)=0;
+ uint16_t fletcher16(uint8_t *data, uint8_t bytes);
+ uint64_t get_last_timestamp(void);
+ public:
+ CModelCarDriverBase(std::string name, ARDUINO_ID arduino_id);
+ void open(void);
+ uint8_t get_id(void);
+ void set_rx_timeout(double time_s);
+ double get_rx_timeout(void);
+ void close(void);
+ std::string get_new_data_event_id(void);
+ virtual ~CModelCarDriverBase(void);
+};
+
+#endif
diff --git a/include/model_car_drivers.h b/include/model_car_drivers.h
deleted file mode 100644
index fe69f8435be4fd09adebe06551af96f903c02884..0000000000000000000000000000000000000000
--- a/include/model_car_drivers.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#ifndef _MODEL_CAR_DRIVERS_H
-#define _MODEL_CAR_DRIVERS_H
-
-/**
- * @class CModel_Car_Drivers
- * @brief CModel_Car_Drivers class brief description text
- * @details CModel_Car_Drivers class detailed description text
- *
- */
-class CModel_Car_Drivers
-{
- public:
- CModel_Car_Drivers();
- ~CModel_Car_Drivers();
-};
-
-#endif
diff --git a/include/model_car_drivers_act.h b/include/model_car_drivers_act.h
deleted file mode 100644
index df0fa22e9c805df2b4e50e9e5123370c581395d0..0000000000000000000000000000000000000000
--- a/include/model_car_drivers_act.h
+++ /dev/null
@@ -1,43 +0,0 @@
-#ifndef _MODEL_CAR_DRIVERS_ACT_H
-#define _MODEL_CAR_DRIVERS_ACT_H
-
-#include "rs232.h"
-#include "commexceptions.h"
-#include "threadserver.h"
-#include "model_car_drivers_base.h"
-#include "model_car_drivers_base_exceptions.h"
-#include "model_car_drivers_protocol.h"
-#include <iostream>
-#include <iomanip>
-#include <vector>
-#include <dirent.h>
-#include <chrono>
-#include <thread>
-#include <map>
-
-/**
- * @class CModel_Car_Drivers_Act
- * @brief CModel_Car_Drivers_Act class brief description text
- * @details CModel_Car_Drivers_Act class detailed description text
- *
- */
-class CModel_Car_Drivers_Act: public CModel_Car_Drivers_Base
-{
- private:
- void process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union);
-
- protected:
-
- public:
- CModel_Car_Drivers_Act(std::string name);
- void send_speed(float speed);
- void send_steering(float angle);
- void send_watchdog();
- void send_emergency_stop();
- void send_light(uint8_t value);
- void send_enable_uss();
- void send_disable_uss();
- ~CModel_Car_Drivers_Act(void);
-};
-
-#endif
diff --git a/include/model_car_drivers_base.h b/include/model_car_drivers_base.h
deleted file mode 100644
index e0cb86d40bb3a6e5e78836455c32ac1dd8bcf8af..0000000000000000000000000000000000000000
--- a/include/model_car_drivers_base.h
+++ /dev/null
@@ -1,76 +0,0 @@
-#ifndef _MODEL_CAR_DRIVERS_BASE_H
-#define _MODEL_CAR_DRIVERS_BASE_H
-
-#include "rs232.h"
-#include "commexceptions.h"
-#include "threadserver.h"
-// #include "eventserver.h"
-#include "model_car_drivers_base_exceptions.h"
-#include "model_car_drivers_protocol.h"
-#include <iostream>
-#include <iomanip>
-#include <vector>
-#include <dirent.h>
-#include <chrono>
-#include <thread>
-
-enum sm_state { WAIT_START, READ_ID, READ_DATA_LENGTH, READ_TIMESTAMP, READ_DATA, READ_CRC, READ_END};
-
-/**
- * @class CModel_Car_Drivers_Base
- * @brief CModel_Car_Drivers_Base class brief description text
- * @details CModel_Car_Drivers_Base class detailed description text
- *
- */
-class CModel_Car_Drivers_Base
-{
- private:
- std::string name;
- int arduino_id;
- bool paired;
- CRS232 *serial_port;
- CMutex port_mutex;
-
- bool debug;
- tDataUnion data_union;
- tArduinoHeader header;
- tInfoData info_data;
- tErrorData error_data;
- sm_state state;
-
- /* event attributes */
- CEventServer *event_server;
- std::string finish_thread_event_id;
- std::string new_data_event_id;
- std::string new_info_event_id;
-
- /* thread attributes */
- double data_rate_hz;
- CThreadServer *thread_server;
- std::string data_thread_id;
-
- protected:
- CMutex data_mutex;
-
- static void *data_thread(void *param);
- void send_info_request();
- void send_request(tArduinoHeader header, uint8_t data);
- void get_next_info_frame(unsigned char frame);
- int read_data();
- virtual void process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union)=0;
- uint16_t fletcher16(uint8_t const *data, uint8_t bytes);
- void delete_thread_and_events(void);
-
- public:
- CModel_Car_Drivers_Base(std::string name, ARDUINO_ID arduino_id);
- void start();
- bool open(std::string &serial_dev);
- void stop(void);
- std::string get_new_data_event_id(void);
- //void test(unsigned char *data, unsigned int len);
- void set_debug(bool debug);
-
- virtual ~CModel_Car_Drivers_Base(void);
-};
-
-#endif
diff --git a/include/model_car_drivers_base_exceptions.h b/include/model_car_drivers_base_exceptions.h
deleted file mode 100644
index 4ba9a7a324b691feecef007abdd393ab88954f33..0000000000000000000000000000000000000000
--- a/include/model_car_drivers_base_exceptions.h
+++ /dev/null
@@ -1,81 +0,0 @@
-#ifndef MODEL_CAR_DRIVERS_BASE_EXCEPTIONS
-#define MODEL_CAR_DRIVERS_BASE_EXCEPTIONS
-
-#include "exceptions.h"
-#include <stdint.h>
-
-/**
- * \brief CModel_Car_Drivers_Base exception class
- *
- * This class implements the exceptions for the CModel_Car_Drivers_Base class.
- *
- * Similarly to other exception classes, it appends a class identifer
- * string ("[CModel_Car_Drivers_Base] - ") to the error message in order to identify the
- * class that generated the exception.
- *
- * The base class can be used to catch any exception thrown by the application
- * or also, this class can be used in order to catch only exceptions generated
- * by CModel_Car_Drivers_Base objects.
- *
- */
-class CModel_Car_Drivers_BaseException : public CException
-{
- public:
- /**
- * \brief Constructor
- *
- * The constructor calls the base class constructor to add the general
- * exception identifier and then adds the class identifier string
- * "[CModel_Car_Drivers_Base class]" and the supplied error message.
- *
- *
- * \verbatim
- * [Exception caught] - <where>
- * [CModel_Car_Drivers_Base class] - <error message>
- * \endverbatim
- *
- * \param where a null terminated string with the information about the name
- * of the function, the source code filename and the line where
- * the exception was generated. This string must be generated
- * by the _HERE_ macro.
- *
- * \param error_msg a null terminated string that contains the error message.
- * This string may have any valid character and there is no
- * limit on its length.
- *
- *
- */
- CModel_Car_Drivers_BaseException(const std::string& where, const std::string& error_msg);
-
- /**
- * \brief Constructor
- *
- * The constructor calls the base class constructor to add the general
- * exception identifier and then adds the class identifier string
- * "[CModel_Car_Drivers_Base class]", the supplied error message and the hexadecimal message received.
- *
- *
- * \verbatim
- * [Exception caught] - <where>
- * [CModel_Car_Drivers_Base class] - <error message>
- * [Serial message] - <serial_msg>
- * \endverbatim
- *
- * \param where a null terminated string with the information about the name
- * of the function, the source code filename and the line where
- * the exception was generated. This string must be generated
- * by the _HERE_ macro.
- *
- * \param error_msg a null terminated string that contains the error message.
- * This string may have any valid character and there is no
- * limit on its length.
- *
- * \param serial_msg Pointer to the message received by Serial port.
- *
- * \param msg_length Serial message length.
- *
- */
- CModel_Car_Drivers_BaseException(const std::string& where, const std::string& error_msg, const uint8_t *serial_msg, uint8_t msg_length);
-};
-
-#endif
diff --git a/include/model_car_drivers_bat.h b/include/model_car_drivers_bat.h
deleted file mode 100644
index db898b7fa3a1839bad05778057a04bc78e970b2f..0000000000000000000000000000000000000000
--- a/include/model_car_drivers_bat.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef _MODEL_CAR_DRIVERS_BAT_H
-#define _MODEL_CAR_DRIVERS_BAT_H
-
-#include "rs232.h"
-#include "commexceptions.h"
-#include "threadserver.h"
-#include "model_car_drivers_base.h"
-#include "model_car_drivers_base_exceptions.h"
-#include "model_car_drivers_protocol.h"
-#include <iostream>
-#include <iomanip>
-#include <vector>
-#include <dirent.h>
-#include <chrono>
-#include <thread>
-#include <map>
-
-/**
- * @class CModel_Car_Drivers_Bat
- * @brief CModel_Car_Drivers_Bat class brief description text
- * @details CModel_Car_Drivers_Bat class detailed description text
- *
- */
-class CModel_Car_Drivers_Bat: public CModel_Car_Drivers_Base
-{
- private:
- void process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union);
- std::map<SENSOR_ID, tVoltageData> m_bat_values;
-
- protected:
-
- public:
- CModel_Car_Drivers_Bat(std::string name);
- bool get_bat(SENSOR_ID id, float & value);
- ~CModel_Car_Drivers_Bat(void);
-};
-
-#endif
diff --git a/include/model_car_drivers_egomotion.h b/include/model_car_drivers_egomotion.h
deleted file mode 100644
index 1991171d999f24acc5de341e1495ff805e5c4d3c..0000000000000000000000000000000000000000
--- a/include/model_car_drivers_egomotion.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef _MODEL_CAR_DRIVERS_EGOMOTION_H
-#define _MODEL_CAR_DRIVERS_EGOMOTION_H
-
-#include "rs232.h"
-#include "commexceptions.h"
-#include "threadserver.h"
-#include "model_car_drivers_base.h"
-#include "model_car_drivers_base_exceptions.h"
-#include "model_car_drivers_protocol.h"
-#include <iostream>
-#include <iomanip>
-#include <vector>
-#include <dirent.h>
-#include <chrono>
-#include <thread>
-#include <map>
-
-/**
- * @class CModel_Car_Drivers_Egomotion
- * @brief CModel_Car_Drivers_Egomotion class brief description text
- * @details CModel_Car_Drivers_Egomotion class detailed description text
- *
- */
-class CModel_Car_Drivers_Egomotion: public CModel_Car_Drivers_Base
-{
- private:
- void process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union);
- tSensWheelData left_wheel;
- tSensWheelData right_wheel;
- tImuData imu;
-
- protected:
-
- public:
- CModel_Car_Drivers_Egomotion(std::string name);
- bool get_left_wheel(tSensWheelData & data);
- bool get_right_wheel(tSensWheelData & data);
- bool get_imu(tImuData & data);
- ~CModel_Car_Drivers_Egomotion(void);
-};
-
-#endif
diff --git a/include/model_car_drivers_protocol.h b/include/model_car_drivers_protocol.h
deleted file mode 100644
index a51cc9af923da14e031ce362d87e752bbba929cf..0000000000000000000000000000000000000000
--- a/include/model_car_drivers_protocol.h
+++ /dev/null
@@ -1,389 +0,0 @@
-/**
-Copyright (c)
-Audi Autonomous Driving Cup. All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
-
-1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
-2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
-3. All advertising materials mentioning features or use of this software must display the following acknowledgement: “This product includes software developed by the Audi AG and its contributors for Audi Autonomous Driving Cup.”
-4. Neither the name of Audi nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL AUDI AG OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-
-**********************************************************************
-* $Author:: spiesra $ $Date:: 2018-05-15 10:15:43#$ $Rev:: 75429 $
-**********************************************************************/
-
-///////////////////////////////////////////////////////////////////////////////
-/// \file
-///
-/// \brief The arduino protocol definition.
-///
-/// This header define all structures which are used in the protocol. It defines
-/// also some arduino specific stuff like arduino ids and samplingrates.
-///
-///////////////////////////////////////////////////////////////////////////////
-
-#ifndef ARDUINO_PROTOCOL_HPP
-#define ARDUINO_PROTOCOL_HPP
-
-#include <stdint.h>
-
-#pragma pack(push, 1)
-
-///////////////////////////////////////////////////////////////////////////////
-//
-// Arduino frame definition
-//
-///////////////////////////////////////////////////////////////////////////////
-/*
-
- +----+------------+------------+------+-------+
- | ID | Datalength | Timestamp | Data | CRC16 |
- +----+------------+------------+------+-------+
-Size (Byte): 1 1 4 var 2
-
-+-------------------------------------------------------------+
-
-Byte: 0 1 2 6 ?
-
-
-*/
-
-/*! A macro that defines start byte. */
-#define START_BYTE 0x02 //STX
-
-/*! A macro that defines end byte. */
-#define END_BYTE 0x03 //ETX
-
-/*! A macro that defines escape byte. */
-#define ESCAPE_BYTE 0x1B //ESCAPE
-
-/*! A define for maximum trys to read a specific byte which is received by the arduino. */
-#define MAX_MSG_LENGTH_TILL_TIMEOUT 64 // uint8_t
-
-/*! The maximum size of the std::vector implementation as implemented in utils_arduino.h */
-#define MAX_VECTOR_BUFFER_SIZE 128
-
-/*!
- * The Frame header definition of the protocol.
- *
- */
-struct tArduinoHeader
-{
- /*! The header id */
- uint8_t ui8ID;
- /*! Length of the data frame */
- uint8_t ui8DataLength;
- /*! The arduino timestamp */
- uint32_t ui32Timestamp;
-};
-
-/*!
- * All the sensor ids, which are set in tArduinoHeader.ui8ID.
- *
- * To construct a frame with the specific id, create a tArduinoHeader struct and set the ui8ID
- * to that specific enum. This enums are used to send sensor information to a processing host.
- * The arduino system never processes these ids.
- */
-enum SENSOR_ID /*: uint8_t*/{
-
- ID_ARD_SENSOR_INFO,
- ID_ARD_SENS_ERROR,
-
- ID_ARD_SENS_US_FRONT_LEFT,
- ID_ARD_SENS_US_FRONT_CENTER_LEFT,
- ID_ARD_SENS_US_FRONT_CENTER,
- ID_ARD_SENS_US_FRONT_CENTER_RIGHT,
- ID_ARD_SENS_US_FRONT_RIGHT,
-
- ID_ARD_SENS_US_REAR_RIGHT,
- ID_ARD_SENS_US_REAR_CENTER_RIGHT,
- ID_ARD_SENS_US_REAR_CENTER,
- ID_ARD_SENS_US_REAR_CENTER_LEFT,
- ID_ARD_SENS_US_REAR_LEFT,
-
- ID_ARD_SENS_US_SIDE_LEFT,
- ID_ARD_SENS_US_SIDE_RIGHT,
-
- ID_ARD_SENS_WHEEL_RIGHT,
- ID_ARD_SENS_WHEEL_LEFT,
-
- ID_ARD_SENS_IMU,
-
- ID_ARD_SENS_VOLT_ACTUATOR,
- ID_ARD_SENS_VOLT_ACTUATOR_CELL1,
- ID_ARD_SENS_VOLT_ACTUATOR_CELL2,
-
- ID_ARD_SENS_VOLT_SENSORS,
- ID_ARD_SENS_VOLT_SENSORS_CELL1,
- ID_ARD_SENS_VOLT_SENSORS_CELL2,
- ID_ARD_SENS_VOLT_SENSORS_CELL3,
- ID_ARD_SENS_VOLT_SENSORS_CELL4,
- ID_ARD_SENS_VOLT_SENSORS_CELL5,
- ID_ARD_SENS_VOLT_SENSORS_CELL6,
-};
-
-/*!
- * The Info struct.
- *
- * This struct is used as payload (data part) in the protocol. See top of file to see its
- * position.
- */
-struct tInfoData
-{
- /*! see enum ARDUINO_ADDRESS for number(id) of the arduinos */
- uint8_t ui8ArduinoAddress;
- /*! version of the arduino software */
- uint16_t ui16ArduinoVersion;
-};
-
-/*!
- * The Error struct.
- *
- * This struct is used as payload (data part) in the protocol. See top of file to see its
- * position. The ui8ErrorNr is filled with an enum (ARD_ERROR) as described below.
- */
-struct tErrorData
-{
- /*! The error nr */
- int8_t ui8ErrorNr;
-};
-
-/*!
- * The Us data struct.
- *
- * This struct is used as payload (data part) in the protocol. See top of file to see its
- * position. Its member i16Distance is filled with the distance and its unit is [cm].
- */
-struct tUsData
-{
- /*! The distance in cm */
- int16_t i16Distance;
-};
-
-/*!
- * The wheel data struct.
- *
- * This struct is used as payload (data part) in the protocol. See top of file to see its
- * position. Its two members are used to indicate the tachometer and the direction of the driving
- * wheels. ui32WheelTach is the tick count of the wheels.
- */
-struct tSensWheelData
-{
- /*! The wheel tach */
- uint32_t ui32WheelTach;
- /*! The wheel direction */
- int8_t i8WheelDir;
-};
-
-/*!
- * The imu data struct.
- *
- * This struct is used as payload (data part) in the protocol. See top of file to see its
- * position. The units are ...acceleration [g], gyroscope [°/s] and for magnetometer [mG].
- */
-struct tImuData
-{
- /*! The ax */
- float f32ax;
- /*! The ay */
- float f32ay;
- /*! The az */
- float f32az;
-
- /*! The gx */
- float f32gx;
- /*! The gy */
- float f32gy;
- /*! The gz */
- float f32gz;
-
- /*! The mx */
- float f32mx;
- /*! The my */
- float f32my;
- /*! The mz */
- float f32mz;
-
- /*! The roll */
- float f32roll;
- /*! The pitch */
- float f32pitch;
- /*! The yaw */
- float f32yaw;
-};
-
-/*!
- * The voltage data struct.
- *
- * This struct is used as payload (data part) in the protocol. See top of file to see its
- * position. The unit is in milliVolt [mV].
- */
-struct tVoltageData
-{
- /*! Information describing the 16 voltage */
- uint16_t ui16VoltageData;
-};
-
-/*!
- * The data union for the protocol frame.
- *
- * This union is a helper define for easier parsing by the receiver. The received frame could be
- * just copied into this union for easy access.
- */
-union tDataUnion
-{
- /*! The raw data */
- unsigned char raw[48];
- /*! The information */
- tInfoData info;
- /*! The error */
- tErrorData error;
- /*! The us distance */
- tUsData us;
- /*! The imu data*/
- tImuData imu;
- /*! The wheel data */
- tSensWheelData wheel;
- /*! The voltage data*/
- tVoltageData voltage;
-};
-
-/*!
- * The error codes as used by the ID_ARD_SENS_ERROR id.
- *
- * The elements of this enums are used to fill the member of the tErrorData. The member
- * ui8ErrorNr should be set to one of this enum.
- */
-enum ARD_ERROR_CODE /*: int8_t*/
-{
- ERROR_STX_NOT_FOUND = -1,
- ERROR_ETX_NOT_FOUND = -2,
- ERROR_ESC_BYTE_BROKEN = -3,
- ERROR_NO_BYTES_AVAILABLE = -4,
- ERROR_CRC_INVALID = -5,
- ERROR_NO_FRAME_DATA = -6,
- ERROR_FRAME_DROPPED = -7,
- ERROR_REMOTE_DETECTED = -8,
- ERROR_NO_GYRO_DETECTED = -9,
- ERROR_INVALID_ACTUATOR_HEADER = -10,
- ERROR_INITIALIZATION_FAILED = -11,
- ERROR_FRAME_NOT_WRITTEN = -12,
-};
-
-/*!
- * The actuator ids. These ids are only processed by ARDUINO_CENTER_ACTUATORS.
- *
- * The actuator ids are used to control the behavior of the arduino. For example send frames
- * from host with ID_ARD_ACT_STEER_SERVO or ID_ARD_ACT_SPEED_CONTR to control the steering and
- * throttle of the model car. Currently only the arduino with the id ARDUINO_CENTER_ACTUATORS
- * processes more than a ID_ARD_ACT_REQUEST frame. If a ID_ARD_ACT_REQUEST is sent to an arduino,
- * the arduino answer this request with an tInfoData.
- */
-enum ACTUATOR_ID /*: uint8_t*/
-{
- ID_ARD_ACT_NO_NAME = 0,
- ID_ARD_ACT_WATCHDOG = 1,
- ID_ARD_ACT_EMERGENCY_STOP = 2,
- ID_ARD_ACT_REQUEST = 3,
-
- ID_ARD_ACT_STEER_SERVO = 4,
- ID_ARD_ACT_SPEED_CONTR = 5,
-
- ID_ARD_ACT_LIGHT = 6,
-
- ID_ARD_DISABLE_USS = 7,
- ID_ARD_ENABLE_USS = 8,
-};
-
-/*!
- * The Watchdog data. Processed by the ARDUINO_CENTER_ACTUATORS.
- *
- * The arduino with the id ARDUINO_CENTER_ACTUATORS processes this data. It should be received
- * every half a second. Otherwise the Servos stop working.
- */
-struct tWatchdogData
-{
- /*! The triggered state 1 = true, 0 = false */
- uint8_t ui8IsTriggerd;
-} ;
-
-/*!
- * The emergeny data. Processed by the ARDUINO_CENTER_ACTUATORS.
- *
- * The arduino with the id ARDUINO_CENTER_ACTUATORS processes this data. If this data is
- * received. The arduino shuts down the servos.
- */
-struct tEmergencyStopData
-{
- /*! The triggered state 1 = true, 0 = false */
- uint8_t ui8IsTriggerd;
-};
-
-/*!
- * The servo data. Processed by the ARDUINO_CENTER_ACTUATORS.
- *
- * The arduino with the id ARDUINO_CENTER_ACTUATORS processes this data. If this data is
- * received. The arduino sets the servo position (used by steering and throttle) accordingly.
- * The range is 0...180 with 90 as zero position.
- */
-struct tServoData
-{
- /*! The angle. Actually a servo number range is [0, 180]*/
- uint8_t ui8Angle;
-};
-
-/*!
- * The light data. Processed by the ARDUINO_CENTER_ACTUATORS.
- *
- * The arduino with the id ARDUINO_CENTER_ACTUATORS processes this data. This data is filled
- * with a member of the tLightMask enum. To set more lights than one the enums are combined with
- * each other.
- */
-struct tLightData
-{
- /*! see enum tLightMask for IDs */
- uint8_t ui8LightMask;
-};
-
-/*!
- * The light mask enum. Processed by the ARDUINO_CENTER_ACTUATORS.
- *
- * The arduino with the id ARDUINO_CENTER_ACTUATORS processes this data. The members of that
- * enum are used to set the ui8LightMask in tLightData.
- */
-enum tLightMask /*: uint8_t*/
-{
- ID_ARD_ACT_LIGHT_MASK_HEAD = 0x01,
- //const uint8_t ID_ARD_ACT_LIGHT_MASK_BACK = 0x02; //same Pin as ID_ARD_ACT_LIGHT_MASK_HEAD
- ID_ARD_ACT_LIGHT_MASK_BRAKE = 0x04,
- ID_ARD_ACT_LIGHT_MASK_TURNLEFT = 0x08,
- ID_ARD_ACT_LIGHT_MASK_TURNRIGHT = 0x10,
- ID_ARD_ACT_LIGHT_MASK_HAZARD = ID_ARD_ACT_LIGHT_MASK_TURNLEFT | ID_ARD_ACT_LIGHT_MASK_TURNRIGHT,
- ID_ARD_ACT_LIGHT_MASK_REVERSE = 0x20,
-};
-
-#pragma pack(pop)
-
-/*!
- * The arduino identifiers.
- *
- * These arduino ids are used to identify which arduino is communicating. When sending an info
- * frame to an arduino, it answers with a tInfoData with an memeber of the enum as id.
- */
-enum ARDUINO_ID /*: uint8_t*/
-{
- ARDUINO_NO_NAME = 0,
- ARDUINO_FRONT_US = 1,
- ARDUINO_FRONT_IMU = 2,
- ARDUINO_CENTER_MEASUREMENT = 3,
- ARDUINO_CENTER_ACTUATORS = 4,
- ARDUINO_REAR_US = 5,
- ARDUINO_REAR_IMU_WHEELENC = 6,
- ARDUINO_TOP_RIGHT_OF_WAY = 7,
-};
-
-#endif
diff --git a/include/model_car_drivers_uss.h b/include/model_car_drivers_uss.h
deleted file mode 100644
index 3883e0652a698e8314eadceec372d5ea490e4067..0000000000000000000000000000000000000000
--- a/include/model_car_drivers_uss.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef _MODEL_CAR_DRIVERS_USS_H
-#define _MODEL_CAR_DRIVERS_USS_H
-
-#include "rs232.h"
-#include "commexceptions.h"
-#include "threadserver.h"
-#include "model_car_drivers_base.h"
-#include "model_car_drivers_base_exceptions.h"
-#include "model_car_drivers_protocol.h"
-#include <iostream>
-#include <iomanip>
-#include <vector>
-#include <dirent.h>
-#include <chrono>
-#include <thread>
-#include <map>
-
-/**
- * @class CModel_Car_Drivers_Uss
- * @brief CModel_Car_Drivers_Uss class brief description text
- * @details CModel_Car_Drivers_Uss class detailed description text
- *
- */
-class CModel_Car_Drivers_Uss: public CModel_Car_Drivers_Base
-{
- private:
- void process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union);
- std::map<SENSOR_ID, tUsData> m_uss_values;
-
- protected:
-
- public:
- CModel_Car_Drivers_Uss(std::string name);
- bool get_uss(SENSOR_ID id, float & value);
- ~CModel_Car_Drivers_Uss(void);
-};
-
-#endif
diff --git a/include/model_car_egomotion.h b/include/model_car_egomotion.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ec120336645ac2fc75bbcad76697bdbcb4f63eb
--- /dev/null
+++ b/include/model_car_egomotion.h
@@ -0,0 +1,42 @@
+#ifndef _MODEL_CAR_EGOMOTION_H
+#define _MODEL_CAR_EGOMOTION_H
+
+#include "model_car_driver_base.h"
+
+/**
+ * @class CModelCarEgomotion
+ * @brief CModelCarEgomotion class brief description text
+ * @details CModelCarEgomotion class detailed description text
+ *
+ */
+class CModelCarEgomotion: public CModelCarDriverBase
+{
+ private:
+ void process_data_frame(uint8_t id, TDataUnion data_union);
+ TWheelData left_wheel;
+ uint64_t left_wheel_timestamp;
+ TWheelData right_wheel;
+ uint64_t right_wheel_timestamp;
+ TImuData imu;
+ uint64_t imu_timestamp;
+ double zero_gx;
+ double zero_gy;
+ double zero_gz;
+ bool gyro_cal_running;
+ bool gyro_cal_stop;
+ unsigned int gyro_cal_num_samples;
+
+ public:
+ CModelCarEgomotion(std::string name);
+ void get_left_wheel(int &tach, bool &dir,unsigned long int ×tamp);
+ void get_right_wheel(int &tach, bool &dir,unsigned long int ×tamp);
+ void get_imu_accelerometer(double &ax, double &ay, double &az,unsigned long int ×tamp);
+ void get_imu_gyroscope(double &gx, double &gy, double &gz,unsigned long int ×tamp);
+ void get_imu_magnetometer(double &mx, double &my, double &mz,unsigned long int ×tamp);
+ void start_gyro_calibration(unsigned int num_samples=100);
+ void stop_gyro_calibration(void);
+ bool is_gyro_calibration_done(void);
+ ~CModelCarEgomotion(void);
+};
+
+#endif
diff --git a/include/model_car_protocol.h b/include/model_car_protocol.h
new file mode 100644
index 0000000000000000000000000000000000000000..a2f4c87590d1ad404fdbb333120b1a6b465e2b7e
--- /dev/null
+++ b/include/model_car_protocol.h
@@ -0,0 +1,187 @@
+#ifndef MODEL_CAR_PROTOCOL_HPP
+#define MODEL_CAR_PROTOCOL_HPP
+
+#include <stdint.h>
+
+#pragma pack(push, 1)
+
+/*
+ | HEADER |
+ +----+------------+------------+------+-------+
+ | ID | Datalength | Timestamp | Data | CRC16 |
+ +----+------------+------------+------+-------+
+Size (Byte): 1 1 4 var 2
++-------------------------------------------------------------+
+Byte: 0 1 2 6 ?
+*/
+
+
+#define START_BYTE 0x02
+#define END_BYTE 0x03
+#define ESCAPE_BYTE 0x1B
+
+struct THeader
+{
+ uint8_t id;
+ uint8_t data_length;
+ uint32_t time_stamp;
+};
+
+
+enum SENSOR_ID /*: int8_t*/
+{
+ ID_ARD_SENSOR_INFO = 0,
+ ID_ARD_SENS_ERROR = 1,
+
+ ID_ARD_SENS_US_FRONT_LEFT = 2,
+ ID_ARD_SENS_US_FRONT_CENTER_LEFT = 3,
+ ID_ARD_SENS_US_FRONT_CENTER = 4,
+ ID_ARD_SENS_US_FRONT_CENTER_RIGHT = 5,
+ ID_ARD_SENS_US_FRONT_RIGHT = 6,
+
+ ID_ARD_SENS_US_REAR_RIGHT = 7,
+ ID_ARD_SENS_US_REAR_CENTER_RIGHT = 8,
+ ID_ARD_SENS_US_REAR_CENTER = 9,
+ ID_ARD_SENS_US_REAR_CENTER_LEFT = 10,
+ ID_ARD_SENS_US_REAR_LEFT = 11,
+
+ ID_ARD_SENS_US_SIDE_LEFT = 12,
+ ID_ARD_SENS_US_SIDE_RIGHT = 13,
+
+ ID_ARD_SENS_WHEEL_RIGHT = 14,
+ ID_ARD_SENS_WHEEL_LEFT = 15,
+
+ ID_ARD_SENS_IMU = 16,
+
+ ID_ARD_SENS_VOLT_ACTUATOR = 17,
+ ID_ARD_SENS_VOLT_ACTUATOR_CELL1 = 18,
+ ID_ARD_SENS_VOLT_ACTUATOR_CELL2 = 19,
+
+ ID_ARD_SENS_VOLT_SENSORS = 20,
+ ID_ARD_SENS_VOLT_SENSORS_CELL1 = 21,
+ ID_ARD_SENS_VOLT_SENSORS_CELL2 = 22,
+ ID_ARD_SENS_VOLT_SENSORS_CELL3 = 23,
+ ID_ARD_SENS_VOLT_SENSORS_CELL4 = 24,
+ ID_ARD_SENS_VOLT_SENSORS_CELL5 = 25,
+ ID_ARD_SENS_VOLT_SENSORS_CELL6 = 26,
+};
+
+struct TInfoData
+{
+ uint8_t arduino_address; //see enum ARDUINO_ADDRESS
+ uint16_t arduino_version;
+};
+
+struct TErrorData
+{
+ int8_t error_number;
+};
+
+struct TUltrasoundData
+{
+ int16_t distance; //cm
+};
+
+struct TWheelData
+{
+ uint32_t wheel_tach; //encoder ticks
+ int8_t wheel_dir; //0,1
+};
+
+struct TImuData
+{
+ float ax; //g
+ float ay;
+ float az;
+
+ float gx; //º/s
+ float gy;
+ float gz;
+
+ float mx; //mG
+ float my;
+ float mz;
+
+ float roll;
+ float pitch;
+ float yaw;
+};
+
+struct TVoltageData
+{
+ uint16_t voltage; //mV
+};
+
+union TDataUnion
+{
+ unsigned char raw[48];
+ TInfoData info;
+ TErrorData error;
+ TUltrasoundData us;
+ TImuData imu;
+ TWheelData wheel;
+ TVoltageData voltage;
+};
+
+enum ARD_ERROR_CODE /*: int8_t*/
+{
+ ERROR_STX_NOT_FOUND = -1,
+ ERROR_ETX_NOT_FOUND = -2,
+ ERROR_ESC_BYTE_BROKEN = -3,
+ ERROR_NO_BYTES_AVAILABLE = -4,
+ ERROR_CRC_INVALID = -5,
+ ERROR_NO_FRAME_DATA = -6,
+ ERROR_FRAME_DROPPED = -7,
+ ERROR_REMOTE_DETECTED = -8,
+ ERROR_NO_GYRO_DETECTED = -9,
+ ERROR_INVALID_ACTUATOR_HEADER = -10,
+ ERROR_INITIALIZATION_FAILED = -11,
+ ERROR_FRAME_NOT_WRITTEN = -12,
+};
+
+enum ACTUATOR_ID /*: uint8_t*/
+{
+ ID_ARD_ACT_NO_NAME = 0,
+ ID_ARD_ACT_WATCHDOG = 1,
+ ID_ARD_ACT_EMERGENCY_STOP = 2,
+ ID_ARD_ACT_REQUEST = 3,
+
+ ID_ARD_ACT_STEER_SERVO = 4,
+ ID_ARD_ACT_SPEED_CONTR = 5,
+
+ ID_ARD_ACT_LIGHT = 6,
+
+ ID_ARD_DISABLE_USS = 7,
+ ID_ARD_ENABLE_USS = 8,
+};
+
+struct TWatchdogData
+{
+ //1:true, 0:false
+ uint8_t is_triggered;
+};
+
+struct tEmergencyStopData
+{
+ //1:true, 0:false
+ uint8_t is_triggered;
+};
+
+struct TServoData
+{
+ // [0, 180]
+ uint8_t angle;
+};
+
+#pragma pack(pop)
+
+enum ARDUINO_ID /*: uint8_t*/
+{
+ ARDUINO_NO_NAME = 0,
+ ARDUINO_CENTER_MEASUREMENT = 3,
+ ARDUINO_CENTER_ACTUATORS = 4,
+ ARDUINO_REAR_US = 5,
+ ARDUINO_REAR_IMU_WHEELENC = 6,
+};
+
+#endif
diff --git a/include/model_car_ultrasounds.h b/include/model_car_ultrasounds.h
new file mode 100644
index 0000000000000000000000000000000000000000..ef658f01f252c9cff1013b1c0e6c849589b4997c
--- /dev/null
+++ b/include/model_car_ultrasounds.h
@@ -0,0 +1,31 @@
+#ifndef _MODEL_CAR_ULTRASOUNDS_H
+#define _MODEL_CAR_ULTRASOUNDS_H
+
+#include "model_car_driver_base.h"
+#include <map>
+
+typedef std::map<const SENSOR_ID, double> ultrasounds_map_t;
+
+/**
+ * @class CModelCarUltrasounds
+ * @brief CModelCarUltrasounds class brief description text
+ * @details CModelCarUltrasounds class detailed description text
+ *
+ */
+class CModelCarUltrasounds: public CModelCarDriverBase
+{
+ private:
+ void process_data_frame(uint8_t id, TDataUnion data_union);
+ ultrasounds_map_t ultrasound_values;
+
+ public:
+ CModelCarUltrasounds(std::string name);
+ double get_side_right_distance(void);
+ double get_side_left_distance(void);
+ double get_rear_right_distance(void);
+ double get_rear_center_distance(void);
+ double get_rear_left_distance(void);
+ ~CModelCarUltrasounds(void);
+};
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 01c9a706b3abc0072109886fadbc1f371291a3b7..a50bed2b854b52285ae9239a3a8b103918dc2dbe 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,21 +1,17 @@
# driver source files
-SET(sources ${PROJECT_NAME}.cpp
- ${PROJECT_NAME}_base.cpp
- ${PROJECT_NAME}_base_exceptions.cpp
- ${PROJECT_NAME}_uss.cpp
- ${PROJECT_NAME}_bat.cpp
- ${PROJECT_NAME}_egomotion.cpp
- ${PROJECT_NAME}_act.cpp)
+SET(sources model_car_driver_base.cpp
+ model_car_actuators.cpp
+ model_car_ultrasounds.cpp
+ model_car_egomotion.cpp
+ model_car_batteries.cpp)
# application header files
-SET(headers ../include/${PROJECT_NAME}.h
- ../include/${PROJECT_NAME}_base.h
- ../include/${PROJECT_NAME}_base_exceptions.h
- ../include/${PROJECT_NAME}_protocol.h
- ../include/${PROJECT_NAME}_uss.h
- ../include/${PROJECT_NAME}_bat.h
- ../include/${PROJECT_NAME}_egomotion.h
- ../include/${PROJECT_NAME}_act.h)
+SET(headers ../include/model_car_driver_base.h
+ ../include/model_car_protocol.h
+ ../include/model_car_actuators.h
+ ../include/model_car_ultrasounds.h
+ ../include/model_car_egomotion.h
+ ../include/model_car_batteries.h)
# locate the necessary dependencies
# FIND_PACKAGE(my_example_library REQUIRED)
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 09db7d58ba540ccac51eddc308c181310772f8f4..75133f2ff7bc511b1b3eef70b22139e59345f5d9 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,20 +1,15 @@
-# create an example application
-ADD_EXECUTABLE(${PROJECT_NAME}_test ${PROJECT_NAME}_test.cpp)
-# link necessary libraries
-TARGET_LINK_LIBRARIES(${PROJECT_NAME}_test ${PROJECT_NAME})
-
-SET(name ${PROJECT_NAME}_test_uss)
+SET(name model_car_ultrasounds_test)
ADD_EXECUTABLE(${name} ${name}.cpp)
TARGET_LINK_LIBRARIES(${name} ${PROJECT_NAME})
-SET(name ${PROJECT_NAME}_test_bat)
+SET(name model_car_batteries_test)
ADD_EXECUTABLE(${name} ${name}.cpp)
TARGET_LINK_LIBRARIES(${name} ${PROJECT_NAME})
-SET(name ${PROJECT_NAME}_test_egomotion)
+SET(name model_car_egomotion_test)
ADD_EXECUTABLE(${name} ${name}.cpp)
TARGET_LINK_LIBRARIES(${name} ${PROJECT_NAME})
-SET(name ${PROJECT_NAME}_test_act)
+SET(name model_car_actuators_test)
ADD_EXECUTABLE(${name} ${name}.cpp)
TARGET_LINK_LIBRARIES(${name} ${PROJECT_NAME})
diff --git a/src/examples/model_car_actuators_test.cpp b/src/examples/model_car_actuators_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91450794f5f37a12a16f8a9d29bac7e36b2c0f7f
--- /dev/null
+++ b/src/examples/model_car_actuators_test.cpp
@@ -0,0 +1,66 @@
+#include "model_car_actuators.h"
+#include <iostream>
+#include <math.h>
+
+int main(int argc, char *argv[])
+{
+ std::string name = "model_car_driver_actuators";
+ CModelCarActuators *act_driver;
+ double vx,steer;
+
+ while(1)
+ {
+ try
+ {
+ act_driver = new CModelCarActuators(name);
+ act_driver->open();
+
+ if(argc!=3)
+ {
+ std::cout << "Insufficient arguments. Usage:" << std::endl;
+ std::cout << " model_car_actuators_test speed steering angle" << std::endl;
+ std::cout << " speed: a value between -100 and 100 corresponding to the minimum and maximum speeds" << std::endl;
+ std::cout << " steering: a value between -100 and 100 corresponding to the minimum and maximum steering angles" << std::endl;
+ return 0;
+ }
+ else
+ {
+ vx=std::stod(argv[1]);
+ steer=std::stod(argv[2]);
+ }
+
+ act_driver->disable_ultrasounds();
+
+ std::cout << "model_car_actuators_test::main: sending speed " << vx << " and steering " << steer << std::endl;
+/*
+ for(unsigned int i=0;i<100;i++)
+ {
+ usleep(100000);
+ vx=20.0*sin((2.0*3.14159*i)/100.0);
+ steer=50.0*sin((2.0*3.14159*i)/100.0);
+ std::cout << vx << "," << steer << std::endl;
+ act_driver->send_command(vx,steer);
+ }
+ act_driver->send_command(0.0,0.0);
+*/
+ while(1)
+ {
+ usleep(100000);
+ act_driver->send_command(vx,steer);
+ }
+ act_driver->send_command(0.0,0.0);
+
+ act_driver->close();
+ delete act_driver;
+ }
+ catch (CException &e)
+ {
+ if(act_driver!=NULL)
+ {
+ delete act_driver;
+ act_driver=NULL;
+ }
+ std::cout << "model_car_actuators_test: " << e.what() << std::endl;
+ }
+ }
+}
diff --git a/src/examples/model_car_batteries_test.cpp b/src/examples/model_car_batteries_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee9ac3929f452c022cbdf5e9fe8516198a5fe0b2
--- /dev/null
+++ b/src/examples/model_car_batteries_test.cpp
@@ -0,0 +1,49 @@
+#include "model_car_batteries.h"
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events;
+ std::string name = "model_car_driver_bat";
+ CModelCarBatteries *bat_driver;
+
+ while(1)
+ {
+ try
+ {
+ bat_driver = new CModelCarBatteries(name);
+ bat_driver->open();
+ events.clear();
+ events.push_back(bat_driver->get_new_data_event_id());
+
+ for(unsigned int i=0;i<100;i++)
+ {
+ event_server->wait_all(events,2000);
+ std::cout << " motors battery voltage: " << bat_driver->get_motors_battery_voltage() << std::endl;
+ std::cout << " motors battery cell1 voltage: " << bat_driver->get_motors_cell1_voltage() << std::endl;
+ std::cout << " motors battery cell2 voltage: " << bat_driver->get_motors_cell2_voltage() << std::endl;
+ std::cout << " payload battery voltage: " << bat_driver->get_payload_battery_voltage() << std::endl;
+ std::cout << " payload battery cell1 voltage: " << bat_driver->get_payload_cell1_voltage() << std::endl;
+ std::cout << " payload battery cell2 voltage: " << bat_driver->get_payload_cell2_voltage() << std::endl;
+ std::cout << " payload battery cell3 voltage: " << bat_driver->get_payload_cell3_voltage() << std::endl;
+ std::cout << " payload battery cell4 voltage: " << bat_driver->get_payload_cell4_voltage() << std::endl;
+ std::cout << " payload battery cell5 voltage: " << bat_driver->get_payload_cell5_voltage() << std::endl;
+ std::cout << " payload battery cell6 voltage: " << bat_driver->get_payload_cell6_voltage() << std::endl;
+ std::cout << std::endl;
+ }
+ bat_driver->close();
+ delete bat_driver;
+ }
+ catch (CException &e)
+ {
+ if(bat_driver!=NULL)
+ {
+ delete bat_driver;
+ bat_driver=NULL;
+ }
+ std::cout << e.what() << std::endl;
+ }
+ }
+
+}
diff --git a/src/examples/model_car_drivers_test.cpp b/src/examples/model_car_drivers_test.cpp
index 2b6873f448a8a5dbaff79807a381ab71478fd125..d5ec9fe47189d08f465bd725d97df4c7a606d0f1 100644
--- a/src/examples/model_car_drivers_test.cpp
+++ b/src/examples/model_car_drivers_test.cpp
@@ -1,20 +1,18 @@
-#include "model_car_drivers_uss.h"
+#include "model_car_ultrasounds.h"
int main(int argc, char *argv[])
{
-
- bool debug = true;
std::string name = "model_car_driver";
- CModel_Car_Drivers_Uss *uss_driver;
+ CModelCarUltrasounds *uss_driver;
try
{
std::cout << "model_car_drivers_test::main: Creating object..." << std::endl;
- uss_driver = new CModel_Car_Drivers_Uss(name);
+ uss_driver = new CModelCarUltrasounds(name);
std::cout << "model_car_drivers_test::main: Object created" << std::endl;
- uss_driver->set_debug(debug);
- uss_driver->start();
+ for(unsigned int i=0;i<100;i++)
+ uss_driver->open();
std::cout << "model_car_drivers_test::main: reading USS values" << std::endl;
for(unsigned int i=0; i<10; i++)
@@ -35,7 +33,7 @@ int main(int argc, char *argv[])
}
std::cout << "model_car_drivers_test::main: stopping uss_driver" << std::endl;
- uss_driver->stop();
+ uss_driver->close();
std::cout << "model_car_drivers_test::main: uss_driver stopped" << std::endl;
}
catch (CException &e)
diff --git a/src/examples/model_car_drivers_test_act.cpp b/src/examples/model_car_drivers_test_act.cpp
deleted file mode 100644
index 406fc8b6fd46466d9f5a5d9f41a18ef16ea861cd..0000000000000000000000000000000000000000
--- a/src/examples/model_car_drivers_test_act.cpp
+++ /dev/null
@@ -1,135 +0,0 @@
-#include "model_car_drivers_act.h"
-
-
-
-int main(int argc, char *argv[])
-{
-
- bool debug = true;
- std::string name = "model_car_driver_act";
- CModel_Car_Drivers_Act *act_driver;
- try
- {
- std::cout << "model_car_drivers_test_act::main: Creating object..." << std::endl;
- act_driver = new CModel_Car_Drivers_Act(name);
- std::cout << "model_car_drivers_test_act::main: Object created" << std::endl;
- act_driver->set_debug(debug);
- act_driver->start();
-
- bool test_steering = true;
- bool test_speed = false;
- bool test_lights = false;
- bool test_watchdog = true;
- bool test_emergency_stop = true;
- bool test_enable_disable_uss = true;
-
- if(test_steering)
- {
- std::cout << "model_car_drivers_test_act::main: about to test steering, in 3..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test steering, 2..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test steering, 1..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: sending steering 0" << std::endl;
- act_driver->send_steering(0.0); //center
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering max left" << std::endl;
- act_driver->send_steering(100.0); //max left (30º)
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering 0" << std::endl;
- act_driver->send_steering(0.0); //center
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering max right" << std::endl;
- act_driver->send_steering(-100.0); //max right (30º)
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering 0" << std::endl;
- act_driver->send_steering(0.0); //center
- }
-
- if(test_speed)
- {
- std::cout << "model_car_drivers_test_act::main: about to test speed, in 3..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test speed, 2..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test speed, 1..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: sending speed 0" << std::endl;
- act_driver->send_speed(0.0); //stop
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed 10%" << std::endl;
- act_driver->send_speed(10.0); //10% of full gas
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed 0" << std::endl;
- act_driver->send_speed(0.0); //stop
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed -10%" << std::endl;
- act_driver->send_speed(-10.0); //10% of reverse
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed 0" << std::endl;
- act_driver->send_speed(0.0); //stop
- }
-
- if(test_lights)
- {
- std::cout << "model_car_drivers_test_act::main: about to test lights, in 3..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test lights, 2..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test lights, 1..." << std::endl;
- sleep(1);
- std::cout << "model_car_drivers_test_act::main: sending light HEAD" << std::endl;
- act_driver->send_light(ID_ARD_ACT_LIGHT_MASK_HEAD);
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending light BRAKE" << std::endl;
- act_driver->send_light(ID_ARD_ACT_LIGHT_MASK_BRAKE);
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending light TURNLEFT" << std::endl;
- act_driver->send_light(ID_ARD_ACT_LIGHT_MASK_TURNLEFT);
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending light TURNRIGHT" << std::endl;
- act_driver->send_light(ID_ARD_ACT_LIGHT_MASK_TURNRIGHT);
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending light HAZARD" << std::endl;
- act_driver->send_light(ID_ARD_ACT_LIGHT_MASK_HAZARD);
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending light REVERSE" << std::endl;
- act_driver->send_light(ID_ARD_ACT_LIGHT_MASK_REVERSE);
- usleep(500e3);
- }
-
- if(test_watchdog)
- {
- std::cout << "model_car_drivers_test_act::main: sending watchdog" << std::endl;
- act_driver->send_watchdog();
- }
-
- if(test_emergency_stop)
- {
- std::cout << "model_car_drivers_test_act::main: sending emergency stop" << std::endl;
- act_driver->send_emergency_stop();
- }
-
- if(test_enable_disable_uss)
- {
- std::cout << "model_car_drivers_test_act::main: sending enable uss" << std::endl;
- act_driver->send_enable_uss();
- std::cout << "model_car_drivers_test_act::main: sending disable uss" << std::endl;
- act_driver->send_disable_uss();
- }
-
- std::cout << "model_car_drivers_test_act::main: END OF TESTING" << std::endl;
-
- std::cout << "model_car_drivers_test_act::main: stopping act_driver" << std::endl;
- act_driver->stop();
- std::cout << "model_car_drivers_test_act::main: act_driver stopped" << std::endl;
- }
- catch (CException &e)
- {
- std::cout << e.what() << std::endl;
- }
-
- delete act_driver;
-
-}
diff --git a/src/examples/model_car_drivers_test_bat.cpp b/src/examples/model_car_drivers_test_bat.cpp
deleted file mode 100644
index efd05e0579e91b6204ff16e00fb5285f1762a014..0000000000000000000000000000000000000000
--- a/src/examples/model_car_drivers_test_bat.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-#include "model_car_drivers_bat.h"
-
-int main(int argc, char *argv[])
-{
-
- bool debug = true;
- std::string name = "model_car_driver_bat";
- CModel_Car_Drivers_Bat *bat_driver;
- try
- {
- std::cout << "model_car_drivers_test_bat::main: Creating object..." << std::endl;
- bat_driver = new CModel_Car_Drivers_Bat(name);
- std::cout << "model_car_drivers_test_bat::main: Object created" << std::endl;
- bat_driver->set_debug(debug);
- bat_driver->start();
-
- std::cout << "model_car_drivers_test_bat::main: reading BAT values" << std::endl;
- for(unsigned int i=0; i<10; i++)
- {
- float value;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR_CELL1, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL1 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR_CELL2, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL2 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR_CELL2, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL2 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL1, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL1 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL2, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL2 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL3, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL3 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL4, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL4 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL5, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL5 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL6, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL6 value: " << value << std::endl;
- std::cout <<" ---" <<std::endl;
- usleep(100000);
- }
- std::cout << "model_car_drivers_test_bat::main: stopping bat_driver" << std::endl;
- bat_driver->stop();
- std::cout << "model_car_drivers_test_bat::main: bat_driver stopped" << std::endl;
- }
- catch (CException &e)
- {
- std::cout << e.what() << std::endl;
- }
-
- delete bat_driver;
-}
diff --git a/src/examples/model_car_drivers_test_egomotion.cpp b/src/examples/model_car_drivers_test_egomotion.cpp
deleted file mode 100644
index 3febe64802bfd74318543a0a772ff9f67de971e3..0000000000000000000000000000000000000000
--- a/src/examples/model_car_drivers_test_egomotion.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-#include "model_car_drivers_egomotion.h"
-
-
-
-int main(int argc, char *argv[])
-{
-
- bool debug = true;
- std::string name = "model_car_driver_egomotion";
- CModel_Car_Drivers_Egomotion *egomotion_driver;
- try
- {
- std::cout << "model_car_drivers_test_egomotion::main: Creating object..." << std::endl;
- egomotion_driver = new CModel_Car_Drivers_Egomotion(name);
- std::cout << "model_car_drivers_test_egomotion::main: Object created" << std::endl;
- egomotion_driver->set_debug(debug);
- egomotion_driver->start();
-
- std::cout << "model_car_drivers_test_egomotion::main: reading EGOMOTION values" << std::endl;
- for(unsigned int i=0; i<10; i++)
- {
- tSensWheelData lwheel;
- if(egomotion_driver->get_left_wheel(lwheel))
- {
- std::cout << "Left wheel tachometer: " << lwheel.ui32WheelTach << std::endl;
- std::cout << "Left wheel direction: " << static_cast<int16_t>(lwheel.i8WheelDir) << std::endl;
- }
-
- tSensWheelData rwheel;
- if(egomotion_driver->get_right_wheel(rwheel))
- {
- std::cout << "Right wheel tachometer: " << rwheel.ui32WheelTach << std::endl;
- std::cout << "Right wheel direction: " << static_cast<int16_t>(rwheel.i8WheelDir) << std::endl;
- }
-
- tImuData imu;
- if(egomotion_driver->get_imu(imu))
- {
- std::cout << " Imu ax: " << imu.f32ax << std::endl;
- std::cout << " Imu ay: " << imu.f32ay << std::endl;
- std::cout << " Imu az: " << imu.f32az << std::endl;
-
- std::cout << " Imu gx: " << imu.f32gx << std::endl;
- std::cout << " Imu gy: " << imu.f32gy << std::endl;
- std::cout << " Imu gz: " << imu.f32gz << std::endl;
-
- std::cout << " Imu mx: " << imu.f32mx << std::endl;
- std::cout << " Imu my: " << imu.f32my << std::endl;
- std::cout << " Imu mz: " << imu.f32mz << std::endl;
-
- std::cout << " Imu roll: " << imu.f32roll << std::endl;
- std::cout << " Imu pitch: " << imu.f32pitch << std::endl;
- std::cout << " Imu yaw: " << imu.f32yaw << std::endl;
- }
- std::cout <<" ---" <<std::endl;
- usleep(1e4);
- }
-
- std::cout << "model_car_drivers_test_egomotion::main: stopping egomotion_driver" << std::endl;
- egomotion_driver->stop();
- std::cout << "model_car_drivers_test_egomotion::main: egomotion_driver stopped" << std::endl;
- }
- catch (CException &e)
- {
- std::cout << e.what() << std::endl;
- }
-
- delete egomotion_driver;
-
-}
diff --git a/src/examples/model_car_drivers_test_uss.cpp b/src/examples/model_car_drivers_test_uss.cpp
deleted file mode 100644
index a34f858da8937b8c57781cc1b085835256415da6..0000000000000000000000000000000000000000
--- a/src/examples/model_car_drivers_test_uss.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-#include "model_car_drivers_uss.h"
-
-
-
-int main(int argc, char *argv[])
-{
-
- bool debug = true;
- std::string name = "model_car_driver_uss";
- CModel_Car_Drivers_Uss *uss_driver;
- try
- {
- std::cout << "model_car_drivers_test_uss::main: Creating object..." << std::endl;
- uss_driver = new CModel_Car_Drivers_Uss(name);
- std::cout << "model_car_drivers_test_uss::main: Object created" << std::endl;
- uss_driver->set_debug(debug);
- uss_driver->start();
-
- std::cout << "model_car_drivers_test_uss::main: reading USS values" << std::endl;
- for(unsigned int i=0; i<10; i++)
- {
- float us_value;
- if(uss_driver->get_uss(ID_ARD_SENS_US_SIDE_RIGHT, us_value))
- std::cout << " ID_ARD_SENS_US_SIDE_RIGHT value: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_REAR_CENTER_RIGHT, us_value))
- std::cout << " ID_ARD_SENS_US_REAR_CENTER_RIGHT value: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_REAR_CENTER, us_value))
- std::cout << " ID_ARD_SENS_US_REAR_CENTER value: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_REAR_CENTER_LEFT, us_value))
- std::cout << " ID_ARD_SENS_US_REAR_CENTER_LEFT value: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_SIDE_LEFT, us_value))
- std::cout << " ID_ARD_SENS_US_SIDE_LEFT value: " << us_value << std::endl;
- std::cout <<" ---" <<std::endl;
- usleep(1e5);
- }
-
- std::cout << "model_car_drivers_test_uss::main: stopping uss_driver" << std::endl;
- uss_driver->stop();
- std::cout << "model_car_drivers_test_uss::main: uss_driver stopped" << std::endl;
- }
- catch (CException &e)
- {
- std::cout << e.what() << std::endl;
- }
-
- delete uss_driver;
-
-}
diff --git a/src/examples/model_car_egomotion_test.cpp b/src/examples/model_car_egomotion_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..429c742e0797c694f00c2decc95e6297f879e304
--- /dev/null
+++ b/src/examples/model_car_egomotion_test.cpp
@@ -0,0 +1,73 @@
+#include "model_car_egomotion.h"
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events;
+ std::string name = "model_car_driver_egomotion";
+ CModelCarEgomotion *egomotion_driver;
+ double ax,ay,az,gx,gy,gz,mx,my,mz;
+ unsigned long int timestamp;
+ int tach;
+ bool dir;
+
+ while(1)
+ {
+ try
+ {
+ egomotion_driver = new CModelCarEgomotion(name);
+ egomotion_driver->open();
+ events.clear();
+ events.push_back(egomotion_driver->get_new_data_event_id());
+
+ for(unsigned int i=0;i<100;i++)
+ {
+ try
+ {
+ event_server->wait_all(events,1000);
+ egomotion_driver->get_left_wheel(tach,dir,timestamp);
+ std::cout << " Left Wheel: " << std::endl;
+ std::cout << " tachometer: " << tach << std::endl;
+ std::cout << " direction: " << dir << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ egomotion_driver->get_right_wheel(tach,dir,timestamp);
+ std::cout << " Right Wheel: " << std::endl;
+ std::cout << " tachometer: " << tach << std::endl;
+ std::cout << " direction: " << dir << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ egomotion_driver->get_imu_accelerometer(ax,ay,az,timestamp);
+ std::cout << " IMU: " << std::endl;
+ std::cout << " ax: " << ax << std::endl;
+ std::cout << " ay: " << ay << std::endl;
+ std::cout << " az: " << az << std::endl;
+ egomotion_driver->get_imu_gyroscope(gx,gy,gz,timestamp);
+ std::cout << " gx: " << gx << std::endl;
+ std::cout << " gy: " << gy << std::endl;
+ std::cout << " gz: " << gz << std::endl;
+ egomotion_driver->get_imu_magnetometer(mx,my,mz,timestamp);
+ std::cout << " mx: " << mx << std::endl;
+ std::cout << " my: " << my << std::endl;
+ std::cout << " mz: " << mz << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ std::cout << std::endl;
+ }
+ catch(CEventTimeoutException &e){
+ std::cout << e.what() << std::endl;
+ }
+ }
+
+ egomotion_driver->close();
+ delete egomotion_driver;
+ }
+ catch (CException &e)
+ {
+ if(egomotion_driver!=NULL)
+ {
+ delete egomotion_driver;
+ egomotion_driver=NULL;
+ }
+ std::cout << e.what() << std::endl;
+ }
+ }
+}
diff --git a/src/examples/model_car_ultrasounds_test.cpp b/src/examples/model_car_ultrasounds_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fe85be3011c0358d3b591bd6a23ff765b3fcefdd
--- /dev/null
+++ b/src/examples/model_car_ultrasounds_test.cpp
@@ -0,0 +1,43 @@
+#include "model_car_ultrasounds.h"
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events;
+ std::string name = "model_car_driver_uss";
+ CModelCarUltrasounds *uss_driver;
+ while(1)
+ {
+ try
+ {
+ uss_driver = new CModelCarUltrasounds(name);
+ uss_driver->open();
+ events.clear();
+ events.push_back(uss_driver->get_new_data_event_id());
+
+ for(unsigned int i=0;i<100;i++)
+ {
+ event_server->wait_all(events,1000);
+ std::cout << " Side right ultrasound distance: " << uss_driver->get_side_right_distance() << std::endl;
+ std::cout << " Side left ultrasound distance: " << uss_driver->get_side_left_distance() << std::endl;
+ std::cout << " Rear right ultrasound distance: " << uss_driver->get_rear_right_distance() << std::endl;
+ std::cout << " Rear center ultrasound distance: " << uss_driver->get_rear_center_distance() << std::endl;
+ std::cout << " Rear left ultrasound distance: " << uss_driver->get_rear_left_distance() << std::endl;
+ std::cout << std::endl;
+ }
+
+ uss_driver->close();
+ delete uss_driver;
+ }
+ catch (CException &e)
+ {
+ if(uss_driver!=NULL)
+ {
+ delete uss_driver;
+ uss_driver=NULL;
+ }
+ std::cout << e.what() << std::endl;
+ }
+ }
+}
diff --git a/src/model_car_actuators.cpp b/src/model_car_actuators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..27a16eb477ef096101441b2617865765e160761e
--- /dev/null
+++ b/src/model_car_actuators.cpp
@@ -0,0 +1,71 @@
+#include "model_car_actuators.h"
+
+CModelCarActuators::CModelCarActuators(std::string name) : CModelCarDriverBase(name, ARDUINO_CENTER_ACTUATORS)
+{
+ this->set_rx_timeout(NO_RX_TIMEOUT);
+}
+
+void CModelCarActuators::process_data_frame(uint8_t id, TDataUnion data_union)
+{
+}
+
+void CModelCarActuators::send_watchdog(void)
+{
+ uint8_t cmd=1;
+
+ this->send_request(ID_ARD_ACT_WATCHDOG,1,&cmd);
+}
+
+void CModelCarActuators::send_command(double speed,double steering)
+{
+ uint8_t cmd;
+
+ this->send_watchdog();
+
+ // remap speed
+ speed = speed > 100.f ? 100.f : speed;
+ speed = speed < -100.f ? -100.f : speed;
+ speed += 100.f;
+ speed *= 0.9f;
+ // 0...180
+ cmd=(uint8_t)speed;
+
+ this->send_request(ID_ARD_ACT_SPEED_CONTR,1, &cmd);
+
+ // remap steering
+ steering = steering > 100.f ? 100.f : steering;
+ steering = steering < -100.f ? -100.f : steering;
+ steering += 100.f;
+ steering *= 0.9f;
+ // 0...180
+ cmd=(uint8_t)steering;
+
+ this->send_request(ID_ARD_ACT_STEER_SERVO,1, &cmd);
+}
+
+void CModelCarActuators::emergency_stop(void)
+{
+ uint8_t cmd=1;
+
+ this->send_request(ID_ARD_ACT_EMERGENCY_STOP,1,&cmd);
+}
+
+void CModelCarActuators::enable_ultrasounds(void)
+{
+ uint8_t cmd=0;
+
+ this->send_request(ID_ARD_ENABLE_USS,1,&cmd);
+}
+
+void CModelCarActuators::disable_ultrasounds(void)
+{
+ uint8_t cmd=0;
+
+ this->send_request(ID_ARD_DISABLE_USS,1,&cmd);
+}
+
+
+CModelCarActuators::~CModelCarActuators()
+{
+
+}
diff --git a/src/model_car_batteries.cpp b/src/model_car_batteries.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2474bfa13fa284682c13faf3f7bf814524d5cf48
--- /dev/null
+++ b/src/model_car_batteries.cpp
@@ -0,0 +1,148 @@
+#include "model_car_batteries.h"
+
+CModelCarBatteries::CModelCarBatteries(std::string name) : CModelCarDriverBase(name, ARDUINO_CENTER_MEASUREMENT)
+{
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR_CELL1, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR_CELL2, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL1, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL2, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL3, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL4, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL5, 0.0));
+ battery_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL6, 0.0));
+
+ this->set_rx_timeout(1);
+}
+
+void CModelCarBatteries::process_data_frame(uint8_t id, TDataUnion data_union)
+{
+ voltages_map_t::iterator it = this->battery_values.find((SENSOR_ID)id);
+ static int num_messages=10;
+
+ if(it!=this->battery_values.end())
+ it->second = data_union.voltage.voltage/1000.0;
+ num_messages--;
+ if(num_messages==0)
+ {
+ num_messages=10;
+ if(!this->event_server->event_is_set(this->new_data_event_id))
+ this->event_server->set_event(this->new_data_event_id);
+ }
+}
+
+double CModelCarBatteries::get_motors_battery_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_ACTUATOR];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_payload_battery_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_SENSORS];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_motors_cell1_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_ACTUATOR_CELL1];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_motors_cell2_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_ACTUATOR_CELL2];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_payload_cell1_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_SENSORS_CELL1];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_payload_cell2_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_SENSORS_CELL2];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_payload_cell3_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_SENSORS_CELL3];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_payload_cell4_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_SENSORS_CELL4];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_payload_cell5_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_SENSORS_CELL5];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+double CModelCarBatteries::get_payload_cell6_voltage(void)
+{
+ double voltage;
+
+ this->data_mutex.enter();
+ voltage=this->battery_values[ID_ARD_SENS_VOLT_SENSORS_CELL6];
+ this->data_mutex.exit();
+
+ return voltage;
+}
+
+CModelCarBatteries::~CModelCarBatteries()
+{
+
+}
diff --git a/src/model_car_driver_base.cpp b/src/model_car_driver_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57d0ac724ab25603b1683df6dd1a3edbfc8958c1
--- /dev/null
+++ b/src/model_car_driver_base.cpp
@@ -0,0 +1,538 @@
+#include "model_car_driver_base.h"
+#include <iostream>
+#include <vector>
+#include <dirent.h>
+
+CModelCarDriverBase::CModelCarDriverBase(std::string name, ARDUINO_ID arduino_id)
+{
+ this->name=name;
+ this->device="";
+ this->serial_port=NULL;
+
+ this->arduino_id=arduino_id;
+ this->detected_id=-1;
+ this->sw_version=-1;
+ this->rx_timeout=DEFAULT_TIMEOUT;
+ this->timestamp_lsb=0;
+ this->timestamp_msb=0;
+
+ /* init events */
+ this->event_server=CEventServer::instance();
+ this->finish_thread_event_id=this->name+"_finish_thread_event";
+ this->event_server->create_event(this->finish_thread_event_id);
+ this->new_data_event_id=this->name+"_new_data_event";
+ this->event_server->create_event(this->new_data_event_id);
+ this->new_info_event_id=this->name+"_new_info_event";
+ this->event_server->create_event(this->new_info_event_id);
+
+ /* initialize threads */
+ this->thread_server=CThreadServer::instance();
+ this->data_thread_id=this->name+"_data_thread";
+ this->thread_server->create_thread(this->data_thread_id);
+ this->thread_server->attach_thread(this->data_thread_id,this->data_thread,this);
+}
+
+bool CModelCarDriverBase::open_dev(std::string &serial_dev)
+{
+ bool ok=false;
+ TRS232_config serial_config;
+ try
+ {
+ serial_config.baud=115200;
+ serial_config.num_bits=8;
+ serial_config.parity=none;
+ serial_config.stop_bits=1;
+ this->serial_port = new CRS232(this->name);
+ this->serial_port->open((void *)&serial_dev);
+ this->serial_port->config(&serial_config);
+
+ this->thread_server->start_thread(this->data_thread_id);
+
+ if(this->get_id()==-1)
+ this->close();
+ else
+ {
+ if(this->arduino_id == this->detected_id)
+ {
+ std::cout << "CModelCarDriverBase: correct arduino found!" << std::endl;
+ this->device=serial_dev;
+ ok=true;
+ }
+ else
+ this->close();
+ }
+ }
+ catch(CException &e)
+ {
+ this->close();
+ throw e;
+ }
+
+ return ok;
+}
+
+void *CModelCarDriverBase::data_thread(void *param)
+{
+ CModelCarDriverBase *driver=(CModelCarDriverBase *)param;
+ uint8_t *frame_data=NULL;
+ TRS232_config serial_config;
+ uint32_t num,index;
+ bool end=false;
+
+ THeader header;
+ TDataUnion data_union;
+ TCRCUnion crc_union;
+
+ std::list<std::string> events;
+ events.push_back(driver->serial_port->get_rx_event_id());
+ events.push_back(driver->finish_thread_event_id);
+
+ while(!end)
+ {
+ try
+ {
+ driver->port_mutex.enter();
+ num=driver->serial_port->get_num_data();
+ if(num==0)
+ {
+ driver->port_mutex.exit();
+ index=driver->event_server->wait_first(events,driver->rx_timeout);
+ driver->port_mutex.enter();
+ num=driver->serial_port->get_num_data();
+ }
+ else
+ index=0;
+
+ if(index==0)
+ {
+ frame_data = new uint8_t[num];
+ driver->serial_port->read(frame_data,num);
+ driver->port_mutex.exit();
+ for(uint32_t i=0;i<num;i++)
+ {
+ if(driver->process_byte(header, data_union, crc_union, frame_data[i]))
+ driver->process_data(header, data_union, crc_union);
+ }
+ delete[] frame_data;
+ }
+ else
+ {
+ driver->port_mutex.exit();
+ end=true;
+ }
+ }
+ catch(CEventTimeoutException &e)
+ {
+ serial_config.baud=115200;
+ serial_config.num_bits=8;
+ serial_config.parity=none;
+ serial_config.stop_bits=1;
+ driver->serial_port->close();
+ driver->serial_port->open((void *)&driver->device);
+ driver->serial_port->config(&serial_config);
+ std::cout << e.what() << std::endl;
+ }
+ catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+ }
+ pthread_exit(NULL);
+}
+
+bool CModelCarDriverBase::process_byte(THeader &header, TDataUnion &data_union, TCRCUnion &crc_union, uint8_t byte)
+{
+ bool frame_ready=false;
+ static sm_state state=WAIT_START;
+ static bool escaped=false;
+ static int ts_count=0;
+ static int data_count=0;
+ static int crc_count=0;
+ static TTimeStampUnion ts_union;
+
+ if(byte==ESCAPE_BYTE && !escaped)
+ escaped=true;
+ else
+ {
+ if(byte==START_BYTE && !escaped && state!=WAIT_START)
+ state=READ_ID;
+ else if(byte==END_BYTE && !escaped && state!=READ_END)
+ state=WAIT_START;
+
+ switch(state)
+ {
+ case WAIT_START:
+ if(byte==START_BYTE)
+ {
+ if(!escaped)
+ state=READ_ID;
+ else
+ {
+ escaped=false;
+ state=WAIT_START;
+ }
+ }
+ else
+ state=WAIT_START;
+ break;
+ case READ_ID:
+ if(escaped)
+ escaped=false;
+ header.id = byte;
+ state=READ_DATA_LENGTH;
+ break;
+ case READ_DATA_LENGTH:
+ if(escaped)
+ escaped=false;
+ header.data_length = byte;
+ ts_count=0;
+ state=READ_TIMESTAMP;
+ break;
+ case READ_TIMESTAMP:
+ if(escaped)
+ escaped=false;
+ ts_union.raw[ts_count] = byte;
+ if(ts_count<3)
+ {
+ ts_count++;
+ state=READ_TIMESTAMP;
+ }
+ else
+ {
+ header.time_stamp = ts_union.time_stamp;
+ data_count=0;
+ state=READ_DATA;
+ }
+ break;
+ case READ_DATA:
+ if(escaped)
+ escaped=false;
+ data_union.raw[data_count] = byte;
+ if(data_count<header.data_length-1)
+ {
+ data_count++;
+ state=READ_DATA;
+ }
+ else
+ {
+ crc_count=0;
+ state=READ_CRC;
+ }
+ break;
+ case READ_CRC:
+ if(escaped)
+ escaped=false;
+ crc_union.raw[crc_count] = byte;
+ if(crc_count<1)
+ {
+ crc_count++;
+ state=READ_CRC;
+ }
+ else
+ state=READ_END;
+ break;
+ case READ_END:
+ if(byte==END_BYTE && !escaped)
+ {
+ frame_ready=true;
+ state=WAIT_START;
+ }
+ else
+ {
+ if(escaped)
+ escaped=false;
+ state=WAIT_START;
+ }
+ break;
+ }
+ }
+ return frame_ready;
+}
+
+void CModelCarDriverBase::process_data(THeader &header, TDataUnion &data_union, TCRCUnion &crc_union)
+{
+ static uint32_t last_timestamp;
+ static bool first=true;
+ uint8_t * single_frame=NULL;
+ uint16_t calculated_crc;
+ uint8_t frame_size;
+ TErrorData error_data;
+
+ frame_size = sizeof(THeader)+header.data_length;
+ single_frame = new uint8_t[frame_size];
+ memcpy(single_frame, &header, sizeof(THeader));
+ memcpy(&single_frame[sizeof(THeader)], &data_union, header.data_length);
+ calculated_crc = this->fletcher16(single_frame, frame_size);
+ delete[] single_frame;
+
+ if(first)
+ {
+ first=false;
+ last_timestamp=header.time_stamp;
+ }
+ else
+ {
+ this->timestamp_lsb=header.time_stamp;
+ if(header.time_stamp<last_timestamp)//32 bit overflow
+ this->timestamp_msb++;
+ last_timestamp=header.time_stamp;
+ }
+
+ if (calculated_crc != crc_union.crc)
+ std::cout << "CModelCarDriverBase: error, frame CRC invalid. Skipping frame" << std::endl;
+ else
+ {
+ switch(header.id)
+ {
+ case ID_ARD_SENSOR_INFO:
+ this->data_mutex.enter();
+ this->detected_id = data_union.info.arduino_address;
+ this->sw_version = data_union.info.arduino_version;
+ this->data_mutex.exit();
+ if(!this->event_server->event_is_set(this->new_info_event_id))
+ this->event_server->set_event(this->new_info_event_id);
+ break;
+ case ID_ARD_SENS_ERROR:
+ error_data.error_number = data_union.error.error_number;
+ switch(error_data.error_number)
+ {
+ case ERROR_STX_NOT_FOUND:
+ std::cout << "CModelCarDriverBase: error received: ERROR_STX_NOT_FOUND" << std::endl;
+ break;
+ case ERROR_ETX_NOT_FOUND:
+ std::cout << "CModelCarDriverBase: error received: ERROR_ETX_NOT_FOUND" << std::endl;
+ break;
+ case ERROR_ESC_BYTE_BROKEN:
+ std::cout << "CModelCarDriverBase: error received: ERROR_ESC_BYTE_BROKEN" << std::endl;
+ break;
+ case ERROR_NO_BYTES_AVAILABLE:
+ std::cout << "CModelCarDriverBase: error received: ERROR_NO_BYTES_AVAILABLE" << std::endl;
+ break;
+ case ERROR_CRC_INVALID:
+ std::cout << "CModelCarDriverBase: error received: ERROR_CRC_INVALID" << std::endl;
+ break;
+ case ERROR_NO_FRAME_DATA:
+ std::cout << "CModelCarDriverBase: error received: ERROR_NO_FRAME_DATA" << std::endl;
+ break;
+ case ERROR_FRAME_DROPPED:
+ std::cout << "CModelCarDriverBase: error received: ERROR_FRAME_DROPPED" << std::endl;
+ break;
+ case ERROR_REMOTE_DETECTED:
+ std::cout << "CModelCarDriverBase: error received: ERROR_REMOTE_DETECTED" << std::endl;
+ break;
+ case ERROR_NO_GYRO_DETECTED:
+ std::cout << "CModelCarDriverBase: error received: ERROR_NO_GYRO_DETECTED" << std::endl;
+ break;
+ case ERROR_INVALID_ACTUATOR_HEADER:
+ std::cout << "CModelCarDriverBase: error received: ERROR_INVALID_ACTUATOR_HEADER" << std::endl;
+ break;
+ case ERROR_INITIALIZATION_FAILED:
+ std::cout << "CModelCarDriverBase: error received: ERROR_INITIALIZATION_FAILED" << std::endl;
+ break;
+ case ERROR_FRAME_NOT_WRITTEN:
+ std::cout << "CModelCarDriverBase: error received: ERROR_FRAME_NOT_WRITTEN" << std::endl;
+ break;
+ default:
+ std::cout << "CModelCarDriverBase: error received: unknown error code" << std::endl;
+ }
+ break;
+ default:
+ this->data_mutex.enter();
+ this->process_data_frame(header.id, data_union);
+ this->data_mutex.exit();
+ break;
+ }
+ }
+}
+
+uint8_t CModelCarDriverBase::get_id(void)
+{
+ std::list<std::string> events;
+
+ events.push_back(this->new_info_event_id);
+ this->send_request(ID_ARD_ACT_REQUEST,0,NULL);
+ try{
+ this->event_server->wait_all(events,100);
+ }
+ catch(CEventTimeoutException &e){
+ return -1;
+ }
+
+ return this->detected_id;
+}
+
+void CModelCarDriverBase::send_request(uint8_t id, uint8_t data_length, uint8_t *data)
+{
+ uint8_t req_frame_size=sizeof(uint8_t)+sizeof(uint8_t)+sizeof(TTimeStampUnion)+data_length+sizeof(uint16_t),stuffed_frame_size;
+ uint16_t crc_value;
+ uint8_t *frame=NULL,*stuffed_frame=NULL;
+
+ frame = new uint8_t[req_frame_size];
+ frame[0]=id;
+ frame[1]=data_length;
+ memset(frame+2*sizeof(uint8_t), 0x00, sizeof(TTimeStampUnion));
+ memcpy(frame+2*sizeof(uint8_t)+sizeof(TTimeStampUnion),data,data_length);
+ crc_value=fletcher16(frame,req_frame_size-sizeof(uint16_t));
+ memcpy(frame+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(TTimeStampUnion)+data_length,&crc_value,sizeof(uint16_t));
+ this->stuff_frame(frame,req_frame_size,&stuffed_frame,stuffed_frame_size);
+
+ this->port_mutex.enter();
+ this->serial_port->write(stuffed_frame,stuffed_frame_size);
+ this->port_mutex.exit();
+ delete[] stuffed_frame;
+ delete[] frame;
+}
+
+void CModelCarDriverBase::stuff_frame(uint8_t *frame_in, uint8_t size_in, uint8_t **frame_out, uint8_t & size_out)
+{
+ uint8_t esc_num=0;
+
+ for(uint8_t i=0;i<size_in; i++)
+ if( (frame_in[i]==START_BYTE) || (frame_in[i]==END_BYTE) || (frame_in[i]==ESCAPE_BYTE ) )
+ esc_num++;
+
+ size_out = size_in+esc_num+2;
+ *frame_out = new uint8_t[size_out];
+
+ (*frame_out)[0]=START_BYTE;
+ for(uint8_t input_byte=0,output_byte=1;input_byte<size_in;input_byte++)
+ {
+ if( (frame_in[input_byte]==START_BYTE) || (frame_in[input_byte]==END_BYTE) || (frame_in[input_byte]==ESCAPE_BYTE ) )
+ {
+ (*frame_out)[output_byte]=ESCAPE_BYTE;
+ output_byte++;
+ }
+ (*frame_out)[output_byte]=frame_in[input_byte];
+ output_byte++;
+ }
+ (*frame_out)[size_out-3]=frame_in[size_in-2];
+ (*frame_out)[size_out-2]=frame_in[size_in-1];
+ (*frame_out)[size_out-1]=END_BYTE;
+}
+
+uint16_t CModelCarDriverBase::fletcher16(uint8_t *data, uint8_t bytes)
+{
+ uint16_t sum1 = 0xff, sum2 = 0xff;
+
+ while (bytes)
+ {
+ uint8_t tlen = bytes > 20 ? 20 : bytes;
+ bytes -= tlen;
+ do
+ {
+ sum2 += sum1 += *data++;
+ }
+ while (--tlen);
+ sum1 = (sum1 & 0xff) + (sum1 >> 8);
+ sum2 = (sum2 & 0xff) + (sum2 >> 8);
+ }
+ /* Second reduction step to reduce sums to 8 bits */
+ sum1 = (sum1 & 0xff) + (sum1 >> 8);
+ sum2 = (sum2 & 0xff) + (sum2 >> 8);
+
+ return sum2 << 8 | sum1;
+}
+
+void CModelCarDriverBase::open(void)
+{
+ std::vector<std::string> dev_list;
+ std::stringstream error;
+ bool found=false;
+
+ this->close();
+
+ std::string path = "/sys/class/tty";
+ std::string str = "ttyACM";
+
+ DIR *dir;
+ struct dirent *ent;
+ if ((dir = opendir (path.c_str())) != NULL)
+ {
+ while ((ent = readdir (dir)) != NULL)
+ {
+ std::size_t found = std::string(ent->d_name).find(str);
+ if (found!=std::string::npos)
+ {
+ std::string dev = "/dev/"+std::string(ent->d_name);
+ dev_list.push_back(dev);
+ }
+ }
+ closedir (dir);
+ }
+ else
+ {
+ error << "Can't open folder " << path;
+ throw CException(_HERE_,error.str());
+ }
+
+ for(uint32_t i=0; i<dev_list.size();i++)
+ {
+ found=this->open_dev(dev_list[i]);
+ if(found)
+ return;
+ }
+ error << "Impossible to find arduino with ID " << (int)this->arduino_id;
+ throw CException(_HERE_,error.str());
+}
+
+void CModelCarDriverBase::close(void)
+{
+ if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
+ this->thread_server->get_thread_state(this->data_thread_id)==active)
+ {
+ this->event_server->set_event(this->finish_thread_event_id);
+ this->thread_server->end_thread(this->data_thread_id);
+ }
+
+ this->port_mutex.enter();
+ if(this->serial_port!=NULL)
+ {
+ this->serial_port->close();
+ delete this->serial_port;
+ this->serial_port=NULL;
+ }
+ this->port_mutex.exit();
+}
+
+void CModelCarDriverBase::set_rx_timeout(double time_s)
+{
+ std::stringstream error;
+
+ if(time_s<0.0 && time_s!=NO_RX_TIMEOUT)
+ {
+ error << "Invalid timeout value " << time_s;
+ throw CException(_HERE_,error.str());
+ }
+ if(time_s==NO_RX_TIMEOUT)
+ this->rx_timeout=-1;
+ else
+ this->rx_timeout=(int)(time_s*1000.0);
+}
+
+double CModelCarDriverBase::get_rx_timeout(void)
+{
+ return this->rx_timeout;
+}
+
+uint64_t CModelCarDriverBase::get_last_timestamp(void)
+{
+ return ((uint64_t)this->timestamp_msb<<32)+this->timestamp_lsb;
+}
+
+std::string CModelCarDriverBase::get_new_data_event_id(void)
+{
+ return this->new_data_event_id;
+}
+
+CModelCarDriverBase::~CModelCarDriverBase()
+{
+ this->close();
+ this->thread_server->detach_thread(this->data_thread_id);
+ this->thread_server->delete_thread(this->data_thread_id);
+ this->data_thread_id="";
+ this->event_server->delete_event(this->finish_thread_event_id);
+ this->finish_thread_event_id="";
+ this->event_server->delete_event(this->new_data_event_id);
+ this->new_data_event_id="";
+ this->event_server->delete_event(this->new_info_event_id);
+ this->new_info_event_id="";
+}
diff --git a/src/model_car_drivers.cpp b/src/model_car_drivers.cpp
deleted file mode 100644
index f25d2d5f80bee91817eb6e1a50bc91995b0cf482..0000000000000000000000000000000000000000
--- a/src/model_car_drivers.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include "model_car_drivers.h"
-
-CModel_Car_Drivers::CModel_Car_Drivers()
-{
-}
-
-CModel_Car_Drivers::~CModel_Car_Drivers()
-{
-}
-
diff --git a/src/model_car_drivers_act.cpp b/src/model_car_drivers_act.cpp
deleted file mode 100644
index e3335aff7b38f4c75078c40f83a9dab5cede253d..0000000000000000000000000000000000000000
--- a/src/model_car_drivers_act.cpp
+++ /dev/null
@@ -1,116 +0,0 @@
-#include "model_car_drivers_act.h"
-
-CModel_Car_Drivers_Act::CModel_Car_Drivers_Act(std::string name) : CModel_Car_Drivers_Base(name, ARDUINO_CENTER_ACTUATORS)
-{
-
-}
-
-void CModel_Car_Drivers_Act::process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union)
-{
- //std::cout << "CModel_Car_Drivers_Act::process_data_frame: id=" << unsigned(id) << std::endl;
-
- //const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id);
- //TODO nothing?
-
-}
-
-void CModel_Car_Drivers_Act::send_speed(float speed)
-{
- tArduinoHeader header;
- header.ui8ID = ID_ARD_ACT_SPEED_CONTR;
- header.ui8DataLength = 1;
- header.ui32Timestamp = 0;
-
- // remap speed
- speed = speed > 100.f ? 100.f : speed;
- speed = speed < -100.f ? -100.f : speed;
- speed += 100.f;
- speed *= 0.9f;
-
- uint8_t value = (uint8_t)speed;
-
-
- this->send_request(header, value);
-}
-
-void CModel_Car_Drivers_Act::send_steering(float angle)
-{
- tArduinoHeader header;
- header.ui8ID = ID_ARD_ACT_STEER_SERVO;
- header.ui8DataLength = 1;
- header.ui32Timestamp = 0;
-
- // remap angle
- angle = angle > 100.f ? 100.f : angle;
- angle = angle < -100.f ? -100.f : angle;
- angle += 100.f;
- angle *= 0.9f;
-
- uint8_t value = (uint8_t)angle;
-
- this->send_request(header, value);
-}
-
-void CModel_Car_Drivers_Act::send_watchdog()
-{
- //do this every half a second or the motors will stop
- tArduinoHeader header;
- header.ui8ID = ID_ARD_ACT_WATCHDOG;
- header.ui8DataLength = 1;
- header.ui32Timestamp = 0;
- uint8_t value=1;
-
- this->send_request(header, value);
-}
-
-void CModel_Car_Drivers_Act::send_emergency_stop()
-{
- //do this every half a second or the motors will stop
- tArduinoHeader header;
- header.ui8ID = ID_ARD_ACT_EMERGENCY_STOP;
- header.ui8DataLength = 1;
- header.ui32Timestamp = 0;
- uint8_t value=1;
-
- this->send_request(header, value);
-}
-
-void CModel_Car_Drivers_Act::send_light(uint8_t value)
-{
- //do this every half a second or the motors will stop
- tArduinoHeader header;
- header.ui8ID = ID_ARD_ACT_LIGHT;
- header.ui8DataLength = 1;
- header.ui32Timestamp = 0;
-
- this->send_request(header, value);
-}
-
-void CModel_Car_Drivers_Act::send_enable_uss()
-{
- //do this every half a second or the motors will stop
- tArduinoHeader header;
- header.ui8ID = ID_ARD_ENABLE_USS;
- header.ui8DataLength = 0;
- header.ui32Timestamp = 0;
- uint8_t value=0;
-
- this->send_request(header, value);
-}
-
-void CModel_Car_Drivers_Act::send_disable_uss()
-{
- //do this every half a second or the motors will stop
- tArduinoHeader header;
- header.ui8ID = ID_ARD_DISABLE_USS;
- header.ui8DataLength = 1;
- header.ui32Timestamp = 0;
- uint8_t value=0;
-
- this->send_request(header, value);
-}
-
-CModel_Car_Drivers_Act::~CModel_Car_Drivers_Act()
-{
-
-}
diff --git a/src/model_car_drivers_base.cpp b/src/model_car_drivers_base.cpp
deleted file mode 100644
index b9236c208c79ff263ea39e691cccdca9e5f5790c..0000000000000000000000000000000000000000
--- a/src/model_car_drivers_base.cpp
+++ /dev/null
@@ -1,796 +0,0 @@
-#include "model_car_drivers_base.h"
-
-union tTimeStampUnion
-{
- unsigned char raw[4];
- uint32_t ui32Timestamp;
-};
-
-union tCRCUnion
-{
- unsigned char raw[2];
- uint16_t crc;
-};
-
-struct HexCharStruct
-{
- unsigned char c;
- HexCharStruct(unsigned char _c) : c(_c) { }
-};
-
-inline std::ostream& operator<<(std::ostream& o, const HexCharStruct& hs)
-{
- return (o << std::setw(2) << std::setfill('0') << std::hex << (int)hs.c << std::dec);
-}
-
-inline HexCharStruct hex(unsigned char _c)
-{
- return HexCharStruct(_c);
-}
-
-CModel_Car_Drivers_Base::CModel_Car_Drivers_Base(std::string name, ARDUINO_ID arduino_id)
-{
- this->name=name;
- this->arduino_id=arduino_id;
- this->serial_port=NULL;
- this->debug=false;
- this->state=WAIT_START;
- this->paired=false;
- /* init events */
- this->event_server=CEventServer::instance();
- this->finish_thread_event_id=this->name+"_finish_thread_event";
- this->event_server->create_event(this->finish_thread_event_id);
- this->new_data_event_id=this->name+"_new_data_event";
- this->event_server->create_event(this->new_data_event_id);
- this->new_info_event_id=this->name+"_new_info_event";
- this->event_server->create_event(this->new_info_event_id);
-
- /* initialize threads */
- this->thread_server=CThreadServer::instance();
- this->data_thread_id=this->name+"_data_thread";
- this->thread_server->create_thread(this->data_thread_id);
- this->thread_server->attach_thread(this->data_thread_id,this->data_thread,this);
-}
-
-void CModel_Car_Drivers_Base::start()
-{
- std::vector<std::string> dev_list;
-
- /*
- dev_list.push_back("/dev/ttyACM0");
- dev_list.push_back("/dev/ttyACM1");
- dev_list.push_back("/dev/ttyACM2");
- dev_list.push_back("/dev/ttyACM3");
- */
-
- std::cout << "CModel_Car_Drivers_Base::start(): scanning ttyACM* devices in /sys/class/tty directory" << std::endl;
- std::string path = "/sys/class/tty";
- std::string str = "ttyACM";
-
- DIR *dir;
- struct dirent *ent;
- if ((dir = opendir (path.c_str())) != NULL)
- {
- while ((ent = readdir (dir)) != NULL)
- {
- std::size_t found = std::string(ent->d_name).find(str);
- if (found!=std::string::npos)
- {
- std::string dev = "/dev/"+std::string(ent->d_name);
- std::cout << " Found ttyACM* dev: " << dev << std::endl;
- dev_list.push_back(dev);
- }
- }
- closedir (dir);
- }
- else
- {
- std::cout << "CModel_Car_Drivers_Base::start: ERROR can't open dir " << path << std::endl;
- }
-
- std::cout << "CModel_Car_Drivers_Base::start(): looking for device matching arduino_id = " << this->arduino_id << std::endl;
- for(unsigned int i=0; i<dev_list.size();i++)
- {
- if(this->open(dev_list[i]))
- {
- break;
- }
- }
-}
-
-bool CModel_Car_Drivers_Base::open(std::string &serial_dev)
-{
- bool ok=false;
- TRS232_config serial_config;
- try
- {
- serial_config.baud=115200;
- serial_config.num_bits=8;
- serial_config.parity=none;
- serial_config.stop_bits=1;
- this->serial_port = new CRS232(this->name);
- std::cout << "CModel_Car_Drivers_Base::start: Opening " << serial_dev << std::endl;
- this->serial_port->open((void *)&serial_dev);
- this->serial_port->config(&serial_config);
-
- if(this->thread_server->get_thread_state(this->data_thread_id)==attached)
- this->thread_server->start_thread(this->data_thread_id);
-
- this->send_info_request();
-
- std::list<std::string> events;
- events.push_back(this->new_info_event_id);
-
- //std::cout << "CModel_Car_Drivers_Base::start: waiting info frame" << std::endl;
- this->event_server->wait_all(events,1000);
-
- //std::cout << "CModel_Car_Drivers_Base::start: received info frame" << std::endl;
-
- this->data_mutex.enter();
- bool match = (this->arduino_id == this->info_data.ui8ArduinoAddress);
- this->data_mutex.exit();
-
- if(match)
- {
- this->paired=true;
- std::cout << "CModel_Car_Drivers_Base::start: correct arduino found!" << std::endl;
- ok=true;
- }
- else
- {
- std::cout << "CModel_Car_Drivers_Base::start: incorrect arduino" << std::endl;
- this->stop();
- }
- }
- catch(CCommException &e)
- {
- std::cout << e.what() << std::endl;
- //this->stop();
- throw;
- }
- return ok;
-}
-
-void CModel_Car_Drivers_Base::stop()
-{
- //std::cout << "CModel_Car_Drivers_Base::stop(): stopping" << std::endl;
- this->event_server->reset_event(this->new_info_event_id);
- this->paired=false;
- if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
- this->thread_server->get_thread_state(this->data_thread_id)==active)
- {
- this->event_server->set_event(this->finish_thread_event_id);
- this->thread_server->end_thread(this->data_thread_id);
- this->event_server->reset_event(this->finish_thread_event_id);
- }
- else
- {
- std::cout << "CModel_Car_Drivers_Base::stop(): data thread already ended" << std::endl;
- }
-
- this->port_mutex.enter();
- this->serial_port->close();
- if(this->serial_port!=NULL)
- {
- delete this->serial_port;
- this->serial_port=NULL;
- }
- else
- {
- std::cout << "CModel_Car_Drivers_Base::stop(): serial port already closed" << std::endl;
- }
- this->port_mutex.exit();
- //std::cout << "CModel_Car_Drivers_Base::stop(): stopped" << std::endl;
-}
-
-void CModel_Car_Drivers_Base::delete_thread_and_events()
-{
- if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
- this->thread_server->get_thread_state(this->data_thread_id)==active)
- {
- this->event_server->set_event(this->finish_thread_event_id);
- this->thread_server->end_thread(this->data_thread_id);
- }
-
- this->thread_server->detach_thread(this->data_thread_id);
- this->thread_server->delete_thread(this->data_thread_id);
- this->data_thread_id="";
- this->event_server->delete_event(this->finish_thread_event_id);
- this->finish_thread_event_id="";
- this->event_server->delete_event(this->new_data_event_id);
- this->new_data_event_id="";
-}
-
-void *CModel_Car_Drivers_Base::data_thread(void *param)
-{
- //std::cout << "CModel_Car_Drivers_Base::data_thread" << std::endl;
- CModel_Car_Drivers_Base *driver=(CModel_Car_Drivers_Base *)param;
- bool end=false;
-
- while(!end)
- {
- try
- {
- driver->read_data();
- if(!driver->event_server->event_is_set(driver->new_data_event_id))
- driver->event_server->set_event(driver->new_data_event_id);
- }
- catch(CException &e)
- {
- std::cout << e.what() << std::endl;
- }
- if(driver->event_server->event_is_set(driver->finish_thread_event_id))
- end=true;
- }
- driver->state=WAIT_START;
- pthread_exit(NULL);
-}
-
-void CModel_Car_Drivers_Base::send_info_request()
-{
- tArduinoHeader header;
- header.ui8ID = ID_ARD_ACT_REQUEST;
- header.ui8DataLength = 0;
- header.ui32Timestamp = 0;
-
- this->send_request(header, 0x00);
-}
-
-void CModel_Car_Drivers_Base::send_request(tArduinoHeader header, uint8_t data)
-{
- //std::cout << "CModel_Car_Drivers_Base::send_request()" <<std::endl;
-
- unsigned int req_frame_size = sizeof(tArduinoHeader)+header.ui8DataLength;
- unsigned char frame[req_frame_size];
-
- memcpy(frame, &header, sizeof(tArduinoHeader));
- if(header.ui8DataLength==1)
- memcpy(frame+sizeof(tArduinoHeader), &data, sizeof(uint8_t));
-
- uint16_t calculated_crc = fletcher16(frame, uint8_t(req_frame_size));
- //std::cout << " calculated_crc="<< hex(((calculated_crc >> (8*0)) & 0xff)) << "," << hex(((calculated_crc >> (8*1)) & 0xff))<< std::endl;
-
- unsigned char frame_with_crc[req_frame_size+sizeof(uint16_t)];
- memcpy(frame_with_crc, &frame, req_frame_size);
- memcpy(frame_with_crc+req_frame_size, &calculated_crc, sizeof(uint16_t));
-
- unsigned int esc_num=0;
- for(unsigned int i=0; i<req_frame_size+sizeof(uint16_t); i++)
- {
- if( (frame_with_crc[i]==START_BYTE) || (frame_with_crc[i]==END_BYTE) || (frame_with_crc[i]==ESCAPE_BYTE ) )
- esc_num++;
- }
-
- unsigned int stuffed_frame_size = req_frame_size+sizeof(uint16_t)+esc_num+2;
- unsigned char stuffed_frame[stuffed_frame_size];
- unsigned int ii=1;
- stuffed_frame[0]=START_BYTE;
- stuffed_frame[stuffed_frame_size-1]=END_BYTE;
- for(unsigned int i=0; i<req_frame_size+sizeof(uint16_t); i++)
- {
- if( (frame_with_crc[i]==START_BYTE) || (frame_with_crc[i]==END_BYTE) || (frame_with_crc[i]==ESCAPE_BYTE ) )
- {
- stuffed_frame[ii]=ESCAPE_BYTE;
- ii++;
- stuffed_frame[ii]=frame_with_crc[i];
- ii++;
- }
- else
- {
- stuffed_frame[ii]=frame_with_crc[i];
- ii++;
- }
- }
-
-// std::cout << " Sent frame: ";
-// for(unsigned int i=0; i<stuffed_frame_size; i++)
-// {
-// std::ostringstream out;
-// out << hex(stuffed_frame[i]) << ",";
-// std::cout << out.str();
-// }
-// std::cout << std::endl;
-
- if(this->debug)
- {
- std::cout << "CModel_Car_Drivers_Base::send_request(): DEBUG: sending frame = ";
- for(unsigned int i=0; i<stuffed_frame_size; i++)
- {
- std::cout << hex(stuffed_frame[i]) << "," << std::dec;
- }
- std::cout << std::endl;
- }
-
- this->port_mutex.enter();
- this->serial_port->write(stuffed_frame,stuffed_frame_size);
- this->port_mutex.exit();
-
- /*
- //MEANWHILE DIRECTLY USE msg[6+2+2+2=12]
- unsigned int len=12;
- unsigned char msg[12]={START_BYTE,ESCAPE_BYTE,ID_ARD_ACT_REQUEST,0x00,0x00,0x00,0x00,0x00,ESCAPE_BYTE,0x03,0x12,END_BYTE};
- std::cout << " Sent frame: ";
- for(unsigned int i=0; i<len; i++)
- {
- std::ostringstream out;
- out << hex(msg[i]) << ",";
- std::cout << out.str();
- }
- std::cout << std::endl;
-
- this->serial_port->write(msg,len);
- */
-
-}
-
-int CModel_Car_Drivers_Base::read_data()
-{
- //std::cout << "CModel_Car_Drivers_Base::read_data" << std::endl;
- unsigned char * frame_data=NULL;
- unsigned char * single_frame=NULL;
- unsigned int num;
- std::list<std::string> events;
-
- events.push_back(this->serial_port->get_rx_event_id());
-
-
- try
- {
- this->port_mutex.enter();
- bool empty = (num=this->serial_port->get_num_data())==0;
- this->port_mutex.exit();
- if(empty)
- {
- this->event_server->wait_all(events,1000);
- this->port_mutex.enter();
- num=this->serial_port->get_num_data();
- this->port_mutex.exit();
- }
-
- frame_data = new unsigned char[num];
- this->port_mutex.enter();
- this->serial_port->read(frame_data,num);
- this->port_mutex.exit();
-
- //std::cout << "CModel_Car_Drivers_Base::read_data: read buffer num = " << num << std::endl;
-
- bool escaped=false;
- int ts_count=0;
- int data_count=0;
- int crc_count=0;
- tTimeStampUnion ts_union;
- tCRCUnion crc_union;
- uint16_t calculated_crc;
-
- if(this->debug)
- {
- std::cout << "CModel_Car_Drivers_Base::read_data: DEBUG: frame data = ";
- for(unsigned int i=0; i<num; i++)
- {
- std::cout << hex(frame_data[i]) << "," << std::dec;
- }
- std::cout << std::endl;
- }
-
- for(unsigned int i=0; i<num; i++)
- {
- //std::cout << " state=" << this->state << std::endl;
- switch(this->state)
- {
- case WAIT_START:
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
- {
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- this->state=WAIT_START;
- }
- else if(frame_data[i]==END_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected END_BYTE" << std::endl;
- this->state=WAIT_START;
- }
- else if(frame_data[i]==START_BYTE)
- {
- if(!escaped)
- {
- //std::cout << " found start byte! " << hex(frame_data[i]) << std::endl;
- this->state=READ_ID;
- }
- else
- {
- escaped=false;
- this->state=WAIT_START;
- }
- }
- else
- {
- this->state=WAIT_START;
- }
- break;
-
- case READ_ID:
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
- {
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- this->state=READ_ID;
- }
- else if(frame_data[i]==START_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected START_BYTE" << std::endl;
- this->state=READ_ID;
- }
- else if(frame_data[i]==END_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected END_BYTE" << std::endl;
- this->state=WAIT_START;
- }
- else
- {
- if(escaped)
- escaped=false;
- this->header.ui8ID = frame_data[i];
- //std::cout << " found id="<< unsigned(this->header.ui8ID) << std::endl;
- this->state=READ_DATA_LENGTH;
- }
- break;
-
- case READ_DATA_LENGTH:
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
- {
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- this->state=READ_DATA_LENGTH;
- }
- else if(frame_data[i]==START_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected START_BYTE" << std::endl;
- this->state=READ_ID;
- }
- else if(frame_data[i]==END_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected END_BYTE" << std::endl;
- this->state=WAIT_START;
- }
- else
- {
- if(escaped)
- escaped=false;
- this->header.ui8DataLength = frame_data[i];
- //std::cout << " data length="<< unsigned(this->header.ui8DataLength) << std::endl;
- ts_count=0;
- this->state=READ_TIMESTAMP;
- }
- break;
- case READ_TIMESTAMP:
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
- {
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- this->state=READ_TIMESTAMP;
- }
- else if(frame_data[i]==START_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected START_BYTE" << std::endl;
- this->state=READ_ID;
- }
- else if(frame_data[i]==END_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected END_BYTE" << std::endl;
- this->state=WAIT_START;
- }
- else
- {
- if(escaped)
- escaped=false;
- ts_union.raw[ts_count] = frame_data[i];
- //std::cout << " reading timestamp " << ts_count+1 << "/4: "<< hex(ts_union.raw[ts_count]) << std::endl;
- if(ts_count<3)
- {
- ts_count++;
- this->state=READ_TIMESTAMP;
- }
- else
- {
- this->header.ui32Timestamp = ts_union.ui32Timestamp;
- //std::cout << " time stamp="<< unsigned(this->header.ui32Timestamp) << std::endl;
- data_count=0;
- this->state=READ_DATA;
- }
- }
- break;
-
- case READ_DATA:
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
- {
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- this->state=READ_DATA;
- }
- else if(frame_data[i]==START_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected START_BYTE" << std::endl;
- this->state=READ_ID;
- }
- else if(frame_data[i]==END_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected END_BYTE" << std::endl;
- this->state=WAIT_START;
- }
- else
- {
- if(escaped)
- escaped=false;
- this->data_union.raw[data_count] = frame_data[i];
- //std::cout << " reading data " << data_count+1 << "/" << unsigned(this->header.ui8DataLength) << ": " << hex(this->data_union.raw[data_count]) << std::endl;
- if(data_count<this->header.ui8DataLength-1)
- {
- data_count++;
- this->state=READ_DATA;
- }
- else
- {
- // std::cout << " data=";
- // for(unsigned int ii=0; ii<this->header.ui8DataLength; ii++)
- // {
- // std::cout << hex(this->data_union.raw[ii]) << ",";
- // }
- // std::cout << std::endl;
-
- crc_count=0;
- this->state=READ_CRC;
- }
- }
- break;
-
- case READ_CRC:
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
- {
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- this->state=READ_CRC;
- }
- else if(frame_data[i]==START_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected START_BYTE" << std::endl;
- this->state=READ_ID;
- }
- else if(frame_data[i]==END_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected END_BYTE" << std::endl;
- this->state=WAIT_START;
- }
- else
- {
- if(escaped)
- escaped=false;
- crc_union.raw[crc_count] = frame_data[i];
- //std::cout << " reading crc " << crc_count+1 << "/2: " << hex(crc_union.raw[crc_count]) << std::endl;
- if(crc_count<1)
- {
- crc_count++;
- this->state=READ_CRC;
- }
- else
- {
- //std::cout << " crc="<< hex(((crc_union.crc >> (8*0)) & 0xff)) << "," << hex(((crc_union.crc >> (8*1)) & 0xff))<< std::endl;
- this->state=READ_END;
- }
- }
- break;
- case READ_END:
- if(frame_data[i]==ESCAPE_BYTE && !escaped)
- {
- //std::cout << " esc_byte!" << std::endl;
- escaped=true;
- this->state=READ_END;
- }
- else if(frame_data[i]==START_BYTE && !escaped)
- {
- std::cout << " WARNING, found unexpected START_BYTE" << std::endl;
- this->state=READ_ID;
- }
- else if(frame_data[i]==END_BYTE && !escaped)
- {
- //std::cout << " found end byte! " << unsigned(frame_data[i]) << std::endl;
- //this->state=CHECK_CRC;
- //CHECK_CRC
-
- unsigned int frame_size = sizeof(tArduinoHeader)+this->header.ui8DataLength;
- single_frame = new unsigned char[frame_size];
-
- memcpy(single_frame, &this->header, sizeof(tArduinoHeader));
- memcpy(&single_frame[sizeof(tArduinoHeader)], &this->data_union, this->header.ui8DataLength);
-
- //std::cout << " crc frame=";
- // for(unsigned int ii=0; ii<frame_size; ii++)
- // {
- // std::cout << hex(single_frame[ii]) << ",";
- // }
- // std::cout << std::endl;
-
- calculated_crc = fletcher16(single_frame, frame_size);
-
- if(single_frame!=NULL)
- {
- delete []single_frame;
- }
-
- //std::cout << " calculated_crc="<< hex(((calculated_crc >> (8*0)) & 0xff)) << "," << hex(((calculated_crc >> (8*1)) & 0xff))<< std::endl;
- //calculated_crc = 0xff << 8 | 0xff;
- if (calculated_crc != crc_union.crc)
- {
- std::cout << " #Error, frame CRC invalid. Skipping frame" << std::endl;
- this->state=WAIT_START;
- }
- else
- {
- //std::cout << " valid CRC!" << std::endl;
- //this->state=PROCESS_DATA;
- //PROCESS_DATA
- switch(this->header.ui8ID)
- {
- case ID_ARD_SENSOR_INFO:
- this->data_mutex.enter();
- this->event_server->set_event(this->new_info_event_id);
- this->info_data.ui8ArduinoAddress = this->data_union.info.ui8ArduinoAddress;
- this->info_data.ui16ArduinoVersion = this->data_union.info.ui16ArduinoVersion;
- this->data_mutex.exit();
- std::cout << "CModel_Car_Drivers_Base::read_data: processed info data, Address="<< unsigned(this->info_data.ui8ArduinoAddress) << ", Version="<< unsigned(this->info_data.ui16ArduinoVersion) << std::endl;
- break;
- case ID_ARD_SENS_ERROR:
- this->error_data.ui8ErrorNr = this->data_union.error.ui8ErrorNr;
- switch(this->error_data.ui8ErrorNr)
- {
- case ERROR_STX_NOT_FOUND:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_STX_NOT_FOUND" << std::endl;
- break;
- case ERROR_ETX_NOT_FOUND:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_ETX_NOT_FOUND" << std::endl;
- break;
- case ERROR_ESC_BYTE_BROKEN:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_ESC_BYTE_BROKEN" << std::endl;
- break;
- case ERROR_NO_BYTES_AVAILABLE:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_NO_BYTES_AVAILABLE" << std::endl;
- break;
- case ERROR_CRC_INVALID:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_CRC_INVALID" << std::endl;
- break;
- case ERROR_NO_FRAME_DATA:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_NO_FRAME_DATA" << std::endl;
- break;
- case ERROR_FRAME_DROPPED:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_FRAME_DROPPED" << std::endl;
- break;
- case ERROR_REMOTE_DETECTED:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_REMOTE_DETECTED" << std::endl;
- break;
- case ERROR_NO_GYRO_DETECTED:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_NO_GYRO_DETECTED" << std::endl;
- break;
- case ERROR_INVALID_ACTUATOR_HEADER:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_INVALID_ACTUATOR_HEADER" << std::endl;
- break;
- case ERROR_INITIALIZATION_FAILED:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_INITIALIZATION_FAILED" << std::endl;
- break;
- case ERROR_FRAME_NOT_WRITTEN:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: ERROR_FRAME_NOT_WRITTEN" << std::endl;
- break;
- default:
- std::cout << "CModel_Car_Drivers_Base::read_data: error received: unknown error code" << std::endl;
- }
- break;
- default:
- if(this->paired)
- {
- //std::cout << "CModel_Car_Drivers_Base::read_data: process data frame" << std::endl;
- this->data_mutex.enter();
- this->process_data_frame(this->header.ui8ID, this->header.ui32Timestamp, this->data_union);
- this->data_mutex.exit();
- }
- else
- {
- //std::cout << "CModel_Car_Drivers_Base::read_data: ignoring data frame, arduino not paired yet (match arduino_id)" << std::endl;
- //Ignoring data, as this may not be the correct arduino device yet
- }
- break;
- }
- }
- this->state=WAIT_START;
- }
- else
- {
- if(escaped)
- escaped=false;
- std::cout << " #Error, no END BYTE found" << std::endl;
- this->state=WAIT_START;
- }
- break;
- }
-
- //enf of buffer
- if(i==num-1)
- {
- if(state!=WAIT_START)
- {
- std::cout<< "buffer ended and frame not fully read, state=" << this->state << std::endl;
- }
- }
- }
- if(frame_data!=NULL)
- delete []frame_data;
- }
- catch(CException &e)
- {
- std::cout << e.what() << std::endl;
- throw;
- }
-
- return ERROR_NO_FRAME_DATA;
-}
-
-// void CModel_Car_Drivers_Base::process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union)
-// {
-// //TODO: implement in child class
-// std::cout << "CModel_Car_Drivers_Base::process_data_frame: id=" << unsigned(id) << std::endl;
-// }
-
-uint16_t CModel_Car_Drivers_Base::fletcher16(uint8_t const *data, uint8_t bytes)
-{
- uint16_t sum1 = 0xff, sum2 = 0xff;
-
- while (bytes)
- {
- uint8_t tlen = bytes > 20 ? 20 : bytes;
- bytes -= tlen;
- do
- {
- sum2 += sum1 += *data++;
- }
- while (--tlen);
- sum1 = (sum1 & 0xff) + (sum1 >> 8);
- sum2 = (sum2 & 0xff) + (sum2 >> 8);
- }
- /* Second reduction step to reduce sums to 8 bits */
- sum1 = (sum1 & 0xff) + (sum1 >> 8);
- sum2 = (sum2 & 0xff) + (sum2 >> 8);
- return sum2 << 8 | sum1;
-}
-
-std::string CModel_Car_Drivers_Base::get_new_data_event_id(void)
-{
- return this->new_data_event_id;
-}
-
-/*
-void CModel_Car_Drivers_Base::test(unsigned char *data, unsigned int len)
-{
- std::cout << "CModel_Car_Drivers_Base::test" << std::endl;
-
-
- std::cout << "Sent: ";
- for(unsigned int i=0; i<len; i++)
- {
- std::ostringstream out;
- out << hex(data[i]) << ",";
- std::cout << out.str();
- }
- std::cout << std::endl;
-
- this->serial_port->write(data,len);
-}
-*/
-
-void CModel_Car_Drivers_Base::set_debug(bool debug)
-{
- this->debug=debug;
-}
-
-
-CModel_Car_Drivers_Base::~CModel_Car_Drivers_Base()
-{
- this->stop();
- this->delete_thread_and_events();
-}
diff --git a/src/model_car_drivers_base_exceptions.cpp b/src/model_car_drivers_base_exceptions.cpp
deleted file mode 100644
index c8c2159195d96a24a7d125c10d8438a71935bc24..0000000000000000000000000000000000000000
--- a/src/model_car_drivers_base_exceptions.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "model_car_drivers_base_exceptions.h"
-#include <string.h>
-#include <stdio.h>
-#include <iomanip>
-
-const std::string model_car_driver_base_exception_msg="[CModel_Car_Drivers_Base class] - ";
-
-CModel_Car_Drivers_BaseException::CModel_Car_Drivers_BaseException(const std::string& where, const std::string& error_msg):CException(where, model_car_driver_base_exception_msg)
-{
- this->error_msg+=error_msg;
-}
-
-CModel_Car_Drivers_BaseException::CModel_Car_Drivers_BaseException(const std::string& where, const std::string& error_msg, const uint8_t *serial_msg, uint8_t msg_length):CException(where, model_car_driver_base_exception_msg)
-{
- this->error_msg += error_msg;
- std::ostringstream ss;
- for (uint8_t i = 0; i < msg_length; i++)
- ss << "0x" << std::hex << std::setw(2) << std::setfill('0') << (int)serial_msg[i];
- std::string msg_string(ss.str());
- this->error_msg += "\n[Serial message] - ";
- this->error_msg += msg_string;
-}
diff --git a/src/model_car_drivers_bat.cpp b/src/model_car_drivers_bat.cpp
deleted file mode 100644
index ab14f911c74c59dcf1113af32f4e4c365bdb1fb5..0000000000000000000000000000000000000000
--- a/src/model_car_drivers_bat.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "model_car_drivers_bat.h"
-
-CModel_Car_Drivers_Bat::CModel_Car_Drivers_Bat(std::string name) : CModel_Car_Drivers_Base(name, ARDUINO_CENTER_MEASUREMENT)
-{
- tVoltageData v;
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR_CELL1, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_ACTUATOR_CELL2, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL1, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL2, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL3, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL4, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL5, v));
- m_bat_values.insert(std::make_pair(ID_ARD_SENS_VOLT_SENSORS_CELL6, v));
-}
-
-void CModel_Car_Drivers_Bat::process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union)
-{
- //std::cout << "CModel_Car_Drivers_Bat::process_data_frame: id=" << unsigned(id) << std::endl;
-
- const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id);
-
- auto it = m_bat_values.find(currentSensor);
- if(it==m_bat_values.end())
- {
- std::cout << "CModel_Car_Drivers_Bat::process_data_frame: error, incorrect id: " << id << std::endl;
- }
- else
- {
- it->second = data_union.voltage;
- //m_bat_values[currentSensor] = data_union.us;
- }
-
-}
-
-bool CModel_Car_Drivers_Bat::get_bat(SENSOR_ID id, float & value)
-{
- bool ok=false;
- this->data_mutex.enter();
- auto it = m_bat_values.find(id);
-
- if ( it == m_bat_values.end() )
- {
- // not found
- }
- else
- {
- value = it->second.ui16VoltageData;
- ok=true;
- }
- this->data_mutex.exit();
-
- return ok;
-}
-
-CModel_Car_Drivers_Bat::~CModel_Car_Drivers_Bat()
-{
-
-}
diff --git a/src/model_car_drivers_egomotion.cpp b/src/model_car_drivers_egomotion.cpp
deleted file mode 100644
index 13f20c8450317ce8fad4a030be4e51b86d1f6d05..0000000000000000000000000000000000000000
--- a/src/model_car_drivers_egomotion.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-#include "model_car_drivers_egomotion.h"
-
-CModel_Car_Drivers_Egomotion::CModel_Car_Drivers_Egomotion(std::string name) : CModel_Car_Drivers_Base(name, ARDUINO_REAR_IMU_WHEELENC)
-{
-
-}
-
-void CModel_Car_Drivers_Egomotion::process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union)
-{
- //std::cout << "CModel_Car_Drivers_Egomotion::process_data_frame: id=" << unsigned(id) << std::endl;
-
- const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id);
-
- switch(currentSensor)
- {
- case ID_ARD_SENS_WHEEL_RIGHT:
- this->right_wheel = data_union.wheel;
- break;
- case ID_ARD_SENS_WHEEL_LEFT:
- this->left_wheel = data_union.wheel;
- break;
- case ID_ARD_SENS_IMU:
- this->imu = data_union.imu;
- break;
- default:
- std::cout << "CModel_Car_Drivers_Egomotion::process_data_frame: error, incorrect id: " << id << std::endl;
- }
-}
-
-bool CModel_Car_Drivers_Egomotion::get_left_wheel(tSensWheelData & data)
-{
- bool ok=false;
- this->data_mutex.enter();
- data = this->left_wheel;
- ok=true;
- this->data_mutex.exit();
-
- return ok;
-}
-
-bool CModel_Car_Drivers_Egomotion::get_right_wheel(tSensWheelData & data)
-{
- bool ok=false;
- this->data_mutex.enter();
- data = this->right_wheel;
- ok=true;
- this->data_mutex.exit();
-
- return ok;
-
-}
-
-bool CModel_Car_Drivers_Egomotion::get_imu(tImuData & data)
-{
- bool ok=false;
- this->data_mutex.enter();
- data = this->imu;
- ok=true;
- this->data_mutex.exit();
-
- return ok;
-}
-
-CModel_Car_Drivers_Egomotion::~CModel_Car_Drivers_Egomotion()
-{
-
-}
diff --git a/src/model_car_drivers_uss.cpp b/src/model_car_drivers_uss.cpp
deleted file mode 100644
index cbb1a340bb8175b70491ea86f806c20dbbbf7297..0000000000000000000000000000000000000000
--- a/src/model_car_drivers_uss.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-#include "model_car_drivers_uss.h"
-
-CModel_Car_Drivers_Uss::CModel_Car_Drivers_Uss(std::string name) : CModel_Car_Drivers_Base(name, ARDUINO_REAR_US)
-{
- tUsData us;
- m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_SIDE_RIGHT, us));
- m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER_RIGHT, us));
- m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER, us));
- m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER_LEFT, us));
- m_uss_values.insert(std::make_pair(ID_ARD_SENS_US_SIDE_LEFT, us));
-}
-
-void CModel_Car_Drivers_Uss::process_data_frame(uint8_t id,uint32_t timestamp, tDataUnion data_union)
-{
- //std::cout << "CModel_Car_Drivers_Uss::process_data_frame: id=" << unsigned(id) << std::endl;
-
- const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id);
-
- auto it = m_uss_values.find(currentSensor);
- if(it==m_uss_values.end())
- {
- std::cout << "CModel_Car_Drivers_Uss::process_data_frame: error, incorrect id: " << id << std::endl;
- }
- else
- {
- it->second = data_union.us;
- //m_uss_values[currentSensor] = data_union.us;
- }
-
-}
-
-bool CModel_Car_Drivers_Uss::get_uss(SENSOR_ID id, float & value)
-{
- bool ok=false;
- this->data_mutex.enter();
- auto it = m_uss_values.find(id);
-
- if ( it == m_uss_values.end() )
- {
- // not found
- }
- else
- {
- value = it->second.i16Distance;
- ok=true;
- }
- this->data_mutex.exit();
-
- return ok;
-}
-
-CModel_Car_Drivers_Uss::~CModel_Car_Drivers_Uss()
-{
-
-}
diff --git a/src/model_car_egomotion.cpp b/src/model_car_egomotion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c9f5a8ad7e3d54de8897d1653c62c74717c9ec47
--- /dev/null
+++ b/src/model_car_egomotion.cpp
@@ -0,0 +1,162 @@
+#include "model_car_egomotion.h"
+#include <list>
+
+CModelCarEgomotion::CModelCarEgomotion(std::string name) : CModelCarDriverBase(name, ARDUINO_REAR_IMU_WHEELENC)
+{
+ this->zero_gx=0.0;
+ this->zero_gy=0.0;
+ this->zero_gz=0.0;
+ this->gyro_cal_running=false;
+ this->gyro_cal_stop=false;
+ this->gyro_cal_num_samples=100;
+}
+
+void CModelCarEgomotion::process_data_frame(uint8_t id, TDataUnion data_union)
+{
+ static bool left_done=false,right_done=false,imu_done=false;
+ static double cal_gx=0.0,cal_gy=0.0,cal_gz=0.0;
+ static unsigned int cal_count=0;
+
+ switch((SENSOR_ID)id)
+ {
+ case ID_ARD_SENS_WHEEL_RIGHT:
+ this->right_wheel = data_union.wheel;
+ this->right_wheel_timestamp=this->get_last_timestamp();
+ right_done=true;
+ break;
+ case ID_ARD_SENS_WHEEL_LEFT:
+ this->left_wheel = data_union.wheel;
+ this->left_wheel_timestamp=this->get_last_timestamp();
+ left_done=true;
+ break;
+ case ID_ARD_SENS_IMU:
+ this->imu.ax = data_union.imu.ax*9.81; // G to m/s2
+ this->imu.ay = data_union.imu.ay*9.81; // G to m/s2
+ this->imu.az = data_union.imu.az*9.81; // G to m/s2
+ this->imu.gx = data_union.imu.gx*3.14159/180; // º/s to rad/s
+ this->imu.gy = data_union.imu.gy*3.14159/180; // º/s to rad/s
+ this->imu.gz = data_union.imu.gz*3.14159/180; // º/s to rad/s
+ this->imu.mx = data_union.imu.mx*1e-7; //miliGauss(mG) to Tesla(T)
+ this->imu.my = data_union.imu.my*1e-7; //miliGauss(mG) to Tesla(T)
+ this->imu.mz = data_union.imu.mz*1e-7; //miliGauss(mG) to Tesla(T)
+ this->imu_timestamp=this->get_last_timestamp();
+ if(this->gyro_cal_running)
+ {
+ cal_gx+=this->imu.gx;
+ cal_gy+=this->imu.gy;
+ cal_gz+=this->imu.gz;
+ cal_count++;
+ if(this->gyro_cal_num_samples==cal_count)
+ {
+ cal_count=0;
+ this->zero_gx=cal_gx/this->gyro_cal_num_samples;
+ this->zero_gy=cal_gy/this->gyro_cal_num_samples;
+ this->zero_gz=cal_gz/this->gyro_cal_num_samples;
+ cal_gx=0.0;
+ cal_gy=0.0;
+ cal_gz=0.0;
+ this->gyro_cal_running=false;
+ }
+ else if(this->gyro_cal_stop)
+ {
+ cal_count=0;
+ this->zero_gx=cal_gx/this->gyro_cal_num_samples;
+ this->zero_gy=cal_gy/this->gyro_cal_num_samples;
+ this->zero_gz=cal_gz/this->gyro_cal_num_samples;
+ cal_gx=0.0;
+ cal_gy=0.0;
+ cal_gz=0.0;
+ this->gyro_cal_running=false;
+ this->gyro_cal_stop=false;
+ }
+ }
+ imu_done=true;
+ break;
+ default:
+ break;
+ }
+ if(left_done && right_done && imu_done)
+ {
+ left_done=false;
+ right_done=false;
+ imu_done=false;
+ if(!this->event_server->event_is_set(this->new_data_event_id))
+ this->event_server->set_event(this->new_data_event_id);
+ }
+}
+
+void CModelCarEgomotion::get_left_wheel(int &tach, bool &dir,unsigned long int ×tamp)
+{
+ this->data_mutex.enter();
+ tach=this->left_wheel.wheel_tach;
+ dir=this->left_wheel.wheel_dir;
+ timestamp=this->left_wheel_timestamp;
+ this->data_mutex.exit();
+}
+
+void CModelCarEgomotion::get_right_wheel(int &tach, bool &dir,unsigned long int ×tamp)
+{
+ this->data_mutex.enter();
+ tach = this->right_wheel.wheel_tach;
+ dir = this->right_wheel.wheel_dir;
+ timestamp=this->right_wheel_timestamp;
+ this->data_mutex.exit();
+}
+
+void CModelCarEgomotion::get_imu_accelerometer(double &ax, double &ay, double &az,unsigned long int ×tamp)
+{
+ this->data_mutex.enter();
+ ax = this->imu.ax;
+ ay = this->imu.ay;
+ az = this->imu.az;
+ timestamp=this->imu_timestamp;
+ this->data_mutex.exit();
+}
+
+void CModelCarEgomotion::get_imu_gyroscope(double &gx, double &gy, double &gz,unsigned long int ×tamp)
+{
+ this->data_mutex.enter();
+ gx = this->imu.gx-this->zero_gx;
+ gy = this->imu.gy-this->zero_gy;
+ gz = this->imu.gz-this->zero_gz;
+ timestamp=this->imu_timestamp;
+ this->data_mutex.exit();
+}
+
+void CModelCarEgomotion::get_imu_magnetometer(double &mx, double &my, double &mz,unsigned long int ×tamp)
+{
+ this->data_mutex.enter();
+ mx = this->imu.mx;
+ my = this->imu.my;
+ mz = this->imu.mz;
+ timestamp=this->imu_timestamp;
+ this->data_mutex.exit();
+}
+
+void CModelCarEgomotion::start_gyro_calibration(unsigned int num_samples)
+{
+ this->data_mutex.enter();
+ this->zero_gx=0.0;
+ this->zero_gy=0.0;
+ this->zero_gy=0.0;
+ this->gyro_cal_running=true;
+ this->gyro_cal_num_samples=num_samples;
+ this->data_mutex.exit();
+}
+
+void CModelCarEgomotion::stop_gyro_calibration(void)
+{
+ this->data_mutex.enter();
+ this->gyro_cal_stop=true;
+ this->data_mutex.exit();
+}
+
+bool CModelCarEgomotion::is_gyro_calibration_done(void)
+{
+ return !this->gyro_cal_running;
+}
+
+CModelCarEgomotion::~CModelCarEgomotion()
+{
+
+}
diff --git a/src/model_car_ultrasounds.cpp b/src/model_car_ultrasounds.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32fcfea0e17b90c7d58518565243605f1f0e84ff
--- /dev/null
+++ b/src/model_car_ultrasounds.cpp
@@ -0,0 +1,86 @@
+#include "model_car_ultrasounds.h"
+
+CModelCarUltrasounds::CModelCarUltrasounds(std::string name) : CModelCarDriverBase(name, ARDUINO_REAR_US)
+{
+ ultrasound_values.insert(std::make_pair(ID_ARD_SENS_US_SIDE_RIGHT, 0.0));
+ ultrasound_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER_RIGHT, 0.0));
+ ultrasound_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER, 0.0));
+ ultrasound_values.insert(std::make_pair(ID_ARD_SENS_US_REAR_CENTER_LEFT, 0.0));
+ ultrasound_values.insert(std::make_pair(ID_ARD_SENS_US_SIDE_LEFT, 0.0));
+}
+
+void CModelCarUltrasounds::process_data_frame(uint8_t id, TDataUnion data_union)
+{
+ ultrasounds_map_t::iterator it = this->ultrasound_values.find((SENSOR_ID)id);
+ static int num_messages=5;
+
+ if(it!=this->ultrasound_values.end())
+ it->second = data_union.us.distance/100.0; //cm
+ num_messages--;
+ if(num_messages==0)
+ {
+ num_messages=5;
+ if(!this->event_server->event_is_set(this->new_data_event_id))
+ this->event_server->set_event(this->new_data_event_id);
+ }
+}
+
+double CModelCarUltrasounds::get_side_right_distance(void)
+{
+ double distance;
+
+ this->data_mutex.enter();
+ distance=this->ultrasound_values[ID_ARD_SENS_US_SIDE_RIGHT];
+ this->data_mutex.exit();
+
+ return distance;
+}
+
+double CModelCarUltrasounds::get_side_left_distance(void)
+{
+ double distance;
+
+ this->data_mutex.enter();
+ distance=this->ultrasound_values[ID_ARD_SENS_US_SIDE_LEFT];
+ this->data_mutex.exit();
+
+ return distance;
+}
+
+double CModelCarUltrasounds::get_rear_right_distance(void)
+{
+ double distance;
+
+ this->data_mutex.enter();
+ distance=this->ultrasound_values[ID_ARD_SENS_US_REAR_CENTER_RIGHT];
+ this->data_mutex.exit();
+
+ return distance;
+}
+
+double CModelCarUltrasounds::get_rear_center_distance(void)
+{
+ double distance;
+
+ this->data_mutex.enter();
+ distance=this->ultrasound_values[ID_ARD_SENS_US_REAR_CENTER];
+ this->data_mutex.exit();
+
+ return distance;
+}
+
+double CModelCarUltrasounds::get_rear_left_distance(void)
+{
+ double distance;
+
+ this->data_mutex.enter();
+ distance=this->ultrasound_values[ID_ARD_SENS_US_REAR_CENTER_LEFT];
+ this->data_mutex.exit();
+
+ return distance;
+}
+
+CModelCarUltrasounds::~CModelCarUltrasounds()
+{
+
+}