mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-04 20:11:36 +08:00
vtol_att avoid unnecessary work and delete unused
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
d3a220f807
commit
66f614435f
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user