void VtolAttitudeControl::task_main()
{
warnx("started");
fflush(stdout);
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_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_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));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
parameters_update(); // initialize parameter cache
/* update vtol vehicle status*/
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
// make sure we start with idle in mc mode
_vtol_type->set_idle_mc();
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
fds[1].fd = _actuator_inputs_fw;
fds[1].events = POLLIN;
fds[2].fd = _params_sub;
fds[2].events = POLLIN;
while (!_task_should_exit) {
/*Advertise/Publish vtol vehicle status*/
if (_vtol_vehicle_status_pub != nullptr) {
orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
} else {
_vtol_vehicle_status.timestamp = hrt_absolute_time();
_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
}
/* wait for up to 100ms for data */
int pret = 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) {
warn("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[2].revents & POLLIN) { //parameters were updated, read them now
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
vehicle_manual_poll(); //Check for changes in manual inputs.
arming_status_poll(); //Check for arming status updates.
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
vehicle_airspeed_poll();
vehicle_battery_poll();
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
// check in which mode we are in and call mode specific functions
if (_vtol_type->get_mode() == ROTARY_WING) {
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
_vtol_type->update_mc_state();
// got data from mc attitude controller
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
_vtol_type->process_mc_data();
fill_mc_att_rates_sp();
}
} else if (_vtol_type->get_mode() == FIXED_WING) {
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
_vtol_type->update_fw_state();
// got data from fw attitude controller
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll();
_vtol_type->process_fw_data();
fill_fw_att_rates_sp();
}
} else if (_vtol_type->get_mode() == TRANSITION) {
// vehicle is doing a transition
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();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
// we are using external module to generate attitude/thrust setpoint
_vtol_type->update_external_state();
}
/* 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 (_actuators_0_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
// 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);
}
}
warnx("exit");
_control_task = -1;
_exit(0);
}
void VtolAttitudeControl::task_main()
{
warnx("started");
fflush(stdout);
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_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_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));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
parameters_update(); // initialize parameter cache
/* update vtol vehicle status*/
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
// make sure we start with idle in mc mode
set_idle_mc();
flag_idle_mc = true;
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
fds[1].fd = _actuator_inputs_fw;
fds[1].events = POLLIN;
fds[2].fd = _params_sub;
fds[2].events = POLLIN;
while (!_task_should_exit) {
/*Advertise/Publish vtol vehicle status*/
if (_vtol_vehicle_status_pub > 0) {
orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
} else {
_vtol_vehicle_status.timestamp = hrt_absolute_time();
_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
}
/* wait for up to 100ms for data */
int pret = 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) {
warn("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[2].revents & POLLIN) { //parameters were updated, read them now
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
vehicle_manual_poll(); //Check for changes in manual inputs.
arming_status_poll(); //Check for arming status updates.
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
vehicle_airspeed_poll();
vehicle_battery_poll();
if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */
_vtol_vehicle_status.vtol_in_rw_mode = true;
if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */
set_idle_mc();
flag_idle_mc = true;
}
/* got data from mc_att_controller */
if (fds[0].revents & POLLIN) {
vehicle_manual_poll(); /* update remote input */
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
// scale pitch control with total airspeed
scale_mc_output();
fill_mc_att_control_output();
fill_mc_att_rates_sp();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */
_vtol_vehicle_status.vtol_in_rw_mode = false;
if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */
set_idle_fw();
flag_idle_mc = false;
}
if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll(); //update remote input
fill_fw_att_control_output();
fill_fw_att_rates_sp();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
// publish the attitude rates setpoint
if(_v_rates_sp_pub > 0) {
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);
}
}
warnx("exit");
_control_task = -1;
_exit(0);
}