diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 0cee13f7de..1efece610f 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -4,11 +4,9 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 uint8 VEHICLE_VTOL_STATE_MC = 3 uint8 VEHICLE_VTOL_STATE_FW = 4 -uint8 VEHICLE_VTOL_STATE_EXTERNAL = 5 bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode bool vtol_in_trans_mode bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW bool vtol_transition_failsafe # vtol in transition failsafe mode bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode -float32 airspeed_tot # Estimated airspeed over control surfaces diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 681f2a64ea..83e5f9802f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1567,7 +1567,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VTOL VEHICLE STATUS --- */ if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) { log_msg.msg_type = LOG_VTOL_MSG; - log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot; log_msg.body.log_VTOL.rw_mode = buf.vtol_status.vtol_in_rw_mode; log_msg.body.log_VTOL.trans_mode = buf.vtol_status.vtol_in_trans_mode; log_msg.body.log_VTOL.failsafe_mode = buf.vtol_status.vtol_transition_failsafe; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7dc9be9cc4..60b58d4fc1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -466,7 +466,6 @@ struct log_ENCD_s { /* --- VTOL - VTOL VEHICLE STATUS */ #define LOG_VTOL_MSG 43 struct log_VTOL_s { - float airspeed_tot; uint8_t rw_mode; uint8_t trans_mode; uint8_t failsafe_mode; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 97b7f6066c..11b6953441 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -50,13 +50,10 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) : VtolType(attc), - _airspeed_tot(0.0f), _min_front_trans_dur(0.5f), _thrust_transition_start(0.0f), _yaw_transition(0.0f), - _pitch_transition_start(0.0f), - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) + _pitch_transition_start(0.0f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -133,7 +130,6 @@ void Tailsitter::update_vtol_state() if (!_attc->is_fixed_wing_requested()) { - switch (_vtol_schedule.flight_mode) { // user switchig to MC mode case MC_MODE: break; @@ -334,9 +330,6 @@ void Tailsitter::update_transition_state() } - - - _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); @@ -358,58 +351,6 @@ void Tailsitter::waiting_on_tecs() _v_att_sp->thrust = _thrust_transition; } -void Tailsitter::calc_tot_airspeed() -{ - float airspeed = math::max(1.0f, _airspeed->indicated_airspeed_m_s); // prevent numerical drama - // calculate momentary power of one engine - float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count; - P = math::constrain(P, 1.0f, _params->power_max); - // calculate prop efficiency - float power_factor = 1.0f - P * _params->prop_eff / _params->power_max; - float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f; - eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side - // calculate induced airspeed by propeller - float v_ind = (airspeed / eta - airspeed) * 2.0f; - // calculate total airspeed - float airspeed_raw = airspeed + v_ind; - // apply low-pass filter - _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; -} - -void Tailsitter::scale_mc_output() -{ - // scale around tuning airspeed - float airspeed; - calc_tot_airspeed(); // estimate air velocity seen by elevons - - // if airspeed is not updating, we assume the normal average speed - if (bool nonfinite = !PX4_ISFINITE(_airspeed->indicated_airspeed_m_s) || - hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { - airspeed = _params->mc_airspeed_trim; - - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } - - } else { - airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max); - } - - _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : - airspeed); - _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling, - -1.0f, 1.0f); -} - void Tailsitter::update_mc_state() { VtolType::update_mc_state(); @@ -504,9 +445,5 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; - - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index aa0a225b58..6c6146eb37 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -96,8 +96,6 @@ private: hrt_abstime transition_start; /**< absoulte time at which front transition started */ } _vtol_schedule; - float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ - /** not sure about it yet ?! **/ float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ @@ -105,20 +103,6 @@ private: float _yaw_transition; // yaw angle in which transition will take place float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) - - /** should this anouncement stay? **/ - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - - /** - * Speed estimation for propwash controlled surfaces. - */ - void calc_tot_airspeed(); - - - /** is this one still needed? */ - void scale_mc_output(); - /** * Update parameters. */ diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c index e4cf2a1c32..34b17b0025 100644 --- a/src/modules/vtol_att_control/tailsitter_params.c +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -39,8 +39,6 @@ * @author David Vorsin */ -#include - /** * Duration of front transition phase 2 * diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 6f4cb7bb45..faef49d031 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -38,8 +38,6 @@ * @author Roman Bapst */ -#include - /** * Position of tilt servo in mc mode * 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 af2843264b..6a87bd9166 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -57,64 +57,9 @@ VtolAttitudeControl *g_control; /** * Constructor */ -VtolAttitudeControl::VtolAttitudeControl() : - _task_should_exit(false), - _control_task(-1), - - // mavlink log - _mavlink_log_pub(nullptr), - - //init subscription handlers - _v_att_sub(-1), - _v_att_sp_sub(-1), - _mc_virtual_att_sp_sub(-1), - _fw_virtual_att_sp_sub(-1), - _mc_virtual_v_rates_sp_sub(-1), - _fw_virtual_v_rates_sp_sub(-1), - _v_control_mode_sub(-1), - _params_sub(-1), - _manual_control_sp_sub(-1), - _local_pos_sub(-1), - _pos_sp_triplet_sub(-1), - _airspeed_sub(-1), - _battery_status_sub(-1), - _vehicle_cmd_sub(-1), - _tecs_status_sub(-1), - _land_detected_sub(-1), - - //init publication handlers - _actuators_0_pub(nullptr), - _actuators_1_pub(nullptr), - _vtol_vehicle_status_pub(nullptr), - _v_rates_sp_pub(nullptr), - _v_att_sp_pub(nullptr), - _v_cmd_ack_pub(nullptr), - _transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), - _abort_front_transition(false) - +VtolAttitudeControl::VtolAttitudeControl() { - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ - memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); - memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp)); - memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); - memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); - memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); - memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); - memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); - memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); - memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); - memset(&_local_pos, 0, sizeof(_local_pos)); - memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); - memset(&_airspeed, 0, sizeof(_airspeed)); - memset(&_batt_status, 0, sizeof(_batt_status)); - memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); - 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; @@ -122,13 +67,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); - _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); - _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); - _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); - _params_handles.power_max = param_find("VT_POWER_MAX"); - _params_handles.prop_eff = param_find("VT_PROP_EFF"); - _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); @@ -247,32 +186,6 @@ void VtolAttitudeControl::actuator_controls_fw_poll() } } -/** -* Check for attitude rates setpoint from mc attitude controller -*/ -void VtolAttitudeControl::vehicle_rates_sp_mc_poll() -{ - bool updated; - orb_check(_mc_virtual_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &_mc_virtual_v_rates_sp); - } -} - -/** -* Check for attitude rates setpoint from fw attitude controller -*/ -void VtolAttitudeControl::vehicle_rates_sp_fw_poll() -{ - bool updated; - orb_check(_fw_virtual_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &_fw_virtual_v_rates_sp); - } -} - /** * Check for airspeed updates. */ @@ -287,21 +200,6 @@ VtolAttitudeControl::vehicle_airspeed_poll() } } -/** -* Check for attitude set points update. -*/ -void -VtolAttitudeControl::vehicle_attitude_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); - } -} - /** * Check for attitude update. */ @@ -317,38 +215,6 @@ VtolAttitudeControl::vehicle_attitude_poll() } } -/** -* Check for battery updates. -*/ -void -VtolAttitudeControl::vehicle_battery_poll() -{ - bool updated; - orb_check(_battery_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(battery_status), _battery_status_sub, &_batt_status); - } -} - -/** -* Check for parameter updates. -*/ -void -VtolAttitudeControl::parameters_update_poll() -{ - bool updated; - - /* Check if parameters have changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - /** * Check for sensor updates. */ @@ -410,7 +276,6 @@ VtolAttitudeControl::mc_virtual_att_sp_poll() if (updated) { orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub, &_mc_virtual_att_sp); } - } /** @@ -426,7 +291,6 @@ VtolAttitudeControl::fw_virtual_att_sp_poll() if (updated) { orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub, &_fw_virtual_att_sp); } - } /** @@ -575,34 +439,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.vtol_fw_permanent_stab, &l); _vtol_vehicle_status.fw_permanent_stab = (l == 1); - /* vtol mc mode min airspeed */ - param_get(_params_handles.mc_airspeed_min, &v); - _params.mc_airspeed_min = v; - - /* vtol mc mode max airspeed */ - param_get(_params_handles.mc_airspeed_max, &v); - _params.mc_airspeed_max = v; - - /* vtol mc mode trim airspeed */ - param_get(_params_handles.mc_airspeed_trim, &v); - _params.mc_airspeed_trim = v; - /* vtol pitch trim for fw mode */ param_get(_params_handles.fw_pitch_trim, &v); _params.fw_pitch_trim = v; - /* vtol maximum power engine can produce */ - param_get(_params_handles.power_max, &v); - _params.power_max = v; - - /* vtol propeller efficiency factor */ - param_get(_params_handles.prop_eff, &v); - _params.prop_eff = v; - - /* vtol total airspeed estimate low pass gain */ - param_get(_params_handles.arsp_lp_gain, &v); - _params.arsp_lp_gain = v; - param_get(_params_handles.vtol_type, &l); _params.vtol_type = l; @@ -660,11 +500,22 @@ VtolAttitudeControl::parameters_update() */ void VtolAttitudeControl::fill_mc_att_rates_sp() { - _v_rates_sp.timestamp = _mc_virtual_v_rates_sp.timestamp; - _v_rates_sp.roll = _mc_virtual_v_rates_sp.roll; - _v_rates_sp.pitch = _mc_virtual_v_rates_sp.pitch; - _v_rates_sp.yaw = _mc_virtual_v_rates_sp.yaw; - _v_rates_sp.thrust = _mc_virtual_v_rates_sp.thrust; + bool updated; + orb_check(_mc_virtual_v_rates_sp_sub, &updated); + + if (updated) { + vehicle_rates_setpoint_s v_rates_sp; + + if (orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) { + // publish the attitude rates setpoint + if (_v_rates_sp_pub != nullptr) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp); + } + } + } } /** @@ -672,22 +523,21 @@ void VtolAttitudeControl::fill_mc_att_rates_sp() */ void VtolAttitudeControl::fill_fw_att_rates_sp() { - _v_rates_sp.timestamp = _fw_virtual_v_rates_sp.timestamp; - _v_rates_sp.roll = _fw_virtual_v_rates_sp.roll; - _v_rates_sp.pitch = _fw_virtual_v_rates_sp.pitch; - _v_rates_sp.yaw = _fw_virtual_v_rates_sp.yaw; - _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; -} + bool updated; + orb_check(_fw_virtual_v_rates_sp_sub, &updated); -void VtolAttitudeControl::publish_att_sp() -{ - if (_v_att_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + if (updated) { + vehicle_rates_setpoint_s v_rates_sp; - } else { - /* advertise and publish */ - _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + if (orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) { + // publish the attitude rates setpoint + if (_v_rates_sp_pub != nullptr) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp); + } + } } } @@ -708,7 +558,6 @@ void VtolAttitudeControl::task_main() _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -716,7 +565,6 @@ void VtolAttitudeControl::task_main() _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); _tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); @@ -730,48 +578,11 @@ void VtolAttitudeControl::task_main() _vtol_type->set_idle_mc(); /* wakeup source*/ - px4_pollfd_struct_t fds[2] = {}; /*input_mc, input_fw, parameters*/ - + px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; - fds[1].fd = _actuator_inputs_fw; - fds[1].events = POLLIN; while (!_task_should_exit) { - /*Advertise/Publish vtol vehicle status*/ - _vtol_vehicle_status.timestamp = hrt_absolute_time(); - - if (_vtol_vehicle_status_pub != nullptr) { - orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); - - } else { - _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); - } - - /* wait for up to 100ms for data */ - int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } - - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - } - - if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - } - /* only update parameters if they changed */ bool params_updated = false; orb_check(_params_sub, ¶ms_updated); @@ -785,25 +596,47 @@ void VtolAttitudeControl::task_main() parameters_update(); } - mc_virtual_att_sp_poll(); - fw_virtual_att_sp_poll(); - vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - vehicle_manual_poll(); //Check for changes in manual inputs. - vehicle_attitude_setpoint_poll();//Check for changes in attitude set points - vehicle_attitude_poll(); //Check for changes in attitude - actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - vehicle_rates_sp_mc_poll(); - vehicle_rates_sp_fw_poll(); - parameters_update_poll(); - vehicle_local_pos_poll(); // Check for new sensor values + /* wait for up to 100ms for data */ + int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + PX4_ERR("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + // run vtol_att on MC actuator publications, unless in full FW mode + switch (_vtol_type->get_mode()) { + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + case ROTARY_WING: + fds[0].fd = _actuator_inputs_mc; + break; + + case FIXED_WING: + fds[0].fd = _actuator_inputs_fw; + break; + } + + vehicle_control_mode_poll(); + vehicle_manual_poll(); + vehicle_attitude_poll(); + vehicle_local_pos_poll(); vehicle_local_pos_sp_poll(); pos_sp_triplet_poll(); vehicle_airspeed_poll(); - vehicle_battery_poll(); vehicle_cmd_poll(); tecs_status_poll(); land_detected_poll(); + actuator_controls_fw_poll(); + actuator_controls_mc_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); @@ -826,67 +659,58 @@ void VtolAttitudeControl::task_main() // check in which mode we are in and call mode specific functions if (_vtol_type->get_mode() == ROTARY_WING) { + + mc_virtual_att_sp_poll(); + // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; _vtol_vehicle_status.vtol_in_trans_mode = false; // got data from mc attitude controller - if (fds[0].revents & POLLIN) { - _vtol_type->update_mc_state(); - - fill_mc_att_rates_sp(); - } + _vtol_type->update_mc_state(); + fill_mc_att_rates_sp(); } else if (_vtol_type->get_mode() == FIXED_WING) { + + fw_virtual_att_sp_poll(); + // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; _vtol_vehicle_status.vtol_in_trans_mode = false; - // got data from fw attitude controller - if (fds[1].revents & POLLIN) { - vehicle_manual_poll(); - - _vtol_type->update_fw_state(); - - fill_fw_att_rates_sp(); - } + _vtol_type->update_fw_state(); + fill_fw_att_rates_sp(); } else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) { + + mc_virtual_att_sp_poll(); + fw_virtual_att_sp_poll(); + // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW); - bool got_new_data = false; - - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - got_new_data = true; - } - - if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - got_new_data = true; - } - - // update transition state if got any new data - if (got_new_data) { - _vtol_type->update_transition_state(); - fill_mc_att_rates_sp(); - } - - } else if (_vtol_type->get_mode() == EXTERNAL) { - // we are using external module to generate attitude/thrust setpoint - _vtol_type->update_external_state(); + _vtol_type->update_transition_state(); + fill_mc_att_rates_sp(); } - publish_att_sp(); _vtol_type->fill_actuator_outputs(); /* Only publish if the proper mode(s) are enabled */ if (_v_control_mode.flag_control_attitude_enabled || _v_control_mode.flag_control_rates_enabled || _v_control_mode.flag_control_manual_enabled) { + + if (_v_att_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + + } else { + /* advertise and publish */ + _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } + if (_actuators_0_pub != nullptr) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); @@ -902,12 +726,14 @@ void VtolAttitudeControl::task_main() } } - // publish the attitude rates setpoint - if (_v_rates_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + /*Advertise/Publish vtol vehicle status*/ + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + + if (_vtol_vehicle_status_pub != nullptr) { + orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); } } @@ -923,7 +749,7 @@ VtolAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, + SCHED_PRIORITY_ATTITUDE_CONTROL + 1, 1230, (px4_main_t)&VtolAttitudeControl::task_main_trampoline, nullptr); 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 80c16eeb02..492259356a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -59,13 +59,10 @@ #include #include #include -#include #include -#include #include #include -#include #include #include #include @@ -80,7 +77,6 @@ #include #include #include -#include #include "tiltrotor.h" #include "tailsitter.h" @@ -101,104 +97,90 @@ public: bool is_fixed_wing_requested(); void abort_front_transition(const char *reason); - struct vehicle_attitude_s *get_att() {return &_v_att;} - struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} - struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} - struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} - struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} - struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} - struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} - struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} - struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} + struct tecs_status_s *get_tecs_status() {return &_tecs_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} + struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;} struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} - struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} - struct airspeed_s *get_airspeed() {return &_airspeed;} - struct battery_status_s *get_batt_status() {return &_batt_status;} - struct tecs_status_s *get_tecs_status() {return &_tecs_status;} - struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;} + struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} struct Params *get_params() {return &_params;} private: //******************flags & handlers****************************************************** - bool _task_should_exit; - int _control_task; //task handle for VTOL attitude controller - orb_advert_t _mavlink_log_pub; // mavlink log uORB handle + bool _task_should_exit{false}; + int _control_task{-1}; //task handle for VTOL attitude controller /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription - int _mc_virtual_att_sp_sub; - int _fw_virtual_att_sp_sub; - int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _v_control_mode_sub; //vehicle control mode subscription - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _local_pos_sub; // sensor subscription - int _local_pos_sp_sub; // setpoint subscription - int _pos_sp_triplet_sub; // local position setpoint subscription - int _airspeed_sub; // airspeed subscription - int _battery_status_sub; // battery status subscription - int _vehicle_cmd_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 + int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs + int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs + int _airspeed_sub{-1}; // airspeed subscription + int _fw_virtual_att_sp_sub{-1}; + int _fw_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription + int _land_detected_sub{-1}; + int _local_pos_sp_sub{-1}; // setpoint subscription + int _local_pos_sub{-1}; // sensor subscription + int _manual_control_sp_sub{-1}; //manual control setpoint subscription + int _mc_virtual_att_sp_sub{-1}; + int _mc_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription + int _params_sub{-1}; //parameter updates subscription + int _pos_sp_triplet_sub{-1}; // local position setpoint subscription + int _tecs_status_sub{-1}; + int _v_att_sp_sub{-1}; //vehicle attitude setpoint subscription + int _v_att_sub{-1}; //vehicle attitude subscription + int _v_control_mode_sub{-1}; //vehicle control mode subscription + int _vehicle_cmd_sub{-1}; //handlers for publishers - orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _actuators_1_pub; - orb_advert_t _vtol_vehicle_status_pub; - orb_advert_t _v_rates_sp_pub; - orb_advert_t _v_att_sp_pub; - orb_advert_t _v_cmd_ack_pub; + orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust) + orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle + orb_advert_t _v_att_sp_pub{nullptr}; + orb_advert_t _v_cmd_ack_pub{nullptr}; + orb_advert_t _v_rates_sp_pub{nullptr}; + orb_advert_t _vtol_vehicle_status_pub{nullptr}; + orb_advert_t _actuators_1_pub{nullptr}; //*******************data containers*********************************************************** - struct vehicle_attitude_s _v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint - struct vehicle_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint - struct vehicle_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint - struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s _v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s _vtol_vehicle_status; - struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct vehicle_local_position_s _local_pos; - struct vehicle_local_position_setpoint_s _local_pos_sp; - struct position_setpoint_triplet_s _pos_sp_triplet; - struct airspeed_s _airspeed; // airspeed - struct battery_status_s _batt_status; // battery status - struct vehicle_command_s _vehicle_cmd; - struct tecs_status_s _tecs_status; - struct vehicle_land_detected_s _land_detected; - Params _params; // struct holding the parameters + vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint + vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint + vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint + + actuator_controls_s _actuators_fw_in{}; //actuator controls from fw_att_control + actuator_controls_s _actuators_mc_in{}; //actuator controls from mc_att_control + actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer + actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons) + + airspeed_s _airspeed{}; // airspeed + manual_control_setpoint_s _manual_control_sp{}; //manual control setpoint + position_setpoint_triplet_s _pos_sp_triplet{}; + tecs_status_s _tecs_status{}; + vehicle_attitude_s _v_att{}; //vehicle attitude + vehicle_command_s _vehicle_cmd{}; + vehicle_control_mode_s _v_control_mode{}; //vehicle control mode + vehicle_land_detected_s _land_detected{}; + vehicle_local_position_s _local_pos{}; + vehicle_local_position_setpoint_s _local_pos_sp{}; + vtol_vehicle_status_s _vtol_vehicle_status{}; + + Params _params{}; // struct holding the parameters struct { param_t idle_pwm_mc; param_t vtol_motor_count; param_t vtol_fw_permanent_stab; - param_t mc_airspeed_min; - param_t mc_airspeed_trim; - param_t mc_airspeed_max; param_t fw_pitch_trim; - param_t power_max; - param_t prop_eff; - param_t arsp_lp_gain; param_t vtol_type; param_t elevons_mc_lock; param_t fw_min_alt; @@ -210,45 +192,42 @@ private: param_t wv_takeoff; param_t wv_loiter; param_t wv_land; - } _params_handles; + } _params_handles{}; /* for multicopters it is usual to have a non-zero idle speed of the engines * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */ - int _transition_command; - bool _abort_front_transition; + int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; + bool _abort_front_transition{false}; - VtolType *_vtol_type = nullptr; // base class for different vtol types + VtolType *_vtol_type{nullptr}; // base class for different vtol types //*****************Member functions*********************************************************************** void task_main(); //main task static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + void land_detected_poll(); + void tecs_status_poll(); + void vehicle_attitude_poll(); //Check for attitude updates. + void vehicle_cmd_poll(); void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. void vehicle_manual_poll(); //Check for changes in manual inputs. - void mc_virtual_att_sp_poll(); - void fw_virtual_att_sp_poll(); - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void vehicle_rates_sp_mc_poll(); - void vehicle_rates_sp_fw_poll(); - void vehicle_local_pos_poll(); // Check for changes in sensor values - void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values + void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + void fw_virtual_att_sp_poll(); + void mc_virtual_att_sp_poll(); void pos_sp_triplet_poll(); // Check for changes in position setpoint values void vehicle_airspeed_poll(); // Check for changes in airspeed - void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates. - void vehicle_attitude_poll(); //Check for attitude updates. - 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_local_pos_poll(); // Check for changes in sensor values + void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values + int parameters_update(); //Update local paraemter cache + void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); + void handle_command(); - void publish_att_sp(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 554c9d2f83..c1b44dd305 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -62,48 +62,6 @@ PARAM_DEFINE_INT32(VT_MOT_COUNT, 0); */ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900); -/** - * Minimum airspeed in multicopter mode - * - * This is the minimum speed of the air flowing over the control surfaces. - * - * @unit m/s - * @min 0.0 - * @max 30.0 - * @increment 0.5 - * @decimal 2 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN, 10.0f); - -/** - * Maximum airspeed in multicopter mode - * - * This is the maximum speed of the air flowing over the control surfaces. - * - * @unit m/s - * @min 0.0 - * @max 30.0 - * @increment 0.5 - * @decimal 2 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX, 30.0f); - -/** - * Trim airspeed when in multicopter mode - * - * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode. - * - * @unit m/s - * @min 0.0 - * @max 30.0 - * @increment 0.5 - * @decimal 2 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM, 10.0f); - /** * Permanent stabilization in fw mode * @@ -128,47 +86,6 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0); */ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM, 0.0f); -/** - * Motor max power - * - * Indicates the maximum power the motor is able to produce. Used to calculate - * propeller efficiency map. - * - * @unit W - * @min 1 - * @max 10000 - * @increment 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f); - -/** - * Propeller efficiency parameter - * - * Influences propeller efficiency at different power settings. Should be tuned beforehand. - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); - -/** - * Total airspeed estimate low-pass filter gain - * - * Gain for tuning the low-pass filter for the total airspeed estimate - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); - /** * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 0e8040caf2..a6dffc088e 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -54,10 +54,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _v_att_sp = _attc->get_att_sp(); _mc_virtual_att_sp = _attc->get_mc_virtual_att_sp(); _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); - _v_rates_sp = _attc->get_rates_sp(); - _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); - _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); - _manual_control_sp = _attc->get_manual_control_sp(); _v_control_mode = _attc->get_control_mode(); _vtol_vehicle_status = _attc->get_vtol_vehicle_status(); _actuators_out_0 = _attc->get_actuators_out0(); @@ -67,7 +63,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _local_pos = _attc->get_local_pos(); _local_pos_sp = _attc->get_local_pos_sp(); _airspeed = _attc->get_airspeed(); - _batt_status = _attc->get_batt_status(); _tecs_status = _attc->get_tecs_status(); _land_detected = _attc->get_land_detected(); _params = _attc->get_params(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index f7ee1cee29..20596f64e4 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -49,13 +49,7 @@ struct Params { int32_t idle_pwm_mc; // pwm value for idle in mc mode int32_t vtol_motor_count; // number of motors - float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) - float mc_airspeed_trim; // trim airspeed in multicopter mode - float mc_airspeed_max; // max airpseed in multicopter mode float fw_pitch_trim; // trim for neutral elevon position in fw mode - float power_max; // maximum power of one engine - float prop_eff; // factor to calculate prop efficiency - float arsp_lp_gain; // total airspeed estimate low pass gain int32_t vtol_type; int32_t elevons_mc_lock; // lock elevons in multicopter mode float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) @@ -74,8 +68,7 @@ enum mode { TRANSITION_TO_FW = 1, TRANSITION_TO_MC = 2, ROTARY_WING = 3, - FIXED_WING = 4, - EXTERNAL = 5 + FIXED_WING = 4 }; enum vtol_type { @@ -116,11 +109,6 @@ public: */ virtual void update_fw_state(); - /** - * Update external state. - */ - virtual void update_external_state() {} - /** * Write control values to actuator output topics. */ @@ -157,10 +145,6 @@ protected: struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint - struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode struct vtol_vehicle_status_s *_vtol_vehicle_status; struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer @@ -170,7 +154,6 @@ protected: struct vehicle_local_position_s *_local_pos; struct vehicle_local_position_setpoint_s *_local_pos_sp; struct airspeed_s *_airspeed; // airspeed - struct battery_status_s *_batt_status; // battery status struct tecs_status_s *_tecs_status; struct vehicle_land_detected_s *_land_detected;