void
GpsFailure::on_active()
{
switch (_gpsf_state) {
case GPSF_STATE_LOITER: {
/* Position controller does not run in this mode:
* navigator has to publish an attitude setpoint */
_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
_navigator->publish_att_sp();
/* Measure time */
hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
if (elapsed > _param_loitertime.get() * 1e6f) {
/* no recovery, adavance the state machine */
warnx("gps not recovered, switch to next state");
advance_gpsf();
}
break;
}
case GPSF_STATE_TERMINATE:
set_gpsf_item();
advance_gpsf();
break;
default:
break;
}
}
void
GpsFailure::on_activation()
{
_gpsf_state = GPSF_STATE_NONE;
_timestamp_activation = hrt_absolute_time();
updateParams();
advance_gpsf();
set_gpsf_item();
}
