mirror of https://github.com/ArduPilot/ardupilot
97 lines
3.6 KiB
C++
97 lines
3.6 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
|
MAVLink enabled mount backend class
|
|
*/
|
|
|
|
#ifndef __AP_MOUNT_MAVLINK_H__
|
|
#define __AP_MOUNT_MAVLINK_H__
|
|
|
|
#include <AP_HAL.h>
|
|
#include <AP_AHRS.h>
|
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
|
#include <AP_Math.h>
|
|
#include <AP_Common.h>
|
|
#include <AP_GPS.h>
|
|
#include <GCS_MAVLink.h>
|
|
#include <RC_Channel.h>
|
|
#include <AP_Mount_Backend.h>
|
|
#include <AP_SmallEKF.h>
|
|
|
|
class AP_Mount_MAVLink : public AP_Mount_Backend
|
|
{
|
|
|
|
public:
|
|
// Constructor
|
|
AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
|
|
|
|
// init - performs any required initialisation for this instance
|
|
virtual void init(const AP_SerialManager& serial_manager);
|
|
|
|
// update mount position - should be called periodically
|
|
virtual void update();
|
|
|
|
// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
|
|
virtual bool has_pan_control() const;
|
|
|
|
// 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);
|
|
|
|
// handle a GIMBAL_REPORT message
|
|
virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
|
|
|
|
// send a GIMBAL_REPORT message to the GCS
|
|
virtual void send_gimbal_report(mavlink_channel_t chan);
|
|
|
|
private:
|
|
// internal variables
|
|
bool _initialised; // true once the driver has been initialised
|
|
|
|
// state of small EKF for gimbal
|
|
SmallEKF _ekf;
|
|
|
|
// keep last gimbal report
|
|
mavlink_gimbal_report_t _gimbal_report;
|
|
|
|
float vehicleYawRateFilt;
|
|
const float K_gimbalRate;
|
|
const float angRateLimit;
|
|
|
|
// circular frequency (rad/sec) constant of filter applied to forward path vehicle yaw rate
|
|
// this frequency must not be larger than the update rate (Hz).
|
|
// reducing it makes the gimbal yaw less responsive to vehicle yaw
|
|
// increasing it makes the gimbal yawe more responsive to vehicle yaw
|
|
const float yawRateFiltPole;
|
|
|
|
// amount of yaw angle that we permit the gimbal to lag the vehicle when operating in slave mode
|
|
// reducing this makes the gimbal respond more to vehicle yaw disturbances
|
|
const float yawErrorLimit;
|
|
|
|
// quaternion demanded at the previous time step
|
|
Quaternion lastQuatDem;
|
|
|
|
Vector3f quaternion_to_vector(const Quaternion &quat);
|
|
Matrix3f vector312_to_rotation_matrix(const Vector3f &vector);
|
|
Vector3f gimbal_update_control1(const Vector3f &ef_target_euler_rad,
|
|
float delta_time,
|
|
const Vector3f &delta_angles,
|
|
const Vector3f &delta_velocity,
|
|
const Vector3f &joint_angles);
|
|
Vector3f gimbal_update_control2(const Vector3f &ef_target_euler_rad,
|
|
float delta_time,
|
|
const Vector3f &delta_angles,
|
|
const Vector3f &delta_velocity,
|
|
const Vector3f &joint_angles);
|
|
Vector3f getGimbalRateDemVecYaw(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst, const Vector3f &joint_angles);
|
|
Vector3f getGimbalRateDemVecTilt(const Vector3f &ef_target_euler_rad, const Quaternion &quatEst);
|
|
Vector3f getGimbalRateDemVecForward(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst);
|
|
};
|
|
|
|
#endif // AP_AHRS_NAVEKF_AVAILABLE
|
|
|
|
#endif // __AP_MOUNT_MAVLINK_H__
|