diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp index e286ae0..c432346 100644 --- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp +++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp @@ -3,7 +3,7 @@ * @Author: L LC @amov * @Date: 2023-11-02 17:50:26 * @LastEditors: L LC @amov - * @LastEditTime: 2023-11-28 11:46:28 + * @LastEditTime: 2023-12-04 09:56:21 * @FilePath: /SpireCV/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp */ #include @@ -11,10 +11,10 @@ /** * The function sets the target position of a gimbal based on the input roll, pitch, and yaw values. - * + * * @param pos The parameter "pos" is of type "amovGimbal::AMOV_GIMBAL_POS_T". It is a structure that * contains the roll, pitch, and yaw values of the gimbal position. - * + * * @return a packed value of type uint32_t. */ uint32_t GX40GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &pos) @@ -29,10 +29,10 @@ uint32_t GX40GimbalDriver::setGimabalPos(const amovGimbal::AMOV_GIMBAL_POS_T &po /** * The function sets the gimbal speed based on the provided roll, pitch, and yaw values. - * + * * @param speed The parameter "speed" is of type "amovGimbal::AMOV_GIMBAL_POS_T". It is a structure * that contains the roll, pitch, and yaw values of the gimbal speed. - * + * * @return the result of the pack() function, which is of type uint32_t. */ uint32_t GX40GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T &speed) @@ -49,7 +49,7 @@ uint32_t GX40GimbalDriver::setGimabalSpeed(const amovGimbal::AMOV_GIMBAL_POS_T & /** * The function sets the gimbal's home position to (0, 0, 0) and sends a command to the gimbal to go to * the home position. - * + * * @return the result of the pack() function call with the arguments GX40::GIMBAL_CMD_HOME, nullptr, and * 0. */ @@ -68,7 +68,7 @@ uint32_t GX40GimbalDriver::setGimabalHome(void) /** * The function `takePic` in the `GX40GimbalDriver` class takes a picture using the GX40 gimbal and * returns the packed command. - * + * * @return a uint32_t value. */ uint32_t GX40GimbalDriver::takePic(void) @@ -80,10 +80,10 @@ uint32_t GX40GimbalDriver::takePic(void) /** * The function `setVideo` toggles the video state of a gimbal driver and returns a packed command. - * + * * @param newState The parameter `newState` is of type `amovGimbal::AMOV_GIMBAL_VIDEO_T`, which is an * enumeration representing the state of the video in the gimbal. It can have two possible values: - * + * * @return the result of the `pack` function, which is a `uint32_t` value. */ uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newState) @@ -107,7 +107,7 @@ uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta /** * The function `attitudeCorrection` updates the state of a gimbal driver with position, velocity, and * acceleration data. - * + * * @param pos The "pos" parameter is of type "amovGimbal::AMOV_GIMBAL_POS_T" and represents the current * position of the gimbal. It likely contains information such as the pitch, yaw, and roll angles of * the gimbal. @@ -120,15 +120,15 @@ uint32_t GX40GimbalDriver::setVideo(const amovGimbal::AMOV_GIMBAL_VIDEO_T newSta * attitudeCorrection function. It can be used to provide any extra information or context that may be * needed for the attitude correction calculation. The specific type and structure of the extenData is * not provided in the code snippet, - * + * * @return the size of the data being passed as arguments. The size is calculated by adding the sizes * of the three types: sizeof(amovGimbal::AMOV_GIMBAL_POS_T), * sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T), and sizeof(amovGimbal::AMOV_GIMBAL_VELOCITY_T). */ uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_T &pos, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, - const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, - void *extenData) + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &seppd, + const amovGimbal::AMOV_GIMBAL_VELOCITY_T &acc, + void *extenData) { carrierStateMutex.lock(); carrierPos = pos; @@ -142,11 +142,11 @@ uint32_t GX40GimbalDriver::attitudeCorrection(const amovGimbal::AMOV_GIMBAL_POS_ /** * The function `extensionFuntions` takes a command as input, converts it to a specific format, and * returns a 32-bit unsigned integer. - * + * * @param cmd The parameter "cmd" is a void pointer, which means it can point to any type of data. In * this case, it is being cast to a uint8_t pointer, which means it is expected to point to an array of * uint8_t (8-bit unsigned integers). - * + * * @return a value of type uint32_t. */ uint32_t GX40GimbalDriver::extensionFuntions(void *cmd) @@ -158,13 +158,13 @@ uint32_t GX40GimbalDriver::extensionFuntions(void *cmd) /** * The function `setGimbalZoom` in the `GX40GimbalDriver` class sets the zoom level of a gimbal based on * the specified zoom type and target rate. - * + * * @param zoom The "zoom" parameter is of type amovGimbal::AMOV_GIMBAL_ZOOM_T, which is an enumeration * type. It represents the zoom action to be performed on the gimbal. The possible values for this * parameter are: * @param targetRate The targetRate parameter is a float value representing the desired zoom rate for * the gimbal. It is used to control the zoom functionality of the gimbal. - * + * * @return a value of type uint32_t. */ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) @@ -203,13 +203,13 @@ uint32_t GX40GimbalDriver::setGimbalZoom(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, fl /** * The function "setGimbalFocus" sets the focus of a gimbal by specifying the zoom level and target * rate. - * + * * @param zoom The zoom parameter is of type amovGimbal::AMOV_GIMBAL_ZOOM_T, which is an enumeration * type representing different zoom levels for the gimbal. It is used to specify the desired zoom level * for the gimbal focus. * @param targetRate The targetRate parameter is a float value representing the desired zoom rate for * the gimbal. - * + * * @return the result of the pack() function, which is of type uint32_t. */ uint32_t GX40GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, float targetRate) @@ -221,7 +221,7 @@ uint32_t GX40GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, f /** * The function sets the GNSS information in the carrierGNSS struct and returns the size of the struct. - * + * * @param lng The "lng" parameter represents the longitude value of the GNSS (Global Navigation * Satellite System) information. * @param lat The "lat" parameter represents the latitude value of the GNSS (Global Navigation @@ -231,7 +231,7 @@ uint32_t GX40GimbalDriver::setGimbalFocus(amovGimbal::AMOV_GIMBAL_ZOOM_T zoom, f * System) information. It is of type uint32_t, which means it is an unsigned 32-bit integer. The * specific values and their meanings for the "nState" parameter are not provided in the code snippet * @param relAlt Relative altitude of the carrier (in meters) - * + * * @return the size of the structure GX40::GIMBAL_SECONDARY_MASTER_FRAME_T. */ uint32_t GX40GimbalDriver::setGNSSInfo(float lng, float lat, float alt, uint32_t nState, float relAlt) @@ -239,11 +239,11 @@ uint32_t GX40GimbalDriver::setGNSSInfo(float lng, float lat, float alt, uint32_t carrierStateMutex.lock(); carrierGNSS.head = 0X01; - carrierGNSS.lng = lng; - carrierGNSS.lat = lat; - carrierGNSS.alt = alt; + carrierGNSS.lng = lng / 1E-7; + carrierGNSS.lat = lat / 1E-7; + carrierGNSS.alt = alt / 1E-3; - carrierGNSS.relAlt = relAlt; + carrierGNSS.relAlt = relAlt / 1E-3; carrierGNSS.nState = nState; carrierStateMutex.unlock();