ardupilot/libraries/AP_Mount/SoloGimbal_Parameters.cpp

294 lines
9.1 KiB
C++

#include "SoloGimbal_Parameters.h"
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
extern const AP_HAL::HAL& hal;
const uint32_t SoloGimbal_Parameters::_retry_period = 3000;
const uint8_t SoloGimbal_Parameters::_max_fetch_attempts = 5;
SoloGimbal_Parameters::SoloGimbal_Parameters()
{
reset();
}
void SoloGimbal_Parameters::reset()
{
memset(_params,0,sizeof(_params));
_last_request_ms = 0;
_last_set_ms = 0;
_flashing_step = GMB_PARAM_NOT_FLASHING;
}
const char* SoloGimbal_Parameters::get_param_name(gmb_param_t param)
{
switch(param) {
case GMB_PARAM_GMB_OFF_ACC_X:
return "GMB_OFF_ACC_X";
case GMB_PARAM_GMB_OFF_ACC_Y:
return "GMB_OFF_ACC_Y";
case GMB_PARAM_GMB_OFF_ACC_Z:
return "GMB_OFF_ACC_Z";
case GMB_PARAM_GMB_GN_ACC_X:
return "GMB_GN_ACC_X";
case GMB_PARAM_GMB_GN_ACC_Y:
return "GMB_GN_ACC_Y";
case GMB_PARAM_GMB_GN_ACC_Z:
return "GMB_GN_ACC_Z";
case GMB_PARAM_GMB_OFF_GYRO_X:
return "GMB_OFF_GYRO_X";
case GMB_PARAM_GMB_OFF_GYRO_Y:
return "GMB_OFF_GYRO_Y";
case GMB_PARAM_GMB_OFF_GYRO_Z:
return "GMB_OFF_GYRO_Z";
case GMB_PARAM_GMB_OFF_JNT_X:
return "GMB_OFF_JNT_X";
case GMB_PARAM_GMB_OFF_JNT_Y:
return "GMB_OFF_JNT_Y";
case GMB_PARAM_GMB_OFF_JNT_Z:
return "GMB_OFF_JNT_Z";
case GMB_PARAM_GMB_K_RATE:
return "GMB_K_RATE";
case GMB_PARAM_GMB_POS_HOLD:
return "GMB_POS_HOLD";
case GMB_PARAM_GMB_MAX_TORQUE:
return "GMB_MAX_TORQUE";
case GMB_PARAM_GMB_SND_TORQUE:
return "GMB_SND_TORQUE";
case GMB_PARAM_GMB_SYSID:
return "GMB_SYSID";
case GMB_PARAM_GMB_FLASH:
return "GMB_FLASH";
default:
return "";
};
}
void SoloGimbal_Parameters::fetch_params()
{
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (_params[i].state != GMB_PARAMSTATE_NOT_YET_READ) {
_params[i].state = GMB_PARAMSTATE_FETCH_AGAIN;
}
}
}
bool SoloGimbal_Parameters::initialized()
{
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ) {
return false;
}
}
return true;
}
bool SoloGimbal_Parameters::received_all()
{
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ || _params[i].state == GMB_PARAMSTATE_FETCH_AGAIN) {
return false;
}
}
return true;
}
void SoloGimbal_Parameters::get_param(gmb_param_t param, float& value, float def_val) {
if (!_params[param].seen) {
value = def_val;
} else {
value = _params[param].value;
}
}
void SoloGimbal_Parameters::set_param(gmb_param_t param, float value) {
if ((_params[param].state == GMB_PARAMSTATE_CONSISTENT && param != GMB_PARAM_GMB_FLASH &&
is_equal(_params[param].value,value)) || _params[param].state == GMB_PARAMSTATE_NONEXISTANT ||
!HAVE_PAYLOAD_SPACE(_chan, PARAM_SET)) {
return;
}
_params[param].state = GMB_PARAMSTATE_ATTEMPTING_TO_SET;
_params[param].value = value;
// make a temporary copy of the ID; mavlink_msg_param_set_send
// expects an array of the full length
char tmp_name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN+1] {};
strncpy(tmp_name, get_param_name(param), MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, tmp_name, _params[param].value, MAV_PARAM_TYPE_REAL32);
_last_set_ms = AP_HAL::millis();
}
void SoloGimbal_Parameters::update()
{
uint32_t tnow_ms = AP_HAL::millis();
// retry initial param retrieval
if(!received_all() && ((tnow_ms - _last_request_ms) > _retry_period) &&
(HAVE_PAYLOAD_SPACE(_chan, PARAM_REQUEST_LIST))) {
_last_request_ms = tnow_ms;
mavlink_msg_param_request_list_send(_chan, 0, MAV_COMP_ID_GIMBAL);
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!_params[i].seen) {
_params[i].fetch_attempts++;
}
}
}
// retry param_set
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!HAVE_PAYLOAD_SPACE(_chan, PARAM_SET)) {
break;
}
if ((_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) && (tnow_ms - _last_set_ms > _retry_period)) {
mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name((gmb_param_t)i), _params[i].value, MAV_PARAM_TYPE_REAL32);
_last_set_ms = AP_HAL::millis();
if (!_params[i].seen) {
_params[i].fetch_attempts++;
}
}
}
// check for nonexistent parameters
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!_params[i].seen && _params[i].fetch_attempts > _max_fetch_attempts) {
_params[i].state = GMB_PARAMSTATE_NONEXISTANT;
hal.console->printf("Gimbal parameter %s timed out\n", get_param_name((gmb_param_t)i));
}
}
if(_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_SET) {
bool done = true;
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) {
done = false;
break;
}
}
if (done) {
_flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_ACK;
set_param(GMB_PARAM_GMB_FLASH,69.0f);
}
}
}
void SoloGimbal_Parameters::handle_param_value(const mavlink_message_t *msg)
{
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
AP_Logger *logger = AP_Logger::get_singleton();
if (logger != nullptr) {
logger->Write_Parameter(packet.param_id, packet.param_value);
}
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
_params[i].seen = true;
switch(_params[i].state) {
case GMB_PARAMSTATE_NONEXISTANT:
case GMB_PARAMSTATE_NOT_YET_READ:
case GMB_PARAMSTATE_FETCH_AGAIN:
_params[i].value = packet.param_value;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
break;
case GMB_PARAMSTATE_CONSISTENT:
_params[i].value = packet.param_value;
break;
case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
if (i == GMB_PARAM_GMB_FLASH) {
if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && (int)packet.param_value == 1) {
_flashing_step = GMB_PARAM_NOT_FLASHING;
}
_params[i].value = 0;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
} else if (is_equal(packet.param_value,_params[i].value)) {
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
}
break;
}
break;
}
}
}
Vector3f SoloGimbal_Parameters::get_accel_bias()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_OFF_ACC_X,ret.x);
get_param(GMB_PARAM_GMB_OFF_ACC_Y,ret.y);
get_param(GMB_PARAM_GMB_OFF_ACC_Z,ret.z);
return ret;
}
Vector3f SoloGimbal_Parameters::get_accel_gain()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_GN_ACC_X,ret.x,1.0f);
get_param(GMB_PARAM_GMB_GN_ACC_Y,ret.y,1.0f);
get_param(GMB_PARAM_GMB_GN_ACC_Z,ret.z,1.0f);
return ret;
}
void SoloGimbal_Parameters::set_accel_bias(const Vector3f& bias)
{
set_param(GMB_PARAM_GMB_OFF_ACC_X, bias.x);
set_param(GMB_PARAM_GMB_OFF_ACC_Y, bias.y);
set_param(GMB_PARAM_GMB_OFF_ACC_Z, bias.z);
}
void SoloGimbal_Parameters::set_accel_gain(const Vector3f& gain)
{
set_param(GMB_PARAM_GMB_GN_ACC_X, gain.x);
set_param(GMB_PARAM_GMB_GN_ACC_Y, gain.y);
set_param(GMB_PARAM_GMB_GN_ACC_Z, gain.z);
}
Vector3f SoloGimbal_Parameters::get_gyro_bias()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_OFF_GYRO_X,ret.x);
get_param(GMB_PARAM_GMB_OFF_GYRO_Y,ret.y);
get_param(GMB_PARAM_GMB_OFF_GYRO_Z,ret.z);
return ret;
}
void SoloGimbal_Parameters::set_gyro_bias(const Vector3f& bias)
{
set_param(GMB_PARAM_GMB_OFF_GYRO_X,bias.x);
set_param(GMB_PARAM_GMB_OFF_GYRO_Y,bias.y);
set_param(GMB_PARAM_GMB_OFF_GYRO_Z,bias.z);
}
Vector3f SoloGimbal_Parameters::get_joint_bias()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_OFF_JNT_X,ret.x);
get_param(GMB_PARAM_GMB_OFF_JNT_Y,ret.y);
get_param(GMB_PARAM_GMB_OFF_JNT_Z,ret.z);
return ret;
}
float SoloGimbal_Parameters::get_K_rate()
{
float ret;
get_param(GMB_PARAM_GMB_K_RATE,ret);
return ret;
}
void SoloGimbal_Parameters::flash()
{
_flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_SET;
}
bool SoloGimbal_Parameters::flashing()
{
return _flashing_step != GMB_PARAM_NOT_FLASHING;
}