SpikeLoader::SpikeLoader( const URIHandler& params )
: EventSource( params )
, _impl( new Impl( *this, params ))
{
if( getDt() < 0.f )
setDt( params.getConfig().getTimestep( ));
}
TestLoader::TestLoader( const URIHandler& params )
: EventSource( params )
, _impl( new TestLoader::Impl( *this ))
{
if( getDt() < 0.f )
setDt( 1.f );
}
VSDLoader::VSDLoader( const URIHandler& params )
: EventSource( params )
, _impl( new VSDLoader::Impl( *this, params ))
{
if( getDt() < 0.f )
setDt( _impl->_voltageReport.getTimestep( ));
}
void ofxRParticleSystem::init()
{
uniqueIDs = 0;
count = new float;
damping = new float;
restitution = new float;
accLimit = new float;
velLimit = new float;
dt = new float;
setDt(1.0);
setCount(0);
setDamping(.25);
setRestitution(1.0);
setAccelerationLimit(5.0);
setVelocityLimit(10.0);
renderer = new ofxRParticleRenderer();
renderer->setParticlesPtr(&particles);
solver = NULL;
solver = new ofxSolver(1);
setSolver(*solver);
}
CompartmentLoader::CompartmentLoader( const URIHandler& params )
: EventSource( params )
, _impl( new CompartmentLoader::Impl( *this, params ))
{
if( getDt() < 0.f )
setDt( _impl->_report.getTimestep( ));
}
void ofxVerletSolver::init()
{
zero.dpdt.set(0);
zero.dvdt.set(0);
dt = new float;
bSelfAllocatedDt = true;
setDt(1.0);
}
void Context::copySimulationContext(const Context& c)
{
worldGravity_.setValue(c.getGravity()); ///< Gravity IN THE WORLD COORDINATE SYSTEM.
setDt(c.getDt());
setTime(c.getTime());
setAnimate(c.getAnimate());
}
void BlockSegwayController::update() {
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.main_state == MAIN_STATE_AUTO) {
// update guidance
}
// compute speed command
float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
// handle autopilot modes
if (_status.main_state == MAIN_STATE_AUTO ||
_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
} else if (_status.main_state == MAIN_STATE_MANUAL) {
if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
_actuators.control[CH_LEFT] = _manual.throttle;
_actuators.control[CH_RIGHT] = _manual.pitch;
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
}
}
// update all publications
updatePublications();
}
void Context::copySimulationContext(const Context& c)
{
worldGravity_.setValue(c.getGravity()); ///< Gravity IN THE WORLD COORDINATE SYSTEM.
setDt(c.getDt());
setTime(c.getTime());
setAnimate(c.getAnimate());
#ifdef SOFA_SMP
if(c.gpuPrioritary.getValue())
gpuPrioritary.setValue(true);
#endif
#ifdef SOFA_SUPPORT_MOVING_FRAMES
setPositionInWorld( c.getPositionInWorld());
spatialVelocityInWorld_ = c.spatialVelocityInWorld_;
velocityBasedLinearAccelerationInWorld_ = c.velocityBasedLinearAccelerationInWorld_;
#endif
#ifdef SOFA_SUPPORT_MULTIRESOLUTION
// for multiresolution
// finestLevel_ = c.finestLevel_;
// coarsestLevel_ = c.coarsestLevel_;
// currentLevel_ = c.currentLevel_;
#endif
#ifdef SOFA_SMP
if(!partition_)
{
if(processor.getValue()!=-1)
is_partition_.setValue(true);
if(is_partition())
{
partition_= new Iterative::IterativePartition();
// partition_->setCPU(processor.getValue());
}
}
if(processor.getValue()==-1&&c.processor.getValue()!=-1)
{
processor.setValue(c.processor.getValue());
is_partition_.setValue(true);
}
if(c.is_partition()&&!partition_)
{
partition_=c.getPartition();
is_partition_.setValue(true);
}
if((gpuPrioritary.getValue())&&partition_)
{
partition_->setGPUPrioritary();
}
#endif
}
void mTecs::updateTimeMeasurement()
{
if (!_dtCalculated) {
float deltaTSeconds = 0.0f;
if (!_firstIterationAfterReset) {
hrt_abstime timestampNow = hrt_absolute_time();
deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
timestampLastIteration = timestampNow;
}
setDt(deltaTSeconds);
_dtCalculated = true;
}
}
void BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
}
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
}
// XXX handle STABILIZED (loiter on spot) as well
// once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// altitude hold
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
// heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
float pCmd = _phi2P.update(phiCmd - _att.roll);
// velocity hold
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(
_vLimit.update(_vCmd.get()) - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.control[CH_THR] = dThrottle + _trimThr.get();
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
// a first binary release can be targeted.
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
// from +1 -> -1 m/s for rate of climb
//float dThrottle = _roc2Thr.update(
//_rocMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.roll);
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currenlty using manual throttle
// XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle;
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
// a first binary release can be targeted.
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
}
// body rates controller, disabled for now
else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}
}
// update all publications
updatePublications();
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(_err_perf);
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
detectDistanceSensors();
}
// reset pos, vel, and terrain on arming
if (!_lastArmedState && armedState) {
// we just armed, we are at home position on the ground
_x(X_x) = 0;
_x(X_y) = 0;
// the pressure altitude of home may have drifted, so we don't
// reset z to zero
// reset flow integral
_flowX = 0;
_flowY = 0;
// we aren't moving, all velocities are zero
_x(X_vx) = 0;
_x(X_vy) = 0;
_x(X_vz) = 0;
// assume we are on the ground, so terrain alt is local alt
_x(X_tz) = _x(X_z);
// reset lowpass filter as well
_xLowPass.setState(_x);
}
_lastArmedState = armedState;
// see which updates are available
bool flowUpdated = _sub_flow.updated();
bool paramsUpdated = _sub_param_update.updated();
bool baroUpdated = _sub_sensor.updated();
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
bool homeUpdated = _sub_home.updated();
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// update home position projection
if (homeUpdated) {
updateHome();
}
// is xy valid?
bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
if (_validXY) {
// if valid and gps has timed out, set to not valid
if (!xy_stddev_ok && !_gpsInitialized) {
_validXY = false;
}
} else {
if (xy_stddev_ok) {
_validXY = true;
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_validZ) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && !_baroInitialized) {
_validZ = false;
}
} else {
if (z_stddev_ok) {
_validZ = true;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_validTZ) {
if (!tz_stddev_ok) {
_validTZ = false;
}
} else {
if (tz_stddev_ok) {
_validTZ = true;
}
}
// timeouts
if (_validXY) {
_time_last_xy = _timeStamp;
}
if (_validZ) {
_time_last_z = _timeStamp;
}
if (_validTZ) {
_time_last_tz = _timeStamp;
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection at 0,0
if (_validXY && !_map_ref.init_done) {
map_projection_init(&_map_ref,
_init_home_lat.get(),
_init_home_lon.get());
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
}
// reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j < n_x; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit_P = true;
break;
}
}
if (reinit_P) { break; }
}
if (reinit_P) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (gpsUpdated) {
if (!_gpsInitialized) {
gpsInit();
} else {
gpsCorrect();
}
}
if (baroUpdated) {
if (!_baroInitialized) {
baroInit();
} else {
baroCorrect();
}
}
if (lidarUpdated) {
if (!_lidarInitialized) {
lidarInit();
} else {
lidarCorrect();
}
}
if (sonarUpdated) {
if (!_sonarInitialized) {
sonarInit();
} else {
sonarCorrect();
}
}
if (flowUpdated) {
if (!_flowInitialized) {
flowInit();
} else {
perf_begin(_loop_perf);// TODO
flowCorrect();
//perf_count(_interval_perf);
perf_end(_loop_perf);
}
}
if (visionUpdated) {
if (!_visionInitialized) {
visionInit();
} else {
visionCorrect();
}
}
if (mocapUpdated) {
if (!_mocapInitialized) {
mocapInit();
} else {
mocapCorrect();
}
}
if (_altHomeInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
if (_validXY) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
void BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
}
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
_stabilization.update(
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// commands
float rCmd = 0;
_backsideAutopilot.update(
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
_pos.alt, v,
_att.roll, _att.pitch, _att.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed
);
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
}
// update all publications
updatePublications();
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(_err_perf);
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
detectDistanceSensors();
}
// reset pos, vel, and terrain on arming
// XXX this will be re-enabled for indoor use cases using a
// selection param, but is really not helping outdoors
// right now.
// if (!_lastArmedState && armedState) {
// // we just armed, we are at origin on the ground
// _x(X_x) = 0;
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
// _x(X_vz) = 0;
// // assume we are on the ground, so terrain alt is local alt
// _x(X_tz) = _x(X_z);
// // reset lowpass filter as well
// _xLowPass.setState(_x);
// _aglLowPass.setState(0);
// }
_lastArmedState = armedState;
// see which updates are available
bool flowUpdated = _sub_flow.updated();
bool paramsUpdated = _sub_param_update.updated();
bool baroUpdated = _sub_sensor.updated();
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
bool landUpdated = (
(_sub_land.get().landed ||
((!_sub_armed.get().armed) && (!_sub_land.get().freefall)))
&& (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized))
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE));
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
vxy_stddev_ok = true;
}
if (_validXY) {
// if valid and gps has timed out, set to not valid
if (!vxy_stddev_ok && !_gpsInitialized) {
_validXY = false;
}
} else {
if (vxy_stddev_ok) {
if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
_validXY = true;
}
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_validZ) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && !_baroInitialized) {
_validZ = false;
}
} else {
if (z_stddev_ok) {
_validZ = true;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_validTZ) {
if (!tz_stddev_ok) {
_validTZ = false;
}
} else {
if (tz_stddev_ok) {
_validTZ = true;
}
}
// timeouts
if (_validXY) {
_time_last_xy = _timeStamp;
}
if (_validZ) {
_time_last_z = _timeStamp;
}
if (_validTZ) {
_time_last_tz = _timeStamp;
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection at 0,0
if (_validXY && !_map_ref.init_done) {
map_projection_init(&_map_ref,
_init_origin_lat.get(),
_init_origin_lon.get());
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
}
// force P symmetry and reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j <= i; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit_P = true;
}
if (i == j) {
// make sure diagonal elements are positive
if (_P(i, i) <= 0) {
reinit_P = true;
}
} else {
// copy elememnt from upper triangle to force
// symmetry
_P(j, i) = _P(i, j);
}
if (reinit_P) { break; }
}
if (reinit_P) { break; }
}
if (reinit_P) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (gpsUpdated) {
if (!_gpsInitialized) {
gpsInit();
} else {
gpsCorrect();
}
}
if (baroUpdated) {
if (!_baroInitialized) {
baroInit();
} else {
baroCorrect();
}
}
if (lidarUpdated) {
if (!_lidarInitialized) {
lidarInit();
} else {
lidarCorrect();
}
}
if (sonarUpdated) {
if (!_sonarInitialized) {
sonarInit();
} else {
sonarCorrect();
}
}
if (flowUpdated) {
if (!_flowInitialized) {
flowInit();
} else {
perf_begin(_loop_perf);// TODO
flowCorrect();
//perf_count(_interval_perf);
perf_end(_loop_perf);
}
}
if (visionUpdated) {
if (!_visionInitialized) {
visionInit();
} else {
visionCorrect();
}
}
if (mocapUpdated) {
if (!_mocapInitialized) {
mocapInit();
} else {
mocapCorrect();
}
}
if (landUpdated) {
if (!_landInitialized) {
landInit();
} else {
landCorrect();
}
}
if (_altOriginInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
_pub_innov.update();
if (_validXY) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
SynapseLoader::SynapseLoader( const URIHandler& params )
: EventSource( params )
, _impl( new Impl( *this, params ))
{
setDt( 1.f );
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
// detect distance sensors
for (int i = 0; i < N_DIST_SUBS; i++) {
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
if (s == _sub_lidar || s == _sub_sonar) { continue; }
if (s->updated()) {
s->update();
if (s->get().timestamp == 0) { continue; }
if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_lidar == nullptr) {
_sub_lidar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %i", msg_label, i);
} else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_sonar == nullptr) {
_sub_sonar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %i", msg_label, i);
}
}
}
}
// reset pos, vel, and terrain on arming
// XXX this will be re-enabled for indoor use cases using a
// selection param, but is really not helping outdoors
// right now.
// if (!_lastArmedState && armedState) {
// // we just armed, we are at origin on the ground
// _x(X_x) = 0;
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
// _x(X_vz) = 0;
// // assume we are on the ground, so terrain alt is local alt
// _x(X_tz) = _x(X_z);
// // reset lowpass filter as well
// _xLowPass.setState(_x);
// _aglLowPass.setState(0);
// }
_lastArmedState = armedState;
// see which updates are available
bool paramsUpdated = _sub_param_update.updated();
_baroUpdated = false;
if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
_sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp != _timeStampLastBaro) {
_baroUpdated = true;
_timeStampLastBaro = baro_timestamp;
}
}
}
_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
_mocapUpdated = _sub_mocap.updated();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
bool targetPositionUpdated = _sub_landing_target_pose.updated();
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) {
vxy_stddev_ok = true;
}
if (_estimatorInitialized & EST_XY) {
// if valid and gps has timed out, set to not valid
if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) {
_estimatorInitialized &= ~EST_XY;
}
} else {
if (vxy_stddev_ok) {
if (!(_sensorTimeout & SENSOR_GPS)
|| !(_sensorTimeout & SENSOR_FLOW)
|| !(_sensorTimeout & SENSOR_VISION)
|| !(_sensorTimeout & SENSOR_MOCAP)
|| !(_sensorTimeout & SENSOR_LAND)
|| !(_sensorTimeout & SENSOR_LAND_TARGET)
) {
_estimatorInitialized |= EST_XY;
}
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_estimatorInitialized & EST_Z) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) {
_estimatorInitialized &= ~EST_Z;
}
} else {
if (z_stddev_ok) {
_estimatorInitialized |= EST_Z;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_estimatorInitialized & EST_TZ) {
if (!tz_stddev_ok) {
_estimatorInitialized &= ~EST_TZ;
}
} else {
if (tz_stddev_ok) {
_estimatorInitialized |= EST_TZ;
}
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) {
map_projection_init(&_map_ref,
_init_origin_lat.get(),
_init_origin_lon.get());
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
}
// force P symmetry and reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j <= i; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%d, %d) not finite", msg_label, i, j);
reinit_P = true;
}
if (i == j) {
// make sure diagonal elements are positive
if (_P(i, i) <= 0) {
mavlink_and_console_log_info(&mavlink_log_pub,
"%sreinit P (%d, %d) negative", msg_label, i, j);
reinit_P = true;
}
} else {
// copy elememnt from upper triangle to force
// symmetry
_P(j, i) = _P(i, j);
}
if (reinit_P) { break; }
}
if (reinit_P) { break; }
}
if (reinit_P) {
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (_gpsUpdated) {
if (_sensorTimeout & SENSOR_GPS) {
gpsInit();
} else {
gpsCorrect();
}
}
if (_baroUpdated) {
if (_sensorTimeout & SENSOR_BARO) {
baroInit();
} else {
baroCorrect();
}
}
if (_lidarUpdated) {
if (_sensorTimeout & SENSOR_LIDAR) {
lidarInit();
} else {
lidarCorrect();
}
}
if (_sonarUpdated) {
if (_sensorTimeout & SENSOR_SONAR) {
sonarInit();
} else {
sonarCorrect();
}
}
if (_flowUpdated) {
if (_sensorTimeout & SENSOR_FLOW) {
flowInit();
} else {
flowCorrect();
}
}
if (_visionUpdated) {
if (_sensorTimeout & SENSOR_VISION) {
visionInit();
} else {
visionCorrect();
}
}
if (_mocapUpdated) {
if (_sensorTimeout & SENSOR_MOCAP) {
mocapInit();
} else {
mocapCorrect();
}
}
if (_landUpdated) {
if (_sensorTimeout & SENSOR_LAND) {
landInit();
} else {
landCorrect();
}
}
if (targetPositionUpdated) {
if (_sensorTimeout & SENSOR_LAND_TARGET) {
landingTargetInit();
} else {
landingTargetCorrect();
}
}
if (_altOriginInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
_pub_innov.get().timestamp = _timeStamp;
_pub_innov.update();
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
Visitor::Result AnimateVisitor::processNodeTopDown(simulation::Node* node)
{
if (!node->isActive()) return Visitor::RESULT_PRUNE;
if (node->isSleeping()) return Visitor::RESULT_PRUNE;
if (!firstNodeVisited)
{
firstNodeVisited=true;
sofa::core::ConstraintParams cparams(*this->params);
MechanicalResetConstraintVisitor resetConstraint(&cparams);
node->execute(&resetConstraint);
}
if (dt == 0) setDt(node->getDt());
else node->setDt(dt);
if (node->collisionPipeline != NULL)
{
processCollisionPipeline(node, node->collisionPipeline);
}
if (!node->solver.empty() )
{
sofa::helper::AdvancedTimer::StepVar timer("Mechanical",node);
SReal nextTime = node->getTime() + dt;
{
IntegrateBeginEvent evBegin;
PropagateEventVisitor eventPropagation( this->params, &evBegin);
eventPropagation.execute(node);
}
MechanicalBeginIntegrationVisitor beginVisitor(this->params, dt);
node->execute(&beginVisitor);
sofa::core::MechanicalParams m_mparams(*this->params);
m_mparams.setDt(dt);
{
unsigned int constraintId=0;
core::ConstraintParams cparams;
simulation::MechanicalAccumulateConstraint(&cparams, core::MatrixDerivId::constraintJacobian(),constraintId).execute(node);
}
for( unsigned i=0; i<node->solver.size(); i++ )
{
ctime_t t0 = begin(node, node->solver[i]);
node->solver[i]->solve(params, getDt());
end(node, node->solver[i], t0);
}
MechanicalProjectPositionAndVelocityVisitor(&m_mparams, nextTime,
sofa::core::VecCoordId::position(), sofa::core::VecDerivId::velocity()
).execute( node );
MechanicalPropagateOnlyPositionAndVelocityVisitor(&m_mparams, nextTime,VecCoordId::position(),VecDerivId::velocity(),
#ifdef SOFA_SUPPORT_MAPPED_MASS
VecDerivId::dx(),
#endif
true).execute( node );
MechanicalEndIntegrationVisitor endVisitor(this->params, dt);
node->execute(&endVisitor);
{
IntegrateEndEvent evBegin;
PropagateEventVisitor eventPropagation(this->params, &evBegin);
eventPropagation.execute(node);
}
return RESULT_PRUNE;
}
{
// process InteractionForceFields
for_each(this, node, node->interactionForceField, &AnimateVisitor::fwdInteractionForceField);
return RESULT_CONTINUE;
}
}
Visitor::Result AnimateVisitor::processNodeTopDown(simulation::Node* node)
{
//cerr<<"AnimateVisitor::process Node "<<node->getName()<<endl;
if (!node->isActive()) return Visitor::RESULT_PRUNE;
#ifdef SOFA_HAVE_EIGEN2
if (!firstNodeVisited)
{
firstNodeVisited=true;
// core::behavior::BaseAnimationLoop* presenceAnimationManager;
// node->get(presenceAnimationManager, core::objectmodel::BaseContext::SearchDown);
// if (!presenceAnimationManager)
// {
// std::cerr << "AnimateVisitor::processNodeTopDown, ERROR: no BaseAnimationLoop found while searching down from node: " << node->getName() << std::endl;
// }
sofa::core::MechanicalParams mparams(*this->params);
mparams.setDt(dt);
MechanicalResetConstraintVisitor resetConstraint(&mparams);
node->execute(&resetConstraint);
}
#endif
if (dt == 0) setDt(node->getDt());
else node->setDt(dt);
if (node->collisionPipeline != NULL)
{
//ctime_t t0 = begin(node, node->collisionPipeline);
#ifndef SOFA_SMP
{
CollisionBeginEvent evBegin;
PropagateEventVisitor eventPropagation(this->params /* PARAMS FIRST */, &evBegin);
eventPropagation.execute(node);
}
processCollisionPipeline(node, node->collisionPipeline);
{
CollisionEndEvent evEnd;
PropagateEventVisitor eventPropagation(this->params /* PARAMS FIRST */, &evEnd);
eventPropagation.execute(node);
}
#endif
//end(node, node->collisionPipeline, t0);
}
/* if (node->solver != NULL)
{
ctime_t t0 = begin(node, node->solver);
processOdeSolver(node, node->solver);
end(node, node->solver, t0);
return RESULT_PRUNE;
}*/
if (!node->solver.empty() )
{
sofa::helper::AdvancedTimer::StepVar timer("Mechanical",node);
double nextTime = node->getTime() + dt;
{
IntegrateBeginEvent evBegin;
PropagateEventVisitor eventPropagation( this->params /* PARAMS FIRST */, &evBegin);
eventPropagation.execute(node);
}
MechanicalBeginIntegrationVisitor beginVisitor(this->params /* PARAMS FIRST */, dt);
node->execute(&beginVisitor);
sofa::core::MechanicalParams m_mparams(*this->params);
m_mparams.setDt(dt);
#ifdef SOFA_HAVE_EIGEN2
{
unsigned int constraintId=0;
core::ConstraintParams cparams;
//MechanicalAccumulateConstraint(&m_mparams /* PARAMS FIRST */, constraintId, VecCoordId::position()).execute(node);
simulation::MechanicalAccumulateConstraint(&cparams /* PARAMS FIRST */, core::MatrixDerivId::holonomicC(),constraintId).execute(node);
}
#endif
for( unsigned i=0; i<node->solver.size(); i++ )
{
ctime_t t0 = begin(node, node->solver[i]);
//cerr<<"AnimateVisitor::processNodeTpDown solver "<<node->solver[i]->getName()<<endl;
node->solver[i]->solve(params /* PARAMS FIRST */, getDt());
end(node, node->solver[i], t0);
}
MechanicalPropagatePositionAndVelocityVisitor(&m_mparams /* PARAMS FIRST */, nextTime,VecCoordId::position(),VecDerivId::velocity(),
#ifdef SOFA_SUPPORT_MAPPED_MASS
VecDerivId::dx(),
#endif
true).execute( node );
MechanicalEndIntegrationVisitor endVisitor(this->params /* PARAMS FIRST */, dt);
node->execute(&endVisitor);
{
IntegrateEndEvent evBegin;
PropagateEventVisitor eventPropagation(this->params /* PARAMS FIRST */, &evBegin);
eventPropagation.execute(node);
}
return RESULT_PRUNE;
}
/*
if (node->mechanicalModel != NULL)
{
std::cerr << "Graph Error: MechanicalState without solver." << std::endl;
return RESULT_PRUNE;
}
*/
{
// process InteractionForceFields
for_each(this, node, node->interactionForceField, &AnimateVisitor::fwdInteractionForceField);
return RESULT_CONTINUE;
}
}
