diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 2eaf89baa00cee..f65e5f581ee383 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -425,6 +425,7 @@ def config_option(self): Feature('MAVLink', 'MAV_DEVICE_OP', 'AP_MAVLINK_MSG_DEVICE_OP_ENABLED', 'Enable DeviceOp MAVLink messages', 0, None), # noqa Feature('MAVLink', 'MAV_SERVO_RELAY', 'AP_MAVLINK_SERVO_RELAY_ENABLED', 'Enable ServoRelay MAVLink messages', 0, 'SERVORELAY_EVENTS'), # noqa Feature('MAVLink', 'MAV_MSG_SERIAL_CONTROL', 'AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'Enable Serial Control MAVLink messages', 0, None), # noqa + Feature('MAVLink', 'MAV_MSG_OPTICAL_FLOW_RAD', 'AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED', 'Enable Optical Flow Rad MAVLink message', 0, 'OPTICALFLOW'), # noqa Feature('MAVLink', 'MAVLINK_MSG_MISSION_REQUEST', 'AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'Enable MISSION_REQUEST MAVLink messages', 0, None), # noqa Feature('MAVLink', 'MAVLINK_MSG_RC_CHANNELS_RAW', 'AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED', 'Enable RC_CHANNELS_RAW MAVLink messages', 0, None), # noqa Feature('MAVLink', 'AP_MAVLINK_FTP_ENABLED', 'AP_MAVLINK_FTP_ENABLED', 'Enable MAVLink FTP protocol', 0, None), # noqa diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index e4637e7da70036..6713738e294e0d 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -258,6 +258,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', 'GCS_MAVLINK::handle_command_request_autopilot_capabilities'), # noqa ('AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', 'GCS_MAVLINK::send_relay_status'), ('AP_MAVLINK_MSG_DEVICE_OP_ENABLED', 'GCS_MAVLINK::handle_device_op_write'), + ('AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED', 'GCS_MAVLINK::handle_optical_flow_rad'), ('AP_MAVLINK_SERVO_RELAY_ENABLED', 'GCS_MAVLINK::handle_servorelay_message'), ('AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'GCS_MAVLINK::handle_serial_control'), ('AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', r'GCS_MAVLINK::handle_mission_request\b'), diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 854918cc9e2114..b098f06db0c847 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -226,6 +226,21 @@ void AP_OpticalFlow::handle_msg(const mavlink_message_t &msg) } } +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + +void AP_OpticalFlow::handle_msg_optical_flow_rad(const mavlink_message_t &msg) +{ + // exit immediately if not enabled + if (!enabled()){ + return; + } + + if (backend != nullptr) { + backend->handle_msg_optical_flow_rad(msg); + } +} +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + #if HAL_MSP_OPTICALFLOW_ENABLED void AP_OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt) { diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index e40aced5047ffc..13c184463aaf57 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -89,6 +89,11 @@ class AP_OpticalFlow // handle optical flow mavlink messages void handle_msg(const mavlink_message_t &msg); +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + // handle optical flow rad mavlink message + void handle_msg_optical_flow_rad(const mavlink_message_t &msg); +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + #if HAL_MSP_OPTICALFLOW_ENABLED // handle optical flow msp messages void handle_msp(const MSP::msp_opflow_data_message_t &pkt); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h index 43a8f552abd200..d43bdbaa84b8e6 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h @@ -42,6 +42,11 @@ class OpticalFlow_backend // handle optical flow mavlink messages virtual void handle_msg(const mavlink_message_t &msg) {} +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + // handle optical flow rad mavlink messages + virtual void handle_msg_optical_flow_rad(const mavlink_message_t &msg) {} +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + #if HAL_MSP_OPTICALFLOW_ENABLED // handle optical flow msp messages virtual void handle_msp(const MSP::msp_opflow_data_message_t &pkt) {} diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp index becae698ab9d90..980d5cf9642a1f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp @@ -35,19 +35,82 @@ void AP_OpticalFlow_MAV::update(void) { // record gyro values as long as they are being used // the sanity check of dt below ensures old gyro values are not used - if (gyro_sum_count < 1000) { + if (gyro_sum_count < 1000 ) { const Vector3f& gyro = AP::ahrs().get_gyro(); gyro_sum.x += gyro.x; gyro_sum.y += gyro.y; gyro_sum_count++; } - // return without updating state if no readings - if (count == 0) { - return; + struct AP_OpticalFlow::OpticalFlow_state state{}; + if (count != 0) { + // obtain data from OPTICAL_FLOW mavlink message + state = update_msg_optical_flow(); + + // reset local variables for OPTICAL_FLOW + flow_sum.zero(); + quality_sum = 0; + count = 0; + gyro_sum.zero(); + gyro_sum_count = 0; + } + +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + if (count_rad != 0 && (quality_rad_sum / count_rad) > state.surface_quality) { + // state quality is better from OPTICAL_FLOW_RAD, so we can take it instead + state = update_msg_optical_flow_rad(); + + // reset local variables for OPTICAL_FLOW_RAD + flow_rad_integral_sum.zero(); + rate_gyro_rad_integral_sum.zero(); + count_rad = 0; + quality_rad_sum = 0; + integration_time_sum_rad_us = 0; } +#endif //AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + + _update_frontend(state); + +} + +// handle OPTICAL_FLOW mavlink messages +void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg) +{ + mavlink_optical_flow_t packet; + mavlink_msg_optical_flow_decode(&msg, &packet); - struct AP_OpticalFlow::OpticalFlow_state state {}; + // record time message was received + // ToDo: add jitter correction + latest_frame_us = AP_HAL::micros64(); + + // use flow_rate_x/y fields if non-zero values are ever provided + if (!flow_sum_is_rads && (!is_zero(packet.flow_rate_x) || !is_zero(packet.flow_rate_y))) { + flow_sum_is_rads = true; + flow_sum.zero(); + quality_sum = 0; + count = 0; + } + + // add sensor values to sum + if (flow_sum_is_rads) { + // higher precision flow_rate_x/y fields are used + flow_sum.x += packet.flow_rate_x; + flow_sum.y += packet.flow_rate_y; + } else { + // lower precision flow_x/y fields are used + flow_sum.x += packet.flow_x; + flow_sum.y += packet.flow_y; + } + quality_sum += packet.quality; + count++; + + // take sensor id from message + sensor_id = packet.sensor_id; +} + +AP_OpticalFlow::OpticalFlow_state AP_OpticalFlow_MAV::update_msg_optical_flow() +{ + struct AP_OpticalFlow::OpticalFlow_state state; state.surface_quality = quality_sum / count; @@ -73,7 +136,7 @@ void AP_OpticalFlow_MAV::update(void) state.bodyRate.zero(); } else { // copy average body rate to state structure - state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count }; + state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count }; } // we only apply yaw to flowRate as body rate comes from AHRS @@ -84,51 +147,52 @@ void AP_OpticalFlow_MAV::update(void) state.bodyRate.zero(); } - _update_frontend(state); + return state; +} + +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED +void AP_OpticalFlow_MAV::handle_msg_optical_flow_rad(const mavlink_message_t &msg) +{ + mavlink_optical_flow_rad_t packet; + mavlink_msg_optical_flow_rad_decode(&msg, &packet); + + flow_rad_integral_sum.x += packet.integrated_x; + flow_rad_integral_sum.y += packet.integrated_y; + + rate_gyro_rad_integral_sum.x += packet.integrated_xgyro; + rate_gyro_rad_integral_sum.y += packet.integrated_ygyro; + + quality_rad_sum += packet.quality; + sensor_id_rad = packet.sensor_id; + + integration_time_sum_rad_us += packet.integration_time_us; - // reset local buffers - flow_sum.zero(); - quality_sum = 0; - count = 0; + count_rad++; - // reset gyro sum - gyro_sum.zero(); - gyro_sum_count = 0; } -// handle OPTICAL_FLOW mavlink messages -void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg) -{ - mavlink_optical_flow_t packet; - mavlink_msg_optical_flow_decode(&msg, &packet); +AP_OpticalFlow::OpticalFlow_state AP_OpticalFlow_MAV::update_msg_optical_flow_rad() { + struct AP_OpticalFlow::OpticalFlow_state state; + const Vector2f flowScaler = _flowScaler(); - // record time message was received - // ToDo: add jitter correction - latest_frame_us = AP_HAL::micros64(); + float scale_factorx = 1.0f + 0.001f * flowScaler.x; + float scale_factory = 1.0f + 0.001f * flowScaler.y; - // use flow_rate_x/y fields if non-zero values are ever provided - if (!flow_sum_is_rads && (!is_zero(packet.flow_rate_x) || !is_zero(packet.flow_rate_y))) { - flow_sum_is_rads = true; - flow_sum.zero(); - quality_sum = 0; - count = 0; - } + float integral_to_rate = 1.0f / (integration_time_sum_rad_us * 1e-6) / count_rad; + state.flowRate = Vector2f{ + flow_rad_integral_sum.x * scale_factorx, + flow_rad_integral_sum.y * scale_factory, + } / count_rad * integral_to_rate; + state.bodyRate = rate_gyro_rad_integral_sum / count_rad * integral_to_rate; + state.surface_quality = quality_rad_sum / count_rad; - // add sensor values to sum - if (flow_sum_is_rads) { - // higher precision flow_rate_x/y fields are used - flow_sum.x += packet.flow_rate_x; - flow_sum.y += packet.flow_rate_y; - } else { - // lower precision flow_x/y fields are used - flow_sum.x += packet.flow_x; - flow_sum.y += packet.flow_y; - } - quality_sum += packet.quality; - count++; + _applyYaw(state.flowRate); + _applyYaw(state.bodyRate); + + return state; - // take sensor id from message - sensor_id = packet.sensor_id; } +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + #endif // AP_OPTICALFLOW_MAV_ENABLED diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h index 5d5f516d82fc69..a00da4b3782967 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h @@ -18,24 +18,47 @@ class AP_OpticalFlow_MAV : public OpticalFlow_backend // read latest values from sensor and fill in x,y and totals. void update(void) override; + AP_OpticalFlow::OpticalFlow_state update_msg_optical_flow(); // get update from mavlink void handle_msg(const mavlink_message_t &msg) override; +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + void handle_msg_optical_flow_rad(const mavlink_message_t &msg) override; + AP_OpticalFlow::OpticalFlow_state update_msg_optical_flow_rad(); +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + // detect if the sensor is available static AP_OpticalFlow_MAV *detect(AP_OpticalFlow &_frontend); private: + // OPTICAL_FLOW state variables uint64_t prev_frame_us; // system time of last message when update was last called uint64_t latest_frame_us; // system time of most recent messages processed Vector2f flow_sum; // sum of sensor's flow_x and flow_y values since last call to update bool flow_sum_is_rads; // true if flow_sum holds values in rad/s, if false flow_sum holds dpi uint16_t quality_sum; // sum of sensor's quality values since last call to update uint16_t count; // number of sensor readings since last call to update - uint8_t sensor_id; // sensor_id received in latest mavlink message + uint8_t sensor_id; // sensor_id received in latest OPTICAL_FLOW message Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor uint16_t gyro_sum_count; // number of gyro sensor values in sum + + + +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + + // OPTICAL_FLOW_RAD state variables + Vector2f flow_rad_integral_sum; // sum of all integrated_x and integrated_y values since last call to update + Vector2f rate_gyro_rad_integral_sum; // sum of all integrated_xgyro/integrated_ygyro values since last update + uint32_t integration_time_sum_rad_us; // sum of all integratation_time_us values used to obtain average flow + uint16_t quality_rad_sum; // sum of sensor quality values since last call to update + uint16_t count_rad; // number of sensor readings since last call to update + uint8_t sensor_id_rad; // sensor_id received in last OPTICAL_FLOW_RAD message + + + +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED }; #endif // AP_OPTICALFLOW_MAV_ENABLED diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_config.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_config.h index 21718fe9637b49..a4cdd78db5dfb5 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_config.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_config.h @@ -51,3 +51,4 @@ #ifndef AP_OPTICALFLOW_UPFLOW_ENABLED #define AP_OPTICALFLOW_UPFLOW_ENABLED AP_OPTICALFLOW_BACKEND_DEFAULT_ENABLED #endif + diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 3b00dbd02776ac..31712e8ce047c0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -717,6 +717,7 @@ class GCS_MAVLINK #endif // AP_MAVLINKCAN_ENABLED void handle_optical_flow(const mavlink_message_t &msg); + void handle_optical_flow_rad(const mavlink_message_t &msg); void handle_manual_control(const mavlink_message_t &msg); void handle_radio_rc_channels(const mavlink_message_t &msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 00ccb77915c8b5..84395f645b3858 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4193,6 +4193,16 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg) } #endif +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED +void GCS_MAVLINK::handle_optical_flow_rad(const mavlink_message_t &msg) +{ + AP_OpticalFlow *optflow = AP::opticalflow(); + if (optflow == nullptr) { + return; + } + optflow->handle_msg_optical_flow_rad(msg); +} +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* @@ -4545,6 +4555,12 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; #endif +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_optical_flow_rad(msg); + break; +#endif + case MAVLINK_MSG_ID_DISTANCE_SENSOR: handle_distance_sensor(msg); break; diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 98b8578fa70d4b..44435bc8c0f34e 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -60,6 +60,10 @@ #define AP_MAVLINK_MSG_WIND_ENABLED AP_AHRS_ENABLED #endif +#ifndef AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED +#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED (HAL_PROGRAM_SIZE_LIMIT_KB > 2048 && defined(AP_OPTICALFLOW_ENABLED) && AP_OPTICALFLOW_ENABLED) +#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + // allow removal of developer-centric mavlink commands #ifndef AP_MAVLINK_FAILURE_CREATION_ENABLED #define AP_MAVLINK_FAILURE_CREATION_ENABLED 1 diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 26400fdb30383f..bd1fbf30320d97 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -114,6 +114,9 @@ enum ap_message : uint8_t { MSG_AVAILABLE_MODES_MONITOR = 99, #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED MSG_FLIGHT_INFORMATION = 100, +#endif +#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED + MSG_OPTICAL_FLOW_RAD = 101, #endif MSG_LAST // MSG_LAST must be the last entry in this enum };