diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index c4132a972f..5fd267d8fd 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -290,6 +290,9 @@ void Sub::fast_loop() // check if we've landed or crashed update_land_and_crash_detectors(); + // camera mount's fast update + camera_mount.update_fast(); + // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index a031306f96..180f98a41d 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1043,6 +1043,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_PARAM_VALUE: + { + sub.camera_mount.handle_param_value(msg); + break; + } + case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38 { handle_mission_write_partial_list(sub.mission, msg); diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index ca90f488da..ccf743bf92 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -204,7 +204,7 @@ void Sub::init_ardupilot() #if MOUNT == ENABLED // initialise camera mount - camera_mount.init(serial_manager); + camera_mount.init(&DataFlash, serial_manager); #endif #if PRECISION_LANDING == ENABLED