From af0c42d3d761736f0b1ae0163ac94bf61c978b09 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 7 May 2016 16:51:05 +0200 Subject: [PATCH] allow direct transition in standard vtol when landed --- src/modules/vtol_att_control/standard.cpp | 2 +- .../vtol_att_control/vtol_att_control_main.cpp | 17 +++++++++++++++++ .../vtol_att_control/vtol_att_control_main.h | 5 +++++ src/modules/vtol_att_control/vtol_type.cpp | 6 ++++++ src/modules/vtol_att_control/vtol_type.h | 6 ++++++ 5 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 3bf8080e63..ba6c0b7187 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -183,7 +183,7 @@ void Standard::update_vtol_state() if ((_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)) || - !_armed->armed) { + can_transition_on_ground()) { _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index b247df4a3f..8cc5014205 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -115,6 +115,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_tecs_status, 0, sizeof(_tecs_status)); + memset(&_land_detected, 0, sizeof(_land_detected)); _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; @@ -444,6 +445,21 @@ VtolAttitudeControl::tecs_status_poll() } } +/** +* Check for land detector updates. +*/ +void +VtolAttitudeControl::land_detected_poll() +{ + bool updated; + + orb_check(_land_detected_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub , &_land_detected); + } +} + /** * Check received command */ @@ -692,6 +708,7 @@ void VtolAttitudeControl::task_main() vehicle_cmd_poll(); vehicle_status_poll(); tecs_status_poll(); + land_detected_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 5a5b5eed57..cf072af71d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -85,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -132,6 +133,7 @@ public: struct battery_status_s *get_batt_status() {return &_batt_status;} struct vehicle_status_s *get_vehicle_status() {return &_vehicle_status;} struct tecs_status_s *get_tecs_status() {return &_tecs_status;} + struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;} struct Params *get_params() {return &_params;} @@ -159,6 +161,7 @@ private: int _vehicle_cmd_sub; int _vehicle_status_sub; int _tecs_status_sub; + int _land_detected_sub; int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -191,6 +194,7 @@ private: struct vehicle_command_s _vehicle_cmd; struct vehicle_status_s _vehicle_status; struct tecs_status_s _tecs_status; + struct vehicle_land_detected_s _land_detected; Params _params; // struct holding the parameters @@ -241,6 +245,7 @@ private: void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); void tecs_status_poll(); + void land_detected_poll(); void parameters_update_poll(); //Check if parameters have changed void vehicle_status_poll(); int parameters_update(); //Update local paraemter cache diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 681c782815..8184d55d2e 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -68,6 +68,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _batt_status = _attc->get_batt_status(); _vehicle_status = _attc->get_vehicle_status(); _tecs_status = _attc->get_tecs_status(); + _land_detected = _attc->get_land_detected(); _params = _attc->get_params(); flag_idle_mc = true; @@ -168,3 +169,8 @@ void VtolType::update_fw_state() waiting_on_tecs(); } } + +bool VtolType::can_transition_on_ground() +{ + return !_armed->armed || _land_detected->landed; +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index ddfed44f57..9a15ff8cfa 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -120,6 +120,11 @@ public: */ virtual void waiting_on_tecs() {}; + /** + * Returns true if we're allowed to do a mode transition on the ground. + */ + bool can_transition_on_ground(); + void set_idle_mc(); void set_idle_fw(); @@ -149,6 +154,7 @@ protected: struct battery_status_s *_batt_status; // battery status struct vehicle_status_s *_vehicle_status; // vehicle status from commander app struct tecs_status_s *_tecs_status; + struct vehicle_land_detected_s *_land_detected; struct Params *_params;