void Device::update_attitude() {
// Update the inertial and calculate attitude
m_pAHRS->update();
// Calculate the attitude based on the accelerometer
calc_acceleration();
#if BENCH_OUT
static int iBCounter = 0;
int iBCurTime = m_pHAL->scheduler->millis();
++iBCounter;
if(iBCurTime - m_t32Inertial >= 1000) {
#ifdef __AVR__
m_pHAL->console->printf_P(PSTR("Benchmark - update_attitude(): %d Hz\n"), iBCounter);
#else
m_pHAL->console->printf("Benchmark - update_attitude(): %d Hz\n", iBCounter);
#endif
iBCounter = 0;
m_t32Inertial = iBCurTime;
}
#endif
m_vAtti_deg.x = ToDeg(m_pAHRS->pitch);
m_vAtti_deg.y = ToDeg(m_pAHRS->roll);
m_vAtti_deg.z = ToDeg(m_pAHRS->yaw);
}
/*
internal rate controller, called by attitude and rate controller
public functions
*/
int32_t AP_SteerController::get_steering_out(float desired_accel)
{
uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
float speed = _ahrs.groundspeed();
if (speed < 1.0e-6) {
// with no speed all we can do is center the steering
return 0;
}
// this is a linear approximation of the inverse steering
// equation for a ground vehicle. It returns steering as an angle from -45 to 45
float scaler = 1.0f / speed;
// Calculate the steering rate error (deg/sec) and apply gain scaler
float desired_rate = desired_accel / speed;
float rate_error = (ToDeg(desired_rate) - ToDeg(_ahrs.get_gyro().z)) * scaler;
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float ki_rate = _K_I * _tau * 45.0f;
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) * 45.0f;
float delta_time = (float)dt * 0.001f;
// Multiply roll rate error by _ki_rate and integrate
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (ki_rate > 0) {
// only integrate if gain and time step are positive.
if (dt > 0) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
// prevent the integrator from increasing if steering defln demand is above the upper limit
if (_last_out < -45) {
integrator_delta = max(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if steering defln demand is below the lower limit
integrator_delta = min(integrator_delta, 0);
}
_integrator += integrator_delta;
}
} else {
_integrator = 0;
}
// Scale the integration limit
float intLimScaled = _imax * 0.01f;
// Constrain the integrator state
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
// Calculate the demanded control surface deflection
_last_out = (rate_error * _K_D * 4.0f) + (desired_rate * kp_ff) * scaler + _integrator;
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
}
Vector3f Device::get_gyro_degps() {
if(!m_pInert->healthy() ) {
#ifdef __AVR__
//m_pHAL->console->printf_P(PSTR("read_gyro_deg(): Inertial not healthy\n") );
m_pHAL->console->printf("read_gyro_deg(): Inertial not healthy\n");
#else
m_pHAL->console->printf("read_gyro_deg(): Inertial not healthy\n");
#endif
m_eErrors = static_cast<DEVICE_ERROR_FLAGS>(tiny::add_flag(m_eErrors, GYROMETER_F) );
return m_vGyro_deg;
}
// Read the current gyrometer value
m_vGyro_deg = m_pInert->get_gyro();
// Save values
float fRol = ToDeg(m_vGyro_deg.x); // in comparison to the accelerometer data swapped
float fPit = ToDeg(m_vGyro_deg.y); // in comparison to the accelerometer data swapped
float fYaw = ToDeg(m_vGyro_deg.z);
// Put them into the right order
m_vGyro_deg.x = fPit; // PITCH
m_vGyro_deg.y = fRol; // ROLL
m_vGyro_deg.z = fYaw; // YAW
return m_vGyro_deg;
}
void
AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
{
roll = _roll;
pitch = _pitch;
yaw = _yaw;
_omega_integ_corr.x = _rollRate;
_omega_integ_corr.y = _pitchRate;
_omega_integ_corr.z = _yawRate;
roll_sensor =ToDeg(roll)*100;
pitch_sensor =ToDeg(pitch)*100;
yaw_sensor =ToDeg(yaw)*100;
// Need the standard C_body<-nav dcm from navigation frame to body frame
// Strapdown Inertial Navigation Technology / Titterton/ pg. 41
float sRoll = sin(roll), cRoll = cos(roll);
float sPitch = sin(pitch), cPitch = cos(pitch);
float sYaw = sin(yaw), cYaw = cos(yaw);
_dcm_matrix.a.x = cPitch*cYaw;
_dcm_matrix.a.y = -cRoll*sYaw+sRoll*sPitch*cYaw;
_dcm_matrix.a.z = sRoll*sYaw+cRoll*sPitch*cYaw;
_dcm_matrix.b.x = cPitch*sYaw;
_dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw;
_dcm_matrix.b.z = -sRoll*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.c.x = -sPitch;
_dcm_matrix.c.y = sRoll*cPitch;
_dcm_matrix.c.z = cRoll*cPitch;
}
// set guided mode angle target
void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;
guided_angle_state.use_yaw_rate = use_yaw_rate;
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
// interpret positive climb rate as triggering take-off
if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
copter.set_auto_armed(true);
}
// log target
copter.Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
}
// the _P_gain raises the gain of the PI controller
// when we are spinning fast. See the fastRotations
// paper from Bill.
float
AP_AHRS_DCM::_P_gain(float spin_rate)
{
if (spin_rate < ToDeg(50)) {
return 1.0;
}
if (spin_rate > ToDeg(500)) {
return 10.0;
}
return spin_rate/ToDeg(50);
}
void
AP_AHRS_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
{
roll = _roll;
pitch = _pitch;
yaw = _yaw;
_omega(_rollRate, _pitchRate, _yawRate);
roll_sensor = ToDeg(roll)*100;
pitch_sensor = ToDeg(pitch)*100;
yaw_sensor = ToDeg(yaw)*100;
_dcm_matrix.from_euler(roll, pitch, yaw);
}
// set guided mode angle target
void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
guided_angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = AP_HAL::millis();
}
// check for ekf yaw reset and adjust target heading
void Copter::check_ekf_yaw_reset()
{
float yaw_angle_change_rad = 0.0f;
if (ahrs.get_NavEKF().getLastYawResetAngle(yaw_angle_change_rad)) {
attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
}
}
static void medium_loop() {
switch(medium_loopCounter) {
case 0:
medium_loopCounter++;
compass.read();
compass.calculate(0,0);
compas=ToDeg(compass.heading);
Serial.print(compas);Serial.print(", ");Serial.print(yawref);Serial.print(", ");Serial.println(yaw);
break;
case 1:
medium_loopCounter++;
yref=(APM_RC.InputCh(CH_2)-1050);yref/=210;yref-=2;yref=constrain(yref,-1,1);
break;
case 2:
medium_loopCounter++;
break;
case 3:
medium_loopCounter++;
break;
case 4:
medium_loopCounter = 0;
break;
default:
medium_loopCounter = 0;
break;
}
}
static void calc_nav_roll()
{
#define NAV_ROLL_BY_RATE 0
#if NAV_ROLL_BY_RATE
// Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a
// desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try
// to turn at 15 degrees per second.
float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error_cd) * .01);
// Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
float speed;
if (!ahrs.airspeed_estimate(&speed)) {
speed = g.airspeed_cruise_cm*0.01;
// Floor the speed so that the user can't enter a bad value
if(speed < 6) {
speed = 6;
}
}
// Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity.
nav_roll = ToDeg(atan(speed*turn_rate/GRAVITY_MSS)*100);
#else
// this is the old nav_roll calculation. We will use this for 2.50
// then remove for a future release
float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed;
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100
#endif
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
}
/*
get the rate offset in degrees/second needed for pitch in body frame
to maintain height in a coordinated turn.
Also returns the inverted flag and the estimated airspeed in m/s for
use by the rest of the pitch controller
*/
float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
{
float rate_offset;
float bank_angle = _ahrs.roll;
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < radians(90)) {
bank_angle = constrain_float(bank_angle,-radians(80),radians(80));
inverted = false;
} else {
inverted = true;
if (bank_angle > 0.0f) {
bank_angle = constrain_float(bank_angle,radians(100),radians(180));
} else {
bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
}
}
if (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
}
if (abs(_ahrs.pitch_sensor) > 7000) {
// don't do turn coordination handling when at very high pitch angles
rate_offset = 0;
} else {
rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / max((aspeed * _ahrs.get_EAS2TAS()) , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
}
if (inverted) {
rate_offset = -rate_offset;
}
return rate_offset;
}
/*
Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
A positive demand is up
Inputs are:
1) demanded pitch rate in degrees/second
2) control gain scaler = scaling_speed / aspeed
3) boolean which is true when stabilise mode is active
4) minimum FBW airspeed (metres/sec)
5) maximum FBW airspeed (metres/sec)
*/
int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed)
{
uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
float delta_time = (float)dt * 0.001f;
// Get body rate vector (radians/sec)
float omega_y = _ahrs.get_gyro().y;
// Calculate the pitch rate error (deg/sec) and scale
float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
// Multiply pitch rate error by _ki_rate and integrate
// Scaler is applied before integrator so that integrator state relates directly to elevator deflection
// This means elevator trim offset doesn't change as the value of scaler changes with airspeed
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!disable_integrator && _K_I > 0) {
float ki_rate = _K_I * _tau;
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit
integrator_delta = max(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = min(integrator_delta , 0);
}
_integrator += integrator_delta;
}
} else {
_integrator = 0;
}
// Scale the integration limit
float intLimScaled = _imax * 0.01f;
// Constrain the integrator state
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) / _ahrs.get_EAS2TAS();
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
_last_out = ( (rate_error * _K_D) + (desired_rate * kp_ff) ) * scaler + _integrator;
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
}
/**************************************************************************
* \brief Filter Get Acc Roll
* Returns the calculated roll angle from the acceleration measurements. \n
*
* \param ---
*
* \return accRoll angle
***************************************************************************/
float filter_get_acc_roll(void)
{
#ifdef FILTER_USE_DCM
filter_predict_accG_roll();
#endif // FILTER_USE_DCM
return ToDeg(accRoll);
}
static int8_t logAhrsCmd(uint8_t argc, const logMenu::arg *argv){
while (!_Console->available()){
//print _Ahrs state
Vector3f drift = _Ahrs->get_gyro_drift();
_Console->printf_P(PSTR("logAhrsCmd :: r:%4.1f p:%4.1f y:%4.1f drift=(%5.1f %5.1f %5.1f) hdg=%.1f \n"),
ToDeg(_Ahrs->roll),
ToDeg(_Ahrs->pitch),
ToDeg(_Ahrs->yaw),
ToDeg(drift.x),
ToDeg(drift.y),
ToDeg(drift.z),
_Compass->use_for_yaw() ? ToDeg(_heading) : 0.0);
//get new readings
_Compass->accumulate();
_Compass->read();
_Ahrs->update();
//delay TODO: non-blocking delay! (scheduler?)
delay(200);
}
return 0;
}
// check for ekf yaw reset and adjust target heading
void Copter::check_ekf_yaw_reset()
{
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
ekfYawReset_ms = new_ekfYawReset_ms;
}
}
void Device::update_attitude() {
m_pAHRS->update();
#if BENCH_OUT
static int iCounter = 0;
static int iTimer = 0;
int iCurTime = m_pHAL->scheduler->millis();
++iCounter;
if(iCurTime - iTimer >= 1000) {
m_pHAL->console->printf("Benchmark - update_attitude(): %d Hz\n", iCounter);
iCounter = 0;
iTimer = iCurTime;
}
#endif
#if SIGM_FOR_ATTITUDE
// Use "m_pInert->update()" only if "m_pAHRS->update()" is not used
//m_pInert->update();
// Calculate time (in s) passed
uint_fast32_t t32CurrentTime = m_pHAL->scheduler->millis();
float dT = static_cast<float>((t32CurrentTime - m_t32Inertial) ) / 1000.f;
m_t32Inertial = t32CurrentTime;
// Calculate attitude from relative gyrometer changes
m_vAtti_deg += read_gyro_deg() * dT;
m_vAtti_deg = wrap180_V3f(m_vAtti_deg);
m_vAtti_deg.z = ToDeg(m_pAHRS->yaw); // Use AHRS for the yaw
// Use a temporary instead of the member variable for acceleration data
Vector3f vRef_deg = read_accl_deg();
#if DEBUG_OUT && !BENCH_OUT
m_pHAL->console->printf("Attitude - x: %.1f/%.1f, y: %.1f/%.1f, z: %.1f\n", m_vAtti_deg.x, vRef_deg.x, m_vAtti_deg.y, vRef_deg.y, m_vAtti_deg.z);
#endif
m_vAtti_deg.x = SFilter::transff_filt_f(m_vAtti_deg.x, vRef_deg.x-m_vAtti_deg.x, dT*INERT_FUSION_RATE, Functor_f(&atti_f, vRef_deg.x) );
m_vAtti_deg.y = SFilter::transff_filt_f(m_vAtti_deg.y, vRef_deg.y-m_vAtti_deg.y, dT*INERT_FUSION_RATE, Functor_f(&atti_f, vRef_deg.y) );
#else
m_vAtti_deg.x = ToDeg(m_pAHRS->pitch);
m_vAtti_deg.y = ToDeg(m_pAHRS->roll);
m_vAtti_deg.z = ToDeg(m_pAHRS->yaw);
#endif
}
/*
* Reads the current altitude changes from the gyroscope in degrees and returns it as a 3D vector
*/
Vector3f Device::read_gyro_deg() {
if(!m_pInert->healthy() ) {
m_pHAL->console->println("read_gyro_deg(): Inertial not healthy\n");
m_eErrors = static_cast<DEVICE_ERROR_FLAGS>(add_flag(m_eErrors, GYROMETER_F) );
return m_vGyro_deg;
}
m_vGyro_deg = m_pInert->get_gyro();
// Save values
float fRol = ToDeg(m_vGyro_deg.x); // in comparison to the accelerometer data swapped
float fPit = ToDeg(m_vGyro_deg.y); // in comparison to the accelerometer data swapped
float fYaw = ToDeg(m_vGyro_deg.z);
// Put them into the right order
m_vGyro_deg.x = fPit; // PITCH
m_vGyro_deg.y = fRol; // ROLL
m_vGyro_deg.z = fYaw; // YAW
return m_vGyro_deg;
}
// set guided mode angle target
void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
guided_angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
// log target
Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
}
/*
lateral acceleration controller. Returns servo value -4500 to 4500
given a desired lateral acceleration
*/
int32_t AP_SteerController::get_steering_out_lat_accel(float desired_accel)
{
float speed = _ahrs.groundspeed();
if (speed < _minspeed) {
// assume a minimum speed. This reduces osciallations when first starting to move
speed = _minspeed;
}
// Calculate the desired steering rate given desired_accel and speed
float desired_rate = ToDeg(desired_accel / speed);
return get_steering_out_rate(desired_rate);
}
float Angle(const float x1, const float y1, const float x2, const float y2)
{
float x = x1 - x2;
float y = y1 - y2;
if (y == 0.f)
{
if (x >= 0.f)
return 270.f;
else
return 90.f;
}
if (x >= 0.f)
{
return ( ToDeg( -std::atan(y/x) ) + 270.f );
}
else
{
return ( ToDeg( -std::atan(y/x) ) + 90.f );
}
}
/*
* Reads the current attitude from the accelerometer in degrees and returns it as a 3D vector
* From: "Tilt Sensing Using a Three-Axis Accelerometer"
*/
Vector3f Device::read_accl_deg() {
if(!m_pInert->healthy() ) {
m_pHAL->console->println("read_accl_deg(): Inertial not healthy\n");
m_eErrors = static_cast<DEVICE_ERROR_FLAGS>(add_flag(m_eErrors, ACCELEROMETR_F) );
return m_vAccel_deg;
}
// Low Pass SFilter
Vector3f vAccelCur_cmss = m_pInert->get_accel() * 100.f;
m_vAccel_deg = m_vAccelPG_cmss = SFilter::low_pass_filt_V3f(vAccelCur_cmss, m_vAccelPG_cmss, INERT_LOWPATH_FILT_f);
// Calculate G-const. corrected acceleration
m_vAccelMG_cmss = vAccelCur_cmss - m_vAccelPG_cmss;
// Calculate roll and pitch in degrees from the filtered acceleration readouts (attitude)
float fpYZ = sqrt(pow2_f(m_vAccel_deg.y) + pow2_f(m_vAccel_deg.z) );
// Pitch
m_vAccel_deg.x = ToDeg(atan2(m_vAccel_deg.x, fpYZ) );
// Roll
m_vAccel_deg.y = ToDeg(atan2(-m_vAccel_deg.y, -m_vAccel_deg.z) );
return m_vAccel_deg;
}
static void sensors_debug() {
static int divider = 0;
if (divider++ == 20) {
divider = 0;
float heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix());
hal.console->printf("ahrs: roll %4.1f pitch %4.1f "
"yaw %4.1f hdg %.1f\r\n",
ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch),
ToDeg(g_ahrs.yaw),
g_compass.use_for_yaw() ? ToDeg(heading):0.0f);
Vector3f accel(g_ins.get_accel());
Vector3f gyro(g_ins.get_gyro());
hal.console->printf("mpu6000: accel %.2f %.2f %.2f "
"gyro %.2f %.2f %.2f\r\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
hal.console->printf("compass: heading %.2f deg\r\n",
ToDeg(g_compass.calculate_heading(0, 0)));
}
}
int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize)
{
uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
if(_ahrs == NULL) return 0; // can't control without a reference
float delta_time = (float)dt / 1000.0f;
int32_t angle_err = angle - _ahrs->roll_sensor;
float rate = _ahrs->get_roll_rate_earth();
float RC = 1/(2*PI*_fCut);
rate = _last_rate +
(delta_time / (RC + delta_time)) * (rate - _last_rate);
_last_rate = rate;
int32_t desired_rate = angle_err * _kp_angle;
if (_max_rate && desired_rate < -_max_rate) desired_rate = -_max_rate;
else if (_max_rate && desired_rate > _max_rate) desired_rate = _max_rate;
if(stabilize) {
desired_rate *= _stabilize_gain;
}
int32_t rate_error = desired_rate - ToDeg(rate)*100;
float out = (rate_error * _kp_rate + desired_rate * _kp_ff) * scaler;
//rate integrator
if (!stabilize) {
if ((fabsf(_ki_rate) > 0) && (dt > 0))
{
_integrator += (rate_error * _ki_rate) * scaler * delta_time;
if (_integrator < -4500-out) _integrator = -4500-out;
else if (_integrator > 4500-out) _integrator = 4500-out;
}
} else {
_integrator = 0;
}
return out + _integrator;
}
/**************************************************************************
* \brief Filter Task
* Process all filter calculations. \n
*
* \param ---
*
* \return ---
***************************************************************************/
void filter_task(unsigned long time)
{
/* Calculate time elapsed since last call (dt) */
/* Please note that overflows are ok, since for example 0x0001 - 0x00FE will be equal to 2 */
dt = (float)(time - lastMicros) / 1000000;
lastMicros = time;
#ifdef FILTER_USE_DCM /* Execute DCM */
filter_dcm_matrix_update();
filter_dcm_normalize();
filter_dcm_drift_correction();
filter_dcm_euler_angles();
#else /* Execute Kalman filter */
filter_predict_accG_roll();
filter_kalman_ars_predict(); // Kalman predict
filter_kalman_ars_update(accRoll); // Kalman update + result (angle)
filterdata->roll = ToDeg(x_angle);
#endif // FILTER_USE_DCM
}
/*
* Return true if compass was healthy
* In heading the heading of the compass is written.
* All units in degrees
*/
float Device::read_comp_deg() {
if (!m_pComp->use_for_yaw() ) {
//m_pHAL->console->println("read_comp_deg(): Compass not healthy\n");
m_eErrors = static_cast<DEVICE_ERROR_FLAGS>(add_flag(m_eErrors, COMPASS_F) );
return m_fCmpH;
}
// Update the compass readout maximally ten times a second
if(m_pHAL->scheduler->millis() - m_t32Compass <= COMPASS_UPDATE_T) {
return m_fCmpH;
}
m_pComp->read();
m_fCmpH = m_pComp->calculate_heading(m_pAHRS->get_dcm_matrix() );
m_fCmpH = ToDeg(m_fCmpH);
m_pComp->learn_offsets();
return m_fCmpH;
}
// check for ekf yaw reset and adjust target heading, also log position reset
void Copter::check_ekf_reset()
{
// check for yaw reset
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control->shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
ekfYawReset_ms = new_ekfYawReset_ms;
Log_Write_Event(DATA_EKF_YAW_RESET);
}
#if AP_AHRS_NAVEKF_AVAILABLE
// check for change in primary EKF (log only, AC_WPNav handles position target adjustment)
if ((EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (EKF2.getPrimaryCoreIndex() != -1)) {
ekf_primary_core = EKF2.getPrimaryCoreIndex();
Log_Write_Error(ERROR_SUBSYSTEM_EKF_PRIMARY, ekf_primary_core);
gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core);
}
#endif
}
static int8_t logCompassCmd(uint8_t argc, const logMenu::arg *argv)
{
static float min[3], max[3], offset[3];
float heading;
_Compass->read();
if (!_Compass->healthy) {
_Console->println("logCompassCmd :: Compass not healthy");
return -1;
}
heading = _Compass->calculate_heading(0,0); // roll = 0, pitch = 0 for this example
_Compass->null_offsets();
// capture min
if( _Compass->mag_x < min[0] ){ min[0] = _Compass->mag_x; }
if( _Compass->mag_y < min[1] ){ min[1] = _Compass->mag_y; }
if( _Compass->mag_z < min[2] ){ min[2] = _Compass->mag_z; }
// capture max
if( _Compass->mag_x > max[0] ){ max[0] = _Compass->mag_x; }
if( _Compass->mag_y > max[1] ){ max[1] = _Compass->mag_y; }
if( _Compass->mag_z > max[2] ){ max[2] = _Compass->mag_z; }
// calculate offsets
offset[0] = -(max[0]+min[0])/2;
offset[1] = -(max[1]+min[1])/2;
offset[2] = -(max[2]+min[2])/2;
// display all to user
_Console->printf("logCompassCmd :: heading: %.2f (%3d,%3d,%3d) ", ToDeg(heading),
_Compass->mag_x,
_Compass->mag_y,
_Compass->mag_z);
// display offsets
_Console->printf(" offsets : (%.2f, %.2f, %.2f)", (double)offset[0], (double)offset[1], (double)offset[2]);
_Console->println();
return 0;
}
void loop(void)
{
static uint16_t counter;
static uint32_t last_t, last_print, last_compass;
uint32_t now = hal.scheduler->micros();
float heading = 0;
if (last_t == 0) {
last_t = now;
return;
}
last_t = now;
if (now - last_compass > 100*1000UL &&
compass.read()) {
heading = compass.calculate_heading(ahrs.get_dcm_matrix());
// read compass at 10Hz
last_compass = now;
#if WITH_GPS
g_gps->update();
#endif
}
ahrs.update();
counter++;
if (now - last_print >= 100000 /* 100ms : 10hz */) {
Vector3f drift = ahrs.get_gyro_drift();
hal.console->printf(
"r:%4.1f p:%4.1f y:%4.1f "
"drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n",
ToDeg(ahrs.roll),
ToDeg(ahrs.pitch),
ToDeg(ahrs.yaw),
ToDeg(drift.x),
ToDeg(drift.y),
ToDeg(drift.z),
compass.use_for_yaw() ? ToDeg(heading) : 0.0f,
(1.0e6f*counter)/(now-last_print));
last_print = now;
counter = 0;
}
}
/*
calculate yaw control for ground steering
*/
void Plane::calc_nav_yaw_ground(void)
{
if (gps.ground_speed() < 1 &&
channel_throttle->get_control_in() == 0 &&
flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
// manual rudder control while still
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
steering_control.steering = rudder_input;
return;
}
float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps;
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
steer_rate = 0;
}
if (!is_zero(steer_rate)) {
// pilot is giving rudder input
steer_state.locked_course = false;
} else if (!steer_state.locked_course) {
// pilot has released the rudder stick or we are still - lock the course
steer_state.locked_course = true;
if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
steer_state.locked_course_err = 0;
}
}
if (!steer_state.locked_course) {
// use a rate controller at the pilot specified rate
steering_control.steering = steerController.get_steering_out_rate(steer_rate);
} else {
// use a error controller on the summed error
int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd);
}
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}
