diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 52 |
1 files changed, 32 insertions, 20 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bc092c7e9..e98d72afe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_int(msg); break; - case MAVLINK_MSG_ID_OPTICAL_FLOW: - handle_message_optical_flow(msg); + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); break; case MAVLINK_MSG_ID_SET_MODE: @@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -389,13 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.integrated_x; - f.flow_raw_y = flow.integrated_y; + f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -538,12 +546,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << - OFB_IGN_BIT_YAW; + if (set_position_target_local_ned.type_mask & (1 << 10)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << - OFB_IGN_BIT_YAWRATE; + if (set_position_target_local_ned.type_mask & (1 << 11)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + } offboard_control_sp.timestamp = hrt_absolute_time(); |