PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTaskManual.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  * @file FlightTaskManual.cpp
35  */
36 
37 #include "FlightTaskManual.hpp"
38 #include <mathlib/mathlib.h>
39 #include <float.h>
40 #include <drivers/drv_hrt.h>
41 
42 using namespace matrix;
43 using namespace time_literals;
44 
46 {
47  bool ret = FlightTask::updateInitialize();
48 
49  _sub_manual_control_setpoint.update();
50 
51  const bool sticks_available = _evaluateSticks();
52 
53  if (_sticks_data_required) {
54  ret = ret && sticks_available;
55  }
56 
57  return ret;
58 }
59 
61 {
62  hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
63 
64  /* Sticks are rescaled linearly and exponentially to [-1,1] */
65  if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
66 
67  /* Linear scale */
68  _sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */
69  _sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */
70  _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
71  _sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */
72 
73  /* Exponential scale */
74  _sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
75  _sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
76  _sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
77  _sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
78 
79  // Only switch the landing gear up if the user switched from gear down to gear up.
80  // If the user had the switch in the gear up position and took off ignore it
81  // until he toggles the switch to avoid retracting the gear immediately on takeoff.
82  int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
83 
84  if (_gear_switch_old != gear_switch) {
85  _applyGearSwitch(gear_switch);
86  }
87 
88  _gear_switch_old = gear_switch;
89 
90  // valid stick inputs are required
91  const bool valid_sticks = PX4_ISFINITE(_sticks(0))
92  && PX4_ISFINITE(_sticks(1))
93  && PX4_ISFINITE(_sticks(2))
94  && PX4_ISFINITE(_sticks(3));
95 
96  return valid_sticks;
97 
98  } else {
99  /* Timeout: set all sticks to zero */
100  _sticks.zero();
101  _sticks_expo.zero();
102  _gear.landing_gear = landing_gear_s::GEAR_KEEP;
103  return false;
104  }
105 }
106 
108 {
109  if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
110  _gear.landing_gear = landing_gear_s::GEAR_DOWN;
111  }
112 
113  if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
114  _gear.landing_gear = landing_gear_s::GEAR_UP;
115  }
116 }
bool _evaluateSticks()
checks and sets stick inputs
High-resolution timer with callouts and timekeeping.
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
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
void _applyGearSwitch(uint8_t gswitch)
Sets gears according to switch.
const T expo_deadzone(const T &value, const T &e, const T &dz)
Definition: Functions.hpp:123
bool updateInitialize() override
Call before activate() or update() to initialize time and input data.
Linear and exponential map from stick inputs to range -1 and 1.