ardupilot/libraries/AP_Mount/SoloGimbal.h

150 lines
4.1 KiB
C++

/************************************************************
* SoloGimbal -- library to control a 3 axis rate gimbal. *
* *
* Author: Arthur Benemann, Paul Riseborough; *
* *
************************************************************/
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include "AP_Mount.h"
#if HAL_SOLO_GIMBAL_ENABLED
#include "SoloGimbalEKF.h"
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_AccelCal/AP_AccelCal.h>
#include "SoloGimbal_Parameters.h"
enum gimbal_state_t {
GIMBAL_STATE_NOT_PRESENT = 0,
GIMBAL_STATE_PRESENT_INITIALIZING,
GIMBAL_STATE_PRESENT_ALIGNING,
GIMBAL_STATE_PRESENT_RUNNING
};
enum gimbal_mode_t {
GIMBAL_MODE_IDLE = 0,
GIMBAL_MODE_POS_HOLD,
GIMBAL_MODE_POS_HOLD_FF,
GIMBAL_MODE_STABILIZE
};
class SoloGimbal : AP_AccelCal_Client
{
public:
//Constructor
SoloGimbal() :
_ekf(),
_state(GIMBAL_STATE_NOT_PRESENT),
_vehicle_yaw_rate_ef_filt(0.0f),
_vehicle_to_gimbal_quat(),
_vehicle_to_gimbal_quat_filt(),
_filtered_joint_angles(),
_last_report_msg_ms(0),
_max_torque(5000.0f),
_ang_vel_mag_filt(0),
_lockedToBody(false),
_log_dt(0),
_log_del_ang(),
_log_del_vel()
{
#if HAL_INS_ACCELCAL_ENABLED
AP_AccelCal::register_client(this);
#endif
}
void update_target(const Vector3f &newTarget);
void receive_feedback(mavlink_channel_t chan, const mavlink_message_t &msg);
void update_fast();
bool present();
bool aligned();
void set_lockedToBody(bool val) { _lockedToBody = val; }
void write_logs();
float get_log_dt() const { return _log_dt; }
void disable_torque_report() { _gimbalParams.set_param(GMB_PARAM_GMB_SND_TORQUE, 0); }
void fetch_params() { _gimbalParams.fetch_params(); }
void handle_param_value(const mavlink_message_t &msg) {
_gimbalParams.handle_param_value(msg);
}
private:
// private methods
void update_estimators();
void send_controls(mavlink_channel_t chan);
void extract_feedback(const mavlink_gimbal_report_t& report_msg);
void update_joint_angle_est();
Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_roll_tilt(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst) const;
Vector3f get_ang_vel_dem_gyro_bias();
Vector3f get_ang_vel_dem_body_lock();
void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates) const;
void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel) const;
void readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng);
void _acal_save_calibrations() override;
bool _acal_get_ready_to_sample() override;
bool _acal_get_saving() override;
AccelCalibrator* _acal_get_calibrator(uint8_t instance) override;
gimbal_mode_t get_mode();
bool joints_near_limits() const;
// private member variables
SoloGimbalEKF _ekf; // state of small EKF for gimbal
gimbal_state_t _state;
struct {
float delta_time;
Vector3f delta_angles;
Vector3f delta_velocity;
Vector3f joint_angles;
} _measurement;
float _vehicle_yaw_rate_ef_filt;
static const uint8_t _compid = MAV_COMP_ID_GIMBAL;
// joint angle filter states
Vector3f _vehicle_delta_angles;
Quaternion _vehicle_to_gimbal_quat;
Quaternion _vehicle_to_gimbal_quat_filt;
Vector3f _filtered_joint_angles;
uint32_t _last_report_msg_ms;
float _max_torque;
float _ang_vel_mag_filt;
Vector3f _ang_vel_dem_rads; // rad/s
Vector3f _att_target_euler_rad; // desired earth-frame roll, tilt and pan angles in radians
bool _lockedToBody;
SoloGimbal_Parameters _gimbalParams;
AccelCalibrator _calibrator;
float _log_dt;
Vector3f _log_del_ang;
Vector3f _log_del_vel;
};
#endif // HAL_SOLO_GIMBAL_ENABLED