mirror of https://github.com/ArduPilot/ardupilot
98 lines
2.4 KiB
C++
98 lines
2.4 KiB
C++
#pragma once
|
|
|
|
#include "AP_Mount_config.h"
|
|
|
|
#if HAL_SOLO_GIMBAL_ENABLED
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
|
|
enum gmb_param_state_t {
|
|
GMB_PARAMSTATE_NOT_YET_READ=0, // parameter has yet to be initialized
|
|
GMB_PARAMSTATE_FETCH_AGAIN=1, // parameter is being fetched
|
|
GMB_PARAMSTATE_ATTEMPTING_TO_SET=2, // parameter is being set
|
|
GMB_PARAMSTATE_CONSISTENT=3, // parameter is consistent
|
|
GMB_PARAMSTATE_NONEXISTANT=4 // parameter does not seem to exist
|
|
};
|
|
|
|
enum gmb_param_t {
|
|
GMB_PARAM_GMB_OFF_ACC_X=0,
|
|
GMB_PARAM_GMB_OFF_ACC_Y,
|
|
GMB_PARAM_GMB_OFF_ACC_Z,
|
|
GMB_PARAM_GMB_GN_ACC_X,
|
|
GMB_PARAM_GMB_GN_ACC_Y,
|
|
GMB_PARAM_GMB_GN_ACC_Z,
|
|
GMB_PARAM_GMB_OFF_GYRO_X,
|
|
GMB_PARAM_GMB_OFF_GYRO_Y,
|
|
GMB_PARAM_GMB_OFF_GYRO_Z,
|
|
GMB_PARAM_GMB_OFF_JNT_X,
|
|
GMB_PARAM_GMB_OFF_JNT_Y,
|
|
GMB_PARAM_GMB_OFF_JNT_Z,
|
|
GMB_PARAM_GMB_K_RATE,
|
|
GMB_PARAM_GMB_POS_HOLD,
|
|
GMB_PARAM_GMB_MAX_TORQUE,
|
|
GMB_PARAM_GMB_SND_TORQUE,
|
|
GMB_PARAM_GMB_SYSID,
|
|
GMB_PARAM_GMB_FLASH,
|
|
MAVLINK_GIMBAL_NUM_TRACKED_PARAMS
|
|
};
|
|
|
|
enum gmb_flashing_step_t {
|
|
GMB_PARAM_NOT_FLASHING=0,
|
|
GMB_PARAM_FLASHING_WAITING_FOR_SET,
|
|
GMB_PARAM_FLASHING_WAITING_FOR_ACK
|
|
};
|
|
|
|
class SoloGimbal_Parameters
|
|
{
|
|
public:
|
|
SoloGimbal_Parameters();
|
|
void reset();
|
|
|
|
bool initialized();
|
|
bool received_all();
|
|
void fetch_params();
|
|
|
|
void get_param(gmb_param_t param, float& value, float def_val = 0.0f);
|
|
void set_param(gmb_param_t param, float value);
|
|
|
|
void update();
|
|
void handle_param_value(const mavlink_message_t &msg);
|
|
|
|
Vector3f get_accel_bias();
|
|
Vector3f get_accel_gain();
|
|
void set_accel_bias(const Vector3f& bias);
|
|
void set_accel_gain(const Vector3f& gain);
|
|
Vector3f get_gyro_bias();
|
|
void set_gyro_bias(const Vector3f& bias);
|
|
Vector3f get_joint_bias();
|
|
|
|
float get_K_rate();
|
|
void flash();
|
|
bool flashing();
|
|
|
|
void set_channel(mavlink_channel_t chan) { _chan = chan; }
|
|
|
|
private:
|
|
static const char* get_param_name(gmb_param_t param);
|
|
|
|
static const uint32_t _retry_period;
|
|
static const uint8_t _max_fetch_attempts;
|
|
|
|
struct {
|
|
float value;
|
|
gmb_param_state_t state;
|
|
uint8_t fetch_attempts;
|
|
bool seen;
|
|
} _params[MAVLINK_GIMBAL_NUM_TRACKED_PARAMS];
|
|
|
|
uint32_t _last_request_ms;
|
|
uint32_t _last_set_ms;
|
|
gmb_flashing_step_t _flashing_step;
|
|
|
|
mavlink_channel_t _chan;
|
|
};
|
|
|
|
#endif // HAL_SOLO_GIMBAL_ENABLED
|