Fix camera trigger via MAVLink when camera capture feedback is enabled

- camera_trigger module always publishes the camera_trigger msg (independent of the camera feedback)
- Use camera_trigger msg and set the feedback flag
- Subscribing modules determine if the message is relevant based on the feedback message
This commit is contained in:
Michael Schaeuble
2021-10-07 11:03:55 +02:00
committed by Beat Küng
parent 3e4031cf0f
commit ebb657bcf4
5 changed files with 37 additions and 36 deletions
+12 -2
View File
@@ -37,6 +37,11 @@ CameraFeedback::CameraFeedback() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
_p_cam_cap_fback = param_find("CAM_CAP_FBACK");
if (_p_cam_cap_fback != PARAM_INVALID) {
param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
}
}
bool
@@ -63,7 +68,7 @@ CameraFeedback::Run()
camera_trigger_s trig{};
if (_trigger_sub.update(&trig)) {
while (_trigger_sub.update(&trig)) {
// update geotagging subscriptions
vehicle_global_position_s gpos{};
@@ -77,7 +82,12 @@ CameraFeedback::Run()
att.timestamp == 0) {
// reject until we have valid data
return;
continue;
}
if ((_cam_cap_fback >= 1) && !trig.feedback) {
// Ignore triggers that are not feedback when camera capture feedback is enabled
continue;
}
camera_capture_s capture{};
@@ -83,4 +83,7 @@ private:
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
uORB::Publication<camera_capture_s> _capture_pub{ORB_ID(camera_capture)};
param_t _p_cam_cap_fback;
int32_t _cam_cap_fback{0};
};