void Navigator_Dcm::updateFast(float dt) {
if (_board->getMode() != AP_Board::MODE_LIVE)
return;
setTimeStamp(micros()); // if running in live mode, record new time stamp
// use range finder if attached and close to the ground
if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) {
setAlt(_rangeFinderDown->distance);
// otherwise if you have a baro attached, use it
} else if (_board->baro) {
/**
* The altitued is read off the barometer by implementing the following formula:
* altitude (in m) = 44330*(1-(p/po)^(1/5.255)),
* where, po is pressure in Pa at sea level (101325 Pa).
* See http://www.sparkfun.com/tutorials/253 or type this formula
* in a search engine for more information.
* altInt contains the altitude in meters.
*
* pressure input is in pascals
* temp input is in deg C *10
*/
_board->baro->Read(); // Get new data from absolute pressure sensor
float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295)));
setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_board->baro->Press/101325.0),0.190295)))) - reference,dt));
//_board->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_board->baro->Press,_board->baro->Temp);
// last resort, use gps altitude
} else if (_board->gps && _board->gps->fix) {
setAlt_intM(_board->gps->altitude * 10); // gps in cm, intM in mm
}
// update dcm calculations and navigator data
//
_dcm.update_DCM_fast();
setRoll(_dcm.roll);
setPitch(_dcm.pitch);
setYaw(_dcm.yaw);
setRollRate(_dcm.get_gyro().x);
setPitchRate(_dcm.get_gyro().y);
setYawRate(_dcm.get_gyro().z);
setXAccel(_dcm.get_accel().x);
setYAccel(_dcm.get_accel().y);
setZAccel(_dcm.get_accel().z);
/*
* accel/gyro debug
*/
/*
Vector3f accel = _board->imu->get_accel();
Vector3f gyro = _board->imu->get_gyro();
Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"),
accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z);
*/
}
//33
void MavSerialPort::global_position_int_handler(){
// qDebug() << "MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n";
mavlink_msg_global_position_int_decode(&message, &global_position_int);
setLat(global_position_int.lat);
setLon(global_position_int.lon);
setAlt(global_position_int.relative_alt);
emit globalChanged();
}
