Merge pull request #3 from LeoRover/devel

1.2.0 version

.gitignore

@@ -1,4 +1,5 @@
-.vscode
+.vscode/*
+!.vscode/c_cpp_properties.json
 CMakeFiles
 CMakeCache.txt
 cmake_install.cmake
@@ -8,4 +9,5 @@ build.ninja
 rules.ninja
 *.bin
 *.hex
-*.elf
+*.elf
+build/

.vscode/c_cpp_properties.json

@@ -0,0 +1,65 @@
+{
+    "env": {
+        "hFrameworkPath": "/opt/hFramework",
+        "DefaultIncludePath": [
+            "${hFrameworkPath}/include",
+            "${hFrameworkPath}/ports/stm32/include",
+            "/usr/include",
+            "${workspaceFolder}/include",
+            "${workspaceFolder}/lib/rosserial",
+            "${workspaceFolder}/lib/sensors"
+        ]
+    },
+    "configurations": [
+        {
+            "name": "Win32",
+            "includePath": [
+                "${DefaultIncludePath}"
+            ],
+            "defines": [
+                "BOARD_TYPE=CORE2",
+                "PORT=STM32",
+                "BOARD_VERSION=1.0.0"
+            ],
+            "intelliSenseMode": "gcc-arm",
+            "cStandard": "c11",
+            "cppStandard": "c++17",
+            "compilerPath": "arm-none-eabi-gcc.exe"
+        },
+        {
+            "name": "Linux",
+            "includePath": [
+                "${DefaultIncludePath}"
+            ],
+            "defines": [
+                "BOARD_TYPE=CORE2",
+                "PORT=STM32",
+                "BOARD_VERSION=1.0.0"
+            ],
+            "intelliSenseMode": "gcc-arm",
+            "cStandard": "c11",
+            "cppStandard": "c++17",
+            "compilerPath": "/usr/bin/arm-none-eabi-gcc"
+        },
+        {
+            "name": "Mac",
+            "includePath": [
+                "${DefaultIncludePath}"
+            ],
+            "defines": [
+                "BOARD_TYPE=CORE2",
+                "PORT=STM32",
+                "BOARD_VERSION=1.0.0"
+            ],
+            "macFrameworkPath": [
+                "/System/Library/Frameworks",
+                "/Library/Frameworks"
+            ],
+            "intelliSenseMode": "clang-arm",
+            "cStandard": "c11",
+            "cppStandard": "c++17",
+            "compilerPath": "/usr/bin/arm-none-eabi-gcc"
+        }
+    ],
+    "version": 4
+}

CMakeLists.txt

@@ -1,11 +1,22 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.0.2)
 project(LeoFirmware NONE)
 
+set(HFRAMEWORK_PATH "/opt/hFramework" CACHE FILEPATH
+    "Path to the hFramework SDK")
+
+set(BOARD_TYPE "core2")
+set(BOARD_VERSION "1.0.0")
+
 include(${HFRAMEWORK_PATH}/hFramework.cmake)
-enable_module(hROS)
+
+if (RELEASE)
+    optimization(3)
+endif()
 
 include_directories(
     include/
+    lib/rosserial/
+    lib/sensors/
 )
 
 add_hexecutable(leo_firmware 
@@ -15,5 +26,8 @@ add_hexecutable(leo_firmware
     src/config.cpp
     src/logging.cpp
     src/imu.cpp
-    src/MPU9250.cpp
-    src/gps.cpp)
+    src/gps.cpp
+    src/parameters.cpp
+    lib/rosserial/time.cpp
+    lib/rosserial/duration.cpp
+    lib/sensors/MPU9250/MPU9250.cpp)

README.md

@@ -12,12 +12,38 @@ The main functionalities include:
 
 It uses [rosserial] client library to expose its functionalities on ROS topics, services and parameters.
 
-## Building
-To build the project, you will need:
+## Building and flashing
+To build the project you will need to install cmake and GNU Arm Embedded Toolchain. On Ubuntu/Debian, you can do:
+```
+sudo apt-get install cmake gcc-arm-none-eabi
+```
+You will also need the [Husarion SDK]. Download it and unpack to the `/opt/hFramework` directory.
+
+To build, enter the following commands on the terminal:
+```
+mkdir build && cd build
+cmake ..
+make
+```
+To flash, you can either use a USB cable to connect your computer to hSerial port on CORE2 board and type:
+```
+make flash
+```
+or you can upload the `leo_firmware.bin` file to Leo Rover and use [leo_fw] package to flash it:
+```
+rosrun leo_fw flash leo_firmware.bin
+```
+
+### Using Husarion extension for VSCode
+You can also use the VSCode extension which already contains the Husarion SDK. This should work on Windows, Linux and MacOS platforms, but unfortunately break the VSCode IntelliSense configuration.
+
+ Make sure you have installed:
 - [Visual Studio Code],
 - [Husarion extension] for VSCode
 
-Open the project on VSCode and click `[Ctrl]+[Shift]+[B]`.
+and followed the instructions on the [Husarion extension] page for installing the requirements.
+
+Open the project in VSCode and click `[Ctrl]+[Shift]+[B]` to build it.
 
 To flash the firmware, use a USB cable to connect your computer to hSerial port on CORE2 board. \
 Then, click `[Ctrl]+[Shift]+[B]` and select `Flash project to CORE2`.
@@ -31,4 +57,5 @@ For the information about exposed ROS topics, services and parameters, visit [le
 [Visual Studio Code]: https://code.visualstudio.com
 [Husarion extension]: https://marketplace.visualstudio.com/items?itemName=husarion.husarion
 [Husarion CORE2]: https://husarion.com/manuals/core2/
-[rosserial]: http://wiki.ros.org/rosserial
+[rosserial]: http://wiki.ros.org/rosserial
+[Husarion SDK]: http://files.fictionlab.pl/husarion/Husarion_SDK-stable.zip

include/leo_firmware/config.h

@@ -4,33 +4,25 @@
 
 #include <cstdint>
 
-#define CONFIG_ADDRESS 0x01
+static const uint16_t CONFIG_ADDRESS = 0x01;
 
 struct Config {
   uint8_t checksum;
 
-  bool debug_logging;
-  bool imu_enabled;
-  bool gps_enabled;
-  float gyro_bias[3];
-  float accel_bias[3];
-  float mag_scale[3];
-  float mag_bias[3];
-
-  Config()
-      : debug_logging(false),
-        imu_enabled(false),
-        gps_enabled(false),
-        gyro_bias{0.0, 0.0, 0.0},
-        accel_bias{0.0, 0.0, 0.0},
-        mag_scale{1.0, 1.0, 1.0},
-        mag_bias{0.0, 0.0, 0.0} {}
+  bool debug_logging = false;
+  bool imu_enabled = false;
+  bool gps_enabled = false;
+  float gyro_bias[3] = {0.0, 0.0, 0.0};
+  float accel_bias[3] = {0.0, 0.0, 0.0};
+  float mag_scale[3] = {1.0, 1.0, 1.0};
+  float mag_bias[3] = {0.0, 0.0, 0.0};
 } __attribute__((packed));
 
 extern Config conf;
 
-void load_config();
-void store_config();
-void reset_config();
+void configPrint();
+void configLoad();
+void configStore();
+void configReset();
 
 #endif  // LEO_FIRMWARE_INCLUDE_CONFIG_H_

include/leo_firmware/diff_drive_controller.h

@@ -4,41 +4,45 @@
 #include <vector>
 
 #include <ros.h>
+#include <hMutex.h>
 
 #include <leo_firmware/wheel_controller.h>
 
+struct Odom {
+  float vel_lin;
+  float vel_ang;
+  float pose_x;
+  float pose_y;
+  float pose_yaw;
+};
+
 class DiffDriveController {
  public:
-  void init(ros::NodeHandle *nh);
-  void start();
-  void setSpeed(float linear, float angular);
-  std::vector<float> getOdom();
-  std::vector<float> getWheelPositions();
-  std::vector<float> getWheelVelocities();
-  std::vector<float> getWheelEfforts();
+  void init();
+  void setSpeed(const float linear, const float angular);
+  Odom getOdom();
+  void resetOdom();
+  void updateWheelStates();
+
+  double positions[4];
+  double velocities[4];
+  double efforts[4];
 
  private:
-  void updateWheelLoop();
-  void updateOdometryLoop();
-  void debugLoop();
+  void controllerLoop();
   void inputWatchdog();
 
+  hFramework::hMutex mutex_wheel_;
+  hFramework::hMutex mutex_odom_;
+
   WheelController *wheel_FL_;
   WheelController *wheel_RL_;
   WheelController *wheel_FR_;
   WheelController *wheel_RR_;
 
-  float lin_vel_;
-  float ang_vel_;
+  Odom odom_;
 
   uint64_t last_update_;
-  uint64_t input_timeout_;
-
-  // Default parameters
-  float encoder_resolution_ = 878.4;
-  float wheel_radius_ = 0.0625;
-  float wheel_separation_ = 0.33;
-  float angular_velocity_multiplier_ = 1.91;
 };
 
 #endif  // LEO_FIRMWARE_INCLUDE_DIFF_DRIVE_CONTROLLER_H_

include/leo_firmware/parameters.h

@@ -0,0 +1,42 @@
+#ifndef LEO_FIRMWARE_INCLUDE_PARAMETERS_H_
+#define LEO_FIRMWARE_INCLUDE_PARAMETERS_H_
+
+#include <ros.h>
+
+struct Parameters {
+  // TF frames
+  char robot_frame_id[50] = "base_link";
+  char odom_frame_id[50] = "odom";
+  char imu_frame_id[50] = "imu";
+  char gps_frame_id[50] = "gpu";
+
+  // Servo
+  int servo_voltage = 2;
+  int servo_period[6] = {20000, 20000, 20000, 20000, 20000, 20000};
+  int servo_angle_min[6] = {-90, -90, -90, -90, -90, -90};
+  int servo_angle_max[6] = {90, 90, 90, 90, 90, 90};
+  int servo_width_min[6] = {1000, 1000, 1000, 1000, 1000, 1000};
+  int servo_width_max[6] = {2000, 2000, 2000, 2000, 2000, 2000};
+
+  // Motor
+  float motor_encoder_resolution = 878.4F;
+  int motor_encoder_pullup = 1;
+  float motor_max_speed = 800.0F;
+  float motor_pid_p = 0.0F;
+  float motor_pid_i = 0.005F;
+  float motor_pid_d = 0.0F;
+  float motor_power_limit = 1000.0F;
+  float motor_torque_limit = 1000.0F;
+
+  // Differential drive
+  float dd_wheel_radius = 0.0625F;
+  float dd_wheel_separation = 0.33F;
+  float dd_angular_velocity_multiplier = 1.91F;
+  float dd_input_timeout = 500.0F;
+
+  void load(ros::NodeHandle &nh);
+};
+
+extern Parameters params;
+
+#endif

include/leo_firmware/sensors/gps.h

@@ -1,7 +1,7 @@
 #ifndef LEO_FIRMWARE_INCLUDE_SENSORS_GPS_H_
 #define LEO_FIRMWARE_INCLUDE_SENSORS_GPS_H_
 
-#include <hFramework.h>
+#include <hSerial.h>
 
 struct gga {
   int time;

include/leo_firmware/sensors/imu.h

@@ -1,11 +1,14 @@
 #ifndef LEO_FIRMWARE_INCLUDE_SENSORS_IMU_H_
 #define LEO_FIRMWARE_INCLUDE_SENSORS_IMU_H_
 
-#include <leo_firmware/sensors/imu/MPU9250.h>
+#include <II2C.h>
+#include <hMutex.h>
+
+#include <MPU9250/MPU9250.h>
 
 class IMU {
  public:
-  explicit IMU(hFramework::hI2C &i2c) : mpu_(i2c) {}
+  explicit IMU(hFramework::II2C &i2c) : mpu_(i2c) {}
 
   void init();
   void update();