fix(at128): allow negative angles in elevation point field to play …

…nicely with visualization
tier4 · Sep 30, 2024 · a1e184e · a1e184e
1 parent 4f54c3b
commit a1e184e
Showing 1 changed file with 13 additions and 9 deletions.
diff --git a/...clude/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/...clude/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp
@@ -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]};