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

add attitudeCorrection function

This commit is contained in:
Eason Yi 2024-07-18 00:16:09 +08:00
parent 535f0f3736
commit d4d26bc565
8 changed files with 30 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gimbal_ctrl/driver/src/GX40/GX40_gimbal_driver.h
View File

@ -56,6 +56,7 @@ public:
uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc,
float lng, float lat, float alt, uint32_t nState, float relAlt,
void *extenData);
uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);

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

@ -134,12 +134,21 @@ uint32_t GX40GimbalDriver::setVideo(const AMOV_GIMBAL_VIDEO_T newState)
uint32_t GX40GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc,
float lng, float lat, float alt, uint32_t nState, float relAlt,
void *extenData)
{
carrierStateMutex.lock();
carrierPos = pos;
carrierSpeed = seppd;
carrierAcc = acc;
carrierGNSS.lng = lng / 1E-7;
carrierGNSS.lat = lat / 1E-7;
carrierGNSS.alt = alt / 1E-3;
carrierGNSS.relAlt = relAlt / 1E-3;
carrierGNSS.nState = nState;
upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
carrierStateMutex.unlock();
return sizeof(AMOV_GIMBAL_POS_T) + sizeof(AMOV_GIMBAL_VELOCITY_T) + sizeof(AMOV_GIMBAL_VELOCITY_T);

4
gimbal_ctrl/driver/src/amovGimbal/amov_gimbal.h
View File

@ -83,10 +83,10 @@ namespace amovGimbal
uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData);
uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData);
uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
uint32_t setDownwardShootingMode();
uint32_t extensionFuntions(void *cmd);

12
gimbal_ctrl/driver/src/amov_gimbal_interface.cpp
View File

@ -142,28 +142,28 @@ uint32_t amovGimbal::IamovGimbalBase::setVideo(const AMOV_GIMBAL_VIDEO_T newStat
uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData)
{
return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(quaterion, speed, acc, extenData);
return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(quaterion, speed, acc,lng, lat, alt, nState, relAlt, extenData);
}
uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData)
{
return 0;
}
uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData)
{
return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(pos, speed, acc, extenData);
return ((amovGimbalBase *)(this->devHandle))->attitudeCorrection(pos, speed, acc, lng, lat, alt, nState, relAlt,extenData);
}
uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &speed,
const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData)
const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData)
{
return 0;
}

8
gimbal_ctrl/driver/src/amov_gimbal_interface_c.cpp
View File

@ -98,16 +98,16 @@ uint32_t amovGimbalSetVideo(AMOV_GIMBAL_VIDEO_T newState, void *handle)
uint32_t amovGimbalAttitudeCorrectionQ(AMOV_GIMBAL_QUATERNION_T *quaterion,
AMOV_GIMBAL_VELOCITY_T *speed,
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle)
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, float lng, float lat, float alt, uint32_t nState, float relAlt,void *handle)
{
return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*quaterion, *speed, *acc, extenData);
return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*quaterion, *speed, *acc, lng, lat, alt, nState, relAlt,extenData);
}
uint32_t amovGimbalAttitudeCorrectionE(AMOV_GIMBAL_POS_T *pos,
AMOV_GIMBAL_VELOCITY_T *speed,
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, void *handle)
AMOV_GIMBAL_VELOCITY_T *acc, void *extenData, float lng, float lat, float alt, uint32_t nState, float relAlt,void *handle)
{
return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*pos, *speed, *acc, extenData);
return ((amovGimbal::gimbal *)handle)->attitudeCorrection(*pos, *speed, *acc, lng, lat, alt, nState, relAlt,extenData);
}
uint32_t amovGimbalSetGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt, void *handle)

4
gimbal_ctrl/driver/src/amov_gimbal_private.h
View File

@ -90,8 +90,8 @@ namespace amovGimbal
virtual uint32_t setGimbalROI(const AMOV_GIMBAL_ROI_T area);
virtual uint32_t takePic(void);
virtual uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState);
virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, void *extenData);
virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, const AMOV_GIMBAL_VELOCITY_T &speed, const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData);
virtual uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData);
virtual uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
virtual uint32_t setDownwardShootingMode();
virtual uint32_t extensionFuntions(void *cmd);

8
gimbal_ctrl/sv_gimbal.cpp
View File

@ -517,7 +517,7 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y
}
void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData)
const GimbalVelocityT &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
AMOV_GIMBAL_QUATERNION_T temp_q;
@ -535,12 +535,12 @@ void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
temp_v2.y = acc.y;
temp_v2.z = acc.z;
pdevTemp->attitudeCorrection(temp_q, temp_v1, temp_v2, extenData);
pdevTemp->attitudeCorrection(temp_q, temp_v1, temp_v2, lng, lat, alt, nState, relAlt,extenData);
}
void sv::Gimbal::attitudeCorrection(const GimbalPosT &pos,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData)
const GimbalVelocityT &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData)
{
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
@ -557,7 +557,7 @@ void sv::Gimbal::attitudeCorrection(const GimbalPosT &pos,
temp_p.yaw = pos.yaw;
temp_p.roll = pos.roll;
pdevTemp->attitudeCorrection(temp_p, temp_v1, temp_v2, extenData);
pdevTemp->attitudeCorrection(temp_p, temp_v1, temp_v2, lng, lat, alt, nState, relAlt,extenData);
}
void sv::Gimbal::setDownwardShootingMode()

4
include/sv_gimbal.h
View File

@ -190,10 +190,10 @@ namespace sv
int getVideoState();
void attitudeCorrection(const GimbalQuaternionT &quaterion,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData);
const GimbalVelocityT &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData);
void attitudeCorrection(const GimbalPosT &pos,
const GimbalVelocityT &speed,
const GimbalVelocityT &acc, void *extenData);
const GimbalVelocityT &acc, float lng, float lat, float alt, uint32_t nState, float relAlt,void *extenData);
void setDownwardShootingMode();