Browse Source

Move land detector changes to vtol

sbg
sanderux 8 years ago committed by Lorenz Meier
parent
commit
6ee24a0c80
  1. 12
      src/modules/land_detector/MulticopterLandDetector.cpp
  2. 2
      src/modules/land_detector/MulticopterLandDetector.h
  3. 19
      src/modules/land_detector/VtolLandDetector.cpp
  4. 5
      src/modules/land_detector/VtolLandDetector.h

12
src/modules/land_detector/MulticopterLandDetector.cpp

@ -123,7 +123,6 @@ void MulticopterLandDetector::_initialize_topics()
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_battery_sub = orb_subscribe(ORB_ID(battery_status)); _battery_sub = orb_subscribe(ORB_ID(battery_status));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
} }
void MulticopterLandDetector::_update_topics() void MulticopterLandDetector::_update_topics()
@ -137,7 +136,6 @@ void MulticopterLandDetector::_update_topics()
_orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); _orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
_orb_update(ORB_ID(battery_status), _battery_sub, &_battery); _orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
} }
void MulticopterLandDetector::_update_params() void MulticopterLandDetector::_update_params()
@ -194,11 +192,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
_arming_time = now; _arming_time = now;
} }
// Only trigger in RW mode
if (!_vehicle_status.is_rotary_wing) {
return false;
}
// If in manual flight mode never report landed if the user has more than idle throttle // If in manual flight mode never report landed if the user has more than idle throttle
// Check if user commands throttle and if so, report no ground contact based on // Check if user commands throttle and if so, report no ground contact based on
// the user intent to take off (even if the system might physically still have // the user intent to take off (even if the system might physically still have
@ -249,11 +242,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
return true; return true;
} }
// Only trigger in RW mode
if (!_vehicle_status.is_rotary_wing) {
return false;
}
// If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff // If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff
if (_state == LandDetectionState::LANDED && _has_manual_control_present()) { if (_state == LandDetectionState::LANDED && _has_manual_control_present()) {
if (_manual.z < _get_takeoff_throttle()) { if (_manual.z < _get_takeoff_throttle()) {

2
src/modules/land_detector/MulticopterLandDetector.h

@ -126,7 +126,6 @@ private:
int _ctrl_state_sub; int _ctrl_state_sub;
int _vehicle_control_mode_sub; int _vehicle_control_mode_sub;
int _battery_sub; int _battery_sub;
int _vehicle_status_sub;
struct vehicle_local_position_s _vehicleLocalPosition; struct vehicle_local_position_s _vehicleLocalPosition;
struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint; struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint;
@ -137,7 +136,6 @@ private:
struct control_state_s _ctrl_state; struct control_state_s _ctrl_state;
struct vehicle_control_mode_s _control_mode; struct vehicle_control_mode_s _control_mode;
struct battery_status_s _battery; struct battery_status_s _battery;
struct vehicle_status_s _vehicle_status;
uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first
uint64_t _arming_time; uint64_t _arming_time;

19
src/modules/land_detector/VtolLandDetector.cpp

@ -50,6 +50,7 @@ VtolLandDetector::VtolLandDetector() :
_paramHandle(), _paramHandle(),
_params(), _params(),
_airspeedSub(-1), _airspeedSub(-1),
_vehicle_status_sub(-1),
_airspeed{}, _airspeed{},
_was_in_air(false), _was_in_air(false),
_airspeed_filtered(0) _airspeed_filtered(0)
@ -62,6 +63,7 @@ void VtolLandDetector::_initialize_topics()
MulticopterLandDetector::_initialize_topics(); MulticopterLandDetector::_initialize_topics();
_airspeedSub = orb_subscribe(ORB_ID(airspeed)); _airspeedSub = orb_subscribe(ORB_ID(airspeed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
} }
void VtolLandDetector::_update_topics() void VtolLandDetector::_update_topics()
@ -69,10 +71,27 @@ void VtolLandDetector::_update_topics()
MulticopterLandDetector::_update_topics(); MulticopterLandDetector::_update_topics();
_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
bool VtolLandDetector::_get_maybe_landed_state()
{
// Only trigger in RW mode
if (!_vehicle_status.is_rotary_wing) {
return false;
}
return MulticopterLandDetector::_get_maybe_landed_state();
} }
bool VtolLandDetector::_get_landed_state() bool VtolLandDetector::_get_landed_state()
{ {
// Only trigger in RW mode
if (!_vehicle_status.is_rotary_wing) {
return false;
}
// this is returned from the mutlicopter land detector // this is returned from the mutlicopter land detector
bool landed = MulticopterLandDetector::_get_landed_state(); bool landed = MulticopterLandDetector::_get_landed_state();

5
src/modules/land_detector/VtolLandDetector.h

@ -58,6 +58,7 @@ protected:
void _update_params() override; void _update_params() override;
void _update_topics() override; void _update_topics() override;
bool _get_landed_state() override; bool _get_landed_state() override;
bool _get_maybe_landed_state() override;
private: private:
@ -70,8 +71,10 @@ private:
} _params; } _params;
int _airspeedSub; int _airspeedSub;
int _vehicle_status_sub;
struct airspeed_s _airspeed; struct airspeed_s _airspeed;
struct vehicle_status_s _vehicle_status;
bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */ bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */
float _airspeed_filtered; /**< low pass filtered airspeed */ float _airspeed_filtered; /**< low pass filtered airspeed */

Loading…
Cancel
Save