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(); }