int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { static int counter = 0; static bool initialized = false; static struct fw_rate_control_params p; static struct fw_rate_control_param_handles h; static PID_t roll_rate_controller; static PID_t pitch_rate_controller; static PID_t yaw_rate_controller; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); if(!initialized) { parameters_init(&h); parameters_update(&h, &p); pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } /* load new parameters with lower rate */ if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); } /* Roll Rate (PI) */ actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer... actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now counter++; return 0; }
void parameters_init() { _params_handles.accel_x_offset = param_find("CAL_ACC0_XOFF"); _params_handles.accel_x_scale = param_find("CAL_ACC0_XSCALE"); _params_handles.accel_y_offset = param_find("CAL_ACC0_YOFF"); _params_handles.accel_y_scale = param_find("CAL_ACC0_YSCALE"); _params_handles.accel_z_offset = param_find("CAL_ACC0_ZOFF"); _params_handles.accel_z_scale = param_find("CAL_ACC0_ZSCALE"); _params_handles.gyro_x_offset = param_find("CAL_GYRO0_XOFF"); _params_handles.gyro_x_scale = param_find("CAL_GYRO0_XSCALE"); _params_handles.gyro_y_offset = param_find("CAL_GYRO0_YOFF"); _params_handles.gyro_y_scale = param_find("CAL_GYRO0_YSCALE"); _params_handles.gyro_z_offset = param_find("CAL_GYRO0_ZOFF"); _params_handles.gyro_z_scale = param_find("CAL_GYRO0_ZSCALE"); _params_handles.mag_x_offset = param_find("CAL_MAG0_XOFF"); _params_handles.mag_x_scale = param_find("CAL_MAG0_XSCALE"); _params_handles.mag_y_offset = param_find("CAL_MAG0_YOFF"); _params_handles.mag_y_scale = param_find("CAL_MAG0_YSCALE"); _params_handles.mag_z_offset = param_find("CAL_MAG0_ZOFF"); _params_handles.mag_z_scale = param_find("CAL_MAG0_ZSCALE"); _params_handles.gyro_lpf_enum = param_find("MPU_GYRO_LPF_ENM"); _params_handles.accel_lpf_enum = param_find("MPU_ACC_LPF_ENM"); _params_handles.imu_sample_rate_enum = param_find("MPU_SAMPLE_R_ENM"); parameters_update(); }
estimator_base::estimator_base() { _time_to_run = -1; _params_sub = orb_subscribe(ORB_ID(parameter_update)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); // _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _gps_init = false; _baro_init = false; fds[0].fd = _sensor_combined_sub; fds[0].events = POLLIN; poll_error_counter = 0; memset(&_params, 0, sizeof(_params)); memset(&_sensor_combined, 0, sizeof(_sensor_combined)); memset(&_gps, 0, sizeof(_gps)); memset(&_vehicle_state, 0, sizeof(_vehicle_state)); _params_handles.gravity = param_find("UAVBOOK_GRAVITY"); _params_handles.rho = param_find("UAVBOOK_RHO"); _params_handles.sigma_accel = param_find("UAVBOOK_SIGMA_ACCEL"); _params_handles.sigma_n_gps = param_find("UAVBOOK_SIGMA_N_GPS"); _params_handles.sigma_e_gps = param_find("UAVBOOK_SIGMA_E_GPS"); _params_handles.sigma_Vg_gps = param_find("UAVBOOK_SIGMA_VG_GPS"); _params_handles.sigma_course_gps = param_find("UAVBOOK_SIGMA_COURSE_GPS"); parameters_update(); _vehicle_state_pub = orb_advertise(ORB_ID(vehicle_state), &_vehicle_state); }
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp) { static int counter = 0; static bool initialized = false; static struct fw_att_control_params p; static struct fw_pos_control_param_handles h; static PID_t roll_controller; static PID_t pitch_controller; if(!initialized) { parameters_init(&h); parameters_update(&h, &p); pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } /* load new parameters with lower rate */ if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); } /* Roll (P) */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0); /* Pitch (P) */ float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan; rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ //TODO counter++; return 0; }
void path_follower_base::parameter_update_poll() { bool updated; /* Check if param status has changed */ orb_check(_params_sub, &updated); if (updated) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); parameters_update(); } }
void MulticopterAttitudeControl::parameter_update_poll() { bool updated; /* Check HIL state if vehicle status has changed */ orb_check(_params_sub, &updated); if (updated) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); parameters_update(); } }
/** * Check for parameter updates. */ void VtolAttitudeControl::parameters_update_poll() { bool updated; /* Check if parameters have changed */ orb_check(_params_sub, &updated); if (updated) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); parameters_update(); } }
GroundRoverAttitudeControl::GroundRoverAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "gnda_dt")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "gnda_nani")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "gnda_nano")) { _parameter_handles.w_p = param_find("GND_WR_P"); _parameter_handles.w_i = param_find("GND_WR_I"); _parameter_handles.w_d = param_find("GND_WR_D"); _parameter_handles.w_imax = param_find("GND_WR_IMAX"); _parameter_handles.trim_yaw = param_find("TRIM_YAW"); _parameter_handles.man_yaw_scale = param_find("GND_MAN_Y_SC"); _parameter_handles.bat_scale_en = param_find("GND_BAT_SCALE_EN"); /* fetch initial parameter values */ parameters_update(); }
/** * Constructor */ VtolAttitudeControl::VtolAttitudeControl() { _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); _params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR"); _params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P"); _params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R"); _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM"); _params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); _params_handles.wv_takeoff = param_find("VT_WV_TKO_EN"); _params_handles.wv_land = param_find("VT_WV_LND_EN"); _params_handles.wv_loiter = param_find("VT_WV_LTR_EN"); /* fetch initial parameter values */ parameters_update(); if (_params.vtol_type == vtol_type::TAILSITTER) { _vtol_type = new Tailsitter(this); } else if (_params.vtol_type == vtol_type::TILTROTOR) { _vtol_type = new Tiltrotor(this); } else if (_params.vtol_type == vtol_type::STANDARD) { _vtol_type = new Standard(this); } else { _task_should_exit = true; } }
path_follower_base::path_follower_base() { _params_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_state_sub = orb_subscribe(ORB_ID(vehicle_state)); _current_path_sub = orb_subscribe(ORB_ID(current_path)); fds[0].fd = _vehicle_state_sub; fds[0].events = POLLIN; poll_error_counter = 0; memset(&_vehicle_state, 0, sizeof(_vehicle_state)); memset(&_current_path, 0, sizeof(_current_path)); memset(&_controller_commands, 0, sizeof(_controller_commands)); memset(&_params, 0, sizeof(_params)); _params_handles.chi_infty = param_find("UAVBOOK_CHI_INFTY"); _params_handles.k_path = param_find("UAVBOOK_K_PATH"); _params_handles.k_orbit = param_find("UAVBOOK_K_ORBIT"); parameters_update(); _controller_commands_pub = orb_advertise(ORB_ID(controller_commands), &_controller_commands); }
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_accel)); _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)); /* 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(); /* 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; 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(); /* 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 { airspeed = _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; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_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); } } /* 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 (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 (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 (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 (loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (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; _exit(0); }
MulticopterAttitudeControl::MulticopterAttitudeControl() : _task_should_exit(false), _control_task(-1), /* subscriptions */ _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; _params.man_pitch_max = 0.0f; _params.man_yaw_max = 0.0f; _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); _I.identity(); _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); /* fetch initial parameter values */ parameters_update(); }
/* * EKF Attitude Estimator main function. * * Estimates the attitude recursively once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ float x_aposteriori_k[12]; /**< states */ float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ float x_aposteriori[12]; float P_aposteriori[144]; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); fflush(stdout); int overloadcounter = 19; /* Initialize filter */ attitudeKalmanfilter_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_vicon_position_s vicon_pos; // Added by Ross Allen memset(&vicon_pos, 0, sizeof(vicon_pos)); uint64_t last_data = 0; uint64_t last_measurement = 0; uint64_t last_vel_t = 0; /* Vicon parameters - Added by Ross Allen */ bool vicon_valid = false; //~ static const hrt_abstime vicon_topic_timeout = 500000; // vicon topic timeout = 0.25s /* current velocity */ math::Vector<3> vel; vel.zero(); /* previous velocity */ math::Vector<3> vel_prev; vel_prev.zero(); /* actual acceleration (by GPS velocity) in body frame */ math::Vector<3> acc; acc.zero(); /* rotation matrix */ math::Matrix<3, 3> R; R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); /* subscribe to GPS */ int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode*/ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to vicon position */ int vehicle_vicon_position_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); // Added by Ross Allen /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; thread_running = true; /* advertise debug value */ // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; // orb_advert_t pub_dbg = -1; /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; struct attitude_estimator_ekf_param_handles ekf_param_handles; /* initialize parameter handles */ parameters_init(&ekf_param_handles); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; /* magnetic declination, in radians */ float mag_decl = 0.0f; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); /* Main loop*/ while (!thread_should_exit) { struct pollfd fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); /* Added by Ross Allen */ //~ hrt_abstime t = hrt_absolute_time(); bool updated; if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); bool gps_updated; orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); } } bool global_pos_updated; orb_check(sub_global_pos, &global_pos_updated); if (global_pos_updated) { orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); } if (!initialized) { // XXX disabling init for now initialized = true; // gyro_offsets[0] += raw.gyro_rad_s[0]; // gyro_offsets[1] += raw.gyro_rad_s[1]; // gyro_offsets[2] += raw.gyro_rad_s[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { // initialized = true; // gyro_offsets[0] /= offset_count; // gyro_offsets[1] /= offset_count; // gyro_offsets[2] /= offset_count; // } } else { perf_begin(ekf_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; bool vel_valid = false; if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { vel_valid = true; if (gps_updated) { vel_t = gps.timestamp_velocity; vel(0) = gps.vel_n_m_s; vel(1) = gps.vel_e_m_s; vel(2) = gps.vel_d_m_s; } } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; vel(0) = global_pos.vel_n; vel(1) = global_pos.vel_e; vel(2) = global_pos.vel_d; } } if (vel_valid) { /* velocity is valid */ if (vel_t != 0) { /* velocity updated */ if (last_vel_t != 0 && vel_t != last_vel_t) { float vel_dt = (vel_t - last_vel_t) / 1000000.0f; /* calculate acceleration in body frame */ acc = R.transposed() * ((vel - vel_prev) / vel_dt); } last_vel_t = vel_t; vel_prev = vel; } } else { /* velocity is valid, reset acceleration */ acc.zero(); vel_prev.zero(); last_vel_t = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc(0); z_k[4] = raw.accelerometer_m_s2[1] - acc(1); z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; last_run = now; if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here // if (overloadcounter == 20) { // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); // overloadcounter = 0; // } overloadcounter++; } static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } else { mag_decl = ekf_params.mag_decl; } /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; x_aposteriori_k[3] = 0.0f; x_aposteriori_k[4] = 0.0f; x_aposteriori_k[5] = 0.0f; x_aposteriori_k[6] = z_k[3]; x_aposteriori_k[7] = z_k[4]; x_aposteriori_k[8] = z_k[5]; x_aposteriori_k[9] = z_k[6]; x_aposteriori_k[10] = z_k[7]; x_aposteriori_k[11] = z_k[8]; const_initialized = true; } /* do not execute the filter if not initialized */ if (!const_initialized) { continue; } attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, euler, Rot_matrix, x_aposteriori, P_aposteriori); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); } else { /* due to inputs or numerical failure the output is invalid, skip it */ continue; } if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; /* Check Vicon for attitude data*/ /* Added by Ross Allen */ orb_check(vehicle_vicon_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_vicon_position), vehicle_vicon_position_sub, &vicon_pos); vicon_valid = vicon_pos.valid; } //~ if (vicon_valid && (t > (vicon_pos.timestamp + vicon_topic_timeout))) { //~ vicon_valid = false; //~ warnx("VICON timeout"); //~ } /* send out */ att.timestamp = raw.timestamp; att.roll = euler[0]; att.pitch = euler[1]; att.yaw = euler[2] + mag_decl; /* Use vicon yaw if valid and just overwrite*/ /* Added by Ross Allen */ if(vicon_valid) { att.yaw = vicon_pos.yaw; } att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; att.rollacc = x_aposteriori[3]; att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); R = R_decl * R_body; /* copy rotation matrix */ memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); } perf_end(ekf_loop_perf); } } } loopcounter++; } thread_running = false; return 0; }
void Tiltrotor::update_vtol_state() { parameters_update(); /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting rotors, picking up * forward speed. After the vehicle has picked up enough speed the rotors are tilted * forward completely. For the backtransition the motors simply rotate back. */ if (!_attc->is_fixed_wing_requested()) { // plane is in multicopter mode switch (_vtol_schedule.flight_mode) { case MC_MODE: break; case FW_MODE: _vtol_schedule.flight_mode = TRANSITION_BACK; _vtol_schedule.transition_start = hrt_absolute_time(); break; case TRANSITION_FRONT_P1: // failsafe into multicopter mode _vtol_schedule.flight_mode = MC_MODE; break; case TRANSITION_FRONT_P2: // failsafe into multicopter mode _vtol_schedule.flight_mode = MC_MODE; break; case TRANSITION_BACK: if (_tilt_control <= _params_tiltrotor.tilt_mc) { _vtol_schedule.flight_mode = MC_MODE; } break; } } else { switch (_vtol_schedule.flight_mode) { case MC_MODE: // initialise a front transition _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; _vtol_schedule.transition_start = hrt_absolute_time(); break; case FW_MODE: break; case TRANSITION_FRONT_P1: // check if we have reached airspeed to switch to fw mode if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); } break; case TRANSITION_FRONT_P2: // if the rotors have been tilted completely we switch to fw mode if (_tilt_control >= _params_tiltrotor.tilt_fw) { _vtol_schedule.flight_mode = FW_MODE; _tilt_control = _params_tiltrotor.tilt_fw; } break; case TRANSITION_BACK: // failsafe into fixed wing mode _vtol_schedule.flight_mode = FW_MODE; break; } } // map tiltrotor specific control phases to simple control modes switch (_vtol_schedule.flight_mode) { case MC_MODE: _vtol_mode = ROTARY_WING; break; case FW_MODE: _vtol_mode = FIXED_WING; break; case TRANSITION_FRONT_P1: case TRANSITION_FRONT_P2: case TRANSITION_BACK: _vtol_mode = TRANSITION; break; } }
Sensors::Sensors() : _fd_adc(-1), _last_adc(0), _task_should_exit(false), _sensors_task(-1), _hil_enabled(false), _publishing(true), /* subscriptions */ _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), /* publications */ _sensor_pub(-1), _manual_control_pub(-1), _actuator_group_3_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), _diff_pres_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _mag_is_external(false), _battery_discharged(0), _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { char nbuf[16]; /* min values */ sprintf(nbuf, "RC%d_MIN", i + 1); _parameter_handles.min[i] = param_find(nbuf); /* trim values */ sprintf(nbuf, "RC%d_TRIM", i + 1); _parameter_handles.trim[i] = param_find(nbuf); /* max values */ sprintf(nbuf, "RC%d_MAX", i + 1); _parameter_handles.max[i] = param_find(nbuf); /* channel reverse */ sprintf(nbuf, "RC%d_REV", i + 1); _parameter_handles.rev[i] = param_find(nbuf); /* channel deadzone */ sprintf(nbuf, "RC%d_DZ", i + 1); _parameter_handles.dz[i] = param_find(nbuf); } /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); /* RC thresholds */ _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); _parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); _parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF"); _parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE"); _parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE"); _parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE"); /* accel offsets */ _parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF"); _parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF"); _parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF"); _parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE"); _parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE"); _parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE"); /* mag offsets */ _parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF"); _parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF"); _parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF"); _parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE"); _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* fetch initial parameter values */ parameters_update(); }
/**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); memset(R_buf, 0, sizeof(R_buf)); memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation float eph = max_eph_epv; float epv = 1.0f; float eph_flow = 1.0f; float eph_vision = 0.5f; float epv_vision = 0.5f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); hrt_abstime t_prev = 0; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; float sonar_prev = 0.0f; hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct home_position_s home; memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); struct vision_position_estimate vision; memset(&vision, 0, sizeof(vision)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_advert_t vehicle_global_position_pub = -1; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); struct pollfd fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, };
void MulticopterAttitudeControl::task_main() { warnx("started"); fflush(stdout); /* * 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)); /* initialize parameters cache */ parameters_update(); /* wakeup source: vehicle attitude */ struct pollfd 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 = 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(); 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 > 0) { orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); } else { _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } } 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] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } } } perf_end(_loop_perf); } warnx("exit"); _control_task = -1; _exit(0); }
FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_should_exit(false), _control_task(-1), /* subscriptions */ _att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), _global_pos_sub(-1), /* publications */ _rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false) { /* safely initialize structs */ _att = {}; _accel = {}; _att_sp = {}; _manual = {}; _airspeed = {}; _vcontrol_mode = {}; _actuators = {}; _actuators_airframe = {}; _global_pos = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); _parameter_handles.p_ff = param_find("FW_PR_FF"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_i = param_find("FW_RR_I"); _parameter_handles.r_ff = param_find("FW_RR_FF"); _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_YR_P"); _parameter_handles.y_i = param_find("FW_YR_I"); _parameter_handles.y_ff = param_find("FW_YR_FF"); _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); _parameter_handles.trim_roll = param_find("TRIM_ROLL"); _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); _parameter_handles.trim_yaw = param_find("TRIM_YAW"); _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); /* fetch initial parameter values */ parameters_update(); }
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; }
MulticopterAttitudeControl::MulticopterAttitudeControl() : _task_should_exit(false), _control_task(-1), /* subscriptions */ _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), _vehicle_status_sub(-1), /* publications */ _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), _controller_status_pub(nullptr), _rates_sp_id(0), _actuators_id(0), _actuators_0_circuit_breaker_enabled(false), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _ts_opt_recovery(nullptr) { memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_motor_limits, 0, sizeof(_motor_limits)); memset(&_controller_status, 0, sizeof(_controller_status)); _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); _params.rate_ff.zero(); _params.yaw_ff = 0.0f; _params.roll_rate_max = 0.0f; _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); _params.rattitude_thres = 1.0f; _rates_prev.zero(); _rates_sp.zero(); _rates_sp_prev.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); _I.identity(); _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.rattitude_thres = param_find("MC_RATT_TH"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.roll_tc = param_find("MC_ROLL_TC"); _params_handles.pitch_tc = param_find("MC_PITCH_TC"); /* fetch initial parameter values */ parameters_update(); if (_params.vtol_type == 0) { // the vehicle is a tailsitter, use optimal recovery control strategy _ts_opt_recovery = new TailsitterRecovery(); } }
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 GroundRoverAttitudeControl::task_main() { _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _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)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); parameters_update(); /* get an initial update for all sensor and status data */ vehicle_attitude_setpoint_poll(); vehicle_control_mode_poll(); manual_control_setpoint_poll(); battery_status_poll(); /* wakeup source */ px4_pollfd_struct_t 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 = 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 || fabsf(deltaT) < 0.00001f || !PX4_ISFINITE(deltaT)) { deltaT = 0.01f; } /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); vehicle_attitude_setpoint_poll(); vehicle_control_mode_poll(); manual_control_setpoint_poll(); battery_status_poll(); /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_rates_enabled) { /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled) { Eulerf euler_angles(matrix::Quatf(_att.q)); /* Calculate the control output for the steering as yaw */ float yaw_u = pid_calculate(&_steering_ctrl, _att_sp.yaw_body, euler_angles.psi(), _att.yawspeed, deltaT); float angle_diff = 0.0f; if (_att_sp.yaw_body * euler_angles.psi() < 0.0f) { if (_att_sp.yaw_body < 0.0f) { angle_diff = euler_angles.psi() - _att_sp.yaw_body ; } else { angle_diff = _att_sp.yaw_body - euler_angles.psi(); } // a switch might have happened if ((double)angle_diff > M_PI) { yaw_u = -yaw_u; } } math::constrain(yaw_u, -1.0f, 1.0f); if (PX4_ISFINITE(yaw_u)) { _actuators.control[actuator_controls_s::INDEX_YAW] = yaw_u + _parameters.trim_yaw; } else { _actuators.control[actuator_controls_s::INDEX_YAW] = _parameters.trim_yaw; 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] = _att_sp.thrust; /* scale effort by battery status */ if (_parameters.bat_scale_en && _battery_status.scale > 0.0f && _actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { _actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale; } } } else { /* manual/direct control */ _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y; _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x; _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; } /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _att.timestamp; /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuators_0_pub, &_actuators); } else { _actuators_0_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &_actuators); } } } loop_counter++; perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _task_running = false; }
/* * EKF Attitude Estimator main function. * * Estimates the attitude recursively once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ float x_aposteriori_k[12]; /**< states */ float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ float x_aposteriori[12]; float P_aposteriori[144]; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ float debugOutput[4] = { 0.0f }; /* Initialize filter */ AttitudeEKF_initialize(); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); gps.eph = 100000; gps.epv = 100000; struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; uint64_t last_vel_t = 0; /* current velocity */ math::Vector<3> vel; vel.zero(); /* previous velocity */ math::Vector<3> vel_prev; vel_prev.zero(); /* actual acceleration (by GPS velocity) in body frame */ math::Vector<3> acc; acc.zero(); /* rotation matrix */ math::Matrix<3, 3> R; R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); /* subscribe to GPS */ int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode */ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to vision estimate */ int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); /* subscribe to mocap data */ int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; thread_running = true; /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; memset(&ekf_params, 0, sizeof(ekf_params)); struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); bool initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; /* magnetic declination, in radians */ float mag_decl = 0.0f; /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); struct vision_position_estimate_s vision {}; struct att_pos_mocap_s mocap {}; /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); /* Main loop*/ while (!thread_should_exit) { px4_pollfd_struct_t fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = px4_poll(fds, 2, 1000); if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { warnx("WARNING: Not getting sensor data - sensor app running?"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); bool gps_updated; orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); } } bool global_pos_updated; orb_check(sub_global_pos, &global_pos_updated); if (global_pos_updated) { orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); } if (!initialized) { // XXX disabling init for now initialized = true; // gyro_offsets[0] += raw.gyro_rad_s[0]; // gyro_offsets[1] += raw.gyro_rad_s[1]; // gyro_offsets[2] += raw.gyro_rad_s[2]; // offset_count++; // if (hrt_absolute_time() - start_time > 3000000LL) { // initialized = true; // gyro_offsets[0] /= offset_count; // gyro_offsets[1] /= offset_count; // gyro_offsets[2] /= offset_count; // } } else { perf_begin(ekf_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.gyro_timestamp[0]; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp[0]; } hrt_abstime vel_t = 0; bool vel_valid = false; if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; vel(0) = global_pos.vel_n; vel(1) = global_pos.vel_e; vel(2) = global_pos.vel_d; } } if (vel_valid) { /* velocity is valid */ if (vel_t != 0) { /* velocity updated */ if (last_vel_t != 0 && vel_t != last_vel_t) { float vel_dt = (vel_t - last_vel_t) / 1000000.0f; /* calculate acceleration in body frame */ acc = R.transposed() * ((vel - vel_prev) / vel_dt); } last_vel_t = vel_t; vel_prev = vel; } } else { /* velocity is valid, reset acceleration */ acc.zero(); vel_prev.zero(); last_vel_t = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc(0); z_k[4] = raw.accelerometer_m_s2[1] - acc(1); z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && /* check that the mag vector is > 0 */ fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp[0]; } bool vision_updated = false; orb_check(vision_sub, &vision_updated); bool mocap_updated = false; orb_check(mocap_sub, &mocap_updated); if (vision_updated) { orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); } if (mocap_updated) { orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap); } if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) { math::Quaternion q(mocap.q); math::Matrix<3, 3> Rmoc = q.to_dcm(); math::Vector<3> v(1.0f, 0.0f, 0.4f); math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); }else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> Rvis = q.to_dcm(); math::Vector<3> v(1.0f, 0.0f, 0.4f); math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); } else { z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; } static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } /* update mag declination rotation matrix */ R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; x_aposteriori_k[3] = 0.0f; x_aposteriori_k[4] = 0.0f; x_aposteriori_k[5] = 0.0f; x_aposteriori_k[6] = z_k[3]; x_aposteriori_k[7] = z_k[4]; x_aposteriori_k[8] = z_k[5]; x_aposteriori_k[9] = z_k[6]; x_aposteriori_k[10] = z_k[7]; x_aposteriori_k[11] = z_k[8]; const_initialized = true; } /* do not execute the filter if not initialized */ if (!const_initialized) { continue; } /* Call the estimator */ AttitudeEKF(false, // approx_prediction (unsigned char)ekf_params.use_moment_inertia, update_vect, dt, z_k, ekf_params.q[0], // q_rotSpeed, ekf_params.q[1], // q_rotAcc ekf_params.q[2], // q_acc ekf_params.q[3], // q_mag ekf_params.r[0], // r_gyro ekf_params.r[1], // r_accel ekf_params.r[2], // r_mag ekf_params.moment_inertia_J, x_aposteriori, P_aposteriori, Rot_matrix, euler, debugOutput); /* swap values for next iteration, check for fatal inputs */ if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); } else { /* due to inputs or numerical failure the output is invalid, skip it */ continue; } if (last_data > 0 && raw.timestamp - last_data > 30000) { warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data)); } last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; att.roll = euler[0]; att.pitch = euler[1]; att.yaw = euler[2] + mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; att.rollacc = x_aposteriori[3]; att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); R = R_decl * R_body; math::Quaternion q; q.from_dcm(R); /* copy rotation matrix */ memcpy(&att.R[0], &R.data[0][0], sizeof(att.R)); memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { warnx("ERR: NaN estimate!"); } perf_end(ekf_loop_perf); } } } loopcounter++; } thread_running = false; return 0; }
void Standard::update_vtol_state() { parameters_update(); /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up * forward speed. After the vehicle has picked up enough speed the rotors shutdown. * For the back transition the pusher motor is immediately stopped and rotors reactivated. */ if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off if (_vtol_schedule.flight_mode == MC_MODE) { // in mc mode _vtol_schedule.flight_mode = MC_MODE; _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode if (_vtol_vehicle_status->vtol_transition_failsafe == true) { // Failsafe event, engage mc motors immediately _vtol_schedule.flight_mode = MC_MODE; _flag_enable_mc_motors = true; } else { // Regular backtransition _vtol_schedule.flight_mode = TRANSITION_TO_MC; _flag_enable_mc_motors = true; _vtol_schedule.transition_start = hrt_absolute_time(); } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // failsafe back to mc mode _vtol_schedule.flight_mode = MC_MODE; _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed // XXX: base this on XY hold velocity of MC if (hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.back_trans_dur * 1000000.0f)) { _vtol_schedule.flight_mode = MC_MODE; } } // the pusher motor should never be powered when in or transitioning to mc mode _pusher_throttle = 0.0f; } else { // the transition to fw mode switch is on if (_vtol_schedule.flight_mode == MC_MODE || _vtol_schedule.flight_mode == TRANSITION_TO_MC) { // start transition to fw mode /* NOTE: The failsafe transition to fixed-wing was removed because it can result in an * unsafe flying state. */ _vtol_schedule.flight_mode = TRANSITION_TO_FW; _vtol_schedule.transition_start = hrt_absolute_time(); } else if (_vtol_schedule.flight_mode == FW_MODE) { // in fw mode _vtol_schedule.flight_mode = FW_MODE; _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; _mc_throttle_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED || _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)) || can_transition_on_ground()) { _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; // don't set pusher throttle here as it's being ramped up elsewhere _trans_finished_ts = hrt_absolute_time(); } } } // map specific control phases to simple control modes switch (_vtol_schedule.flight_mode) { case MC_MODE: _vtol_mode = mode::ROTARY_WING; break; case FW_MODE: _vtol_mode = mode::FIXED_WING; break; case TRANSITION_TO_FW: _vtol_mode = mode::TRANSITION_TO_MC; break; case TRANSITION_TO_MC: _vtol_mode = mode::TRANSITION_TO_FW; break; } }
/* * Nonliner Exciplicit complementary filter (ECF), attitude estimator main function. * * Estimates the attitude once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ int unibo_att_esti_ECF_s_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds //! Time constant float dt = 0.005f; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; /* Initialization */ float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ float acc[3] = {0.0f, 0.0f, 0.0f}; float gyro[3] = {0.0f, 0.0f, 0.0f}; float mag[3] = {0.0f, 0.0f, 0.0f}; warnx("main thread started"); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); //! Initialize attitude vehicle uORB message. struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); /* subscribe to control mode */ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); //orb_advert_t att_pub = -1; orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; int printcounter = 0; thread_running = true; float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs /* keep track of sensor updates */ uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct unibo_att_esti_ECF_s_params so3_comp_params; struct unibo_att_esti_ECF_s_param_handles so3_comp_param_handles; /* initialize parameter handles */ parameters_init(&so3_comp_param_handles); parameters_update(&so3_comp_param_handles, &so3_comp_params); uint64_t start_time = hrt_absolute_time(); bool initialized = false; bool state_initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; /* register the perf counter */ perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "unibo_att_esti_ECF_s"); /*----------------------------------------------- * Main loop * ----------------------------------------------*/ while (!thread_should_exit) { struct pollfd fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { warnx("WARNING: Not getting sensors - sensor app running?"); } } else { /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), sub_params, &update); /* update parameters */ parameters_update(&so3_comp_param_handles, &so3_comp_params); } /* only run filter if sensor values changed */ if (fds[0].revents & POLLIN) { /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { gyro_offsets[0] += raw.gyro_rad_s[0]; gyro_offsets[1] += raw.gyro_rad_s[1]; gyro_offsets[2] += raw.gyro_rad_s[2]; offset_count++; if (hrt_absolute_time() > start_time + 3000000l) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); } } else { perf_begin(so3_comp_loop_perf); /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; acc[1] = raw.accelerometer_m_s2[1]; acc[2] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; mag[1] = raw.magnetometer_ga[1]; mag[2] = raw.magnetometer_ga[2]; /* initialize with good values once we have a reasonable dt estimate */ if (!state_initialized && dt < 0.05f && dt > 0.001f) { state_initialized = true; warnx("state initialized"); } /* do not execute the filter if not initialized */ if (!state_initialized) { continue; } uint64_t timing_start = hrt_absolute_time(); // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. standardECFnew(gyro[0], gyro[1], gyro[2], -acc[0], -acc[1], -acc[2], mag[0], mag[1], mag[2], so3_comp_params.Ka, so3_comp_params.Km, so3_comp_params.Kp, so3_comp_params.Ki, dt); // direction cosine matrix of 1-2-3 sequence // Resulted Rotation matrix convert from Inertial frame to body frame Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3; // R11 Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // R12 Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // R13 Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // R21 Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3; // R22 Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // R23 Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // R31 Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // R32 Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3; // R33 // Euler Angles using Unit Quaternions; euler[0]= atan2f((2.0f*(q2q3+q0q1)),1.0f-2.0f*(q1q1+q2q2)); // Roll euler[1]= asinf(-2.0f*(q1q3-q0q2)); // Pitch euler[2]= atan2f((2.0f*(q1q2+q0q3)),1.0f-2.0f*(q2q2+q3q3)); // Yaw /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { // Publish only finite euler angles att.roll = euler[0] - so3_comp_params.roll_off; att.pitch = euler[1] - so3_comp_params.pitch_off; att.yaw = euler[2] - so3_comp_params.yaw_off; } else { /* due to inputs or numerical failure the output is invalid, skip it */ // Due to inputs or numerical failure the output is invalid warnx("infinite euler angles, rotation matrix:"); warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); // Don't publish anything continue; } if (last_data > 0 && raw.timestamp > last_data + 12000) { warnx("sensor data missed"); } last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; // Quaternion att.q[0] = q0; att.q[1] = q1; att.q[2] = q2; att.q[3] = q3; att.q_valid = true; // Euler angle rate. But it needs to be investigated again. /* att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); */ att.rollspeed = gyro[0]; att.pitchspeed = gyro[1]; att.yawspeed = gyro[2]; att.rollacc = 0; att.pitchacc = 0; att.yawacc = 0; /* TODO: Bias estimation required */ memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; // Publish if (att_pub > 0) { orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); orb_advertise(ORB_ID(vehicle_attitude), &att); } perf_end(so3_comp_loop_perf); } } } loopcounter++; } // here the main loop stop thread_running = false; return 0; }
/** * Constructor */ VtolAttitudeControl::VtolAttitudeControl() : _task_should_exit(false), _control_task(-1), //init subscription handlers _v_att_sub(-1), _v_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), _local_pos_sub(-1), _airspeed_sub(-1), _battery_status_sub(-1), //init publication handlers _actuators_0_pub(nullptr), _actuators_1_pub(nullptr), _vtol_vehicle_status_pub(nullptr), _v_rates_sp_pub(nullptr) { memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); memset(&_local_pos,0,sizeof(_local_pos)); memset(&_airspeed,0,sizeof(_airspeed)); memset(&_batt_status,0,sizeof(_batt_status)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; _params.vtol_fw_permanent_stab = 0; _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); _params_handles.power_max = param_find("VT_POWER_MAX"); _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); /* fetch initial parameter values */ parameters_update(); if (_params.vtol_type == 0) { _tailsitter = new Tailsitter(this); _vtol_type = _tailsitter; } else if (_params.vtol_type == 1) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; } else { _task_should_exit = true; } }
void Standard::update_vtol_state() { parameters_update(); /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up * forward speed. After the vehicle has picked up enough speed the rotors shutdown. * For the back transition the pusher motor is immediately stopped and rotors reactivated. */ if (_manual_control_sp->aux1 < 0.0f) { // the transition to fw mode switch is off if (_vtol_schedule.flight_mode == MC_MODE) { // in mc mode _vtol_schedule.flight_mode = MC_MODE; _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode _vtol_schedule.flight_mode = TRANSITION_TO_MC; _flag_enable_mc_motors = true; _vtol_schedule.transition_start = hrt_absolute_time(); } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // failsafe back to mc mode _vtol_schedule.flight_mode = MC_MODE; _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // keep transitioning to mc mode _vtol_schedule.flight_mode = MC_MODE; } // the pusher motor should never be powered when in or transitioning to mc mode _pusher_throttle = 0.0f; } else { // the transition to fw mode switch is on if (_vtol_schedule.flight_mode == MC_MODE) { // start transition to fw mode _vtol_schedule.flight_mode = TRANSITION_TO_FW; _vtol_schedule.transition_start = hrt_absolute_time(); } else if (_vtol_schedule.flight_mode == FW_MODE) { // in fw mode _vtol_schedule.flight_mode = FW_MODE; _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) { _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; // don't set pusher throttle here as it's being ramped up elsewhere } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transitioning to mc mode & transition switch on - failsafe back into fw mode _vtol_schedule.flight_mode = FW_MODE; } } // map specific control phases to simple control modes switch(_vtol_schedule.flight_mode) { case MC_MODE: _vtol_mode = ROTARY_WING; break; case FW_MODE: _vtol_mode = FIXED_WING; break; case TRANSITION_TO_FW: case TRANSITION_TO_MC: _vtol_mode = TRANSITION; break; } }
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { static uint64_t last_run = 0; static uint64_t last_input = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; last_run = hrt_absolute_time(); if (last_input != att_sp->timestamp) { last_input = att_sp->timestamp; } static int sensor_delay; sensor_delay = hrt_absolute_time() - att->timestamp; static int motor_skip_counter = 0; static PID_t pitch_controller; static PID_t roll_controller; static struct mc_att_control_params p; static struct mc_att_control_param_handles h; static bool initialized = false; /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); parameters_update(&h, &p); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } /* load new parameters with lower rate */ if (motor_skip_counter % 1000 == 0) { /* update parameters from storage */ parameters_update(&h, &p); //printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); /* apply parameters */ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } /* calculate current control outputs */ /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , att->pitch, att->pitchspeed, deltaT); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); /* control yaw rate */ rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed); rates_sp->thrust = att_sp->thrust; motor_skip_counter++; }
void VtolAttitudeControl::task_main() { warnx("started"); fflush(stdout); /* do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); parameters_update(); // initialize parameter cache /* update vtol vehicle status*/ _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; // make sure we start with idle in mc mode _vtol_type->set_idle_mc(); /* wakeup source*/ struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; fds[1].fd = _actuator_inputs_fw; fds[1].events = POLLIN; fds[2].fd = _params_sub; fds[2].events = POLLIN; while (!_task_should_exit) { /*Advertise/Publish vtol vehicle status*/ if (_vtol_vehicle_status_pub != nullptr) { orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); } else { _vtol_vehicle_status.timestamp = hrt_absolute_time(); _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); } /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } if (fds[2].revents & POLLIN) { //parameters were updated, read them now /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); vehicle_battery_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); // check in which mode we are in and call mode specific functions if (_vtol_type->get_mode() == ROTARY_WING) { // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; _vtol_type->update_mc_state(); // got data from mc attitude controller if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); _vtol_type->process_mc_data(); fill_mc_att_rates_sp(); } } else if (_vtol_type->get_mode() == FIXED_WING) { // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; _vtol_type->update_fw_state(); // got data from fw attitude controller if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); vehicle_manual_poll(); _vtol_type->process_fw_data(); fill_fw_att_rates_sp(); } } else if (_vtol_type->get_mode() == TRANSITION) { // vehicle is doing a transition bool got_new_data = false; if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); got_new_data = true; } if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); got_new_data = true; } // update transition state if got any new data if (got_new_data) { _vtol_type->update_transition_state(); } } else if (_vtol_type->get_mode() == EXTERNAL) { // we are using external module to generate attitude/thrust setpoint _vtol_type->update_external_state(); } /* Only publish if the proper mode(s) are enabled */ if(_v_control_mode.flag_control_attitude_enabled || _v_control_mode.flag_control_rates_enabled || _v_control_mode.flag_control_manual_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); } else { _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); } if (_actuators_1_pub != nullptr) { orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); } else { _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } // publish the attitude rates setpoint if(_v_rates_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp); } else { _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp); } } warnx("exit"); _control_task = -1; _exit(0); }
void Sensors::parameter_update_poll(bool forced) { bool param_updated; /* Check if any parameter has changed */ orb_check(_params_sub, ¶m_updated); if (param_updated || forced) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters */ parameters_update(); /* update sensor offsets */ int fd = open(GYRO_DEVICE_PATH, 0); struct gyro_scale gscale = { _parameters.gyro_offset[0], _parameters.gyro_scale[0], _parameters.gyro_offset[1], _parameters.gyro_scale[1], _parameters.gyro_offset[2], _parameters.gyro_scale[2], }; if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) { warn("WARNING: failed to set scale / offsets for gyro"); } close(fd); fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { _parameters.accel_offset[0], _parameters.accel_scale[0], _parameters.accel_offset[1], _parameters.accel_scale[1], _parameters.accel_offset[2], _parameters.accel_scale[2], }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) { warn("WARNING: failed to set scale / offsets for accel"); } close(fd); fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale = { _parameters.mag_offset[0], _parameters.mag_scale[0], _parameters.mag_offset[1], _parameters.mag_scale[1], _parameters.mag_offset[2], _parameters.mag_scale[2], }; if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) { warn("WARNING: failed to set scale / offsets for mag"); } close(fd); fd = open(AIRSPEED_DEVICE_PATH, 0); /* this sensor is optional, abort without error */ if (fd > 0) { struct airspeed_scale airscale = { _parameters.diff_pres_offset_pa, 1.0f, }; if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { warn("WARNING: failed to set scale / offsets for airspeed sensor"); } close(fd); } #if 0 printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100)); fflush(stdout); usleep(5000); #endif } }