floratest1
/
SpireCV
1
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add GX40's attitudeCorrection and fix GX40's direction of control.

This commit is contained in:
Daniel 2024-06-11 17:12:02 +08:00
parent 1788c858b2
commit fb9b65401d
3 changed files with 76 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
View File

@ -20,9 +20,9 @@
uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
{
carrierStateMutex.lock();
targetPos[0] = (int16_t)(-pos.roll / 0.01f);
targetPos[0] = (int16_t)(pos.roll / 0.01f);
targetPos[1] = (int16_t)(-pos.pitch / 0.01f);
targetPos[2] = (int16_t)(-pos.yaw / 0.01f);
targetPos[2] = (int16_t)(pos.yaw / 0.01f);
carrierStateMutex.unlock();
return pack(GX40::GIMBAL_CMD_MODE_EULER, nullptr, 0);
}
@ -38,9 +38,9 @@ uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
uint32_t GX40GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
{
carrierStateMutex.lock();
targetPos[0] = (int16_t)(-speed.roll / 0.1f);
targetPos[0] = (int16_t)(speed.roll / 0.1f);
targetPos[1] = (int16_t)(-speed.pitch / 0.1f);
targetPos[2] = (int16_t)(-speed.yaw / 0.1f);
targetPos[2] = (int16_t)(speed.yaw / 0.1f);
carrierStateMutex.unlock();
return pack(GX40::GIMBAL_CMD_MODE_FOLLOW, nullptr, 0);

44
gimbal_ctrl/sv_gimbal.cpp
View File

@ -509,6 +509,50 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y
temp.yaw = yaw_rate;
pdevTemp->setGimabalSpeed(temp);
}
void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
AMOV_GIMBAL_QUATERNION_T temp_q;
temp_q.q0 = quaterion.q0;
temp_q.q1 = quaterion.q1;
temp_q.q2 = quaterion.q2;
temp_q.q3 = quaterion.q3;
AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
temp_v1.x = speed.x;
temp_v1.y = speed.y;
temp_v1.z = speed.z;
temp_v2.x = acc.x;
temp_v2.y = acc.y;
temp_v2.z = acc.z;
pdevTemp->attitudeCorrection(temp_q, temp_v1, temp_v2, extenData);
}
void sv::Gimbal::attitudeCorrection(const GimbalPosT &pos,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
temp_v1.x = speed.x;
temp_v1.y = speed.y;
temp_v1.z = speed.z;
temp_v2.x = acc.x;
temp_v2.y = acc.y;
temp_v2.z = acc.z;
AMOV_GIMBAL_POS_T temp_p;
temp_p.pitch = pos.pitch;
temp_p.yaw = pos.yaw;
temp_p.roll = pos.roll;
pdevTemp->attitudeCorrection(temp_p, temp_v1, temp_v2, extenData);
}
sv::Gimbal::~Gimbal()
{

28
include/sv_gimbal.h
View File

@ -78,6 +78,28 @@ namespace sv
FLOWCONTROL_HARDWARE = 2,
};
typedef struct
{
double q0;
double q1;
double q2;
double q3;
} GimbalQuaternionT;
typedef struct
{
double x; // or N
double y; // or E
double z; // or UP
} GimbalVelocityT;
typedef struct
{
double yaw;
double roll;
double pitch;
} GimbalPosT;
static inline void emptyCallback(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
double &fovX, double &fovY)
@ -165,6 +187,12 @@ namespace sv
bool takePhoto();
bool takeVideo(int state);
int getVideoState();
void attitudeCorrection(const GimbalQuaternionT &quaterion,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData);
void attitudeCorrection(const GimbalPosT &pos,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData);
//! Set gimbal angles
/*!