#include "SoloGimbal_Parameters.h" #if HAL_SOLO_GIMBAL_ENABLED #include <AP_HAL/AP_HAL.h> #include <AP_Logger/AP_Logger.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; DEV_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; } #endif // HAL_SOLO_GIMBAL_ENABLED