PX4 Firmware
PX4 Autopilot Software http://px4.io
gpsfailure.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 /**
34  * @file gpsfailure.cpp
35  * Helper class for gpsfailure mode according to the OBC rules
36  *
37  * @author Thomas Gubler <thomasgubler@gmail.com>
38  */
39 
40 #include "gpsfailure.h"
41 #include "navigator.h"
42 
43 #include <systemlib/mavlink_log.h>
44 #include <lib/ecl/geo/geo.h>
45 #include <navigator/navigation.h>
46 #include <uORB/uORB.h>
47 #include <uORB/topics/mission.h>
50 #include <mathlib/mathlib.h>
51 
52 using matrix::Eulerf;
53 using matrix::Quatf;
54 
56  MissionBlock(navigator),
57  ModuleParams(navigator)
58 {
59 }
60 
61 void
63 {
64  /* reset GPSF state only if setpoint moved */
66  _gpsf_state = GPSF_STATE_NONE;
67  }
68 }
69 
70 void
72 {
73  _gpsf_state = GPSF_STATE_NONE;
75  advance_gpsf();
76  set_gpsf_item();
77 }
78 
79 void
81 {
82  switch (_gpsf_state) {
83  case GPSF_STATE_LOITER: {
84  /* Position controller does not run in this mode:
85  * navigator has to publish an attitude setpoint */
86  vehicle_attitude_setpoint_s att_sp = {};
87  att_sp.timestamp = hrt_absolute_time();
88  att_sp.roll_body = math::radians(_param_nav_gpsf_r.get());
89  att_sp.pitch_body = math::radians(_param_nav_gpsf_p.get());
90  att_sp.thrust_body[0] = _param_nav_gpsf_tr.get();
91 
92  Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
93  q.copyTo(att_sp.q_d);
94  att_sp.q_d_valid = true;
95 
96  if (_navigator->get_vstatus()->is_vtol) {
98 
99  } else {
100  _att_sp_pub.publish(att_sp);
101 
102  }
103 
104  /* Measure time */
105  if ((_param_nav_gpsf_lt.get() > FLT_EPSILON) &&
106  (hrt_elapsed_time(&_timestamp_activation) > _param_nav_gpsf_lt.get() * 1e6f)) {
107  /* no recovery, advance the state machine */
108  PX4_WARN("GPS not recovered, switching to next failure state");
109  advance_gpsf();
110  }
111 
112  break;
113  }
114 
115  case GPSF_STATE_TERMINATE:
116  set_gpsf_item();
117  advance_gpsf();
118  break;
119 
120  default:
121  break;
122  }
123 }
124 
125 void
127 {
129 
130  /* Set pos sp triplet to invalid to stop pos controller */
131  pos_sp_triplet->previous.valid = false;
132  pos_sp_triplet->current.valid = false;
133  pos_sp_triplet->next.valid = false;
134 
135  switch (_gpsf_state) {
136  case GPSF_STATE_TERMINATE: {
137  /* Request flight termination from commander */
140  PX4_WARN("GPS failure: request flight termination");
141  }
142  break;
143 
144  default:
145  break;
146  }
147 
149 }
150 
151 void
153 {
154  switch (_gpsf_state) {
155  case GPSF_STATE_NONE:
156  _gpsf_state = GPSF_STATE_LOITER;
157  mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: fixed bank loiter");
158  break;
159 
160  case GPSF_STATE_LOITER:
161  _gpsf_state = GPSF_STATE_TERMINATE;
162  mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight");
163  break;
164 
165  case GPSF_STATE_TERMINATE:
166  PX4_WARN("terminate");
167  _gpsf_state = GPSF_STATE_END;
168  break;
169 
170  default:
171  break;
172  }
173 }
void set_gpsf_item()
Set the GPSF item.
Definition: gpsfailure.cpp:126
void on_activation() override
This function is called one time when mode becomes active, pos_sp_triplet must be initialized here...
Definition: gpsfailure.cpp:71
API for the uORB lightweight object broker.
Definition of geo / math functions to perform geodesic calculations.
struct position_setpoint_s next
struct position_setpoint_s previous
#define FLT_EPSILON
bool get_can_loiter_at_sp()
Definition: navigator.h:170
Navigator * _navigator
uORB::Publication< vehicle_attitude_setpoint_s > _fw_virtual_att_sp_pub
Definition: gpsfailure.h:79
struct position_setpoint_s current
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
uORB::Publication< vehicle_attitude_setpoint_s > _att_sp_pub
Definition: gpsfailure.h:78
constexpr T radians(T degrees)
Definition: Limits.hpp:85
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
Euler< float > Eulerf
Definition: Euler.hpp:156
struct position_setpoint_triplet_s * get_position_setpoint_triplet()
Definition: navigator.h:153
GpsFailure(Navigator *navigator)
Definition: gpsfailure.cpp:55
struct mission_result_s * get_mission_result()
Definition: navigator.h:152
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
Helper class for Data Link Loss Mode according to the OBC rules.
hrt_abstime _timestamp_activation
Definition: gpsfailure.h:76
void advance_gpsf()
Move to next GPSF item.
Definition: gpsfailure.cpp:152
orb_advert_t * get_mavlink_log_pub()
Definition: navigator.h:261
struct vehicle_status_s * get_vstatus()
Definition: navigator.h:159
void set_mission_result_updated()
Definition: navigator.h:146
void on_inactive() override
This function is called while the mode is inactive.
Definition: gpsfailure.cpp:62
void on_active() override
This function is called while the mode is active.
Definition: gpsfailure.cpp:80
void set_position_setpoint_triplet_updated()
Definition: navigator.h:145
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).