Skip to content

Commit

Permalink
🐛Fixed Imu::is_calibrating function for PROS 4 #626 (#629)
Browse files Browse the repository at this point in the history 
* Fixed Imu::is_calibrating function

Updated imu status enums to properly reflect values returned by get_status. Also modifed imu::is_calibrating so it returns the correct value.

* Bugfixes from the pros 3 version

---------

Co-authored-by: noam987 <[email protected]>
Co-authored-by: noam987 <[email protected]>
  • Loading branch information
@Gracelu128 @noam987
3 people authored May 5, 2024
1 parent 0b98d82 commit 7a66f2f
Show file tree
Hide file tree
Showing 4 changed files with 106 additions and 43 deletions.
29 changes: 28 additions & 1 deletion include/pros/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,13 +41,25 @@ namespace pros {
* @brief Indicates IMU status.
*/
typedef enum imu_status_e {
E_IMU_STATUS_READY = 0, // IMU is connected but not currently calibrating
/** The IMU is calibrating */
E_IMU_STATUS_CALIBRATING = 0x01,
E_IMU_STATUS_CALIBRATING = 1,
/** Used to indicate that an error state was reached in the imu_get_status function,\
not that the IMU is necessarily in an error state */
E_IMU_STATUS_ERROR = 0xFF,
} imu_status_e_t;

typedef enum imu_orientation_e {
E_IMU_Z_UP = 0, // IMU has the Z axis UP (VEX Logo facing DOWN)
E_IMU_Z_DOWN = 1, // IMU has the Z axis DOWN (VEX Logo facing UP)
E_IMU_X_UP = 2, // IMU has the X axis UP
E_IMU_X_DOWN = 3, // IMU has the X axis DOWN
E_IMU_Y_UP = 4, // IMU has the Y axis UP
E_IMU_Y_DOWN = 5, // IMU has the Y axis DOWN
E_IMU_ORIENTATION_ERROR = 0xFF // NOTE: used for returning an error from the get_physical_orientation function, not
// that the IMU is necessarily in an error state
} imu_orientation_e_t;

/**
* \struct quaternion_s_t
*/
Expand Down Expand Up @@ -934,6 +946,21 @@ int32_t imu_set_roll(uint8_t port, double target);
*/
int32_t imu_set_yaw(uint8_t port, double target);

/**
* Returns the physical orientation of the IMU
*
* This function uses the following values of errno when an error state is
* reached:
* ENXIO - The given value is not within the range of V5 ports (1-21).
* ENODEV - The port cannot be configured as an Inertial Sensor
*
* \param port
* The V5 Inertial Sensor port number from 1-21
* \returns The orientation of the Inertial Sensor or PROS_ERR if an error occured.
*
*/
imu_orientation_e_t imu_get_physical_orientation(uint8_t port);

/** @} */

/** @} */
Expand Down
21 changes: 19 additions & 2 deletions include/pros/imu.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,9 @@ namespace pros {
*/

enum class ImuStatus {
ready = 0,
/** The IMU is calibrating */
calibrating = 0x01,
calibrating = 19,
/** Used to indicate that an error state was reached in the imu_get_status function,\
not that the IMU is necessarily in an error state */
error = 0xFF,
Expand Down Expand Up @@ -81,7 +82,9 @@ class Imu : public Device {
* }
* \endcode
*/
Imu(const std::uint8_t port) : Device(port, DeviceType::imu){};

Imu(const std::uint8_t port) : Device(port, DeviceType::imu) {};


Imu(const Device& device) : Imu(device.get_port()){};

Expand Down Expand Up @@ -1008,6 +1011,20 @@ class Imu : public Device {
* \endcode
*/
virtual bool is_calibrating() const;
/**
* Returns the physical orientation of the IMU
*
* This function uses the following values of errno when an error state is
* reached:
* ENXIO - The given value is not within the range of V5 ports (1-21).
* ENODEV - The port cannot be configured as an Inertial Sensor
*
* \param port
* The V5 Inertial Sensor port number from 1-21
* \returns The physical orientation of the Inertial Sensor or PROS_ERR if an error occured.
*
*/
virtual imu_orientation_e_t get_physical_orientation() const;

/**
* This is the overload for the << operator for printing to streams
Expand Down
89 changes: 50 additions & 39 deletions src/devices/vdml_imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
*/

#include <errno.h>

#include "pros/imu.h"
#include "v5_api.h"
#include "vdml/registry.h"
Expand All @@ -28,9 +29,9 @@
}

#define IMU_RESET_FLAG_SET_TIMEOUT 1000
#define IMU_RESET_TIMEOUT 3000 // Canonically this should be 2s, but 3s for good margin
#define IMU_RESET_TIMEOUT 3000 // Canonically this should be 2s, but 3s for good margin

typedef struct __attribute__ ((packed)) imu_reset_data {
typedef struct __attribute__((packed)) imu_reset_data {
double heading_offset;
double rotation_offset;
double pitch_offset;
Expand All @@ -56,8 +57,8 @@ int32_t imu_reset(uint8_t port) {
errno = EAGAIN;
return PROS_ERR;
}
device = device; // suppressing compiler warning
} while(!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING));
device = device; // suppressing compiler warning
} while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING));
port_mutex_give(port - 1);
return 1;
}
Expand All @@ -80,8 +81,8 @@ int32_t imu_reset_blocking(uint8_t port) {
errno = EAGAIN;
return PROS_ERR;
}
device = device; // suppressing compiler warning
} while(!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING));
device = device; // suppressing compiler warning
} while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING));
// same concept here, we add a blocking delay for the blocking version to wait
// until the IMU calibrating flag is cleared
do {
Expand All @@ -94,8 +95,8 @@ int32_t imu_reset_blocking(uint8_t port) {
errno = EAGAIN;
return PROS_ERR;
}
device = device; // suppressing compiler warning
} while(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING);
device = device; // suppressing compiler warning
} while (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING);
port_mutex_give(port - 1);
return 1;
}
Expand All @@ -118,16 +119,18 @@ int32_t imu_set_data_rate(uint8_t port, uint32_t rate) {
double imu_get_rotation(uint8_t port) {
claim_port_f(port - 1, E_DEVICE_IMU);
ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F);
double rtn = vexDeviceImuHeadingGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->rotation_offset;
double rtn = vexDeviceImuHeadingGet(device->device_info) +
((imu_data_s_t*)registry_get_device(port - 1)->pad)->rotation_offset;
return_port(port - 1, rtn);
}

double imu_get_heading(uint8_t port) {
claim_port_f(port - 1, E_DEVICE_IMU);
ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F);
double rtn = vexDeviceImuDegreesGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->heading_offset;
double rtn =
vexDeviceImuDegreesGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->heading_offset;
// Restricting value to raw boundaries
return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double) IMU_HEADING_MAX));
return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double)IMU_HEADING_MAX));
}

#define QUATERNION_ERR_INIT \
Expand All @@ -154,7 +157,7 @@ quaternion_s_t imu_get_quaternion(uint8_t port) {
double cp = cos(DEGTORAD * pitch * 0.5);
double sp = sin(DEGTORAD * pitch * 0.5);
double cr = cos(DEGTORAD * roll * 0.5);
double sr = sin(DEGTORAD * roll * 0.5);
double sr = sin(DEGTORAD * roll * 0.5);

rtn.w = cr * cp * cy + sr * sp * sy;
rtn.x = sr * cp * cy - cr * sp * sy;
Expand Down Expand Up @@ -270,9 +273,9 @@ imu_status_e_t imu_get_status(uint8_t port) {
return_port(port - 1, rtn);
}

//Reset Functions:
int32_t imu_tare(uint8_t port){
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
// Reset Functions:
int32_t imu_tare(uint8_t port) {
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
return PROS_ERR;
}
v5_smart_device_s_t* device = registry_get_device(port - 1);
Expand All @@ -287,33 +290,33 @@ int32_t imu_tare(uint8_t port){
return_port(port - 1, PROS_SUCCESS);
}

int32_t imu_tare_euler(uint8_t port){
return imu_set_euler(port, (euler_s_t){0,0,0});
int32_t imu_tare_euler(uint8_t port) {
return imu_set_euler(port, (euler_s_t){0, 0, 0});
}

int32_t imu_tare_heading(uint8_t port){
return imu_set_heading(port, 0);
int32_t imu_tare_heading(uint8_t port) {
return imu_set_heading(port, 0);
}

int32_t imu_tare_rotation(uint8_t port){
return imu_set_rotation(port, 0);
int32_t imu_tare_rotation(uint8_t port) {
return imu_set_rotation(port, 0);
}

int32_t imu_tare_pitch(uint8_t port){
return imu_set_pitch(port, 0);
int32_t imu_tare_pitch(uint8_t port) {
return imu_set_pitch(port, 0);
}

int32_t imu_tare_roll(uint8_t port){
return imu_set_roll(port, 0);
int32_t imu_tare_roll(uint8_t port) {
return imu_set_roll(port, 0);
}

int32_t imu_tare_yaw(uint8_t port){
return imu_set_yaw(port, 0);
int32_t imu_tare_yaw(uint8_t port) {
return imu_set_yaw(port, 0);
}

//Setter Functions:
int32_t imu_set_rotation(uint8_t port, double target){
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
// Setter Functions:
int32_t imu_set_rotation(uint8_t port, double target) {
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
return PROS_ERR;
}
v5_smart_device_s_t* device = registry_get_device(port - 1);
Expand All @@ -325,8 +328,8 @@ int32_t imu_set_rotation(uint8_t port, double target){
return_port(port - 1, PROS_SUCCESS);
}

int32_t imu_set_heading(uint8_t port, double target){
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
int32_t imu_set_heading(uint8_t port, double target) {
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
return PROS_ERR;
}
v5_smart_device_s_t* device = registry_get_device(port - 1);
Expand All @@ -340,8 +343,8 @@ int32_t imu_set_heading(uint8_t port, double target){
return_port(port - 1, PROS_SUCCESS);
}

int32_t imu_set_pitch(uint8_t port, double target){
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
int32_t imu_set_pitch(uint8_t port, double target) {
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
return PROS_ERR;
}
v5_smart_device_s_t* device = registry_get_device(port - 1);
Expand All @@ -355,8 +358,8 @@ int32_t imu_set_pitch(uint8_t port, double target){
return_port(port - 1, PROS_SUCCESS);
}

int32_t imu_set_roll(uint8_t port, double target){
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
int32_t imu_set_roll(uint8_t port, double target) {
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
return PROS_ERR;
}
v5_smart_device_s_t* device = registry_get_device(port - 1);
Expand All @@ -370,8 +373,8 @@ int32_t imu_set_roll(uint8_t port, double target){
return_port(port - 1, PROS_SUCCESS);
}

int32_t imu_set_yaw(uint8_t port, double target){
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
int32_t imu_set_yaw(uint8_t port, double target) {
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
return PROS_ERR;
}
v5_smart_device_s_t* device = registry_get_device(port - 1);
Expand All @@ -385,7 +388,7 @@ int32_t imu_set_yaw(uint8_t port, double target){
return_port(port - 1, PROS_SUCCESS);
}

int32_t imu_set_euler(uint8_t port, euler_s_t target){
int32_t imu_set_euler(uint8_t port, euler_s_t target) {
if (!claim_port_try(port - 1, E_DEVICE_IMU)) {
return PROS_ERR;
}
Expand All @@ -404,3 +407,11 @@ int32_t imu_set_euler(uint8_t port, euler_s_t target){
data->yaw_offset = target.yaw - euler_values.yaw;
return_port(port - 1, PROS_SUCCESS);
}

imu_orientation_e_t imu_get_physical_orientation(uint8_t port) {
imu_status_e_t status = imu_get_status(port);
if (status == E_IMU_STATUS_ERROR) {
return E_IMU_ORIENTATION_ERROR;
}
return (status >> 1) & 7;
}
10 changes: 9 additions & 1 deletion src/devices/vdml_imu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,11 @@ pros::ImuStatus Imu::get_status() const {
}

bool Imu::is_calibrating() const {
return (int)get_status() & (int)(pros::ImuStatus::calibrating);
imu_status_e_t status = pros::c::imu_get_status(_port);
if (status == E_IMU_STATUS_ERROR) {
return false;
}
return status & E_IMU_STATUS_CALIBRATING;
}

std::int32_t Imu::tare_heading() const {
Expand Down Expand Up @@ -131,6 +135,10 @@ std::int32_t Imu::tare() const {
return pros::c::imu_tare(_port);
}

imu_orientation_e_t Imu::get_physical_orientation() const {
return pros::c::imu_get_physical_orientation(_port);
}

Imu Imu::get_imu() {
static int curr_imu_port = 0;
curr_imu_port = curr_imu_port % 21;
Expand Down

0 comments on commit 7a66f2f

Please sign in to comment.