start adding ugv_io

main/CMakeLists.txt

@@ -2,17 +2,17 @@ include(${CMAKE_CURRENT_LIST_DIR}/../components/nanopb/functions.cmake)
 
 set(PB_OUT ${CMAKE_CURRENT_SOURCE_DIR}/pb_out)
 
+make_directory(${PB_OUT})
 nanopb_generate(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
 
 list(APPEND COMPONENT_SRCS "ugv_main.c"
     "ugv_comms.c"
+    "ugv_io.c"
     "u8g2_esp32_hal.c"
     ${PROTO_SRCS})
 set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
 set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb")
 
-make_directory(${PB_OUT})
-
 register_component()
 
 component_compile_options("-Werror=incompatible-pointer-types")

main/ugv_io.c

@@ -0,0 +1,45 @@
+#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);
+}

main/ugv_io.h

@@ -0,0 +1,44 @@
+#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);