PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTaskTransition.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 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 /**
35  * @file FlightTaskTranstion.cpp
36  */
37 
38 #include "FlightTaskTransition.hpp"
39 
41 {
43 }
44 
46 {
47  checkSetpoints(last_setpoint);
48  _transition_altitude = last_setpoint.z;
49  _transition_yaw = last_setpoint.yaw;
52  return FlightTask::activate(last_setpoint);
53 }
54 
56 {
57  // If the setpoint is unknown, set to the current estimate
58  if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
59 
60  if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
61 }
62 
64 {
65  // Estimate the acceleration by filtering the raw derivative of the velocity estimate
66  // This is done to provide a good estimate of the current acceleration to the next flight task after back-transition
68 
69  if (!PX4_ISFINITE(_acceleration_setpoint(0)) ||
70  !PX4_ISFINITE(_acceleration_setpoint(1)) ||
71  !PX4_ISFINITE(_acceleration_setpoint(2))) {
73  }
74 
76 }
77 
79 {
80  // level wings during the transition, altitude should be controlled
81  _thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
82  _thrust_setpoint(2) = NAN;
83  _position_setpoint *= NAN;
84  _velocity_setpoint *= NAN;
86 
88 
90  return true;
91 }
matrix::Vector3f _acceleration_setpoint
Definition: FlightTask.hpp:222
matrix::Vector3f _velocity
current vehicle velocity
Definition: FlightTask.hpp:208
float _yaw_setpoint
Definition: FlightTask.hpp:225
virtual bool activate(vehicle_local_position_setpoint_s last_setpoint)
Call once on the event where you switch to the task.
Definition: FlightTask.cpp:12
float _deltatime
passed time in seconds since the task was last updated
Definition: FlightTask.hpp:201
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
Call once on the event where you switch to the task.
Flight task for automatic VTOL transitions between hover and forward flight and vice versa...
matrix::Vector3f _velocity_prev
matrix::Vector3f _position_setpoint
Setpoints which the position controller has to execute.
Definition: FlightTask.hpp:220
matrix::Vector3f _thrust_setpoint
Definition: FlightTask.hpp:224
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
virtual bool updateInitialize()
Call before activate() or update() to initialize time and input data.
Definition: FlightTask.cpp:27
void checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
matrix::Vector3f _position
current vehicle position
Definition: FlightTask.hpp:207
matrix::Vector3f _velocity_setpoint
Definition: FlightTask.hpp:221
bool update() override
To be called regularly in the control loop cycle to execute the task.
bool updateInitialize() override
Call before activate() or update() to initialize time and input data.
float _yaw
current vehicle yaw heading
Definition: FlightTask.hpp:209
void setAll(Type val)
Definition: Matrix.hpp:426