void VtolAttitudeControl::task_main()
{
PX4_WARN("started");
fflush(stdout);
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_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_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));
_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));
_vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
_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*/
px4_pollfd_struct_t 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 = 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) {
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;
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.
arming_status_poll(); //Check for arming status updates.
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
vehicle_airspeed_poll();
vehicle_battery_poll();
vehicle_cmd_poll();
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
// reset transition command if not in offboard control
if (!_v_control_mode.flag_control_offboard_enabled) {
if (_vtol_type->get_mode() == ROTARY_WING) {
_transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
} else if (_vtol_type->get_mode() == FIXED_WING) {
_transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
}
}
// 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_vehicle_status.vtol_in_trans_mode = false;
// 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->update_mc_state();
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_vehicle_status.vtol_in_trans_mode = false;
// 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->update_fw_state();
fill_fw_att_rates_sp();
}
} else if (_vtol_type->get_mode() == TRANSITION) {
// 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
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();
publish_att_sp();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
// we are using external module to generate attitude/thrust setpoint
_vtol_type->update_external_state();
}
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 (_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;
return;
}
void
FixedwingPositionControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
/* abort on a nonzero return value from the parameter init */
if (parameters_update()) {
/* parameter setup went wrong, abort */
warnx("aborting startup due to errors.");
_task_should_exit = true;
}
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
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);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* 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();
}
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
/* XXX Hack to get mavlink output going */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
range_finder_poll();
// vehicle_baro_poll();
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
/* XXX check if radius makes sense here */
float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;
navigation_capabilities_publish();
}
}
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_exit(0);
}