void MulticopterAttitudeControl::task_main() { /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _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)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /* initialize parameters cache */ parameters_update(); /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* 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; } perf_begin(_loop_perf); /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy attitude and control state topics */ orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || fabsf(_manual_control_sp.x) > _params.rattitude_thres) { _v_control_mode.flag_control_attitude_enabled = false; } } if (_v_control_mode.flag_control_attitude_enabled) { if (_ts_opt_recovery == nullptr) { // the tailsitter recovery instance has not been created, thus, the vehicle // is not a tailsitter, do normal attitude control control_attitude(dt); } else { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); /* limit rates */ for (int i = 0; i < 3; i++) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } //} } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ if (_controller_status_pub != nullptr) { orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } } } perf_end(_loop_perf); } _control_task = -1; return; }
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; }
void MulticopterAttitudeControl::task_main() { /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_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)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /* initialize parameters cache */ parameters_update(); /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; fds[0].fd = _v_att_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* 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; } perf_begin(_loop_perf); /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy attitude topic */ orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = _manual_control_sp.z; /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ if(_controller_status_pub != nullptr) { orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } } } perf_end(_loop_perf); } _control_task = -1; return; }
void VtolAttitudeControl::task_main() { 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)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _tecs_status_sub = orb_subscribe(ORB_ID(tecs_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*/ 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(); vehicle_status_poll(); tecs_status_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); // reset transition command if not auto control if (_v_control_mode.flag_control_manual_enabled) { if (_vtol_type->get_mode() == ROTARY_WING) { _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; } else if (_vtol_type->get_mode() == FIXED_WING) { _transition_command = vtol_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 FixedwingAttitudeControl::task_main() { /* * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _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)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); /* do not limit the attitude updates in order to minimize latency. * actuator outputs are still limited by the individual drivers * properly to not saturate IO or physical limitations */ parameters_update(); /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); vehicle_status_poll(); /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _att_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 = 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(vehicle_attitude), _att_sub, &_att); 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; //original rotation matrix math::Matrix<3, 3> R_adapted; //modified rotation matrix R.set(_att.R); R_adapted.set(_att.R); /* 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); math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation euler_angles = R_adapted.to_euler(); /* fill in new attitude data */ _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); PX4_R(_att.R, 0, 0) = R_adapted(0, 0); PX4_R(_att.R, 0, 1) = R_adapted(0, 1); PX4_R(_att.R, 0, 2) = R_adapted(0, 2); PX4_R(_att.R, 1, 0) = R_adapted(1, 0); PX4_R(_att.R, 1, 1) = R_adapted(1, 1); PX4_R(_att.R, 1, 2) = R_adapted(1, 2); PX4_R(_att.R, 2, 0) = R_adapted(2, 0); PX4_R(_att.R, 2, 1) = R_adapted(2, 1); PX4_R(_att.R, 2, 2) = R_adapted(2, 2); /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; _att.rollspeed = -_att.yawspeed; _att.yawspeed = helper; } vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); global_pos_poll(); vehicle_status_poll(); /* 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;"); } /* default flaps to center */ float flaps_control = 0.0f; /* map flaps by default to manual if valid */ if (isfinite(_manual.flaps)) { flaps_control = _manual.flaps; } /* 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 = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { 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, _airspeed.true_airspeed_m_s); } /* * 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); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_manual = 0.0f; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if * - velocity control or position control is enabled (pos controller is running) * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ if (_vcontrol_mode.flag_control_auto_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* 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(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; } pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* 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(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* 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(); } } else { /* * Scale down roll and pitch as the setpoints are radians * and a typical remote can only do around 45 degrees, the mapping is * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; /* * in manual mode no external source should / does emit attitude setpoints. * emit the manual setpoint here to allow attitude controller tuning * in attitude control mode. */ struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = roll_sp; att_sp.pitch_body = pitch_sp; att_sp.yaw_body = 0.0f - _parameters.trim_yaw; att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); } else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } /* If the aircraft is on ground reset the integrators */ if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; control_input.roll = _att.roll; control_input.pitch = _att.pitch; control_input.yaw = _att.yaw; control_input.roll_rate = _att.rollspeed; control_input.pitch_rate = _att.pitchspeed; control_input.yaw_rate = _att.yawspeed; 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.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; /* Run attitude controllers */ if (isfinite(roll_sp) && 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 /* 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[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!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[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!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 = _yaw_ctrl.control_bodyrate(control_input); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_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[3] = (isfinite(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; if (!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 > 0) { /* 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.trim_roll; _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch; _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw; _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; _actuators.control[6] = _manual.aux2; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _att.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.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 > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); } else if (_actuators_id) { _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub > 0) { /* 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; _exit(0); }
void FixedwingAttitudeControl::task_main() { /* inform about start */ warnx("Initializing.."); fflush(stdout); /* * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _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)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */ orb_set_interval(_att_sub, 17); parameters_update(); /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); vehicle_status_poll(); /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _att_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 = 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(vehicle_attitude), _att_sub, &_att); vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); global_pos_poll(); vehicle_status_poll(); /* lock integrator until control is started */ bool lock_integrator; if (_vcontrol_mode.flag_control_attitude_enabled) { 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[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[1] = 0.0f; // warnx("_actuators_airframe.control[1] = -1.0f;"); } /* 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 = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { 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, _airspeed.true_airspeed_m_s); } /* * 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); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if * - velocity control or position control is enabled (pos controller is running) * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* 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(); } } else { /* * Scale down roll and pitch as the setpoints are radians * and a typical remote can only do around 45 degrees, the mapping is * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* * in manual mode no external source should / does emit attitude setpoints. * emit the manual setpoint here to allow attitude controller tuning * in attitude control mode. */ struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = roll_sp; att_sp.pitch_body = pitch_sp; att_sp.yaw_body = 0.0f - _parameters.trim_yaw; att_sp.thrust = throttle_sp; /* 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); } } /* If the aircraft is on ground reset the integrators */ if (_vehicle_status.condition_landed) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(roll_sp, _att.roll); _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!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(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!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 = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { _yaw_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[3] = (isfinite(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; if (!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 */ vehicle_rates_setpoint_s rates_sp; 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 > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); } else { /* advertise and publish */ _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } } else { /* manual/direct control */ _actuators.control[0] = _manual.y; _actuators.control[1] = -_manual.x; _actuators.control[2] = _manual.r; _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } _actuators.control[5] = _manual.aux1; _actuators.control[6] = _manual.aux2; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { /* advertise and publish */ _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); // warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", // (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], // (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], // (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); } else { /* advertise and publish */ _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); } } loop_counter++; perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _task_running = false; _exit(0); }
void AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); _filter_start_time = hrt_absolute_time(); if (!_ekf) { warnx("OUT OF MEM!"); return; } /* * do subscriptions */ _distance_sub = orb_subscribe(ORB_ID(distance_sensor)); _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); _armedSub = orb_subscribe(ORB_ID(actuator_armed)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); /* sets also parameters in the EKF object */ parameters_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; _task_running = true; while (!_task_should_exit) { /* 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, 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 ERR %d, %d", pret, errno); continue; } perf_begin(_loop_perf); perf_count(_loop_intvl); /* 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 estimator if gyro updated */ if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); /* set sensors to de-initialized state */ _gyro_valid = false; _accel_valid = false; _mag_valid = false; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } /** * PART ONE: COLLECT ALL DATA **/ pollData(); /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { continue; } /** * PART TWO: EXECUTE THE FILTER * * We run the filter only once all data has been fetched **/ if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); // perf_reset(_perf_baro); // warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", // (double)_baro_gps_offset, // (double)_baro_alt_filt, // (double)_gps_alt_filt, // (double)_global_pos.alt, // (double)_local_pos.z); // } /* Initialize the filter first */ if (!_ekf->statesInitialised) { // North, East Down position (m) float initVelNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = 0.0f; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else { if (!_gps_initialized && _gpsIsGood) { initializeGPS(); continue; } // Check if on ground - status is used by covariance prediction _ekf->setOnGround(_landDetector.landed); // We're apparently initialized in this case now // check (and reset the filter as needed) int check = check_filter_state(); if (check) { // Let the system re-initialize itself continue; } //Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); //Publish attitude estimations publishAttitude(); //Publish Local Position estimations publishLocalPosition(); //Publish Global Position, but only if it's any good if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { publishGlobalPosition(); } //Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { publishWindEstimate(); } } } } perf_end(_loop_perf); } _task_running = false; _estimator_task = -1; return; }
void FixedwingEstimator::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); float dt = 0.0f; // time lapsed since last covariance prediction _filter_start_time = hrt_absolute_time(); if (!_ekf) { errx(1, "OUT OF MEM!"); } /* * do subscriptions */ _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); #ifndef SENSOR_COMBINED_SUB _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_gyro_sub, 4); #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ parameters_update(); Vector3f lastAngRate; Vector3f lastAccel; /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; #ifndef SENSOR_COMBINED_SUB fds[1].fd = _gyro_sub; fds[1].events = POLLIN; #else fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; #endif bool newDataGps = false; bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) uint64_t last_gps = 0; _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; 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 ERR %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 estimator if gyro updated */ if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; bool mag_updated; perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); #else /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); #endif /* set sensors to de-initialized state */ _gyro_valid = false; _accel_valid = false; _mag_valid = false; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } /** * PART ONE: COLLECT ALL DATA **/ /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_check(_accel_sub, &accel_updated); if (accel_updated) { orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } _last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - _last_run) / 1e6f; _last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; if (isfinite(_gyro.x) && isfinite(_gyro.y) && isfinite(_gyro.z)) { _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; if (!_gyro_valid) { lastAngRate = _ekf->angRate; } _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; if (!_accel_valid) { lastAccel = _ekf->accel; } _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->lastAngRate = angRate; _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; _ekf->lastAccel = accel; #else orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; static hrt_abstime last_mag = 0; if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; } else { accel_updated = false; } last_accel = _sensor_combined.accelerometer_timestamp; // Copy gyro and accel _last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } _last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; if (isfinite(_sensor_combined.gyro_rad_s[0]) && isfinite(_sensor_combined.gyro_rad_s[1]) && isfinite(_sensor_combined.gyro_rad_s[2])) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; if (!_gyro_valid) { lastAngRate = _ekf->angRate; } _gyro_valid = true; perf_count(_perf_gyro); } if (accel_updated) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; if (!_accel_valid) { lastAccel = _ekf->accel; } _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; lastAccel = _ekf->accel; if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; newDataMag = true; } else { newDataMag = false; } last_mag = _sensor_combined.magnetometer_timestamp; #endif //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); _ekf->VtasMeas = _airspeed.true_airspeed_m_s; newAdsData = true; } else { newAdsData = false; } bool gps_updated; orb_check(_gps_sub, &gps_updated); if (gps_updated) { last_gps = _gps.timestamp_position; orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); if (_gps.fix_type < 3) { newDataGps = false; } else { /* store time of valid GPS measurement */ _gps_start_time = hrt_absolute_time(); /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); _ekf->ResetVelocity(); _ekf->ResetStoredStates(); } /* fuse GPS updates */ //_gps.timestamp / 1e3; _ekf->GPSstatus = _gps.fix_type; _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { // _ekf->vneSigma = _parameters.velne_noise; // } // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); // } else { // _ekf->posNeSigma = _parameters.posne_noise; // } // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); newDataGps = true; } } bool baro_updated; orb_check(_baro_sub, &baro_updated); if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _ekf->baroHgt = _baro.altitude; if (!_baro_init) { _baro_ref = _baro.altitude; _baro_init = true; warnx("ALT REF INIT"); } perf_count(_perf_baro); newHgtData = true; } else { newHgtData = false; } #ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); #endif if (mag_updated) { _mag_valid = true; perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); // XXX we compensate the offsets upfront - should be close to zero. // 0.001f _ekf->magData.x = _mag.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = _mag.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = _mag.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #endif newDataMag = true; } else { newDataMag = false; } /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { continue; } /** * PART TWO: EXECUTE THE FILTER * * We run the filter only once all data has been fetched **/ if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; initVelNED[0] = _gps.vel_n_m_s; initVelNED[1] = _gps.vel_e_m_s; initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame _baro_gps_offset = _baro.altitude - gps_alt; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; _ekf->gpsLat = math::radians(lat); _ekf->gpsLon = math::radians(lon) - M_PI; _ekf->gpsHgt = gps_alt; // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = lat; _local_pos.ref_lon = lon; _local_pos.ref_alt = gps_alt; _local_pos.ref_timestamp = _gps.timestamp_position; map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif _gps_initialized = true; } else if (!_ekf->statesInitialised) { initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; initVelNED[2] = 0.0f; _ekf->posNE[0] = posNED[0]; _ekf->posNE[1] = posNED[1]; _local_pos.ref_alt = _baro_ref; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { // We're apparently initialized in this case now int check = check_filter_state(); if (check) { // Let the system re-initialize itself continue; } // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 // debug code - could be tunred into a filter mnitoring/watchdog function float tempQuat[4]; for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; quat2eul(eulerEst, tempQuat); for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; #endif // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction _ekf->OnGroundCheck(); // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; dt += _ekf->dtIMU; // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); _ekf->summedDelAng.zero(); _ekf->summedDelVel.zero(); dt = 0.0f; } // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; // Calculate acceleration predicted by GPS velocity change if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; } _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); _ekf->posNE[0] = posNED[0]; _ekf->posNE[1] = posNED[1]; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else if (_ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; _ekf->velNED[2] = 0.0f; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else { _ekf->fuseVelData = false; _ekf->fusePosData = false; } if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else { _ekf->fuseHgtData = false; } // Fuse Magnetometer Measurements if (newDataMag && _ekf->statesInitialised) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); } else { _ekf->fuseMagData = false; } // Fuse Airspeed Measurements if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); } else { _ekf->fuseVtasData = false; } // Output results math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; _att.q[1] = _ekf->states[1]; _att.q[2] = _ekf->states[2]; _att.q[3] = _ekf->states[3]; _att.q_valid = true; _att.R_valid = true; _att.timestamp = _last_sensor_timestamp; _att.roll = euler(0); _att.pitch = euler(1); _att.yaw = euler(2); _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; // gyro offsets _att.rate_offsets[0] = _ekf->states[10]; _att.rate_offsets[1] = _ekf->states[11]; _att.rate_offsets[2] = _ekf->states[12]; /* lazily publish the attitude only once available */ if (_att_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); } else { /* advertise and publish */ _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } if (_gps_initialized) { _local_pos.timestamp = _last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly _local_pos.z = _ekf->states[9] - _baro_ref_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; _local_pos.vz = _ekf->states[6]; _local_pos.xy_valid = _gps_initialized; _local_pos.z_valid = true; _local_pos.v_xy_valid = _gps_initialized; _local_pos.v_z_valid = true; _local_pos.xy_global = true; _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, * TODO: adapt so that it works for both, maybe move to another location */ if (_velocity_xy_filtered < 5 && _velocity_z_filtered < 10 && _airspeed_filtered < 10) { _local_pos.landed = true; } else { _local_pos.landed = false; } _local_pos.z_global = false; _local_pos.yaw = _att.yaw; /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); } else { /* advertise and publish */ _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); } _global_pos.timestamp = _local_pos.timestamp; if (_local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; } if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; } else { _global_pos.vel_n = 0.0f; _global_pos.vel_e = 0.0f; } /* local pos alt is negative, change sign and add alt offsets */ _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } _global_pos.yaw = _local_pos.yaw; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; _global_pos.timestamp = _local_pos.timestamp; /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; _wind.covariance_north = 0.0f; // XXX get form filter _wind.covariance_east = 0.0f; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { /* publish the wind estimate */ orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); } else { /* advertise and publish */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } } } } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; _wind.covariance_north = _ekf->P[14][14]; _wind.covariance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { /* publish the wind estimate */ orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); } else { /* advertise and publish */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } } } } perf_end(_loop_perf); } warnx("exiting.\n"); _estimator_task = -1; _exit(0); }