Skip to content

Commit

Permalink
SENS: SF45: Scale the measured distance with pitch and roll, under th…
Browse files Browse the repository at this point in the history
…e assumption that the measured obstacles are vertical walls,
  • Loading branch information
Claudio-Chies committed Dec 20, 2024
1 parent 6196135 commit 2b50b61
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,13 @@
#include <termios.h>
#include <lib/crc/crc.h>
#include <lib/mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>

#include <float.h>

using namespace time_literals;
using matrix::Quatf;
using matrix::Vector3f;

/* Configuration Constants */

Expand Down Expand Up @@ -649,11 +652,22 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
(double)*distance_m);

if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude;

if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
_vehicle_attitude = Quatf(vehicle_attitude.q);
}
}

_scale_dist(*distance_m, scaled_yaw, _vehicle_attitude);

if (_current_bin_dist > _obstacle_distance.max_distance) {
_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
}

hrt_abstime now = hrt_absolute_time();

_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);

_publish_obstacle_msg(now);
Expand Down Expand Up @@ -727,6 +741,20 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
}
}
}
void SF45LaserSerial::_scale_dist(float &distance, const int16_t &yaw, const matrix::Quatf &attitude)
{
const Quatf q_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
const Vector3f forward_vector(1.0f, 0.0f, 0.0f);

const Quatf q_sensor_rotation = attitude * q_sensor;

const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);

float sensor_dist_scale = rotated_sensor_vector.xy().norm();
sensor_dist_scale = math::constrain(sensor_dist_scale, FLT_EPSILON, 1.0f); // limit it to the expected range.
distance = distance * sensor_dist_scale;
}

uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{
uint8_t mapped_sector = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,9 @@
#include <lib/perf/perf_counter.h>

#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/vehicle_attitude.h>

#include "sf45_commands.h"

Expand Down Expand Up @@ -112,7 +114,9 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
bool _crc_valid{false};

void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
void _scale_dist(float &distance, const int16_t &yaw, const matrix::Quatf &attitude);
void _publish_obstacle_msg(hrt_abstime now);
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uint64_t _data_timestamps[BIN_COUNT];


Expand Down Expand Up @@ -141,6 +145,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
int32_t _orient_cfg{0};
uint8_t _previous_bin{0};
uint16_t _current_bin_dist{UINT16_MAX};
matrix::Quatf _vehicle_attitude{};

// end of SF45/B data members

Expand Down

0 comments on commit 2b50b61

Please sign in to comment.