/*
Set the distance based on a MAVLINK message
*/
void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg)
{
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(msg, &packet);
// only accept distances for downward facing sensors
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
last_update_ms = AP_HAL::millis();
distance_cm = packet.current_distance;
}
}
void handle_distance_sensor(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_distance_sensor_t distance_sensor;
mavlink_msg_distance_sensor_decode(msg, &distance_sensor);
auto distance_msg = boost::make_shared<mavros::Sonar>();
distance_msg->distance = (int)(distance_sensor.current_distance*10*cos(attitude_msg.roll)*cos(attitude_msg.pitch)); //from mavlink received in centimiters, converted in milimiters and corrected with attitude
//ROS_INFO("Sonar received. Distance: %d - Type: %d - MaxDist: %d",distance_msg->distance, distance_sensor.type, distance_sensor.max_distance);
distance_sensor_pub.publish(distance_msg);
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
{
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(msg, &packet);
// store distance to appropriate sector based on orientation field
if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
uint8_t sector = packet.orientation;
_angle[sector] = sector * 45;
_distance[sector] = packet.current_distance / 100.0f;
_distance_valid[sector] = true;
_distance_min = packet.min_distance / 100.0f;
_distance_max = packet.max_distance / 100.0f;
_last_update_ms = AP_HAL::millis();
}
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
{
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(msg, &packet);
// store distance to appropriate sector based on orientation field
if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
uint8_t sector = packet.orientation;
_angle[sector] = sector * 45;
_distance[sector] = packet.current_distance / 100.0f;
_distance_min = packet.min_distance / 100.0f;
_distance_max = packet.max_distance / 100.0f;
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
_last_update_ms = AP_HAL::millis();
update_boundary_for_sector(sector);
}
// store upward distance
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
_distance_upward = packet.current_distance / 100.0f;
_last_upward_update_ms = AP_HAL::millis();
}
}
/**
* Receive distance sensor data from FCU.
*/
void handle_distance_sensor(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_distance_sensor_t dist_sen;
mavlink_msg_distance_sensor_decode(msg, &dist_sen);
auto it = sensor_map.find(dist_sen.id);
if (it == sensor_map.end()) {
ROS_ERROR_NAMED("distance_sensor",
"DS: no mapping for sensor id: %d, type: %d, orientation: %d",
dist_sen.id, dist_sen.type, dist_sen.orientation);
return;
}
auto sensor = it->second;
if (sensor->is_subscriber) {
ROS_ERROR_NAMED("distance_sensor",
"DS: %s (id %d) is subscriber, but i got sensor data for that id from FCU",
sensor->topic_name.c_str(), sensor->sensor_id);
return;
}
if (sensor->orientation >= 0 && dist_sen.orientation != sensor->orientation) {
ROS_ERROR_NAMED("distance_sensor",
"DS: %s: received sensor data has different orientation (%s) than in config (%s)!",
sensor->topic_name.c_str(),
UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation)).c_str(),
UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(sensor->orientation)).c_str());
}
auto range = boost::make_shared<sensor_msgs::Range>();
range->header = uas->synchronized_header(sensor->frame_id, dist_sen.time_boot_ms);
range->min_range = dist_sen.min_distance * 1E-2; // in meters
range->max_range = dist_sen.max_distance * 1E-2;
range->field_of_view = sensor->field_of_view;
if (dist_sen.type == MAV_DISTANCE_SENSOR_LASER) {
range->radiation_type = sensor_msgs::Range::INFRARED;
}
else if (dist_sen.type == MAV_DISTANCE_SENSOR_ULTRASOUND) {
range->radiation_type = sensor_msgs::Range::ULTRASOUND;
}
else {
ROS_ERROR_NAMED("distance_sensor",
"DS: %s: Wrong/undefined type of sensor (type: %d). Droping!...",
sensor->topic_name.c_str(), dist_sen.type);
return;
}
range->range = dist_sen.current_distance * 1E-2; // in meters
if (sensor->send_tf) {
/* variables init */
auto q = UAS::sensor_orientation_matching(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation));
geometry_msgs::TransformStamped transform;
transform.header = uas->synchronized_header("fcu", dist_sen.time_boot_ms);
transform.child_frame_id = sensor->frame_id;
/* rotation and position set */
tf::quaternionEigenToMsg(q, transform.transform.rotation);
tf::vectorEigenToMsg(sensor->position, transform.transform.translation);
/* transform broadcast */
uas->tf2_broadcaster.sendTransform(transform);
}
sensor->pub.publish(range);
}
