Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: report MAVLink gimbal angle to the GCS

Browse Source
This commit is contained in:
Arthur Benemann 2015-04-01 16:39:09 -07:00 committed by Randy Mackay
parent b3c35aee4e
commit 255c5b3025
3 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Gimbal/AP_Gimbal.cpp
View File

@ -178,6 +178,12 @@ void AP_Gimbal::update_target(Vector3f newTarget)
}
Vector3f AP_Gimbal::getGimbalEstimateEF()
{
Quaternion quatEst;_ekf.getQuat(quatEst);Vector3f eulerEst;quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
return eulerEst;
}
uint8_t AP_Gimbal::isCopterFliped(){
return fabs(_ahrs.roll)>1.0f || fabs(_ahrs.pitch)>1.0f;
}

2
libraries/AP_Gimbal/AP_Gimbal.h
View File

@ -43,6 +43,8 @@ public:
void update_target(Vector3f newTarget);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
Vector3f getGimbalEstimateEF();
struct Measurament {
float delta_time;
Vector3f delta_angles;

3
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -87,7 +87,8 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
{
// do nothing - we rely on the mount sending the messages directly
Vector3f est = _gimbal.getGimbalEstimateEF();
mavlink_msg_mount_status_send(chan, 0, 0, degrees(est.y)*100, degrees(est.x)*100, degrees(est.z)*100);
}
/*