AP_Mount: add general function for handling mavlink messages

AP_Mount: const parameters, add overrides, remove bad virtual declarations

AP_Mount: use AHRS singleton

AP_Mount: make status_msg pure-virtual and add override keyword

AP_Mount: handle deprecated mavlink control and configure msgs

AP_Mount: handle MAV_CMD_DO_MOUNT_CONFIGURE

AP_Mount: rename status_msg method to send_mount_status

libraries/AP_Mount/AP_Mount.cpp

@@ -393,8 +393,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
     AP_GROUPEND
 };
 
-AP_Mount::AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location &current_loc) :
-    _ahrs(ahrs),
+AP_Mount::AP_Mount(const struct Location &current_loc) :
     _current_loc(current_loc)
 {
     if (_singleton != nullptr) {
@@ -560,47 +559,79 @@ void AP_Mount::set_angle_targets(uint8_t instance, float roll, float tilt, float
     _backends[instance]->set_angle_targets(roll, tilt, pan);
 }
 
-/// Change the configuration of the mount
-/// triggered by a MavLink packet.
-void AP_Mount::configure_msg(uint8_t instance, mavlink_message_t* msg)
+MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_long_t &packet)
 {
-    if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == nullptr) {
-        return;
+    if (_primary >= AP_MOUNT_MAX_INSTANCES || _backends[_primary] == nullptr) {
+        return MAV_RESULT_FAILED;
+    }
+    _backends[_primary]->set_mode((MAV_MOUNT_MODE)packet.param1);
+    state[0]._stab_roll = packet.param2;
+    state[0]._stab_tilt = packet.param3;
+    state[0]._stab_pan = packet.param4;
+
+    return MAV_RESULT_ACCEPTED;
+}
+
+
+MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_long_t &packet)
+{
+    if (_primary >= AP_MOUNT_MAX_INSTANCES || _backends[_primary] == nullptr) {
+        return MAV_RESULT_FAILED;
     }
 
     // send message to backend
-    _backends[instance]->configure_msg(msg);
+    _backends[_primary]->control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
+
+    return MAV_RESULT_ACCEPTED;
+}
+
+MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet)
+{
+    switch (packet.command) {
+    case MAV_CMD_DO_MOUNT_CONFIGURE:
+        return handle_command_do_mount_configure(packet);
+    case MAV_CMD_DO_MOUNT_CONTROL:
+        return handle_command_do_mount_control(packet);
+    default:
+        return MAV_RESULT_UNSUPPORTED;
+    }
+}
+
+/// Change the configuration of the mount
+void AP_Mount::handle_mount_configure(const mavlink_message_t *msg)
+{
+    if (_primary >= AP_MOUNT_MAX_INSTANCES || _backends[_primary] == nullptr) {
+        return;
+    }
+
+    mavlink_mount_configure_t packet;
+    mavlink_msg_mount_configure_decode(msg, &packet);
+
+    // send message to backend
+    _backends[_primary]->handle_mount_configure(packet);
 }
 
 /// Control the mount (depends on the previously set mount configuration)
-/// triggered by a MavLink packet.
-void AP_Mount::control_msg(uint8_t instance, mavlink_message_t *msg)
+void AP_Mount::handle_mount_control(const mavlink_message_t *msg)
 {
-    if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == nullptr) {
+    if (_primary >= AP_MOUNT_MAX_INSTANCES || _backends[_primary] == nullptr) {
         return;
     }
 
-    // send message to backend
-    _backends[instance]->control_msg(msg);
-}
-
-void AP_Mount::control(uint8_t instance, int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, enum MAV_MOUNT_MODE mount_mode)
-{
-    if (instance >= AP_MOUNT_MAX_INSTANCES || _backends[instance] == nullptr) {
-        return;
-    }
+    mavlink_mount_control_t packet;
+    mavlink_msg_mount_control_decode(msg, &packet);
 
     // send message to backend
-    _backends[instance]->control(pitch_or_lat, roll_or_lon, yaw_or_alt, mount_mode);
+    _backends[_primary]->handle_mount_control(packet);
 }
 
 /// Return mount status information
-void AP_Mount::status_msg(mavlink_channel_t chan)
+void AP_Mount::send_mount_status(mavlink_channel_t chan)
 {
-    // call status_msg for  each instance
+    // call send_mount_status for  each instance
     for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
-            _backends[instance]->status_msg(chan);
+            _backends[instance]->send_mount_status(chan);
         }
     }
 }
@@ -615,7 +646,7 @@ void AP_Mount::set_roi_target(uint8_t instance, const struct Location &target_lo
 }
 
 // pass a GIMBAL_REPORT message to the backend
-void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
+void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t *msg)
 {
     for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
@@ -624,8 +655,28 @@ void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *m
     }
 }
 
+void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t *msg)
+{
+    switch (msg->msgid) {
+    case MAVLINK_MSG_ID_GIMBAL_REPORT:
+        handle_gimbal_report(chan, msg);
+        break;
+    case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
+        handle_mount_configure(msg);
+        break;
+    case MAVLINK_MSG_ID_MOUNT_CONTROL:
+        handle_mount_control(msg);
+        break;
+    default:
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        AP_HAL::panic("Unhandled mount case");
+#endif
+        break;
+    }
+}
+
 // handle PARAM_VALUE
-void AP_Mount::handle_param_value(mavlink_message_t *msg)
+void AP_Mount::handle_param_value(const mavlink_message_t *msg)
 {
     for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {

libraries/AP_Mount/AP_Mount.h

@@ -55,7 +55,7 @@ class AP_Mount
     friend class AP_Mount_SToRM32_serial;
 
 public:
-    AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location &current_loc);
+    AP_Mount(const struct Location &current_loc);
 
     /* Do not allow copies */
     AP_Mount(const AP_Mount &other) = delete;
@@ -115,29 +115,16 @@ public:
     void set_roi_target(const struct Location &target_loc) { set_roi_target(_primary,target_loc); }
     void set_roi_target(uint8_t instance, const struct Location &target_loc);
 
-    // control - control the mount
-    void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, enum MAV_MOUNT_MODE mount_mode) { control(_primary, pitch_or_lat, roll_or_lon, yaw_or_alt, mount_mode); }
-    void control(uint8_t instance, int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, enum MAV_MOUNT_MODE mount_mode);
-
-    // configure_msg - process MOUNT_CONFIGURE messages received from GCS
-    void configure_msg(mavlink_message_t* msg) { configure_msg(_primary, msg); }
-    void configure_msg(uint8_t instance, mavlink_message_t* msg);
-
-    // control_msg - process MOUNT_CONTROL messages received from GCS
-    void control_msg(mavlink_message_t* msg) { control_msg(_primary, msg); }
-    void control_msg(uint8_t instance, mavlink_message_t* msg);
-
-    // handle a PARAM_VALUE message
-    void handle_param_value(mavlink_message_t *msg);
-
-    // handle a GIMBAL_REPORT message
-    void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
+    // mavlink message handling:
+    MAV_RESULT handle_command_long(const mavlink_command_long_t &packet);
+    void handle_param_value(const mavlink_message_t *msg);
+    void handle_message(mavlink_channel_t chan, const mavlink_message_t *msg);
 
     // send a GIMBAL_REPORT message to GCS
     void send_gimbal_report(mavlink_channel_t chan);
 
-    // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    void status_msg(mavlink_channel_t chan);
+    // send a MOUNT_STATUS message to GCS:
+    void send_mount_status(mavlink_channel_t chan);
 
     // parameter var table
     static const struct AP_Param::GroupInfo        var_info[];
@@ -147,7 +134,6 @@ protected:
     static AP_Mount *_singleton;
 
     // private members
-    const AP_AHRS_TYPE     &_ahrs;
     const struct Location   &_current_loc;  // reference to the vehicle's current location
 
     // frontend parameters
@@ -189,6 +175,16 @@ protected:
         MAV_MOUNT_MODE  _mode;              // current mode (see MAV_MOUNT_MODE enum)
         struct Location _roi_target;        // roi target location
     } state[AP_MOUNT_MAX_INSTANCES];
+
+private:
+
+    void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t *msg);
+    void handle_mount_configure(const mavlink_message_t *msg);
+    void handle_mount_control(const mavlink_message_t *msg);
+
+    MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
+    MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
+
 };
 
 namespace AP {

libraries/AP_Mount/AP_Mount_Alexmos.cpp

@@ -73,8 +73,8 @@ void AP_Mount_Alexmos::set_mode(enum MAV_MOUNT_MODE mode)
     _state._mode = mode;
 }
 
-// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_Alexmos::status_msg(mavlink_channel_t chan)
+// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+void AP_Mount_Alexmos::send_mount_status(mavlink_channel_t chan)
 {
     if (!_initialised) {
         return;

libraries/AP_Mount/AP_Mount_Alexmos.h

@@ -82,8 +82,8 @@ public:
     // set_mode - sets mount's mode
     virtual void set_mode(enum MAV_MOUNT_MODE mode) ;
 
-    // status_msg - called to allow mounts to send their status to GCS via MAVLink
-    virtual void status_msg(mavlink_channel_t chan) ;
+    // send_mount_status - called to allow mounts to send their status to GCS via MAVLink
+    virtual void send_mount_status(mavlink_channel_t chan) override;
 
 private:
 

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -24,21 +24,18 @@ void AP_Mount_Backend::set_roi_target(const struct Location &target_loc)
     _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT);
 }
 
-// configure_msg - process MOUNT_CONFIGURE messages received from GCS.  deprecated.
-void AP_Mount_Backend::configure_msg(mavlink_message_t* msg)
+// process MOUNT_CONFIGURE messages received from GCS.  deprecated.
+void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &packet)
 {
-    __mavlink_mount_configure_t packet;
-    mavlink_msg_mount_configure_decode(msg, &packet);
-
     set_mode((MAV_MOUNT_MODE)packet.mount_mode);
+    _state._stab_roll = packet.stab_roll;
+    _state._stab_tilt = packet.stab_pitch;
+    _state._stab_pan = packet.stab_yaw;
 }
 
-// control_msg - process MOUNT_CONTROL messages received from GCS. deprecated.
-void AP_Mount_Backend::control_msg(mavlink_message_t *msg)
+// process MOUNT_CONTROL messages received from GCS. deprecated.
+void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet)
 {
-    __mavlink_mount_control_t packet;
-    mavlink_msg_mount_control_decode(msg, &packet);
-
     control((int32_t)packet.input_a, (int32_t)packet.input_b, (int32_t)packet.input_c, _state._mode);
 }
 
@@ -159,7 +156,7 @@ void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec
         // calc absolute heading and then onvert to vehicle relative yaw
         angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
         if (relative_pan) {
-            angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - _frontend._ahrs.yaw);
+            angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - AP::ahrs().yaw);
         }
     }
 }

libraries/AP_Mount/AP_Mount_Backend.h

@@ -59,23 +59,23 @@ public:
     // control - control the mount
     virtual void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode);
     
-    // configure_msg - process MOUNT_CONFIGURE messages received from GCS
-    virtual void configure_msg(mavlink_message_t* msg);
+    // process MOUNT_CONFIGURE messages received from GCS:
+    void handle_mount_configure(const mavlink_mount_configure_t &msg);
 
-    // control_msg - process MOUNT_CONTROL messages received from GCS
-    virtual void control_msg(mavlink_message_t* msg);
+    // process MOUNT_CONTROL messages received from GCS:
+    void handle_mount_control(const mavlink_mount_control_t &packet);
 
-    // status_msg - called to allow mounts to send their status to GCS via MAVLink
-    virtual void status_msg(mavlink_channel_t chan) {}
+    // send_mount_status - called to allow mounts to send their status to GCS via MAVLink
+    virtual void send_mount_status(mavlink_channel_t chan) = 0;
 
     // handle a GIMBAL_REPORT message
-    virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {}
+    virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t *msg) {}
 
     // handle a PARAM_VALUE message
-    virtual void handle_param_value(mavlink_message_t *msg) {}
+    virtual void handle_param_value(const mavlink_message_t *msg) {}
 
     // send a GIMBAL_REPORT message to the GCS
-    virtual void send_gimbal_report(mavlink_channel_t chan) {}
+    virtual void send_gimbal_report(const mavlink_channel_t chan) {}
 
 protected:
 

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -93,8 +93,8 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode)
     _state._mode = mode;
 }
 
-// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_SToRM32::status_msg(mavlink_channel_t chan)
+// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+void AP_Mount_SToRM32::send_mount_status(mavlink_channel_t chan)
 {
     // return target angles as gimbal's actual attitude.  To-Do: retrieve actual gimbal attitude and send these instead
     mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100);

libraries/AP_Mount/AP_Mount_SToRM32.h

@@ -36,8 +36,8 @@ public:
     // set_mode - sets mount's mode
     virtual void set_mode(enum MAV_MOUNT_MODE mode);
 
-    // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    virtual void status_msg(mavlink_channel_t chan);
+    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+    virtual void send_mount_status(mavlink_channel_t chan) override;
 
 private:
 

libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp

@@ -123,8 +123,8 @@ void AP_Mount_SToRM32_serial::set_mode(enum MAV_MOUNT_MODE mode)
     _state._mode = mode;
 }
 
-// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_SToRM32_serial::status_msg(mavlink_channel_t chan)
+// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+void AP_Mount_SToRM32_serial::send_mount_status(mavlink_channel_t chan)
 {
     // return target angles as gimbal's actual attitude.
     mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y, _current_angle.x, _current_angle.z);

libraries/AP_Mount/AP_Mount_SToRM32_serial.h

@@ -34,8 +34,8 @@ public:
     // set_mode - sets mount's mode
     virtual void set_mode(enum MAV_MOUNT_MODE mode);
 
-    // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    virtual void status_msg(mavlink_channel_t chan);
+    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+    virtual void send_mount_status(mavlink_channel_t chan);
 
 private:
 

libraries/AP_Mount/AP_Mount_Servo.cpp

@@ -112,8 +112,8 @@ void AP_Mount_Servo::check_servo_map()
     _flags.pan_control = SRV_Channels::function_assigned(_pan_idx);
 }
 
-// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_Servo::status_msg(mavlink_channel_t chan)
+// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+void AP_Mount_Servo::send_mount_status(mavlink_channel_t chan)
 {
     mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100);
 }
@@ -123,12 +123,13 @@ void AP_Mount_Servo::status_msg(mavlink_channel_t chan)
 //  output: _angle_bf_output_deg (body frame angles in degrees)
 void AP_Mount_Servo::stabilize()
 {
+    AP_AHRS &ahrs = AP::ahrs();
     // only do the full 3D frame transform if we are doing pan control
     if (_state._stab_pan) {
         Matrix3f m;                         ///< holds 3 x 3 matrix, var is used as temp in calcs
         Matrix3f cam;                       ///< Rotation matrix earth to camera. Desired camera from input.
         Matrix3f gimbal_target;             ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
-        m = _frontend._ahrs.get_rotation_body_to_ned();
+        m = ahrs.get_rotation_body_to_ned();
         m.transpose();
         cam.from_euler(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z);
         gimbal_target = m * cam;
@@ -143,24 +144,24 @@ void AP_Mount_Servo::stabilize()
         _angle_bf_output_deg.y = degrees(_angle_ef_target_rad.y);
         _angle_bf_output_deg.z = degrees(_angle_ef_target_rad.z);
         if (_state._stab_roll) {
-            _angle_bf_output_deg.x -= degrees(_frontend._ahrs.roll);
+            _angle_bf_output_deg.x -= degrees(ahrs.roll);
         }
         if (_state._stab_tilt) {
-            _angle_bf_output_deg.y -= degrees(_frontend._ahrs.pitch);
+            _angle_bf_output_deg.y -= degrees(ahrs.pitch);
         }
 
         // lead filter
-        const Vector3f &gyro = _frontend._ahrs.get_gyro();
+        const Vector3f &gyro = ahrs.get_gyro();
 
-        if (_state._stab_roll && !is_zero(_state._roll_stb_lead) && fabsf(_frontend._ahrs.pitch) < M_PI/3.0f) {
+        if (_state._stab_roll && !is_zero(_state._roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) {
             // Compute rate of change of euler roll angle
-            float roll_rate = gyro.x + (_frontend._ahrs.sin_pitch() / _frontend._ahrs.cos_pitch()) * (gyro.y * _frontend._ahrs.sin_roll() + gyro.z * _frontend._ahrs.cos_roll());
+            float roll_rate = gyro.x + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll());
             _angle_bf_output_deg.x -= degrees(roll_rate) * _state._roll_stb_lead;
         }
 
         if (_state._stab_tilt && !is_zero(_state._pitch_stb_lead)) {
             // Compute rate of change of euler pitch angle
-            float pitch_rate = _frontend._ahrs.cos_pitch() * gyro.y - _frontend._ahrs.sin_roll() * gyro.z;
+            float pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z;
             _angle_bf_output_deg.y -= degrees(pitch_rate) * _state._pitch_stb_lead;
         }
     }

libraries/AP_Mount/AP_Mount_Servo.h

@@ -36,8 +36,8 @@ public:
     // set_mode - sets mount's mode
     virtual void set_mode(enum MAV_MOUNT_MODE mode);
 
-    // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    virtual void status_msg(mavlink_channel_t chan);
+    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+    virtual void send_mount_status(mavlink_channel_t chan) override;
 
 private:
 

libraries/AP_Mount/AP_Mount_SoloGimbal.cpp

@@ -12,7 +12,7 @@ extern const AP_HAL::HAL& hal;
 
 AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
     AP_Mount_Backend(frontend, state, instance),
-    _gimbal(frontend._ahrs)
+    _gimbal()
 {}
 
 // init - performs any required initialisation for this instance
@@ -99,8 +99,8 @@ void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode)
     _state._mode = mode;
 }
 
-// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_SoloGimbal::status_msg(mavlink_channel_t chan)
+// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+void AP_Mount_SoloGimbal::send_mount_status(mavlink_channel_t chan)
 {
     if (_gimbal.aligned()) {
         mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100);
@@ -113,7 +113,7 @@ void AP_Mount_SoloGimbal::status_msg(mavlink_channel_t chan)
 /*
   handle a GIMBAL_REPORT message
  */
-void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
+void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t *msg)
 {
     _gimbal.update_target(_angle_ef_target_rad);
     _gimbal.receive_feedback(chan,msg);
@@ -133,7 +133,7 @@ void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, mavlink_m
     }
 }
 
-void AP_Mount_SoloGimbal::handle_param_value(mavlink_message_t *msg)
+void AP_Mount_SoloGimbal::handle_param_value(const mavlink_message_t *msg)
 {
     _gimbal.handle_param_value(msg);
 }
@@ -141,7 +141,7 @@ void AP_Mount_SoloGimbal::handle_param_value(mavlink_message_t *msg)
 /*
   handle a GIMBAL_REPORT message
  */
-void AP_Mount_SoloGimbal::handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg)
+void AP_Mount_SoloGimbal::handle_gimbal_torque_report(mavlink_channel_t chan, const mavlink_message_t *msg)
 {
     _gimbal.disable_torque_report();
 }

libraries/AP_Mount/AP_Mount_SoloGimbal.h

@@ -36,16 +36,16 @@ public:
     // set_mode - sets mount's mode
     virtual void set_mode(enum MAV_MOUNT_MODE mode);
 
-    // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    virtual void status_msg(mavlink_channel_t chan);
+    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+    virtual void send_mount_status(mavlink_channel_t chan) override;
 
     // handle a GIMBAL_REPORT message
-    virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
-    virtual void handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg);
-    virtual void handle_param_value(mavlink_message_t *msg);
+    void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t *msg) override;
+    void handle_gimbal_torque_report(mavlink_channel_t chan, const mavlink_message_t *msg);
+    void handle_param_value(const mavlink_message_t *msg) override;
 
     // send a GIMBAL_REPORT message to the GCS
-    virtual void send_gimbal_report(mavlink_channel_t chan);
+    void send_gimbal_report(mavlink_channel_t chan) override;
 
     virtual void update_fast();
 

libraries/AP_Mount/SoloGimbal.cpp

@@ -38,7 +38,7 @@ gimbal_mode_t SoloGimbal::get_mode()
     }
 }
 
-void SoloGimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
+void SoloGimbal::receive_feedback(mavlink_channel_t chan, const mavlink_message_t *msg)
 {
     mavlink_gimbal_report_t report_msg;
     mavlink_msg_gimbal_report_decode(msg, &report_msg);

libraries/AP_Mount/SoloGimbal.h

@@ -38,9 +38,9 @@ class SoloGimbal : AP_AccelCal_Client
 {
 public:
     //Constructor
-    SoloGimbal(const AP_AHRS_NavEKF &ahrs) :
-        _ekf(ahrs),
-        _ahrs(ahrs),
+    SoloGimbal() :
+        _ekf(AP::ahrs_navekf()),
+        _ahrs(AP::ahrs_navekf()),
         _state(GIMBAL_STATE_NOT_PRESENT),
         _vehicle_yaw_rate_ef_filt(0.0f),
         _vehicle_to_gimbal_quat(),
@@ -58,7 +58,7 @@ public:
     }
 
     void    update_target(Vector3f newTarget);
-    void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
+    void    receive_feedback(mavlink_channel_t chan, const mavlink_message_t *msg);
 
     void update_fast();
 
@@ -74,7 +74,7 @@ public:
     void disable_torque_report() { _gimbalParams.set_param(GMB_PARAM_GMB_SND_TORQUE, 0); }
     void fetch_params() { _gimbalParams.fetch_params(); }
 
-    void handle_param_value(mavlink_message_t *msg) {
+    void handle_param_value(const mavlink_message_t *msg) {
         _gimbalParams.handle_param_value(msg);
     }
 

libraries/AP_Mount/SoloGimbal_Parameters.cpp

@@ -172,7 +172,7 @@ void SoloGimbal_Parameters::update()
     }
 }
 
-void SoloGimbal_Parameters::handle_param_value(mavlink_message_t *msg)
+void SoloGimbal_Parameters::handle_param_value(const mavlink_message_t *msg)
 {
     mavlink_param_value_t packet;
     mavlink_msg_param_value_decode(msg, &packet);

libraries/AP_Mount/SoloGimbal_Parameters.h

@@ -54,7 +54,7 @@ public:
     void set_param(gmb_param_t param, float value);
 
     void update();
-    void handle_param_value(mavlink_message_t *msg);
+    void handle_param_value(const mavlink_message_t *msg);
 
     Vector3f get_accel_bias();
     Vector3f get_accel_gain();