aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: Update attitude control calculations and debug printing

Browse Source
This commit is contained in:
Paul Riseborough 2015-01-31 22:28:48 +11:00 committed by Andrew Tridgell
parent 84029f8f7c
commit 8d6f0d08c9
1 changed files with 22 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -4,7 +4,7 @@
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#define MOUNT_DEBUG 1 #define MOUNT_DEBUG 0
#if MOUNT_DEBUG #if MOUNT_DEBUG
#include <stdio.h> #include <stdio.h>
@ -185,24 +185,35 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
Quaternion quatEst; Quaternion quatEst;
_ekf.getQuat(quatEst); _ekf.getQuat(quatEst);
// set the demanded quaternion // set the demanded quaternion - tilt down with a roll and yaw of zero
Quaternion quatDem; Quaternion quatDem;
quatDem[0] = 1.0f; quatDem[0] = sqrtf(0.75f);
quatDem[2] = -sqrtf(0.25f);
//divide the demanded quaternion by the estimated to get the error //divide the demanded quaternion by the estimated to get the error
Quaternion quatError = quatDem / quatEst; Quaternion quatErr = quatDem / quatEst;
// convert the quaternion to an angle error vector // convert the quaternion to an angle error vector
Vector3f deltaAngErr; Vector3f deltaAngErr;
float scaler = 2.0f*acosf(quatError[0]); float scaler = 1.0f-quatErr[0]*quatErr[0];
scaler = scaler/sin(0.5f*scaler); if (scaler > 1e-12) {
deltaAngErr.x = quatError[1] * scaler; scaler = 1.0f/sqrtf(scaler);
deltaAngErr.y = quatError[2] * scaler; deltaAngErr.x = quatErr[1] * scaler;
deltaAngErr.z = quatError[3] * scaler; deltaAngErr.y = quatErr[2] * scaler;
deltaAngErr.z = quatErr[3] * scaler;
} else {
deltaAngErr.zero();
}
// multiply the angle error vector by a gain to calculate a demanded gimbal rate // multiply the angle error vector by a gain to calculate a demanded gimbal rate
Vector3f rateDemand = deltaAngErr * 1.0f; Vector3f rateDemand = deltaAngErr * 1.0f;
// Constrain the demanded rate to a length of 0.5 rad /sec
float length = rateDemand.length();
if (length > 0.5f) {
rateDemand = rateDemand * (0.5f / length);
}
// send the gimbal control message // send the gimbal control message
mavlink_msg_gimbal_control_send(chan, mavlink_msg_gimbal_control_send(chan,
msg->sysid, msg->sysid,
@ -232,10 +243,10 @@ void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
Vector3f velocity, euler, gyroBias; Vector3f velocity, euler, gyroBias;
_ekf.getDebug(tilt, velocity, euler, gyroBias); _ekf.getDebug(tilt, velocity, euler, gyroBias);
#if MOUNT_DEBUG #if MOUNT_DEBUG
::printf("tilt=%.2f euler(%.2f, %.2f, %.2f) bias=(%.2f, %.2f %.2f)\n", ::printf("tilt=%.2f euler=(%.2f, %.2f, %.2f) vel=(%.2f, %.2f %.2f)\n",
tilt, tilt,
degrees(euler.x), degrees(euler.y), degrees(euler.z), degrees(euler.x), degrees(euler.y), degrees(euler.z),
degrees(gyroBias.x), degrees(gyroBias.y), degrees(gyroBias.z)); (velocity.x), (velocity.y), (velocity.z));
#endif #endif
} }