odometry_serial.h

#pragma once
 
// These are required for Eigen to compile
// https://www.vexforum.com/t/eigen-integration-issue/61474/5
#undef __ARM_NEON__
#undef __ARM_NEON
#include <Eigen/Dense>
 
#include "core/subsystems/custom_encoder.h"
#include "core/subsystems/odometry/odometry_base.h"
#include "core/utils/math_util.h"
 
#include "core/utils/math/geometry/pose2d.h"

class OdometrySerial : public OdometryBase {
  public:
    
    OdometrySerial(
      bool is_async, bool calc_vel_acc_on_brain, Pose2d initial_pose, Pose2d sensor_offset, int32_t port,
      int32_t baudrate
    );
 
    void send_config(const Pose2d &initial_pose, const Pose2d &sensor_offset, const bool &calc_vel_acc_on_brain);
 
    int background_task(void *ptr);

    Pose2d update() override;
 
    
    void set_position(const Pose2d &new_pose) override;
 
    int receive_cobs_packet(uint32_t port, uint8_t *buffer, size_t buffer_size);
 
    Pose2d get_position(void) override;
 
    Pose2d get_pose2d(void);
 
    size_t cobs_decode(const uint8_t *buffer, size_t length, void *data);
 
    size_t cobs_encode(const void *data, size_t length, uint8_t *buffer);
 
    double get_speed() override;
 
    double get_accel() override;
 
  private:
    int32_t _port;
 
    bool calc_vel_acc_on_brain;
 
    Pose2d pose;
 
    Pose2d pose_offset;
 
    double speed;
    double accel;
    double ang_speed_deg;
    double ang_accel_deg;
};