PX4 Firmware
PX4 Autopilot Software http://px4.io
takeoff.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 takeoff.cpp
35  *
36  * Helper class to Takeoff
37  *
38  * @author Lorenz Meier <lorenz@px4.io>
39  */
40 
41 #include "takeoff.h"
42 #include "navigator.h"
43 
45  MissionBlock(navigator)
46 {
47 }
48 
49 void
51 {
53 }
54 
55 void
57 {
59 
60  if (rep->current.valid) {
61  // reset the position
63 
67 
68  // set loiter item so position controllers stop doing takeoff logic
74  }
75 }
76 
77 void
79 {
81 
82  float abs_altitude = 0.0f;
83 
84  float min_abs_altitude;
85 
86  if (_navigator->home_position_valid()) { //only use home position if it is valid
88 
89  } else { //e.g. flow
90  min_abs_altitude = _navigator->get_takeoff_min_alt();
91  }
92 
93  // Use altitude if it has been set. If home position is invalid use min_abs_altitude
94  if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) {
95  abs_altitude = rep->current.alt;
96 
97  // If the altitude suggestion is lower than home + minimum clearance, raise it and complain.
98  if (abs_altitude < min_abs_altitude) {
99  if (abs_altitude < min_abs_altitude - 0.1f) { // don't complain if difference is smaller than 10cm
101  "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt());
102  }
103 
104  abs_altitude = min_abs_altitude;
105  }
106 
107  } else {
108  // Use home + minimum clearance but only notify.
109  abs_altitude = min_abs_altitude;
111  "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt());
112  }
113 
114 
115  if (abs_altitude < _navigator->get_global_position()->alt) {
116  // If the suggestion is lower than our current alt, let's not go down.
117  abs_altitude = _navigator->get_global_position()->alt;
118  mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude");
119  }
120 
121  // set current mission item to takeoff
122  set_takeoff_item(&_mission_item, abs_altitude);
126 
127  // convert mission item to current setpoint
131  pos_sp_triplet->previous.valid = false;
132  pos_sp_triplet->current.yaw_valid = true;
133  pos_sp_triplet->next.valid = false;
134 
135  if (rep->current.valid) {
136 
137  // Go on and check which changes had been requested
138  if (PX4_ISFINITE(rep->current.yaw)) {
139  pos_sp_triplet->current.yaw = rep->current.yaw;
140  }
141 
142  // Set the current latitude and longitude even if they are NAN
143  // NANs are handled in FlightTaskAuto.cpp
144  pos_sp_triplet->current.lat = rep->current.lat;
145  pos_sp_triplet->current.lon = rep->current.lon;
146 
147  // mark this as done
148  memset(rep, 0, sizeof(*rep));
149  }
150 
152 
154 }
bool home_position_valid()
Definition: navigator.h:166
void on_active() override
This function is called while the mode is active.
Definition: takeoff.cpp:56
void set_loiter_item(struct mission_item_s *item, float min_clearance=-1.0f)
Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current positi...
struct position_setpoint_s next
struct position_setpoint_triplet_s * get_takeoff_triplet()
Definition: navigator.h:155
struct position_setpoint_s previous
Navigator * _navigator
void on_activation() override
This function is called one time when mode becomes active, pos_sp_triplet must be initialized here...
Definition: takeoff.cpp:50
struct position_setpoint_s current
void set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch=0.0f)
Set a takeoff mission item.
struct vehicle_global_position_s * get_global_position()
Definition: navigator.h:156
mission_item_s _mission_item
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void mission_apply_limitation(mission_item_s &item)
General function used to adjust the mission item based on vehicle specific limitations.
void reset_mission_item_reached()
Reset all reached flags.
void set_takeoff_position()
Definition: takeoff.cpp:78
Helper class to take off.
struct position_setpoint_triplet_s * get_position_setpoint_triplet()
Definition: navigator.h:153
struct mission_result_s * get_mission_result()
Definition: navigator.h:152
bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
Convert a mission item to a position setpoint.
bool is_mission_item_reached()
Check if mission item has been reached.
orb_advert_t * get_mavlink_log_pub()
Definition: navigator.h:261
void set_mission_result_updated()
Definition: navigator.h:146
float get_takeoff_min_alt() const
Definition: navigator.h:286
void set_can_loiter_at_sp(bool can_loiter)
Setters.
Definition: navigator.h:144
void set_position_setpoint_triplet_updated()
Definition: navigator.h:145
Takeoff()=default