From de185726b3eb65e5461faac1a41fa58a7a33d43e Mon Sep 17 00:00:00 2001
From: Simone Guscetti <simone@yuneecresearch.com>
Date: Thu, 1 Nov 2018 18:00:27 +0100
Subject: [PATCH] vehicle constraints: remove landing gear
- landing_gear: refactor state name
- Add the keep state to the landing gear message
- Adapt FlightTaskManual, FlightTaskAutoMapper, mc_pos_control,
to review message definition
---
msg/landing_gear.msg | 7 ++++---
msg/vehicle_constraints.msg | 5 -----
.../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 8 ++++----
.../tasks/AutoMapper2/FlightTaskAutoMapper2.cpp | 6 +++---
.../FlightTasks/tasks/Manual/FlightTaskManual.cpp | 8 ++++----
src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++--
src/modules/mc_pos_control/mc_pos_control_main.cpp | 13 +++++++------
7 files changed, 24 insertions(+), 27 deletions(-)
diff --git a/msg/landing_gear.msg b/msg/landing_gear.msg
index ce98933531..5ef9ee52f2 100644
--- a/msg/landing_gear.msg
+++ b/msg/landing_gear.msg
@@ -1,6 +1,7 @@
-uint64 timestamp # time since system start (microseconds)
+uint64 timestamp # time since system start (microseconds)
-int8 LANDING_GEAR_UP = 1 # landing gear up
-int8 LANDING_GEAR_DOWN = -1 # landing gear down
+int8 GEAR_UP = 1 # landing gear up
+int8 GEAR_DOWN = -1 # landing gear down
+int8 GEAR_KEEP = 0 # keep the current state
int8 landing_gear
diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg
index 9c241536d6..50a145cf9a 100644
--- a/msg/vehicle_constraints.msg
+++ b/msg/vehicle_constraints.msg
@@ -10,8 +10,3 @@ float32 speed_down # in meters/sec
float32 tilt # in radians [0, PI]
float32 min_distance_to_ground # in meters
float32 max_distance_to_ground # in meters
-
-int8 GEAR_DOWN = -1
-int8 GEAR_UP = 1
-int8 GEAR_KEEP = 0
-int8 landing_gear # Down, UP or KEEP
diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
index 238d8e7306..f7e881b05d 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
@@ -89,7 +89,7 @@ bool FlightTaskAutoMapper::update()
// during mission and reposition, raise the landing gears but only
// if altitude is high enough
if (_highEnoughForLandingGear()) {
- _constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
+ _gear.landing_gear = landing_gear_s::GEAR_UP;
}
// update previous type
@@ -121,7 +121,7 @@ void FlightTaskAutoMapper::_generateLandSetpoints()
// set constraints
_constraints.tilt = MPC_TILTMAX_LND.get();
_constraints.speed_down = MPC_LAND_SPEED.get();
- _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+ _gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_generateTakeoffSetpoints()
@@ -134,7 +134,7 @@ void FlightTaskAutoMapper::_generateTakeoffSetpoints()
_constraints.speed_up = math::gradual(_alt_above_ground, MPC_LAND_ALT2.get(),
MPC_LAND_ALT1.get(), MPC_TKO_SPEED.get(), _constraints.speed_up);
- _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+ _gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_generateVelocitySetpoints()
@@ -173,4 +173,4 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
{
// return true if altitude is above two meters
return _alt_above_ground > 2.0f;
-}
\ No newline at end of file
+}
diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
index a1dcf51a18..83aff05f51 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
@@ -96,7 +96,7 @@ bool FlightTaskAutoMapper2::update()
// during mission and reposition, raise the landing gears but only
// if altitude is high enough
if (_highEnoughForLandingGear()) {
- _constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
+ _gear.landing_gear = landing_gear_s::GEAR_UP;
}
// update previous type
@@ -129,7 +129,7 @@ void FlightTaskAutoMapper2::_prepareLandSetpoints()
// set constraints
_constraints.tilt = MPC_TILTMAX_LND.get();
- _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+ _gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
@@ -139,7 +139,7 @@ void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
const float speed_tko = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_up : MPC_TKO_SPEED.get();
_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
- _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+ _gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper2::_prepareVelocitySetpoints()
diff --git a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
index 68ebd7e3f1..0246ef56c3 100644
--- a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
+++ b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
@@ -91,7 +91,7 @@ bool FlightTaskManual::_evaluateSticks()
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
int8_t gear_switch = _sub_manual_control_setpoint->get().gear_switch;
- if (!_constraints.landing_gear) {
+ if (!_gear.landing_gear) {
if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_applyGearSwitch(gear_switch);
}
@@ -112,7 +112,7 @@ bool FlightTaskManual::_evaluateSticks()
/* Timeout: set all sticks to zero */
_sticks.zero();
_sticks_expo.zero();
- _constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
+ _gear.landing_gear = landing_gear_s::GEAR_KEEP;
return false;
}
}
@@ -120,10 +120,10 @@ bool FlightTaskManual::_evaluateSticks()
void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
{
if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
- _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
+ _gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
- _constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
+ _gear.landing_gear = landing_gear_s::GEAR_UP;
}
}
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index a00b5920f7..27c2df2b94 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -415,9 +415,9 @@ MulticopterAttitudeControl::get_landing_gear_state()
if (_vehicle_land_detected.landed) {
_gear_state_initialized = false;
}
- float landing_gear = landing_gear_s::LANDING_GEAR_DOWN; // default to down
+ float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
- landing_gear = landing_gear_s::LANDING_GEAR_UP;
+ landing_gear = landing_gear_s::GEAR_UP;
} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
// Switching the gear off does put it into a safe defined state
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 5d9f910724..e2bfe52646 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -691,6 +691,7 @@ MulticopterPositionControl::run()
update_avoidance_waypoint_desired(_states, setpoint);
vehicle_constraints_s constraints = _flight_tasks.getConstraints();
+ landing_gear_s gear = _flight_tasks.getGear();
// check if all local states are valid and map accordingly
set_vehicle_states(setpoint.vz);
@@ -732,7 +733,7 @@ MulticopterPositionControl::run()
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.yawspeed = NAN;
setpoint.yaw = _states.yaw;
- constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
+ gear.landing_gear = landing_gear_s::GEAR_KEEP;
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
}
@@ -801,11 +802,11 @@ MulticopterPositionControl::run()
// they might conflict with each other such as in offboard attitude control.
publish_attitude();
- // if theres any change in landing gear setpoint publish it
- if (_old_landing_gear_position != constraints.landing_gear
- && constraints.landing_gear != vehicle_constraints_s::GEAR_KEEP) {
+ // if there's any change in landing gear setpoint publish it
+ if (gear.landing_gear != _old_landing_gear_position
+ && gear.landing_gear != landing_gear_s::GEAR_KEEP) {
- _landing_gear.landing_gear = constraints.landing_gear;
+ _landing_gear.landing_gear = gear.landing_gear;
_landing_gear.timestamp = hrt_absolute_time();
if (_landing_gear_pub != nullptr) {
@@ -816,7 +817,7 @@ MulticopterPositionControl::run()
}
}
- _old_landing_gear_position = constraints.landing_gear;
+ _old_landing_gear_position = gear.landing_gear;
} else {
// no flighttask is active: set attitude setpoint to idle
--
GitLab