int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); param_get(_parameter_handles.r_tc, &(_parameters.r_tc)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.w_p, &(_parameters.w_p)); param_get(_parameter_handles.w_i, &(_parameters.w_i)); param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max)); param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); param_get(_parameter_handles.trim_steer, &(_parameters.trim_steer)); param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale)); param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale)); param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale)); param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.p_tc); _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.r_tc); _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); /* wheel control parameters */ _wheel_ctrl.set_k_p(_parameters.w_p); _wheel_ctrl.set_k_i(_parameters.w_i); _wheel_ctrl.set_k_ff(_parameters.w_ff); _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); return PX4_OK; }
void FixedwingAttitudeControl::task_main() { /* * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); parameters_update(); /* get an initial update for all sensor and status data */ vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_land_detected_poll(); /* wakeup source */ px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _ctrl_state_sub; fds[1].events = POLLIN; _task_running = true; while (!_task_should_exit) { static int loop_counter = 0; /* wait for up to 500ms for data */ int pret = px4_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); /* 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 attitude changed */ if (fds[1].revents & POLLIN) { 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(control_state), _ctrl_state_sub, &_ctrl_state); /* get current rotation matrix and euler angles from control state quaternions */ math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); _R = q_att.to_dcm(); math::Vector<3> euler_angles; euler_angles = _R.to_euler(); _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around * the pitch axis compared to the neutral position of the vehicle in multicopter mode * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. * Additionally, in order to get the correct sign of the pitch, we need to multiply * the new x axis of the rotation matrix with -1 * * original: modified: * * Rxx Ryx Rzx -Rzx Ryx Rxx * Rxy Ryy Rzy -Rzy Ryy Rxy * Rxz Ryz Rzz -Rzz Ryz Rxz * */ math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix /* move z to x */ R_adapted(0, 0) = _R(0, 2); R_adapted(1, 0) = _R(1, 2); R_adapted(2, 0) = _R(2, 2); /* move x to z */ R_adapted(0, 2) = _R(0, 0); R_adapted(1, 2) = _R(1, 0); R_adapted(2, 2) = _R(2, 0); /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation /* fill in new attitude data */ _R = R_adapted; _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); /* lastly, roll- and yawspeed have to be swaped */ float helper = _ctrl_state.roll_rate; _ctrl_state.roll_rate = -_ctrl_state.yaw_rate; _ctrl_state.yaw_rate = helper; } vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); global_pos_poll(); vehicle_status_poll(); vehicle_land_detected_poll(); // the position controller will not emit attitude setpoints in some modes // we need to make sure that this flag is reset _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; /* lock integrator until control is started */ bool lock_integrator; if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) { lock_integrator = false; } else { lock_integrator = true; } /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; //warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[7] = 0.0f; //warnx("_actuators_airframe.control[1] = -1.0f;"); } /* if we are in rotary wing mode, do nothing */ if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { continue; } /* default flaps to center */ float flap_control = 0.0f; /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_parameters.flaps_scale) > 0.01f) { flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale; } else if (_vcontrol_mode.flag_control_auto_enabled && fabsf(_parameters.flaps_scale) > 0.01f) { flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } // move the actual control value continuous with time, full flap travel in 1sec if (fabsf(_flaps_applied - flap_control) > 0.01f) { _flaps_applied += (_flaps_applied - flap_control) < 0 ? deltaT : -deltaT; } else { _flaps_applied = flap_control; } /* default flaperon to center */ float flaperon_control = 0.0f; /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled && fabsf(_parameters.flaperon_scale) > 0.01f) { flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; } else if (_vcontrol_mode.flag_control_auto_enabled && fabsf(_parameters.flaperon_scale) > 0.01f) { flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } // move the actual control value continuous with time, full flap travel in 1sec if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) { _flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? deltaT : -deltaT; } else { _flaperons_applied = flaperon_control; } /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { /* scale around tuning airspeed */ float airspeed; /* if airspeed is not updating, we assume the normal average speed */ if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); } } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ airspeed = math::max(0.5f, _ctrl_state.airspeed); } /* * 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 = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); /* Use min airspeed to calculate ground speed scaling region. * Don't scale below gspd_scaling_trim */ float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_sp = 0.0f; float yaw_manual = 0.0f; float throttle_sp = 0.0f; // in STABILIZED mode we need to generate the attitude setpoint // from manual user inputs if (!_vcontrol_mode.flag_control_climb_rate_enabled) { _att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad; _att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max); _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad; _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max); _att_sp.yaw_body = 0.0f; _att_sp.thrust = _manual.z; int instance; orb_publish_auto(_attitude_setpoint_id, &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT); } roll_sp = _att_sp.roll_body; pitch_sp = _att_sp.pitch_body; yaw_sp = _att_sp.yaw_body; throttle_sp = _att_sp.thrust; /* allow manual yaw in manual modes */ if (_vcontrol_mode.flag_control_manual_enabled) { yaw_manual = _manual.r; } /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } /* If the aircraft is on ground reset the integrators */ if (_vehicle_land_detected.landed || _vehicle_status.is_rotary_wing) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; control_input.roll = _roll; control_input.pitch = _pitch; control_input.yaw = _yaw; control_input.roll_rate = _ctrl_state.roll_rate; control_input.pitch_rate = _ctrl_state.pitch_rate; control_input.yaw_rate = _ctrl_state.yaw_rate; control_input.speed_body_u = speed_body_u; control_input.speed_body_v = speed_body_v; control_input.speed_body_w = speed_body_w; control_input.acc_body_x = _accel.x; control_input.acc_body_y = _accel.y; control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; control_input.yaw_setpoint = yaw_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; control_input.groundspeed = groundspeed; control_input.groundspeed_scaler = groundspeed_scaler; _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude _wheel_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } float pitch_u = _pitch_ctrl.control_bodyrate(control_input); _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," " _roll_ctrl.get_desired_rate() %.4f," " _pitch_ctrl.get_desired_rate() %.4f" " att_sp.roll_body %.4f", (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } } float yaw_u = 0.0f; if (_att_sp.fw_control_yaw == true) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } else { yaw_u = _yaw_ctrl.control_bodyrate(control_input); } _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[actuator_controls_s::INDEX_YAW] += yaw_manual; if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ _rates_sp.roll = _roll_ctrl.get_desired_rate(); _rates_sp.pitch = _pitch_ctrl.get_desired_rate(); _rates_sp.yaw = _yaw_ctrl.get_desired_rate(); _rates_sp.timestamp = hrt_absolute_time(); if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } } else { /* manual/direct control */ _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll; _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale + _parameters.trim_pitch; _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw; _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; _actuators.control[actuator_controls_s::INDEX_SPOILERS] = _manual.aux1; _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future _actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _actuators.control[actuator_controls_s::INDEX_YAW] - _parameters.trim_yaw + _parameters.trim_steer + _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_rates_enabled || _vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub != nullptr) { /* publish the actuator controls*/ orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); } else { /* advertise and publish */ _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); } } } loop_counter++; perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _task_running = false; }
int FixedwingAttitudeControl::parameters_update() { param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.tconst); _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); return OK; }