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, uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
float lng, float lat, float alt, uint32_t nState, float relAlt,
void *extenData); void *extenData);
uint32_t setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt); 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, uint32_t GX40GimbalDriver::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd, const AMOV_GIMBAL_VELOCITY_T &seppd,
const AMOV_GIMBAL_VELOCITY_T &acc, const AMOV_GIMBAL_VELOCITY_T &acc,
float lng, float lat, float alt, uint32_t nState, float relAlt,
void *extenData) void *extenData)
{ {
carrierStateMutex.lock(); carrierStateMutex.lock();
carrierPos = pos; carrierPos = pos;
carrierSpeed = seppd; carrierSpeed = seppd;
carrierAcc = acc; 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()); upDataTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
carrierStateMutex.unlock(); carrierStateMutex.unlock();
return sizeof(AMOV_GIMBAL_POS_T) + sizeof(AMOV_GIMBAL_VELOCITY_T) + sizeof(AMOV_GIMBAL_VELOCITY_T); 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 setVideo(const AMOV_GIMBAL_VIDEO_T newState);
uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion, uint32_t attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed, 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, uint32_t attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &seppd, 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 setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
uint32_t setDownwardShootingMode(); uint32_t setDownwardShootingMode();
uint32_t extensionFuntions(void *cmd); 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, uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed, 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, uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_QUATERNION_T &quaterion,
const AMOV_GIMBAL_VELOCITY_T &speed, 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; return 0;
} }
uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos, uint32_t amovGimbal::gimbal::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &speed, 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, uint32_t amovGimbal::IamovGimbalBase::attitudeCorrection(const AMOV_GIMBAL_POS_T &pos,
const AMOV_GIMBAL_VELOCITY_T &speed, 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; 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, uint32_t amovGimbalAttitudeCorrectionQ(AMOV_GIMBAL_QUATERNION_T *quaterion,
AMOV_GIMBAL_VELOCITY_T *speed, 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, uint32_t amovGimbalAttitudeCorrectionE(AMOV_GIMBAL_POS_T *pos,
AMOV_GIMBAL_VELOCITY_T *speed, 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) 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 setGimbalROI(const AMOV_GIMBAL_ROI_T area);
virtual uint32_t takePic(void); virtual uint32_t takePic(void);
virtual uint32_t setVideo(const AMOV_GIMBAL_VIDEO_T newState); 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_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, 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 setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt);
virtual uint32_t setDownwardShootingMode(); virtual uint32_t setDownwardShootingMode();
virtual uint32_t extensionFuntions(void *cmd); 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, void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
const GimbalVelocityT &speed, 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; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
AMOV_GIMBAL_QUATERNION_T temp_q; AMOV_GIMBAL_QUATERNION_T temp_q;
@ -535,12 +535,12 @@ void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
temp_v2.y = acc.y; temp_v2.y = acc.y;
temp_v2.z = acc.z; 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, void sv::Gimbal::attitudeCorrection(const GimbalPosT &pos,
const GimbalVelocityT &speed, 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; amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2; 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.yaw = pos.yaw;
temp_p.roll = pos.roll; 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() void sv::Gimbal::setDownwardShootingMode()

4
include/sv_gimbal.h
View File

@ -190,10 +190,10 @@ namespace sv
int getVideoState(); int getVideoState();
void attitudeCorrection(const GimbalQuaternionT &quaterion, void attitudeCorrection(const GimbalQuaternionT &quaterion,
const GimbalVelocityT &speed, 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, void attitudeCorrection(const GimbalPosT &pos,
const GimbalVelocityT &speed, 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(); void setDownwardShootingMode();