diff --git a/include/pros/gps.h b/include/pros/gps.h index 1b2e7e7b..917f6938 100644 --- a/include/pros/gps.h +++ b/include/pros/gps.h @@ -188,6 +188,91 @@ double gps_get_error(uint8_t port); */ gps_status_s_t gps_get_status(uint8_t port); +/** + * Gets the X position in meters of the robot relative to the starting position. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS port number from 1-21 + * + * \return The X position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ +double gps_get_x_position(uint8_t port); + +/** + * Gets the Y position in meters of the robot relative to the starting position. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS port number from 1-21 + * + * \return The Y position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ +double gps_get_y_position(uint8_t port); + +/** + * Gets the pitch of the GPS in degrees relative to the starting orientation. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS port number from 1-21 + * + * \return The pitch in (-90,90] degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ +double gps_get_pitch(uint8_t port); + +/** + * Gets the roll of the GPS in degrees relative to the starting orientation. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS port number from 1-21 + * + * \return The roll in (-180,180] degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ +double gps_get_roll(uint8_t port); + +/** + * Gets the yaw of the GPS in degrees relative to the starting orientation. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS port number from 1-21 + * + * \return The yaw in (-180,180] degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ +double gps_get_yaw(uint8_t port); + /** * Get the heading in [0,360) degree values. * diff --git a/include/pros/gps.hpp b/include/pros/gps.hpp index fce40c2a..f25bd608 100644 --- a/include/pros/gps.hpp +++ b/include/pros/gps.hpp @@ -173,6 +173,81 @@ class Gps { */ virtual pros::c::gps_status_s_t get_status() const; + /** + * Gets the X position in meters of the robot relative to the starting position. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * + * \return The X position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ + virtual double get_x_position() const; + + /** + * Gets the Y position in meters of the robot relative to the starting position. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * + * \return The Y position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ + virtual double get_y_position() const; + + /** + * Gets the pitch of the GPS in degrees relative to the starting orientation. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * + * \return The pitch in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ + virtual double get_pitch() const; + + /** + * Gets the roll of the GPS in degrees relative to the starting orientation. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * + * \return The roll in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ + virtual double get_roll() const; + + /** + * Gets the yaw of the GPS in degrees relative to the starting orientation. + * + * 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 a GPS + * EAGAIN - The sensor is still calibrating + * + * + * \return The yaw in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ + virtual double get_yaw() const; + /** * Get the heading in [0,360) degree values. * diff --git a/src/devices/vdml_gps.c b/src/devices/vdml_gps.c index 93b15500..32d044de 100644 --- a/src/devices/vdml_gps.c +++ b/src/devices/vdml_gps.c @@ -87,6 +87,51 @@ gps_status_s_t gps_get_status(uint8_t port) { return_port(port - 1, rtv); } +double gps_get_x_position(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = PROS_ERR_F; + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + rtv = data.position_x; + return_port(port - 1, rtv); +} + +double gps_get_y_position(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = PROS_ERR_F; + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + rtv = data.position_y; + return_port(port - 1, rtv); +} + +double gps_get_pitch(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = PROS_ERR_F; + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + rtv = data.pitch; + return_port(port - 1, rtv); +} + +double gps_get_roll(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = PROS_ERR_F; + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + rtv = data.roll; + return_port(port - 1, rtv); +} + +double gps_get_yaw(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = PROS_ERR_F; + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + rtv = data.yaw; + return_port(port - 1, rtv); +} + double gps_get_heading(uint8_t port) { claim_port_f(port - 1, E_DEVICE_GPS); double rtv = vexDeviceGpsDegreesGet(device->device_info); @@ -101,7 +146,7 @@ double gps_get_heading_raw(uint8_t port) { double gps_get_rotation(uint8_t port) { claim_port_f(port - 1, E_DEVICE_GPS); - double rtv = vexDeviceGpsRotationGet(device->device_info); + double rtv = vexDeviceGpsHeadingGet(device->device_info); return_port(port - 1, rtv); } diff --git a/src/devices/vdml_gps.cpp b/src/devices/vdml_gps.cpp index bc0d4e85..df3d3efd 100644 --- a/src/devices/vdml_gps.cpp +++ b/src/devices/vdml_gps.cpp @@ -43,6 +43,26 @@ pros::c::gps_status_s_t Gps::get_status() const { return pros::c::gps_get_status(_port); } +double Gps::get_x_position() const { + return pros::c::gps_get_x_position(_port); +} + +double Gps::get_y_position() const { + return pros::c::gps_get_y_position(_port); +} + +double Gps::get_pitch() const { + return pros::c::gps_get_pitch(_port); +} + +double Gps::get_roll() const { + return pros::c::gps_get_roll(_port); +} + +double Gps::get_yaw() const { + return pros::c::gps_get_yaw(_port); +} + double Gps::get_heading() const { return pros::c::gps_get_heading(_port); }