Skip to content

Commit

Permalink
fix(at128): allow negative angles in elevation point field to play …
Browse files Browse the repository at this point in the history
…nicely with visualization (#193)
  • Loading branch information
mojomex authored Oct 1, 2024
1 parent 6677df3 commit a9c24e3
Showing 1 changed file with 13 additions and 9 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -181,19 +181,23 @@ class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
{
int field = findField(block_azimuth);

int32_t elevation =
correction_->elevation[channel_id] +
correction_->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100);
int32_t elevation = correction_->elevation[channel_id] +
correction_->getElevationAdjustV3(channel_id, block_azimuth) *
static_cast<int32_t>(AngleUnit / 100);

// Allow negative angles in the radian value. This makes visualization of this field nicer and
// should have no other mathematical implications in downstream modules.
float elevation_rad = 2.f * elevation * M_PI / MAX_AZIMUTH;
// Then, normalize the integer value to the positive [0, MAX_AZIMUTH] range for array indexing
elevation = (MAX_AZIMUTH + elevation) % MAX_AZIMUTH;

int32_t azimuth =
(block_azimuth + MAX_AZIMUTH - correction_->startFrame[field]) * 2 -
correction_->azimuth[channel_id] +
correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100);
int32_t azimuth = (block_azimuth + MAX_AZIMUTH - correction_->startFrame[field]) * 2 -
correction_->azimuth[channel_id] +
correction_->getAzimuthAdjustV3(channel_id, block_azimuth) *
static_cast<int32_t>(AngleUnit / 100);
azimuth = (MAX_AZIMUTH + azimuth) % MAX_AZIMUTH;

float azimuth_rad = 2.f * azimuth * M_PIf / MAX_AZIMUTH;
float elevation_rad = 2.f * elevation * M_PIf / MAX_AZIMUTH;
float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH;

return {azimuth_rad, elevation_rad, sin_[azimuth],
cos_[azimuth], sin_[elevation], cos_[elevation]};
Expand Down

0 comments on commit a9c24e3

Please sign in to comment.