float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ if (!(ISFINITE(ctl_data.pitch_setpoint) && ISFINITE(ctl_data.roll) && ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.airspeed))) { ECL_WARN("not controlling pitch"); return _rate_setpoint; } /* Calculate the error */ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = pitch_error / _tc; return _rate_setpoint; }
void Ekf::controlGpsFusion() { // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { // If the heading is not aligned, reset the yaw and magnetic field states if (!_control_status.flags.yaw_align) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } // If the heading is valid start using gps aiding if (_control_status.flags.yaw_align) { _control_status.flags.gps = true; _time_last_gps = _time_last_imu; // if we are not already aiding with optical flow, then we need to reset the position and velocity if (!_control_status.flags.opt_flow) { if (resetPosition() && resetVelocity()) { _control_status.flags.gps = true; } else { _control_status.flags.gps = false; } } if (_control_status.flags.gps) { ECL_INFO("EKF commencing GPS aiding"); } } } } else if (!(_params.fusion_mode & MASK_USE_GPS)) { _control_status.flags.gps = false; } // handle the case when we are relying on GPS fusion and lose it if (_control_status.flags.gps && !_control_status.flags.opt_flow) { // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { if (_time_last_imu - _time_last_gps > 5e5) { // if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states // and set the synthetic GPS position to the current estimate _control_status.flags.gps = false; _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); ECL_WARN("EKF GPS fusion timout - stopping GPS aiding"); } else { // Reset states to the last GPS measurement resetPosition(); resetVelocity(); ECL_WARN("EKF GPS fusion timout - resetting to GPS"); // Reset the timeout counters _time_last_pos_fuse = _time_last_imu; _time_last_vel_fuse = _time_last_imu; } } } // Only use GPS data for position and velocity aiding if enabled if (_control_status.flags.gps) { _fuse_pos = true; _fuse_vert_vel = true; _fuse_hor_vel = true; // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; // correct position and height for offset relative to IMU Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _gps_sample_delayed.pos(0) -= pos_offset_earth(0); _gps_sample_delayed.pos(1) -= pos_offset_earth(1); _gps_sample_delayed.hgt += pos_offset_earth(2); } // Determine if GPS should be used as the height source if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS)) && !_gps_hgt_faulty) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; _fuse_height = true; } } }