Skip to content

Commit

Permalink
Added survey in data
Browse files Browse the repository at this point in the history
  • Loading branch information
@flybrianfly
flybrianfly committed Feb 24, 2022
1 parent fbf7490 commit 63f50a6
Show file tree
Hide file tree
Showing 3 changed files with 85 additions and 38 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ Methods are provided in this library for automatically configuring the receiver

## Methods

**Ubx(HardwareSerial* bus)** Creates a Ubx object. This constructor is used for the serial communication interface and a pointer to the serial bus object is passed to the constructor.
**Ubx(HardwareSerial* bus)** Creates a Ubx object. This constructor is used for the serial communication interface and a pointer to the serial bus object is passed to the constructor.

```C++
bfs::Ubx ubx(&Serial1);
Expand Down
101 changes: 65 additions & 36 deletions src/ubx.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,10 +101,18 @@ bool Ubx::Read() {
}
break;
}
case UBX_NAV_SVIN_ID_: {
if (rx_msg_.len == ubx_nav_svin_.len) {
memcpy(&ubx_nav_svin_.payload, rx_msg_.payload, rx_msg_.len);
svin_data_ = true;
}
break;
}
case UBX_NAV_RELPOSNED_ID_: {
if (rx_msg_.len == ubx_nav_rel_pos_ned_.len) {
memcpy(&ubx_nav_rel_pos_ned_.payload, rx_msg_.payload,
rx_msg_.len);
rel_pos_data_ = true;
}
break;
}
Expand Down Expand Up @@ -288,42 +296,63 @@ void Ubx::ProcessNavData() {
}
}
/* Relative position */
rel_pos_avail_ = ubx_nav_rel_pos_ned_.payload.flags & 0x04;
rel_pos_moving_baseline_ = ubx_nav_rel_pos_ned_.payload.flags & 0x20;
rel_pos_ref_pos_miss_ = ubx_nav_rel_pos_ned_.payload.flags & 0x40;
rel_pos_ref_obs_miss_ = ubx_nav_rel_pos_ned_.payload.flags & 0x80;
rel_pos_heading_valid_ = ubx_nav_rel_pos_ned_.payload.flags & 0x100;
rel_pos_norm_ = ubx_nav_rel_pos_ned_.payload.flags & 0x200;
if (rel_pos_avail_) {
rel_pos_ned_m_[0] =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_n) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_n) * 1e-2) *
1e-2;
rel_pos_ned_m_[1] =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_e) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_e) * 1e-2) *
1e-2;
rel_pos_ned_m_[2] =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_d) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_d) * 1e-2) *
1e-2;
rel_pos_len_m_ =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_length) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_length) *
1e-2) * 1e-2;
rel_pos_heading_deg_ =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.rel_pos_heading) /
100000.0f;
rel_pos_ned_acc_m_[0] =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_n) / 10000.0f;
rel_pos_ned_acc_m_[1] =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_e) / 10000.0f;
rel_pos_ned_acc_m_[2] =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_d) / 10000.0f;
rel_pos_len_acc_m_ =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_length) / 10000.0f;
rel_pos_heading_acc_deg_ =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_heading) / 100000.0f;
if (rel_pos_data_) {
rel_pos_avail_ = ubx_nav_rel_pos_ned_.payload.flags & 0x04;
rel_pos_moving_baseline_ = ubx_nav_rel_pos_ned_.payload.flags & 0x20;
rel_pos_ref_pos_miss_ = ubx_nav_rel_pos_ned_.payload.flags & 0x40;
rel_pos_ref_obs_miss_ = ubx_nav_rel_pos_ned_.payload.flags & 0x80;
rel_pos_heading_valid_ = ubx_nav_rel_pos_ned_.payload.flags & 0x100;
rel_pos_norm_ = ubx_nav_rel_pos_ned_.payload.flags & 0x200;
if (rel_pos_avail_) {
rel_pos_ned_m_[0] =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_n) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_n) * 1e-2) *
1e-2;
rel_pos_ned_m_[1] =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_e) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_e) * 1e-2) *
1e-2;
rel_pos_ned_m_[2] =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_d) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_d) * 1e-2) *
1e-2;
rel_pos_len_m_ =
(static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_length) +
static_cast<double>(ubx_nav_rel_pos_ned_.payload.rel_pos_hp_length) *
1e-2) * 1e-2;
rel_pos_heading_deg_ =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.rel_pos_heading) /
100000.0f;
rel_pos_ned_acc_m_[0] =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_n) / 10000.0f;
rel_pos_ned_acc_m_[1] =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_e) / 10000.0f;
rel_pos_ned_acc_m_[2] =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_d) / 10000.0f;
rel_pos_len_acc_m_ =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_length) / 10000.0f;
rel_pos_heading_acc_deg_ =
static_cast<float>(ubx_nav_rel_pos_ned_.payload.acc_heading) /
100000.0f;
}
}
/* Survey in data */
if (svin_data_) {
svin_dur_s_ = ubx_nav_svin_.payload.dur;
svin_ecef_m_[0] = (static_cast<double>(ubx_nav_svin_.payload.mean_x) +
static_cast<double>(ubx_nav_svin_.payload.mean_x_hp)
* 1e-2) * 1e-2;
svin_ecef_m_[1] = (static_cast<double>(ubx_nav_svin_.payload.mean_y) +
static_cast<double>(ubx_nav_svin_.payload.mean_y_hp)
* 1e-2) * 1e-2;
svin_ecef_m_[2] = (static_cast<double>(ubx_nav_svin_.payload.mean_z) +
static_cast<double>(ubx_nav_svin_.payload.mean_z_hp)
* 1e-2) * 1e-2;
svin_acc_m_ = static_cast<float>(ubx_nav_svin_.payload.mean_acc) /
10000.0f;
svin_valid_ = ubx_nav_svin_.payload.valid;
svin_in_progress_ = ubx_nav_svin_.payload.active;
svin_num_obs_ = ubx_nav_svin_.payload.obs;
}
}
bool Ubx::ParseMsg() {
Expand Down
20 changes: 19 additions & 1 deletion src/ubx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,6 +138,16 @@ class Ubx {
inline float rel_pos_heading_acc_rad() const {
return deg2rad(rel_pos_heading_acc_deg_);
}
/* Survey-in data */
inline bool svin_valid() const {return svin_valid_;}
inline bool svin_in_progress() const {return svin_in_progress_;}
uint32_t svin_dur_s() const {return svin_dur_s_;}
inline double svin_ecef_pos_x_m() const {return svin_ecef_m_[0];}
inline double svin_ecef_pos_y_m() const {return svin_ecef_m_[1];}
inline double svin_ecef_pos_z_m() const {return svin_ecef_m_[2];}
inline Eigen::Vector3d svin_ecef_pos_m() const {return svin_ecef_m_;}
inline float svin_ecef_pos_acc_m() const {return svin_acc_m_;}
inline uint32_t svin_num_obs() const {return svin_num_obs_;}

private:
/* Parse messages, return true on valid msg received */
Expand All @@ -148,7 +158,6 @@ class Ubx {
HardwareSerial* bus_;
elapsedMillis t_ms_;
static const uint32_t COMM_TIMEOUT_MS_ = 10000;
bool use_hp_pos_ = false;
/* Max payload bytes supported */
static constexpr std::size_t UBX_MAX_PAYLOAD_ = 1024;
/* Parsing */
Expand All @@ -165,6 +174,9 @@ class Ubx {
std::size_t parser_state_ = 0;
/* Data members */
bool eoe_ = false;
bool use_hp_pos_ = false;
bool svin_data_ = false;
bool rel_pos_data_ = false;
Fix fix_;
bool gnss_fix_ok_, diff_soln_;
bool valid_date_, valid_time_, fully_resolved_, validity_confirmed_;
Expand All @@ -177,6 +189,8 @@ class Ubx {
bool rel_pos_ref_obs_miss_ = false;
bool rel_pos_heading_valid_ = false;
bool rel_pos_norm_ = false;
bool svin_valid_ = false;
bool svin_in_progress_ = false;
int8_t carr_soln_;
int8_t num_sv_;
int8_t month_, day_, hour_, min_, sec_;
Expand All @@ -185,6 +199,7 @@ class Ubx {
int16_t week_;
int32_t nano_;
uint32_t t_acc_ns_;
uint32_t svin_dur_s_, svin_num_obs_;
float alt_msl_m_;
float gnd_spd_mps_;
float track_deg_;
Expand All @@ -193,6 +208,7 @@ class Ubx {
float rel_pos_heading_deg_;
float rel_pos_heading_acc_deg_;
float rel_pos_len_acc_m_;
float svin_acc_m_;
double rel_pos_len_m_;
double tow_s_;
Eigen::Vector3f ecef_vel_mps_;
Expand All @@ -201,6 +217,7 @@ class Ubx {
Eigen::Vector3d ecef_m_;
Eigen::Vector3d llh_;
Eigen::Vector3d rel_pos_ned_m_;
Eigen::Vector3d svin_ecef_m_;
/* Class to compute UBX checksum */
class Checksum {
public:
Expand Down Expand Up @@ -247,6 +264,7 @@ class Ubx {
UbxNavVelecef ubx_nav_vel_ecef_;
UbxNavPvt ubx_nav_pvt_;
UbxNavTimegps ubx_nav_time_gps_;
UbxNavSvin ubx_nav_svin_;
};

} // namespace bfs
Expand Down

0 comments on commit 63f50a6

Please sign in to comment.