dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

publish a minimal message for logging/debugging

This commit is contained in:
baumanta 2019-05-17 10:32:48 +02:00 committed by Mathieu Bresciani
parent 09e1d4888c
commit c497d94616
3 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/collision_constraints.msg
View File

@ -3,10 +3,6 @@
uint64 timestamp # time since system start (microseconds)
#value of 0 means no constraint along this axis, value of 1 means no movements alowed, value bigger than 1 forces negative movement.
float32[2] constraints_normalized_x # constraints determined by range sensor measurements [x negative, x positive]
float32[2] constraints_normalized_y # constraintss determined by range sensor measurements [y negative, y positive]
float32[2] original_setpoint # velocities demanded
float32[2] adapted_setpoint # velocities allowed

24
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -65,6 +65,29 @@ bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscriptio
return true;
}
void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint,
const Vector2f &adapted_setpoint)
{
collision_constraints_s constraints; /**< collision constraints message */
//fill in values
constraints.timestamp = hrt_absolute_time();
constraints.original_setpoint[0] = original_setpoint(0);
constraints.original_setpoint[1] = original_setpoint(1);
constraints.adapted_setpoint[0] = adapted_setpoint(0);
constraints.adapted_setpoint[1] = adapted_setpoint(1);
// publish constraints
if (_constraints_pub != nullptr) {
orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints);
} else {
_constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints);
}
}
void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, const float max_acc,
const Vector2f &curr_vel)
{
@ -165,5 +188,6 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
}
_interfering = currently_interfering;
publishConstrainedSetpoint(original_setpoint, new_setpoint);
original_setpoint = new_setpoint;
}

3
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@ -75,6 +75,7 @@ private:
bool _interfering{false}; /**< states if the collision prevention interferes with the user input */
orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
uORB::SubscriptionPollable<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
@ -93,4 +94,6 @@ private:
void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const float max_acc, const matrix::Vector2f &curr_vel);
void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
};