zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

增加一些仿真测试打印数据

tf_i2c_collis_pre
zbr3550 3 years ago
parent
dca054f85b
commit
a279d3fbdb
2 changed files with 126 additions and 97 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 139
      src/lib/collision_prevention/CollisionPrevention.cpp
  2. 36
      src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

139
src/lib/collision_prevention/CollisionPrevention.cpp
Unescape View File

@ -384,6 +384,15 @@ void @@ -384,6 +384,15 @@ void
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
const Vector2f &curr_vel)
{
bool need_prt = false;
static uint8_t cnt;
cnt+=1;
if (cnt > 100)
{
// need_prt = true;
cnt = 0;
}
//////////////////////////////
_updateObstacleMap();
// read parameters
@ -401,6 +410,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec @@ -401,6 +410,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
const hrt_abstime constrain_time = getTime();
int num_fov_bins = 0;
if (need_prt)
{
printf("[clc]setpoint_length:%.2f,contime:%d,obtime:%d,delt:%d\n",(double)setpoint_length,constrain_time , _obstacle_map_body_frame.timestamp,constrain_time - _obstacle_map_body_frame.timestamp);
}
if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
if (setpoint_length > 0.001f) {
@ -416,6 +429,11 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec @@ -416,6 +429,11 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
if (need_prt)
{
printf("[clc]setpoint_dir = %.2f,%.2f\n",(double)setpoint_dir(0),(double)setpoint_dir(1));
}
// limit speed for safe flight
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message
@ -483,6 +501,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec @@ -483,6 +501,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
}
setpoint = setpoint_dir * vel_max;
if (need_prt)
{
printf("[clc]setpoint = %.2f,%.2f\n",(double)setpoint(0),(double)setpoint(1));
}
}
} else {
@ -509,10 +531,21 @@ void @@ -509,10 +531,21 @@ void
CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
const Vector2f &curr_vel)
{
bool need_prt = false;
static uint8_t cnt;
cnt+=1;
if (cnt > 100)
{
// need_prt = true;
cnt = 0;
}
/////////////////////////////////////
//calculate movement constraints based on range data
Vector2f new_setpoint = original_setpoint;
_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
//warn user if collision prevention starts to interfere
bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
|| new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
@ -521,6 +554,10 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max @@ -521,6 +554,10 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
_interfering = currently_interfering;
if (need_prt)
{
printf("setpoint:%.2f,%.2f,%d\n",(double)original_setpoint(0),(double)new_setpoint(0),_interfering);
}
// publish constraints
collision_constraints_s constraints{};
constraints.timestamp = getTime();
@ -535,51 +572,41 @@ void @@ -535,51 +572,41 @@ void
CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_speed, const float &curr_pos,
const float &curr_vel)
{
static uint64_t delay_1s = hrt_absolute_time();
bool need_prt = false;
if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
{
need_prt = true;
delay_1s = hrt_absolute_time();
}
//calculate movement constraints based on range data
float setpoint = original_setpoint;
/******************************************************************/
// _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
// read parameters
static float min_distance;
static float max_distance;
static float min_distance = 0.2f;
static float max_distance = 12.0f;
static float up_distance;
static uint64_t _data_h_timestamps;
static uint64_t _dist_timestamps;
bool need_prt = false;
static uint8_t cnt;
cnt+=1;
if (cnt > 100)
{
// need_prt = true;
cnt = 0;
}
// 获取测距距离值相关
for (auto &dist_sens_sub : _distance_sensor_subs) {
distance_sensor_s distance_sensor;
if (dist_sens_sub.update(&distance_sensor)) {
// consider only instances with valid data and orientations useful for collision prevention
// 距离值有效，并且是朝上的距离
// if (need_prt)
// printf("0.1:ts:%lld,rot:%d\n",distance_sensor.timestamp,distance_sensor.orientation);
if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
(distance_sensor.orientation == distance_sensor_s::ROTATION_FORWARD_FACING)) {
// (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_FACING)) {
// update message description
_dist_timestamps = distance_sensor.timestamp;
max_distance = math::max(max_distance,(distance_sensor.max_distance ));
min_distance = math::min(min_distance,(distance_sensor.min_distance ));
max_distance = math::min(max_distance,(distance_sensor.max_distance ));
min_distance = math::max(min_distance,(distance_sensor.min_distance ));
up_distance = distance_sensor.current_distance;
// if (need_prt)
// printf("1:dist up:%.2f,max:%.2f,min:%.2f\n",(double)up_distance,(double)max_distance,(double)min_distance);
}
}
}
// 获取一些参数设置
const float col_prev_d = _param_cp_dist.get();
@ -588,12 +615,16 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s @@ -588,12 +615,16 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
const float xy_p = _param_mpc_xy_p.get();
const float max_jerk = _param_mpc_jerk_max.get();
const float max_accel = _param_mpc_acc_hor.get();
const float setpoint_length = -setpoint;
const hrt_abstime constrain_time = getTime();
if ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US) {
if (need_prt)
{
printf("dist:%.2f,dt:%d,%.2f,%.2f\n",(double)up_distance,constrain_time-_dist_timestamps,(double)min_distance,(double)max_distance);
}
if ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US && up_distance > min_distance && up_distance < max_distance) {
if (setpoint_length > 0.001f) {
float vel_max = setpoint_length;
// 最小停留距离，测距最小值或者参数设置停留值
@ -606,10 +637,8 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s @@ -606,10 +637,8 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
const float distance = up_distance;
const float max_range = max_distance + 0.005f;
if (need_prt)
printf("2:age:%lld,dist:%.2f,max:%.2f\n",data_age,(double)distance,(double)max_range);
if (distance > min_distance && distance < UINT16_MAX * 0.01f) {
if (distance > min_distance && distance < UINT16_MAX * 0.01f) {
// calculate max allowed velocity with a P-controller (same gain as in the position controller)
const float curr_vel_parallel = math::max(0.f, curr_vel);
float delay_distance = curr_vel_parallel * col_prev_dly;
@ -626,55 +655,41 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s @@ -626,55 +655,41 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s
vel_max = math::min(vel_max, vel_max_bin);
}
if (need_prt)
printf("3:parallel:%.2f,posctrl:%.2f,stop_d:%.2f,vmax:%.2f\n",(double)curr_vel_parallel,(double)vel_max_posctrl,(double)stop_distance,(double)vel_max);
}else if (distance >= UINT16_MAX * 0.01f ) {
if (!move_no_data || (move_no_data)) {
if (!move_no_data) {
vel_max = 0.f;
}
}
setpoint = -vel_max;
if (need_prt)
printf("4:vmax:%.2f\n",(double)vel_max);
}
} else {
//allow no movement
float vel_max = 0.f;
setpoint = setpoint * vel_max;
// if distance data is stale, switch to Loiter
if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
if ((constrain_time - _dist_timestamps) > TIMEOUT_HOLD_US
&& getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
_publishVehicleCmdDoLoiter();
}
_last_timeout_warning = getTime();
}
if (need_prt)
printf("5:setpoint:%.2f,vmax:%.2f\n", (double)setpoint,(double)vel_max);
// float vel_max = 0.f;
// setpoint = setpoint * vel_max;
// // if distance data is stale, switch to Loiter
// if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
// if ((constrain_time - _dist_timestamps) > TIMEOUT_HOLD_US
// && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
// _publishVehicleCmdDoLoiter();
// }
// _last_timeout_warning = getTime();
// }
}
_data_h_timestamps = getTime();
/******************************************************************/
//warn user if collision prevention starts to interfere
bool currently_interfering = (setpoint < original_setpoint - 0.05f * max_speed
|| setpoint > original_setpoint + 0.05f * max_speed);
_interfering = currently_interfering;
original_setpoint = setpoint;
if (need_prt)
printf("6:intf:%d,setpoint:%.2f\n", _interfering,(double)original_setpoint);
// if(_interfering){
// static uint8_t not_cnt;
// not_cnt += 1;
// if(not_cnt > 10){
// not_cnt = 0;
// printf("notic:%.2f\n",(double)original_setpoint);
// }
// }
}
void CollisionPrevention::_publishVehicleCmdDoLoiter()

36
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
Unescape View File

@ -106,23 +106,23 @@ void FlightTaskManualAltitude::_scaleSticks() @@ -106,23 +106,23 @@ void FlightTaskManualAltitude::_scaleSticks()
const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
/////////////////////////////////////////////////////////////////////
float vel_z = vel_max_z * _sticks.getPositionExpo()(2);
// collision prevention
float vel_z ;
if (_collision_prevention.is_active()) {
vel_z = vel_max_z * _sticks.getPositionExpo()(2);
_collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2));
_velocity_setpoint(2) = vel_z;
// static uint8_t cnt;
// cnt+=1;
// if (cnt > 100)
// {
// printf(" vx:%.3f,vy:%.3f,vz:%.3f,vz2:%.3f\n",(double)_velocity_setpoint(0),(double)_velocity_setpoint(1),(double)_velocity_setpoint(2),(double)vel_z);
// printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2));
// cnt = 0;
// }
}
static uint8_t cnt;
cnt+=1;
if (cnt > 50)
{
printf(" vf:%.3f,vb:%.3f\n",(double)_velocity_setpoint(2), (double)vel_z);
cnt = 0;
}
_velocity_setpoint(2) = vel_z;
}
float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@ -377,6 +377,20 @@ void FlightTaskManualAltitude::_updateSetpoints() @@ -377,6 +377,20 @@ void FlightTaskManualAltitude::_updateSetpoints()
_acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
Vector2f setpoint = _acceleration_setpoint.xy();
if (_collision_prevention.is_active()) {
_collision_prevention.modifySetpoint(setpoint, 1.0f, _position.xy(), _velocity.xy());
// static uint8_t cnt;
// cnt+=1;
// if (cnt > 100)
// {
// printf(" ax:%.3f,ay:%.3f,ax2:%.3f,ay2:%.3f\n",(double)_acceleration_setpoint(0),(double)_acceleration_setpoint(1),(double)setpoint(0),(double)setpoint(1));
// // printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2));
// cnt = 0;
// }
}
_updateAltitudeLock();
_respectGroundSlowdown();
}

Write Preview
Loading…
Cancel
Save