amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

begin using mpu driver lib

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-15 10:37:22 -08:00
parent f34c32a19e
commit 46e86e05d0
11 changed files with 173 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
.gitmodules vendored
View File

@ -1,3 +1,12 @@
[submodule "components/nanopb/nanopb"] [submodule "components/nanopb/nanopb"]
path = components/nanopb/nanopb path = components/nanopb/nanopb
url = https://github.com/nanopb/nanopb.git url = https://github.com/nanopb/nanopb.git
[submodule "components/u8g2"]
path = components/u8g2
url = https://github.com/olikraus/u8g2.git
[submodule "MPU-driver"]
path = components/MPU-driver
url = https://github.com/natanaeljr/esp32-MPU-driver.git
[submodule "components/I2Cbus"]
path = components/I2Cbus
url = https://github.com/natanaeljr/esp32-I2Cbus.git

1
components/I2Cbus Submodule

@ -0,0 +1 @@
Subproject commit 5afed1c1db9d87bf4efb79f76a0dbc0f93771bc6

1
components/MPU-driver Submodule

@ -0,0 +1 @@
Subproject commit 82a609236b7773ceaafc11f817ca117371314ce2

7
components/mpu_driver/CMakeLists.txt
View File

@ -1,7 +0,0 @@
set(COMPONENT_SRCS
"mpu_driver.c")
set(COMPONENT_ADD_INCLUDEDIRS ".")
register_component()
component_compile_options("-Werror=incompatible-pointer-types")

18
components/mpu_driver/mpu_driver.c
View File

@ -1,18 +0,0 @@
#include "mpu_driver.h"
typedef struct mpu_s {
} mpu_t;
esp_err_t mpu_init(mpu_config_t *config, mpu_t **hndl_out) {
mpu_t *hndl = malloc(sizeof(mpu_t));
if (!hndl) return ESP_ERR_NO_MEM;
*hndl_out = hndl;
return ESP_OK;
}
esp_err_t mpu_free(mpu_t *hndl) {
free(hndl);
return ESP_OK;
}

11
components/mpu_driver/mpu_driver.h
View File

@ -1,11 +0,0 @@
#pragma once
#include <esp_err.h>
typedef struct mpu_config_s {
} mpu_config_t;
typedef struct mpu_s *mpu_hndl_t;
esp_err_t mpu_init(mpu_config_t *config, mpu_hndl_t *hndl_out);
esp_err_t mpu_free(mpu_hndl_t hndl);

4
main/CMakeLists.txt
View File

@ -7,12 +7,12 @@ nanopb_generate(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
list(APPEND COMPONENT_SRCS "ugv_main.cc" list(APPEND COMPONENT_SRCS "ugv_main.cc"
"ugv_comms.c" "ugv_comms.c"
"ugv_io.c" "ugv_io.cc"
"u8g2_esp32_hal.c" "u8g2_esp32_hal.c"
"Print.cpp" "Print.cpp"
${PROTO_SRCS}) ${PROTO_SRCS})
set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb" "mpu_driver") set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb" "MPU-driver")
register_component() register_component()

45
main/ugv_io.c
View File

@ -1,45 +0,0 @@
#include "ugv_io.h"
#include <driver/gpio.h>
#include <driver/mcpwm.h>
#include <math.h>
#define IO_MCPWM_UNIT MCPWM_UNIT_0
#define IO_MOTOR_LEFT_PWM 0
#define IO_MOTOR_LEFT_DIR 0
#define IO_MOTOR_RIGHT_PWM 0
#define IO_MOTOR_RIGHT_DIR 0
typedef struct ugv_io_s {
} ugv_io_t;
static ugv_io_t io;
void ugv_io_init() {
gpio_set_direction(IO_MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT);
gpio_set_direction(IO_MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT);
gpio_set_direction(IO_MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT);
gpio_set_direction(IO_MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT);
mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0A, IO_MOTOR_LEFT_PWM);
mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0B, IO_MOTOR_RIGHT_PWM);
mcpwm_config_t mcpwm_config;
mcpwm_config.frequency = 20000; // 20khz
mcpwm_config.cmpr_a = 0;
mcpwm_config.cmpr_b = 0;
mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // for symmetric pwm
mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0; // active high
mcpwm_init(IO_MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config);
}
void ugv_io_write_outputs(ugv_outputs_t *outputs) {
mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
fabs(outputs->left_motor));
mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B,
fabs(outputs->right_motor));
bool left_dir, right_dir;
left_dir = outputs->left_motor < 0.f;
right_dir = outputs->right_motor < 0.f;
gpio_set_level(IO_MOTOR_LEFT_DIR, left_dir);
gpio_set_level(IO_MOTOR_RIGHT_DIR, right_dir);
}

94
main/ugv_io.cc Normal file
View File

@ -0,0 +1,94 @@
#include "ugv_io.hh"
#include <driver/gpio.h>
#include <driver/mcpwm.h>
#include <esp_log.h>
#include <math.h>
#include "MPU.hpp"
#include "mpu/math.hpp"
namespace ugv {
namespace io {
static constexpr mcpwm_unit_t MCPWM_UNIT = MCPWM_UNIT_0;
static constexpr gpio_num_t MOTOR_LEFT_PWM = GPIO_NUM_0;
static constexpr gpio_num_t MOTOR_LEFT_DIR = GPIO_NUM_0;
static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_0;
static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_0;
static constexpr mpud::accel_fs_t MPU_ACCEL_FS = mpud::ACCEL_FS_2G;
static constexpr mpud::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS;
static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
static const char *TAG = "ugv_io";
IO io;
IO::IO() { mpu_ = new mpud::MPU(); }
IO::~IO() {
delete mpu_;
}
void IO::Init() {
esp_err_t ret;
gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT);
gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT);
gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT);
gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT);
mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM);
mcpwm_gpio_init(MCPWM_UNIT, MCPWM0B, MOTOR_RIGHT_PWM);
mcpwm_config_t mcpwm_config;
mcpwm_config.frequency = 20000; // 20khz
mcpwm_config.cmpr_a = 0;
mcpwm_config.cmpr_b = 0;
mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // for symmetric pwm
mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0; // active high
mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config);
do {
ret = mpu_->testConnection();
if (ret != ESP_OK) {
ESP_LOGE(TAG, "MPU not connected");
break;
}
ret = mpu_->initialize();
if (ret != ESP_OK) {
ESP_LOGE(TAG, "MPU initialization error");
break;
}
mpu_->setAccelFullScale(MPU_ACCEL_FS);
mpu_->setGyroFullScale(MPU_GYRO_FS);
ESP_LOGI(TAG, "MPU initialized");
} while (0);
} // namespace io
void IO::ReadInputs(Inputs &inputs) {
esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error reading MPU");
}
inputs.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
inputs.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
inputs.mag.x = ((float)mag_.x) * MPU_MAG_TO_FLUX;
inputs.mag.y = ((float)mag_.y) * MPU_MAG_TO_FLUX;
inputs.mag.z = ((float)mag_.z) * MPU_MAG_TO_FLUX;
}
void IO::WriteOutputs(const Outputs &outputs) {
mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
fabs(outputs.left_motor));
mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B,
fabs(outputs.right_motor));
bool left_dir, right_dir;
left_dir = outputs.left_motor < 0.f;
right_dir = outputs.right_motor < 0.f;
gpio_set_level(MOTOR_LEFT_DIR, left_dir);
gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
}
}; // namespace io
}; // namespace ugv

44
main/ugv_io.h
View File

@ -1,44 +0,0 @@
#pragma once
#include <stdint.h>
typedef enum gps_fix_qual_e {
GPS_FIX_INVALID = 0, // invalid gps fix
GPS_FIX_GPS = 1, // GPS fix
GPS_FIX_DGPS = 2, // differential GPS fix
GPS_FIX_PPS = 3, // PPS fix
GPS_FIX_RTK = 4, // Real Time Kinematic fix
GPS_FIX_FRTK = 5, // Float Real Time Kinematic fix
GPS_FIX_ESTIMATED = 6, // Estimated fix
GPS_FIX_MANUAL = 7, // Manual fix
GPS_FIX_SIMULATED = 8, // Simulated fix
} gps_fix_qual_t;
typedef struct ugv_inputs_s {
float gyro_x; // degrees
float gyro_y; // degrees
float gyro_z; // degrees
float accel_x; // gs
float accel_y; // gs
float accel_z; // gs
float mag_x; // flux density uT
float mag_y; // flux density uT
float mag_z; // flux density uT
gps_fix_qual_t gps_fix_quality;
uint8_t gps_num_satellites;
double gps_lat; // degrees +/-
double gps_long; // degrees +/-
double gps_altitude; // meters
} ugv_inputs_t;
typedef struct ugv_outputs_s {
float left_motor; // left motor power -1.0 to +1.0
float right_motor; // right motor power -1.0 to +1.0
} ugv_outputs_t;
void ugv_io_init();
void ugv_io_read_inputs(ugv_inputs_t *inputs);
void ugv_io_write_outputs(ugv_outputs_t *outputs);

66
main/ugv_io.hh Normal file
View File

@ -0,0 +1,66 @@
#pragma once
#include <stdint.h>
#include <MPU.hpp>
namespace ugv {
namespace io {
using mpud::float_axes_t;
enum GpsFixQual {
GPS_FIX_INVALID = 0, // invalid gps fix
GPS_FIX_GPS = 1, // GPS fix
GPS_FIX_DGPS = 2, // differential GPS fix
GPS_FIX_PPS = 3, // PPS fix
GPS_FIX_RTK = 4, // Real Time Kinematic fix
GPS_FIX_FRTK = 5, // Float Real Time Kinematic fix
GPS_FIX_ESTIMATED = 6, // Estimated fix
GPS_FIX_MANUAL = 7, // Manual fix
GPS_FIX_SIMULATED = 8, // Simulated fix
};
struct GpsInfo {
GpsFixQual fix_quality;
uint8_t num_satellites;
double latitude; // degrees +/-
double longitude; // degrees +/-
double altitude; // meters
};
struct Inputs {
// G's
float_axes_t accel;
// degrees/s
float_axes_t gyro_rate;
// flux density uT
float_axes_t mag;
GpsInfo gps_info;
};
struct Outputs {
float left_motor; // left motor power -1.0 to +1.0
float right_motor; // right motor power -1.0 to +1.0
};
class IO {
public:
explicit IO();
~IO();
void Init();
void ReadInputs(Inputs &inputs);
void WriteOutputs(const Outputs &outputs);
private:
mpud::MPU * mpu_;
mpud::raw_axes_t accel_, gyro_, mag_;
};
extern IO io;
} // namespace io
} // namespace ugv