fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 01:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

RoverPositionController: remove some unused stuff

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-03-02 20:41:00 +01:00
parent feec8b2036
commit 9a038281c5
5 changed files with 1 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_pos_control/RoverPositionControl.cpp
+1 -13
View File
@@ -45,8 +45,6 @@
#include "RoverPositionControl.hpp"
#include <lib/geo/geo.h>
#define ACTUATOR_PUBLISH_PERIOD_MS 4
using namespace matrix;
/**
@@ -266,11 +264,6 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
(double)curr_wp(0), (double)curr_wp(1)); // pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
//PX4_INFO("Setpoint type %d", (int) pos_sp_triplet.current.type );
//PX4_INFO(" State machine state %d", (int) _pos_ctrl_state);
//PX4_INFO(" Setpoint Lat %f, Lon %f", (double) curr_wp(0), (double)curr_wp(1));
//PX4_INFO(" Distance to target %f", (double) dist_target);
switch (_pos_ctrl_state) {
case GOTO_WAYPOINT: {
if (dist_target < _param_nav_loiter_rad.get()) {
@@ -305,8 +298,6 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
if (dist_between_waypoints > 0) {
_pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
}
//PX4_INFO(" Distance between prev and curr waypoints %f", (double)dist_between_waypoints);
}
break;
@@ -422,8 +413,6 @@ RoverPositionControl::Run()
_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
_global_local_alt0 = _local_pos.ref_alt;
}
// Convert Local setpoints to global setpoints
@@ -435,7 +424,6 @@ RoverPositionControl::Run()
_trajectory_setpoint.position[0], _trajectory_setpoint.position[1],
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.position[2];
_pos_sp_triplet.current.valid = true;
}
@@ -451,7 +439,7 @@ RoverPositionControl::Run()
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
//TODO: check if radius makes sense here
float turn_distance = _param_l1_distance.get(); //_gnd_control.switch_distance(100.0f);
float turn_distance = _param_l1_distance.get();
// publish status
position_controller_status_s pos_ctrl_status{};