void Ekf::controlGpsAiding() { // GPS fusion mode selection logic // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _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) { _control_status.flags.gps = resetPosition(); _control_status.flags.gps = resetVelocity(); } } } } 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(); } else { // Reset states to the last GPS measurement resetPosition(); resetVelocity(); // Reset the timeout counters _time_last_pos_fuse = _time_last_imu; _time_last_vel_fuse = _time_last_imu; } } } }
void Projectile::moveProjectile(sf::Time dt) { sf::Vector2f pos = getPosition(); if (bHasTarget) { if (!reachedTarget) { fDistance = sqrt(pow(targetPos.x - pos.x,2) + pow(targetPos.y - pos.y,2)); //Calculate new velocity velocity.x = getSpeed() * (targetPos.x - pos.x) / fDistance; velocity.y = getSpeed() * (targetPos.y - pos.y) / fDistance; if (fDistance > -1 && fDistance < 1) { reachedTarget = true; resetVelocity(); } } } //Rotate to face the target float dx = targetPos.x - pos.x; float dy = targetPos.y - pos.y; const float Pi = 3.141; setRotation(atan2(dy,dx) * (180 / Pi) + 90); //Calculates rotation move(getVelocity() * dt.asSeconds()); }
void Ball::resetPosition() { bounds.left = 385; bounds.top = 285; changeXDirection(); m_rect.setPosition(bounds.left, bounds.top); resetMoveCountdown(); resetVelocity(); }
/** * @brief Process individual keyboard inputs. * * @param c keyboard input. */ void KeyOpCore::processKeyboardInput(char c) { /* * Arrow keys are a bit special, they are escape characters - meaning they * trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We * ignore the esc-[ and just parse the last one. So long as we avoid using * the last one for its actual purpose (e.g. left arrow corresponds to * esc-[-D) we can keep the parsing simple. */ switch (c) { case kobuki_msgs::KeyboardInput::KeyCode_Left: { incrementAngularVelocity(); break; } case kobuki_msgs::KeyboardInput::KeyCode_Right: { decrementAngularVelocity(); break; } case kobuki_msgs::KeyboardInput::KeyCode_Up: { incrementLinearVelocity(); break; } case kobuki_msgs::KeyboardInput::KeyCode_Down: { decrementLinearVelocity(); break; } case kobuki_msgs::KeyboardInput::KeyCode_Space: { resetVelocity(); break; } case 'q': { quit_requested = true; break; } case 'd': { disable(); break; } case 'e': { enable(); break; } default: { break; } } }
/** * @brief Process individual keyboard inputs. * * @param c keyboard input. */ void KeyOp::processKeyboardInput(char c) { /* * Arrow keys are a bit special, they are escape characters - meaning they * trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We * ignore the esc-[ and just parse the last one. So long as we avoid using * the last one for its actual purpose (e.g. left arrow corresponds to * esc-[-D) we can keep the parsing simple. */ switch (c) { case KEYCODE_LEFT: { incrementAngularVelocity(); break; } case KEYCODE_RIGHT: { decrementAngularVelocity(); break; } case KEYCODE_UP: { incrementLinearVelocity(); break; } case KEYCODE_DOWN: { decrementLinearVelocity(); break; } case KEYCODE_SPACE: { resetVelocity(); break; } case 'q': { quit_requested_ = true; break; } case 'd': { disable(); break; } case 'e': { enable(); break; } default: { break; } } }
//figures out what needs to happen at an intersection based on the direction we want to turn void Robot::handleIntersection(Direction direction){ switch (direction) { case StraightAhead: break; case Left: Tape::resetErrors(); resetVelocity(); turnAtIntersection(false, millis()); break; case Right: Tape::resetErrors(); resetVelocity(); turnAtIntersection(true, millis()); break; case TurnAround: Tape::resetErrors(); resetVelocity(); turnOntoTape(direction); break; case SlightRight: Actuators::drive(velocity(), SLIGHT_RIGHT_AT_SPECIAL_INTERSECTION_TURN); delay(SLIGHT_RIGHT_AT_SPECIAL_INTERSECTION_DURATION); Tape::resetErrors(); resetVelocity(); findTape(); break; case SlightLeft: Actuators::drive(VELOCITY_SLOW, -SLIGHT_LEFT_AT_SPECIAL_INTERSECTION_TURN); delay(SLIGHT_LEFT_AT_SPECIAL_INTERSECTION_DURATION); Tape::resetErrors(); resetVelocity(); findTape(); break; } }
void SahWorldInterface::create(CudaDynamicWorld * world) { #if COLLIDEJUST return DynamicWorldInterface::create(world); #endif world->setBvhBuilder(new SahBuilder); SahTetrahedronSystem * tetra = new SahTetrahedronSystem; if(!readMeshFromFile(tetra)) createTestMesh(tetra); resetVelocity(tetra); tetra->setTotalMass(4000.f); world->addTetrahedronSystem(tetra); }
void Robot::turnOntoTape(bool turnRight) { resetVelocity(); Actuators::turnInPlace(turnRight); delay(TURN_OFF_TAPE_DURATION); while (Tape::tapePresentCentreWithUpdate()) {} delay(10); unsigned long timeStamp = millis(); while (!Tape::tapePresentCentreWithUpdate()) { if (millis() - timeStamp > GETTING_UNSTUCK_STARTING_TIME) { Actuators::drive(VELOCITY_SLOW, Actuators::Straight); delay(GETTING_UNSTUCK_DELAY); timeStamp = millis(); Actuators::turnInPlace(turnRight); } } }
void Robot::turnOntoTape(Direction direction) { switch (direction) { case StraightAhead: break; case Left: turnOntoTape(false); break; case Right: turnOntoTape(true); break; case TurnAround: resetVelocity(); Actuators::turnInPlace(TURN_180_DURATOIN, true); while (!Tape::tapePresentCentreWithUpdate()) {} break; } }
bool Ekf::initialiseFilter(void) { _state.ang_error.setZero(); _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); _state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f; _state.accel_z_bias = 0.0f; _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); // get initial roll and pitch estimate from accel vector, assuming vehicle is static Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt; float pitch = 0.0f; float roll = 0.0f; if (accel_init.norm() > 0.001f) { accel_init.normalize(); pitch = asinf(accel_init(0)); roll = -asinf(accel_init(1) / cosf(pitch)); } matrix::Euler<float> euler_init(roll, pitch, 0.0f); // Get the latest magnetic field measurement. // If we don't have a measurement then we cannot initialise the filter magSample mag_init = _mag_buffer.get_newest(); if (mag_init.time_us == 0) { return false; } // rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination // TODO use declination if available matrix::Dcm<float> R_to_earth_zeroyaw(euler_init); Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag; float declination = 0.0f; euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0)); // calculate initial quaternion states _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; // calculate initial earth magnetic field states matrix::Dcm<float> R_to_earth(euler_init); _state.mag_I = R_to_earth * mag_init.mag; resetVelocity(); resetPosition(); // initialize vertical position with newest baro measurement baroSample baro_init = _baro_buffer.get_newest(); if (baro_init.time_us == 0) { return false; } _state.pos(2) = -baro_init.hgt; _output_new.pos(2) = -baro_init.hgt; initialiseCovariance(); return true; }
void Ekf::controlExternalVisionFusion() { // Check for new exernal vision data if (_ev_data_ready) { // external vision position aiding selection logic if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position and height _control_status.flags.ev_pos = true; ECL_INFO("EKF switching to external vision position fusion"); // turn off other forms of height aiding _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; // reset the position, height and velocity resetPosition(); resetVelocity(); resetHeight(); } } // external vision yaw aiding selection logic if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observaton quaternion // get the roll, pitch, yaw estimates from the quaternion states matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), _state.quat_nominal(3)); matrix::Euler<float> euler_init(q_init); // get initial yaw from the observation quaternion extVisionSample ev_newest = _ext_vision_buffer.get_newest(); matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); matrix::Euler<float> euler_obs(q_obs); euler_init(2) = euler_obs(2); // save a copy of the quaternion state for later use in calculating the amount of reset change Quaternion quat_before_reset = _state.quat_nominal; // calculate initial quaternion states for the ekf _state.quat_nominal = Quaternion(euler_init); // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); // add the reset amount to the output observer buffered data outputSample output_states; unsigned output_length = _output_buffer.get_length(); for (unsigned i=0; i < output_length; i++) { output_states = _output_buffer.get_from_index(i); output_states.quat_nominal *= _state_reset_status.quat_change; _output_buffer.push_to_index(i,output_states); } // capture the reset event _state_reset_status.quat_counter++; // flag the yaw as aligned _control_status.flags.yaw_align = true; // turn on fusion of external vision yaw measurements and disable all magnetoemter fusion _control_status.flags.ev_yaw = true; _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; _control_status.flags.mag_dec = false; ECL_INFO("EKF switching to external vision yaw fusion"); } } // determine if we should use the height observation if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = true; _fuse_height = true; } // determine if we should use the horizontal position observations if (_control_status.flags.ev_pos) { _fuse_pos = true; // correct position and height for offset relative to IMU Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); } // determine if we should use the yaw observation if (_control_status.flags.ev_yaw) { fuseHeading(); } } }
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; } } }
void Ekf::controlExternalVisionAiding() { // external vision position aiding selection logic if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position and height _control_status.flags.ev_pos = true; printf("EKF switching to external vision position fusion\n"); // turn off other forms of height aiding _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; // reset the position, height and velocity resetPosition(); resetVelocity(); resetHeight(); } } // external vision yaw aiding selection logic if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observaton quaternion // get the roll, pitch, yaw estimates from the quaternion states matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), _state.quat_nominal(3)); matrix::Euler<float> euler_init(q_init); // get initial yaw from the observation quaternion extVisionSample ev_newest = _ext_vision_buffer.get_newest(); matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); matrix::Euler<float> euler_obs(q_obs); euler_init(2) = euler_obs(2); // save a copy of the quaternion state for later use in calculating the amount of reset change Quaternion quat_before_reset = _state.quat_nominal; // calculate initial quaternion states for the ekf _state.quat_nominal = Quaternion(euler_init); // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); // add the reset amount to the output observer buffered data outputSample output_states; unsigned output_length = _output_buffer.get_length(); for (unsigned i=0; i < output_length; i++) { output_states = _output_buffer.get_from_index(i); output_states.quat_nominal *= _state_reset_status.quat_change; _output_buffer.push_to_index(i,output_states); } // capture the reset event _state_reset_status.quat_counter++; // flag the yaw as aligned _control_status.flags.yaw_align = true; // turn on fusion of external vision yaw measurements and disable all magnetoemter fusion _control_status.flags.ev_yaw = true; _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; _control_status.flags.mag_dec = false; printf("EKF switching to external vision yaw fusion\n"); } } }
//Main function of the robot. Drives, detects IR, and detects collisions. //Most importantly, gives status updates to the controller so the controller //can decide what to do. Status Robot::cruise(Direction direction) { handleIntersection(direction); while (true) { //tape follows always Tape::update(); followTape(); //we must be at an intersection, tell this to the controller if (Tape::atIntersection() && (millis() - lastIntersectionTime) > TIME_MIN_BETWEEN_INTERSECTIONS) { lastIntersectionTime = millis(); Actuators::stop(); return Intersection; } Collision::update(); //tell the controller we collided if (Collision::occured()) { resetVelocity(); return Collided; } IR::update(); //tell the controller the status of IR signals switch (IR::check()) { case IR::None: setVelocity(VELOCITY_NORMAL); break; case IR::WeakLeft: if(!hasPassenger){ setVelocity(VELOCITY_SLOW); } break; case IR::WeakRight: if(!hasPassenger){ setVelocity(VELOCITY_SLOW); } break; case IR::StrongLeft: if(!hasPassenger){ Actuators::stop(); resetVelocity(); IR::resetIR(); return IRLeft; } break; case IR::StrongRight: if(!hasPassenger){ Actuators::stop(); resetVelocity(); IR::resetIR(); return IRRight; } break; } } }
void Ekf::controlFusionModes() { // Determine the vehicle status calculateVehicleStatus(); // Get the magnetic declination calcMagDeclination(); // Check for tilt convergence during initial alignment // filter the tilt error vector using a 1 sec time constant LPF float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt; _tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt; // Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } // optical flow fusion mode selection logic // to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align && (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) { // 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 optical flow aiding if (_control_status.flags.yaw_align) { // set the flag and reset the fusion timeout _control_status.flags.opt_flow = true; _time_last_of_fuse = _time_last_imu; // if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading if (!_control_status.flags.gps) { // calculate the rotation matrix from body to earth frame matrix::Dcm<float> body_to_earth(_state.quat_nominal); // constrain height above ground to be above minimum possible float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); // calculate absolute distance from focal point to centre of frame assuming a flat earth float range = heightAboveGndEst / body_to_earth(2, 2); if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { // calculate X and Y body relative velocities from OF measurements Vector3f vel_optflow_body; vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt; vel_optflow_body(2) = 0.0f; // rotate from body to earth frame Vector3f vel_optflow_earth; vel_optflow_earth = body_to_earth * vel_optflow_body; // take x and Y components _state.vel(0) = vel_optflow_earth(0); _state.vel(1) = vel_optflow_earth(1); } else { _state.vel.setZero(); } } } } else if (!(_params.fusion_mode & MASK_USE_OF)) { _control_status.flags.opt_flow = false; } // GPS fusion mode selection logic // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _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) { _control_status.flags.gps = resetPosition(); _control_status.flags.gps = resetVelocity(); } } } } 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 veloity 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(); } else { // Reset states to the last GPS measurement resetPosition(); resetVelocity(); } } } /* * Handle the case where we have not fused height measurements recently and * uncertainty exceeds the max allowable. Reset using the best available height * measurement source, continue using it after the reset and declare the current * source failed if we have switched. */ if ((P[8][8] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) { // handle the case where we are using baro for height if (_control_status.flags.baro_hgt) { // check if GPS height is available gpsSample gps_init = _gps_buffer.get_newest(); bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); baroSample baro_init = _baro_buffer.get_newest(); bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // use the gps if it is accurate or there is no baro data available if (gps_hgt_available && (gps_hgt_accurate || !baro_hgt_available)) { // declare the baro as unhealthy _baro_hgt_faulty = true; // set the height mode to the GPS _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; // adjust the height offset so we can use the GPS _hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref; if (!baro_hgt_available) { printf("EKF baro hgt timeout - switching to gps\n"); } } } // handle the case we are using GPS for height if (_control_status.flags.gps_hgt) { // check if GPS height is available gpsSample gps_init = _gps_buffer.get_newest(); bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); // check the baro height source for consistency and freshness baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); // if baro data is consistent and fresh or GPS height is unavailable or inaccurate, we switch to baro for height if ((baro_data_consistent && baro_data_fresh) || !gps_hgt_available || !gps_hgt_accurate) { // declare the GPS height unhealthy _gps_hgt_faulty = true; // set the height mode to the baro _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; printf("EKF gps hgt timeout - switching to baro\n"); } } // handle the case we are using range finder for height if (_control_status.flags.rng_hgt) { // check if range finder data is available rangeSample rng_init = _range_buffer.get_newest(); bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL); // check if baro data is available baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // check if baro data is consistent float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); // switch to baro if necessary or preferable bool switch_to_baro = (!rng_data_available && baro_data_available) || (baro_data_consistent && baro_data_available); if (switch_to_baro) { // declare the range finder height unhealthy _rng_hgt_faulty = true; // set the height mode to the baro _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; printf("EKF rng hgt timeout - switching to baro\n"); } } // Reset vertical position and velocity states to the last measurement resetHeight(); } // handle the case when we are relying on optical flow fusion and lose it if (_control_status.flags.opt_flow && !_control_status.flags.gps) { // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something if ((_time_last_imu - _time_last_of_fuse > 5e6)) { // Switch to the non-aiding mode, zero the veloity states // and set the synthetic position to the current estimate _control_status.flags.opt_flow = false; _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); } } // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances // or the more accurate 3-axis fusion if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { if (!_control_status.flags.armed) { // use heading fusion for initial startup _control_status.flags.mag_hdg = true; _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } else { if (_control_status.flags.in_air) { // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } // use 3D mag fusion when airborne _control_status.flags.mag_hdg = false; _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = true; } else { // use heading fusion when on the ground _control_status.flags.mag_hdg = true; _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } } } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { // always use heading fusion _control_status.flags.mag_hdg = true; _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) { // always use 2D mag fusion _control_status.flags.mag_hdg = false; _control_status.flags.mag_2D = true; _control_status.flags.mag_3D = false; } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } // always use 3-axis mag fusion _control_status.flags.mag_hdg = false; _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = true; } else { // do no magnetometer fusion at all _control_status.flags.mag_hdg = false; _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } // if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift // fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) { _control_status.flags.mag_dec = true; } else { _control_status.flags.mag_dec = false; } // Control the soure of height measurements for the main filter if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; } else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; } // Placeholder for control of wind velocity states estimation // TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement if (false) { _control_status.flags.wind = false; } // Store the status to enable change detection _control_status_prev.value = _control_status.value; }