Skip to content

Commit

Permalink
Split Run() to enhance modularity and readability
Browse files Browse the repository at this point in the history
  • Loading branch information
jonas committed Sep 29, 2024
1 parent 7d21f16 commit d7ade2b
Show file tree
Hide file tree
Showing 2 changed files with 165 additions and 126 deletions.
244 changes: 134 additions & 110 deletions src/modules/vision_target_estimator/VisionTargetEst.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -299,45 +299,27 @@ void VisionTargetEst::update_task_topics()
void VisionTargetEst::Run()
{
if (should_exit()) {
_vehicle_attitude_sub.unregisterCallback();
exit_and_cleanup();
handle_exit();
return;
}

update_task_topics();

// If a new task is available, stop the estimators if they were already running
if (new_task_available()) {
if (_vte_position_enabled && _position_estimator_running) {
stop_position_estimator();
}

if (_vte_orientation_enabled && _orientation_estimator_running) {
stop_orientation_estimator();
}

stop_all_estimators();
}

// No task running, early return
if (_vte_current_task == VisionTargetEstTask::VTE_NO_TASK) {
if (no_active_task()) {
return;
}

// Task is running, check if an estimator must be started or re-started
if ((!_position_estimator_running && _vte_position_enabled)) {

_position_estimator_running = start_position_estimator();
}

if ((!_orientation_estimator_running && _vte_orientation_enabled)) {

_orientation_estimator_running = start_orientation_estimator();
}
start_estimators_if_needed();

// Early return if no estimator is running or activated
if ((!_vte_orientation_enabled || !_orientation_estimator_running) && (!_vte_position_enabled
|| !_position_estimator_running)) {

if (no_estimator_running()) {
if (is_current_task_done()) {
_vte_current_task = VisionTargetEstTask::VTE_NO_TASK;
}
Expand All @@ -347,147 +329,189 @@ void VisionTargetEst::Run()

// Stop estimators once task is completed
if (is_current_task_done()) {
stop_all_estimators();
_vte_current_task = VisionTargetEstTask::VTE_NO_TASK;
return;
}

if (_vte_position_enabled && _position_estimator_running) {
stop_position_estimator();
}
if (estimators_stopped_due_to_timeout()) {
return;
}

if (_vte_orientation_enabled && _orientation_estimator_running) {
stop_orientation_estimator();
}
// Early return checks passed, start filter computations.
perform_estimations();
}

_vte_current_task = VisionTargetEstTask::VTE_NO_TASK;
void VisionTargetEst::handle_exit()
{
_vehicle_attitude_sub.unregisterCallback();
exit_and_cleanup();
}

return;
void VisionTargetEst::stop_all_estimators()
{
if (_vte_position_enabled && _position_estimator_running) {
stop_position_estimator();
}

if (_vte_orientation_enabled && _orientation_estimator_running) {
stop_orientation_estimator();
}
}

void VisionTargetEst::stop_position_estimator()
{

PX4_INFO("Stopping Position Vision Target Estimator.");

if (_vte_position_enabled) {
_vte_position->resetFilter();
}

_position_estimator_running = false;
_vte_position_stop_time = hrt_absolute_time();
}

void VisionTargetEst::stop_orientation_estimator()
{

PX4_INFO("Stopping Orientation Vision Target Estimator.");

if (_vte_orientation_enabled) {
_vte_orientation->resetFilter();
}

_orientation_estimator_running = false;
_vte_orientation_stop_time = hrt_absolute_time();
}

void VisionTargetEst::start_estimators_if_needed()
{

if (!_position_estimator_running && _vte_position_enabled) {
_position_estimator_running = start_position_estimator();
}

if (!_orientation_estimator_running && _vte_orientation_enabled) {
_orientation_estimator_running = start_orientation_estimator();
}

return;
}

bool VisionTargetEst::estimators_stopped_due_to_timeout()
{
bool all_estimators_stopped = true;

// Stop computations if the position estimator timedout
if (_position_estimator_running && _vte_position->has_timed_out()) {
stop_position_estimator();
PX4_INFO("Estimator TIMEOUT, position VTE stopped.");

// Early return if no other estimator is running
if (!_orientation_estimator_running) {
return;
}
} else {
all_estimators_stopped = false;
}

// Stop computations if the position estimator timedout
if (_orientation_estimator_running && _vte_orientation->has_timed_out()) {
stop_orientation_estimator();
PX4_INFO("Estimator TIMEOUT, orientation VTE stopped.");

// Early return if no other estimator is running
if (!_position_estimator_running) {
return;
}
} else {
all_estimators_stopped = false;
}

// Early return checks passed, start filter computations.
// Early return if all estimators are stopped
return all_estimators_stopped;
}

void VisionTargetEst::perform_estimations()
{
perf_begin(_cycle_perf);

localPose local_pose;

const bool local_pose_updated = get_local_pose(local_pose);

/* Update position filter at vte_pos_UPDATE_RATE_HZ */
if (_vte_position_enabled) {
perform_position_update(local_pose, local_pose_updated);
}

matrix::Vector3f gps_pos_offset_ned;
matrix::Vector3f vel_offset;
const bool vel_offset_updated = get_gps_velocity_offset(vel_offset);

matrix::Vector3f vehicle_acc_ned;

/* Downsample acceleration ned */
if (get_input(vehicle_acc_ned, gps_pos_offset_ned, vel_offset, vel_offset_updated)) {
if (_vte_orientation_enabled && has_elapsed(_last_update_yaw, (1_s / vte_yaw_UPDATE_RATE_HZ))) {
perform_orientation_update(local_pose, local_pose_updated);
}

/* If the acceleration has been averaged for too long, early return */
if ((hrt_absolute_time() - _last_acc_reset) > acc_downsample_TIMEOUT_US) {
PX4_INFO("Forced acc downsample reset");
reset_acc_downsample();
return;
}
perf_end(_cycle_perf);
}

_vehicle_acc_ned_sum += vehicle_acc_ned;
_loops_count ++;
void VisionTargetEst::perform_position_update(const localPose &local_pose, const bool local_pose_updated)
{
matrix::Vector3f gps_pos_offset_ned;
matrix::Vector3f vel_offset;
const bool vel_offset_updated = get_gps_velocity_offset(vel_offset);

if (has_elapsed(_last_update_pos, (1_s / vte_pos_UPDATE_RATE_HZ))) {
matrix::Vector3f vehicle_acc_ned;

perf_begin(_cycle_perf_pos);
/* Downsample acceleration ned */
if (get_input(vehicle_acc_ned, gps_pos_offset_ned, vel_offset, vel_offset_updated)) {

if (local_pose_updated) {
_vte_position->set_local_velocity(local_pose.vel_xyz, local_pose.vel_valid, local_pose.timestamp);
_vte_position->set_local_position(local_pose.xyz, local_pose.pos_valid, local_pose.timestamp);
_vte_position->set_range_sensor(local_pose.dist_bottom, local_pose.dist_valid, local_pose.timestamp);
}
/* If the acceleration has been averaged for too long, early return */
if ((hrt_absolute_time() - _last_acc_reset) > acc_downsample_TIMEOUT_US) {
PX4_INFO("Forced acc downsample reset");
reset_acc_downsample();
return;
}

_vte_position->set_gps_pos_offset(gps_pos_offset_ned, _gps_pos_is_offset);
_vehicle_acc_ned_sum += vehicle_acc_ned;
_loops_count ++;

if (vel_offset_updated) {
_vte_position->set_velocity_offset(vel_offset);
}
if (has_elapsed(_last_update_pos, (1_s / vte_pos_UPDATE_RATE_HZ))) {

const matrix::Vector3f vehicle_acc_ned_sampled = _vehicle_acc_ned_sum / _loops_count;
perf_begin(_cycle_perf_pos);

_vte_position->update(vehicle_acc_ned_sampled);
if (local_pose_updated) {
_vte_position->set_local_velocity(local_pose.vel_xyz, local_pose.vel_valid, local_pose.timestamp);
_vte_position->set_local_position(local_pose.xyz, local_pose.pos_valid, local_pose.timestamp);
_vte_position->set_range_sensor(local_pose.dist_bottom, local_pose.dist_valid, local_pose.timestamp);
}

/* Publish downsampled acceleration*/
vehicle_acceleration_s vte_acc_input_report;
vte_acc_input_report.timestamp = hrt_absolute_time();
_vte_position->set_gps_pos_offset(gps_pos_offset_ned, _gps_pos_is_offset);

for (int i = 0; i < 3; i++) {
vte_acc_input_report.xyz[i] = vehicle_acc_ned_sampled(i);
}
if (vel_offset_updated) {
_vte_position->set_velocity_offset(vel_offset);
}

_vte_acc_input_pub.publish(vte_acc_input_report);
const matrix::Vector3f vehicle_acc_ned_sampled = _vehicle_acc_ned_sum / _loops_count;

reset_acc_downsample();
perf_end(_cycle_perf_pos);
}
}
}
_vte_position->update(vehicle_acc_ned_sampled);
publish_acceleration(vehicle_acc_ned_sampled);

/* Update orientation filter at vte_yaw_UPDATE_RATE_HZ */
if (_vte_orientation_enabled && has_elapsed(_last_update_yaw, (1_s / vte_yaw_UPDATE_RATE_HZ))) {
perf_begin(_cycle_perf_yaw);
reset_acc_downsample();

if (local_pose_updated) {
_vte_orientation->set_range_sensor(local_pose.dist_bottom, local_pose.dist_valid);
perf_end(_cycle_perf_pos);
}

_vte_orientation->update();
perf_end(_cycle_perf_yaw);
}

perf_end(_cycle_perf);

}

void VisionTargetEst::stop_position_estimator()
void VisionTargetEst::perform_orientation_update(const localPose &local_pose, const bool local_pose_updated)
{
perf_begin(_cycle_perf_yaw);

PX4_INFO("Stopping Position Vision Target Estimator.");

if (_vte_position_enabled) {
_vte_position->resetFilter();
if (local_pose_updated) {
_vte_orientation->set_range_sensor(local_pose.dist_bottom, local_pose.dist_valid);
}

_position_estimator_running = false;
_vte_position_stop_time = hrt_absolute_time();
_vte_orientation->update();
perf_end(_cycle_perf_yaw);
}

void VisionTargetEst::stop_orientation_estimator()
void VisionTargetEst::publish_acceleration(const matrix::Vector3f &vehicle_acc_ned_sampled)
{
vehicle_acceleration_s vte_acc_input_report;
vte_acc_input_report.timestamp = hrt_absolute_time();

PX4_INFO("Stopping Orientation Vision Target Estimator.");

if (_vte_orientation_enabled) {
_vte_orientation->resetFilter();
for (int i = 0; i < 3; i++) {
vte_acc_input_report.xyz[i] = vehicle_acc_ned_sampled(i);
}

_orientation_estimator_running = false;
_vte_orientation_stop_time = hrt_absolute_time();
_vte_acc_input_pub.publish(vte_acc_input_report);
}

bool VisionTargetEst::get_gps_velocity_offset(matrix::Vector3f &vel_offset_body)
Expand Down
47 changes: 31 additions & 16 deletions src/modules/vision_target_estimator/VisionTargetEst.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,8 +99,39 @@ class VisionTargetEst : public ModuleBase<VisionTargetEst>, ModuleParams, px4::S
static int task_spawn(int argc, char *argv[]);

private:
struct localPose {
bool pos_valid = false;
matrix::Vector3f xyz;

bool vel_valid = false;
matrix::Vector3f vel_xyz;

bool dist_valid = false;
float dist_bottom = 0, f;

bool yaw_valid = false;
float yaw = 0.f;

hrt_abstime timestamp{0};
};

void Run() override;
void updateParams() override;
void handle_exit();
void stop_all_estimators();
void start_estimators_if_needed();
bool estimators_stopped_due_to_timeout();
void perform_estimations();
void perform_position_update(const localPose &local_pose, const bool local_pose_updated);
void perform_orientation_update(const localPose &local_pose, const bool local_pose_updated);
void publish_acceleration(const matrix::Vector3f &vehicle_acc_ned_sampled);

inline bool no_active_task() {return _vte_current_task == VisionTargetEstTask::VTE_NO_TASK;};
inline bool no_estimator_running()
{
return (!_vte_orientation_enabled || !_orientation_estimator_running) && (!_vte_position_enabled
|| !_position_estimator_running);
};

void update_task_topics();
bool new_task_available();
Expand Down Expand Up @@ -155,22 +186,6 @@ class VisionTargetEst : public ModuleBase<VisionTargetEst>, ModuleParams, px4::S
bool _vte_position_enabled{false};
hrt_abstime _last_update_pos{0};

struct localPose {
bool pos_valid = false;
matrix::Vector3f xyz;

bool vel_valid = false;
matrix::Vector3f vel_xyz;

bool dist_valid = false;
float dist_bottom = 0, f;

bool yaw_valid = false;
float yaw = 0.f;

hrt_abstime timestamp{0};
};

matrix::Vector3f _gps_pos_offset;
bool _gps_pos_is_offset;

Expand Down

0 comments on commit d7ade2b

Please sign in to comment.