// Called by Avatar::simulate after it has set the joint states (fullUpdate true if changed),
// but just before head has been simulated.
void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
updateAttitude(_owningAvatar->getWorldOrientation());
if (fullUpdate) {
setBlendshapeCoefficients(_owningAvatar->getHead()->getSummedBlendshapeCoefficients());
Parent::simulate(deltaTime, fullUpdate);
// let rig compute the model offset
glm::vec3 registrationPoint;
if (_rig.getModelRegistrationPoint(registrationPoint)) {
setOffset(registrationPoint);
}
} else {
Parent::simulate(deltaTime, fullUpdate);
}
// FIXME: This texture loading logic should probably live in Avatar, to mirror RenderableModelEntityItem and ModelOverlay,
// but Avatars don't get updates in the same way
if (!_texturesLoaded && getGeometry() && getGeometry()->areTexturesLoaded()) {
_texturesLoaded = true;
updateRenderItems();
}
if (!isActive() || !_owningAvatar->isMyAvatar()) {
return; // only simulate for own avatar
}
auto player = DependencyManager::get<recording::Deck>();
if (player->isPlaying()) {
return;
}
}
ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent)
: QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)), parent)
, m_Light()
, m_World()
, m_GlView(this)
, m_MoverController()
, m_ModelBoundingBox()
, m_MotionTimer()
, acFilename()
, bgFilename()
, vboEnable(false)
{
setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
CreateScene();
QColor repColor;
repColor.setRgbF(1.0, 0.11372, 0.11372, 0.0);
m_MoverController = GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView);
// Get required UAVObjects
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
attActual = AttitudeActual::GetInstance(objManager);
connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(updateAttitude()));
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Main task loop
while (1) {
if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
// Called by Avatar::simulate after it has set the joint states (fullUpdate true if changed),
// but just before head has been simulated.
void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
updateAttitude();
if (fullUpdate) {
setBlendshapeCoefficients(_owningAvatar->getHead()->getSummedBlendshapeCoefficients());
Parent::simulate(deltaTime, fullUpdate);
// let rig compute the model offset
glm::vec3 registrationPoint;
if (_rig.getModelRegistrationPoint(registrationPoint)) {
setOffset(registrationPoint);
}
} else {
Parent::simulate(deltaTime, fullUpdate);
}
if (!isActive() || !_owningAvatar->isMyAvatar()) {
return; // only simulate for own avatar
}
auto player = DependencyManager::get<recording::Deck>();
if (player->isPlaying()) {
return;
}
}
ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent)
: QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)), parent)
, m_Light()
, m_World()
, m_GlView()
, m_MoverController()
, m_ModelBoundingBox()
, m_MotionTimer()
, acFilename()
, bgFilename()
, vboEnable(false)
{
connect(&m_GlView, SIGNAL(updateOpenGL()), this, SLOT(updateGL()));
setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
m_Light.setPosition(4000.0, 40000.0, 80000.0);
// m_GlView.setBackgroundColor(Qt::white);
m_Light.setAmbientColor(Qt::lightGray);
m_GlView.cameraHandle()->setDefaultUpVector(glc::Z_AXIS);
m_GlView.cameraHandle()->setRearView();
QColor repColor;
repColor.setRgbF(1.0, 0.11372, 0.11372, 0.0);
m_MoverController = GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView);
CreateScene();
// Get required UAVObjects
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
attState = AttitudeActual::GetInstance(objManager);
connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(updateAttitude()));
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
/**
* @brief -- parse mavlink_msg after the char is processed
* @param msg -- the msg to be parsed
* @return -- NULL
*/
void UAS_comm::parseApmMessage(mavlink_message_t* pMsg){
switch (pMsg->msgid){
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(pMsg, &heartbeat);
bypassMessage(chan_gcs, pMsg);
break;
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(pMsg, &attitude);
updateAttitude(attitude.roll,
attitude.pitch,
attitude.yaw,
attitude.rollspeed,
attitude.pitchspeed,
attitude.yawspeed);
bypassMessage(chan_gcs, pMsg);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
mavlink_global_position_int_t global_position;
mavlink_msg_global_position_int_decode(pMsg, &global_position);
updateGlobalPosition(global_position.lat,
global_position.lon,
global_position.alt,
global_position.relative_alt,
global_position.vx,
global_position.vy,
global_position.vz,
global_position.hdg);
bypassMessage(chan_gcs, pMsg);
break;
default:
if (isGcsOpen()) {
bypassMessage(chan_gcs, pMsg); // pass the message to gcs
}
break;
} // end switch
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
enum complimentary_filter_status complimentary_filter_status;
complimentary_filter_status = CF_POWERON;
uint32_t arming_count = 0;
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (complimentary_filter_status == CF_POWERON) {
complimentary_filter_status = (xTaskGetTickCount() > 1000) ?
CF_INITIALIZING : CF_POWERON;
} else if(complimentary_filter_status == CF_INITIALIZING &&
(xTaskGetTickCount() < 7000) &&
(xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 0.1f + 0.1f * (xTaskGetTickCount() < 4000);
accelKi = 0.1;
yawBiasRate = 0.1;
accel_filter_enabled = false;
} else if (zero_during_arming &&
(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
// Use a rapidly decrease accelKp to force the attitude to snap back
// to level and then converge more smoothly
if (arming_count < 20)
accelKp = 1.0f;
else if (accelKp > 0.1f)
accelKp -= 0.01f;
arming_count++;
accelKi = 0.1f;
yawBiasRate = 0.1f;
accel_filter_enabled = false;
// Indicate arming so that after arming it reloads
// the normal settings
if (complimentary_filter_status != CF_ARMING) {
accumulate_gyro_zero();
complimentary_filter_status = CF_ARMING;
accumulating_gyro = true;
}
} else if (complimentary_filter_status == CF_ARMING ||
complimentary_filter_status == CF_INITIALIZING) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
if(accel_alpha > 0.0f)
accel_filter_enabled = true;
// If arming that means we were accumulating gyro
// samples. Compute new bias.
if (complimentary_filter_status == CF_ARMING) {
accumulate_gyro_compute();
accumulating_gyro = false;
arming_count = 0;
}
// Indicate normal mode to prevent rerunning this code
complimentary_filter_status = CF_NORMAL;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AccelsData accels;
GyrosData gyros;
int32_t retval = 0;
retval = updateSensorsCC3D(&accels, &gyros);
// During power on set to angle from accel
if (complimentary_filter_status == CF_POWERON) {
float RPY[3];
float theta = atan2f(accels.x, -accels.z);
RPY[1] = theta * RAD2DEG;
RPY[0] = atan2f(-accels.y, -accels.z / cosf(theta)) * RAD2DEG;
RPY[2] = 0;
RPY2Quaternion(RPY, q);
}
// Only update attitude when sensor data is good
if (retval != 0)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else {
// Do not update attitude data in simulation mode
if (!AttitudeActualReadOnly())
updateAttitude(&accels, &gyros);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
}
}
