From de185726b3eb65e5461faac1a41fa58a7a33d43e Mon Sep 17 00:00:00 2001 From: Simone Guscetti 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