void AttitudePositionEstimatorEKF::pollData()
{
//Update arming status
bool armedUpdate;
orb_check(_armedSub, &armedUpdate);
if (armedUpdate) {
orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
}
//Update Gyro and Accelerometer
static Vector3f lastAngRate;
static Vector3f lastAccel;
bool accel_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0;
static hrt_abstime last_mag = 0;
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
}
last_accel = _sensor_combined.accelerometer_timestamp;
// Copy gyro and accel
_last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3;
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
_last_run = _sensor_combined.timestamp;
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
int last_gyro_main = _gyro_main;
if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
_gyro_main = 0;
_gyro_valid = true;
} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
_gyro_main = 1;
_gyro_valid = true;
} else {
_gyro_valid = false;
}
if (last_gyro_main != _gyro_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
}
if (!_gyro_valid) {
/* keep last value if he hit an out of band value */
lastAngRate = _ekf->angRate;
} else {
perf_count(_perf_gyro);
}
if (accel_updated) {
int last_accel_main = _accel_main;
/* fail over to the 2nd accel if we know the first is down */
if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
_accel_main = 0;
} else {
_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
_accel_main = 1;
}
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
if (last_accel_main != _accel_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
_newDataMag = true;
} else {
_newDataMag = false;
}
last_mag = _sensor_combined.magnetometer_timestamp;
//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
//Update Land Detector
bool newLandData;
orb_check(_landDetectorSub, &newLandData);
if (newLandData) {
orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
}
//Update AirSpeed
orb_check(_airspeed_sub, &_newAdsData);
if (_newAdsData) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
}
bool gps_update;
orb_check(_gps_sub, &gps_update);
if (gps_update) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
//We are more strict for our first fix
float requiredAccuracy = _parameters.pos_stddev_threshold;
if (_gpsIsGood) {
requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
}
//Check if the GPS fix is good enough for us to use
if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
_gpsIsGood = true;
} else {
_gpsIsGood = false;
}
if (_gpsIsGood) {
//Calculate time since last good GPS fix
const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
//Stop dead-reckoning mode
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning");
}
_global_pos.dead_reckoning = false;
//Fetch new GPS data
_ekf->GPSstatus = _gps.fix_type;
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
if (_previousGPSTimestamp != 0) {
//Calculate average time between GPS updates
_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
// update LPF
_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
}
//check if we had a GPS outage for a long time
if (_gps_initialized) {
//Convert from global frame to local frame
map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]);
if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
_ekf->ResetPosition();
_ekf->ResetVelocity();
}
}
//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS);
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
// } else {
// _ekf->vneSigma = _parameters.velne_noise;
// }
// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
// _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
// } else {
// _ekf->posNeSigma = _parameters.posne_noise;
// }
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
_previousGPSTimestamp = _gps.timestamp_position;
}
}
// If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
// then something is very wrong with the GPS (possibly a hardware failure or comlink error)
const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout");
}
_gpsIsGood = false;
_global_pos.dead_reckoning = false;
}
//If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds)
else if (dtLastGoodGPS >= 0.5f) {
if (_armed.armed) {
if (!_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled");
}
_global_pos.dead_reckoning = true;
} else {
_global_pos.dead_reckoning = false;
}
}
//Update barometer
orb_check(_baro_sub, &_newHgtData);
if (_newHgtData) {
static hrt_abstime baro_last = 0;
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
// init lowpass filters for baro and gps altitude
float baro_elapsed;
if (baro_last == 0) {
baro_elapsed = 0.0f;
} else {
baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
}
baro_last = _baro.timestamp;
if(!_baro_init) {
_baro_init = true;
_baroAltRef = _baro.altitude;
}
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
_ekf->baroHgt = _baro.altitude;
_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
perf_count(_perf_baro);
}
//Update Magnetometer
if (_newDataMag) {
_mag_valid = true;
perf_count(_perf_mag);
int last_mag_main = _mag_main;
Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
_sensor_combined.magnetometer_ga[2]);
Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
_sensor_combined.magnetometer1_ga[2]);
const unsigned mag_timeout_us = 200000;
/* fail over to the 2nd mag if we know the first is down */
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
mag0.length() > 0.1f) {
_ekf->magData.x = mag0.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = mag0.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = mag0.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 0;
} else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us &&
mag1.length() > 0.1f) {
_ekf->magData.x = mag1.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = mag1.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = mag1.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 1;
} else {
_mag_valid = false;
}
if (last_mag_main != _mag_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
}
}
//Update range data
orb_check(_distance_sub, &_newRangeData);
if (_newRangeData) {
orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
if ((_distance.current_distance > _distance.min_distance)
&& (_distance.current_distance < _distance.max_distance)) {
_ekf->rngMea = _distance.current_distance;
_distance_last_valid = _distance.timestamp;
} else {
_newRangeData = false;
}
}
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(_err_perf);
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
detectDistanceSensors();
}
// reset pos, vel, and terrain on arming
if (!_lastArmedState && armedState) {
// we just armed, we are at home position on the ground
_x(X_x) = 0;
_x(X_y) = 0;
// the pressure altitude of home may have drifted, so we don't
// reset z to zero
// reset flow integral
_flowX = 0;
_flowY = 0;
// we aren't moving, all velocities are zero
_x(X_vx) = 0;
_x(X_vy) = 0;
_x(X_vz) = 0;
// assume we are on the ground, so terrain alt is local alt
_x(X_tz) = _x(X_z);
// reset lowpass filter as well
_xLowPass.setState(_x);
}
_lastArmedState = armedState;
// see which updates are available
bool flowUpdated = _sub_flow.updated();
bool paramsUpdated = _sub_param_update.updated();
bool baroUpdated = _sub_sensor.updated();
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
bool homeUpdated = _sub_home.updated();
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// update home position projection
if (homeUpdated) {
updateHome();
}
// is xy valid?
bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
if (_validXY) {
// if valid and gps has timed out, set to not valid
if (!xy_stddev_ok && !_gpsInitialized) {
_validXY = false;
}
} else {
if (xy_stddev_ok) {
_validXY = true;
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_validZ) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && !_baroInitialized) {
_validZ = false;
}
} else {
if (z_stddev_ok) {
_validZ = true;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_validTZ) {
if (!tz_stddev_ok) {
_validTZ = false;
}
} else {
if (tz_stddev_ok) {
_validTZ = true;
}
}
// timeouts
if (_validXY) {
_time_last_xy = _timeStamp;
}
if (_validZ) {
_time_last_z = _timeStamp;
}
if (_validTZ) {
_time_last_tz = _timeStamp;
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection at 0,0
if (_validXY && !_map_ref.init_done) {
map_projection_init(&_map_ref,
_init_home_lat.get(),
_init_home_lon.get());
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
}
// reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j < n_x; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit_P = true;
break;
}
}
if (reinit_P) { break; }
}
if (reinit_P) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (gpsUpdated) {
if (!_gpsInitialized) {
gpsInit();
} else {
gpsCorrect();
}
}
if (baroUpdated) {
if (!_baroInitialized) {
baroInit();
} else {
baroCorrect();
}
}
if (lidarUpdated) {
if (!_lidarInitialized) {
lidarInit();
} else {
lidarCorrect();
}
}
if (sonarUpdated) {
if (!_sonarInitialized) {
sonarInit();
} else {
sonarCorrect();
}
}
if (flowUpdated) {
if (!_flowInitialized) {
flowInit();
} else {
perf_begin(_loop_perf);// TODO
flowCorrect();
//perf_count(_interval_perf);
perf_end(_loop_perf);
}
}
if (visionUpdated) {
if (!_visionInitialized) {
visionInit();
} else {
visionCorrect();
}
}
if (mocapUpdated) {
if (!_mocapInitialized) {
mocapInit();
} else {
mocapCorrect();
}
}
if (_altHomeInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
if (_validXY) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.pitch_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
/* apply turning offset to desired bodyrate setpoint*/
/* flying inverted (wings upside down)*/
bool inverted = false;
float constrained_roll;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
} else {
/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
inverted = true;
/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
if (ctl_data.roll > 0.0f) {
/* right hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
} else {
/* left hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
}
}
/* input conditioning */
float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
Availible on google books 8/11/2015:
https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(constrained_roll) * sinf(constrained_roll)));
if (inverted) {
body_fixed_turn_offset = -body_fixed_turn_offset;
}
/* Finally add the turn offset to your bodyrate setpoint*/
_bodyrate_setpoint += body_fixed_turn_offset;
_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
if (!lock_integrator && _k_i > 0.0f) {
float id = _rate_error * dt * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
//Calculate D term
float diff_error = -ctl_data.pitch_rate;
float d_term = (_k_d * _k_f) * (diff_error - _rate_diff_error_state);
_rate_diff_error_state += _k_f * (diff_error - _rate_diff_error_state);
if (d_term > 1.0f)
{
d_term = 1.0f;
}
else if (d_term < -1.0f)
{
d_term = -1.0;
}
/* Apply PID rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained //scaler is proportional to 1/airspeed
+ d_term;
//warnx("pitch: _bodyrate_setpoint: %.4f, _k_ff: %.4f", (double)_bodyrate_setpoint, (double)_k_ff);
//warnx("pitch: _rate_error: %.4f, _k_p: %.4f, ctl_data.scaler: %.4f", (double)_rate_error, (double)_k_p, (double)ctl_data.scaler);
//warnx("pitch: integrator_constrained: %.4f, _integrator_max: %.4f, _k_i %.4f", (double)integrator_constrained, (double)_integrator_max, (double)_k_i);
//warnx("roll: d_term %.4f _k_d %.4f _k_f %.4f", (double)d_term, (double)_k_d, (double)_k_f);
//warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
}
void
L3GD20::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
int8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_report;
#pragma pack(pop)
gyro_report report;
/* start the performance counter */
perf_begin(_sample_perf);
check_registers();
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
if (!(raw_report.status & STATUS_ZYXDA)) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_bad_registers);
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */
report.x_raw = raw_report.x;
report.y_raw = raw_report.y;
break;
case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */
report.x_raw = raw_report.y;
report.y_raw = raw_report.x;
break;
case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */
report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
break;
case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */
report.x_raw = raw_report.y;
report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break;
}
report.z_raw = raw_report.z;
report.temperature_raw = raw_report.temp;
float xraw_f = report.x_raw;
float yraw_f = report.y_raw;
float zraw_f = report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = _gyro_filter_x.apply(report.x);
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
/* stop the perf counter */
perf_end(_sample_perf);
}
int
MEASAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[4] = {0, 0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint8_t status = (val[0] & 0xC0) >> 6;
switch (status) {
case 0:
break;
case 1:
/* fallthrough */
case 2:
/* fallthrough */
case 3:
perf_count(_comms_errors);
perf_end(_sample_perf);
return -EAGAIN;
}
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
/* mask the used bits */
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200.0f * dT_raw) / 2047) - 50;
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative
const float P_min = -1.0f;
const float P_max = 1.0f;
const float PSI_to_Pa = 6894.757f;
/*
this equation is an inversion of the equation in the
pressure transfer function figure on page 4 of the datasheet
We negate the result so that positive differential pressures
are generated when the bottom port is used as the static
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void *
fat_dma_alloc(size_t size)
{
perf_count(g_dma_perf);
return gran_alloc(dma_allocator, size);
}
void
MPU9250_mag::measure(struct ak8963_regs data)
{
bool mag_notify = true;
if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
perf_count(_mag_duplicates);
return;
}
/* monitor for if data overrun flag is ever set */
if (data.st1 & 0x02) {
perf_count(_mag_overruns);
}
/* monitor for if magnetic sensor overflow flag is ever set noting that st2
* is usually not even refreshed, but will always be in the same place in the
* mpu's buffers regardless, hence the actual count would be bogus
*/
if (data.st2 & 0x08) {
perf_count(_mag_overflows);
}
mag_report mrb;
mrb.timestamp = hrt_absolute_time();
/*
* Align axes - note the accel & gryo are also re-aligned so this
* doesn't look obvious with the datasheet
*/
mrb.x_raw = data.x;
mrb.y_raw = -data.y;
mrb.z_raw = -data.z;
float xraw_f = data.x;
float yraw_f = -data.y;
float zraw_f = -data.z;
/* apply user specified rotation */
rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f);
mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale;
mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale;
mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale;
mrb.range_ga = (float)48.0;
mrb.scaling = _mag_range_scale;
mrb.temperature = _parent->_last_temperature;
mrb.error_count = perf_event_count(_mag_errors);
_mag_reports->force(&mrb);
/* notify anyone waiting for data */
if (mag_notify) {
poll_notify(POLLIN);
}
if (mag_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb);
}
}
void
LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
}
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_accel_report;
#pragma pack(pop)
accel_report accel_report;
/* start the performance counter */
perf_begin(_accel_sample_perf);
/* fetch data from the sensor */
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
accel_report.timestamp = hrt_absolute_time();
accel_report.error_count = 0; // not reported
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
accel_report.z_raw = raw_accel_report.z;
float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
_accel_reports->force(&accel_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_accel_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
/* stop the perf counter */
perf_end(_accel_sample_perf);
}
void
LSM303D::mag_measure()
{
if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
perf_count(_reg7_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_mag_report;
#pragma pack(pop)
mag_report mag_report;
/* start the performance counter */
perf_begin(_mag_sample_perf);
/* fetch data from the sensor */
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
mag_report.timestamp = hrt_absolute_time();
mag_report.x_raw = raw_mag_report.x;
mag_report.y_raw = raw_mag_report.y;
mag_report.z_raw = raw_mag_report.z;
mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report.scaling = _mag_range_scale;
mag_report.range_ga = (float)_mag_range_ga;
_mag_reports->force(&mag_report);
/* XXX please check this poll_notify, is it the right one? */
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
/* stop the perf counter */
perf_end(_mag_sample_perf);
}
static int
flow_position_control2_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow position control] starting\n");
uint32_t counter = 0;
const float time_scale = powf(10.0f,-6.0f);
/* structures */
struct vehicle_status_s vstatus;
struct vehicle_attitude_s att;
struct manual_control_setpoint_s manual;
struct filtered_bottom_flow_s filtered_flow;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
struct debug_key_value_s debug_value = { .key = "height_sp", .value = 0.0f };
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int vehicle_local_position_setpoint_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_advert_t speed_sp_pub;
bool speed_setpoint_adverted = false;
orb_advert_t debug_value_pub;
bool debug_value_adverted = false;
/* parameters init*/
struct flow_position_control2_params params;
struct flow_position_control2_param_handles param_handles;
parameters_init(¶m_handles);
parameters_update(¶m_handles, ¶ms);
/* init setpoint */
float bodyframe_sp_x = 0.0f;
float bodyframe_sp_y = 0.0f;
local_pos_sp.x = 0.0f;
local_pos_sp.y = 0.0f;
local_pos_sp.z = 0.0f;
local_pos_sp.yaw = 0.0f;
/* init yaw setpoint */
float yaw_sp = 0.0f;
/* init height setpoint */
float height_sp = params.height_min;
/* height controller states */
bool start_phase = true;
bool landing_initialized = false;
float landing_thrust_start = 0.0f;
/* states */
float integrated_h_error = 0.0f;
float integrated_bodyframe_pos_err_x = 0.0f;
float integrated_bodyframe_pos_err_y = 0.0f;
float last_local_pos_z = 0.0f;
bool manage_position_sp = true;
bool update_position_sp = false;
uint64_t last_time = 0.0f;
float dt = 0.0f; // s
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
static bool sensors_ready = false;
while (!thread_should_exit)
{
/* wait for first attitude msg to be sure all data are available */
if (sensors_ready)
{
/* polling */
struct pollfd fds[3] = {
{ .fd = filtered_bottom_flow_sub, .events = POLLIN },
{ .fd = vehicle_local_position_setpoint_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN }
};
/* wait for a position update, check for exit condition every 500 ms */
int ret = poll(fds, 3, 500);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
// printf("[flow position control] no filtered flow updates\n");
}
else
{
/* parameter update available? */
if (fds[2].revents & POLLIN)
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(¶m_handles, ¶ms);
printf("[flow position control] parameters updated.\n");
}
/* new local position setpoint */
if(fds[1].revents & POLLIN)
{
/* got a position setpoint from another app
* now the other app is responsible for the setpoint!
* */
// float old_sp_x = local_pos_sp.x;
// float old_sp_y = local_pos_sp.y;
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
/* get a local copy of local posiition setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), vehicle_local_position_setpoint_sub, &local_pos_sp);
/* make calculation for bodyframe controlling */
float local_diff_x = local_pos_sp.x-local_pos.x;
float local_diff_y = local_pos_sp.y-local_pos.y;
bodyframe_sp_x = att.R[0][0] * local_diff_x + att.R[1][0] * local_diff_y;
bodyframe_sp_y = att.R[0][1] * local_diff_x + att.R[1][1] * local_diff_y;
// float local_diff_x = local_pos_sp.x - old_sp_x;
// float local_diff_y = local_pos_sp.y - old_sp_y;
// bodyframe_sp_x = bodyframe_sp_x + att.R[0][0] * local_diff_x + att.R[1][0] * local_diff_y;
// bodyframe_sp_y = bodyframe_sp_y + att.R[0][1] * local_diff_x + att.R[1][1] * local_diff_y;
yaw_sp = local_pos_sp.yaw;
/* TODO for start ignore z setpoint */
manage_position_sp = false;
}
/* only run controller if position/speed changed */
if (fds[0].revents & POLLIN)
{
perf_begin(mc_loop_perf);
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
/* get a local copy of filtered bottom flow */
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
if (vstatus.state_machine == SYSTEM_STATE_AUTO)
{
float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
/* calc dt */
if(last_time == 0)
{
last_time = hrt_absolute_time();
continue;
}
dt = ((float) (hrt_absolute_time() - last_time)) * time_scale;
last_time = hrt_absolute_time();
/* update flow sum setpoint */
if (manage_position_sp && update_position_sp && !params.debug)
{
bodyframe_sp_x = 0.0f;
bodyframe_sp_y = 0.0f;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
update_position_sp = false;
}
else
{
float local_diff_x = local_pos_sp.x-local_pos.x;
float local_diff_y = local_pos_sp.y-local_pos.y;
if (fabsf(local_diff_x) < params.setpoint_radius && fabsf(local_diff_y) < params.setpoint_radius)
{
/* this is in hover mode, the setpoint is almost at local position */
/* integrate bodyframe speed */
float diff_x = filtered_flow.vx * dt;
float diff_y = filtered_flow.vy * dt;
/* calc new bodyframe setpoint */
bodyframe_sp_x = bodyframe_sp_x - diff_x;
bodyframe_sp_y = bodyframe_sp_y - diff_y;
/* calc integrated bodyframe position error */
integrated_bodyframe_pos_err_x += bodyframe_sp_x * dt;
integrated_bodyframe_pos_err_y += bodyframe_sp_y * dt;
}
else
{
/* far away from setpoint, calculate stepwise the new bodyframe setpoint */
/* calc new bodyframe setpoint */
/* except of some small differences (because of roll and pitch angles)
* we can take the rotation matrix
*/
bodyframe_sp_x = att.R[0][0] * local_diff_x + att.R[1][0] * local_diff_y;
bodyframe_sp_y = att.R[0][1] * local_diff_x + att.R[1][1] * local_diff_y;
}
}
/* calc new bodyframe speed setpoints */
float speed_body_x = bodyframe_sp_x * params.pos_p +
integrated_bodyframe_pos_err_x * params.pos_i -
filtered_flow.vx * params.pos_d;
float speed_body_y = bodyframe_sp_y * params.pos_p +
integrated_bodyframe_pos_err_y * params.pos_i -
filtered_flow.vy * params.pos_d;
float speed_limit_height_factor = height_sp; // the settings are for 1 meter
/* overwrite with rc input if there is any */
if(isfinite(manual_pitch) && isfinite(manual_roll))
{
if(fabsf(manual_pitch) > params.manual_threshold)
{
speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor;
update_position_sp = true;
}
if(fabsf(manual_roll) > params.manual_threshold)
{
speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor;
update_position_sp = true;
}
}
/* manual yaw change (only if we manage setpoint)*/
if (manage_position_sp)
{
if(isfinite(manual_yaw) && isfinite(manual.throttle))
{
if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f)
{
yaw_sp += manual_yaw * params.limit_yaw_step;
/* modulo for rotation -pi +pi */
if(yaw_sp < -M_PI_F)
yaw_sp = yaw_sp + M_TWOPI_F;
else if(yaw_sp > M_PI_F)
yaw_sp = yaw_sp - M_TWOPI_F;
}
}
}
/* forward yaw setpoint */
speed_sp.yaw_sp = yaw_sp;
/* limit speed setpoints */
if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) &&
(speed_body_x >= -params.limit_speed_x * speed_limit_height_factor))
{
speed_sp.vx = speed_body_x;
}
else
{
if(speed_body_x > params.limit_speed_x * speed_limit_height_factor)
speed_sp.vx = params.limit_speed_x * speed_limit_height_factor;
if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor)
speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor;
}
if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) &&
(speed_body_y >= -params.limit_speed_y * speed_limit_height_factor))
{
speed_sp.vy = speed_body_y;
}
else
{
if(speed_body_y > params.limit_speed_y * speed_limit_height_factor)
speed_sp.vy = params.limit_speed_y * speed_limit_height_factor;
if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor)
speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor;
}
/* manual height control
* 0-20%: thrust linear down
* 20%-40%: down
* 40%-60%: stabilize altitude
* 60-100%: up
*/
float thrust_control = 0.0f;
if (isfinite(manual.throttle))
{
if (start_phase)
{
/* control start thrust with stick input */
if (manual.throttle < 0.4f)
{
/* first 40% for up to feedforward */
thrust_control = manual.throttle / 0.4f * params.thrust_feedforward;
}
else
{
/* second 60% for up to feedforward + 10% */
thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward;
}
/* exit start phase if setpoint is reached */
if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower)
{
start_phase = false;
/* switch to stabilize */
thrust_control = params.thrust_feedforward;
}
}
else
{
if (manual.throttle < 0.2f)
{
/* landing initialization */
if (!landing_initialized)
{
/* consider last thrust control to avoid steps */
landing_thrust_start = speed_sp.thrust_sp;
landing_initialized = true;
}
/* set current height as setpoint to avoid steps */
if (-local_pos.z > params.height_min)
height_sp = -local_pos.z;
else
height_sp = params.height_min;
/* lower 20% stick range controls thrust down */
thrust_control = manual.throttle / 0.2f * landing_thrust_start;
/* assume ground position here */
if (thrust_control < 0.1f)
{
/* reset integral if on ground */
integrated_h_error = 0.0f;
/* switch to start phase */
start_phase = true;
/* reset height setpoint */
height_sp = params.height_min;
/* reset position error integrals */
integrated_bodyframe_pos_err_x = 0.0f;
integrated_bodyframe_pos_err_y = 0.0f;
}
}
else
{
/* stabilized mode */
landing_initialized = false;
/* calc new thrust with PID */
float height_error = (local_pos.z - (-height_sp));
/* update height setpoint if needed*/
if (manual.throttle < 0.4f)
{
/* down */
if (height_sp > params.height_min + params.height_rate &&
fabsf(height_error) < params.limit_height_error)
height_sp -= params.height_rate * dt;
}
if (manual.throttle > 0.6f)
{
/* up */
if (height_sp < params.height_max &&
fabsf(height_error) < params.limit_height_error)
height_sp += params.height_rate * dt;
}
/* instead of speed limitation, limit height error (downwards) */
if(height_error > params.limit_height_error)
height_error = params.limit_height_error;
else if(height_error < -params.limit_height_error)
height_error = -params.limit_height_error;
integrated_h_error = integrated_h_error + height_error;
float integrated_thrust_addition = integrated_h_error * params.height_i;
if(integrated_thrust_addition > params.limit_thrust_int)
integrated_thrust_addition = params.limit_thrust_int;
if(integrated_thrust_addition < -params.limit_thrust_int)
integrated_thrust_addition = -params.limit_thrust_int;
// float height_speed = last_local_pos_z - local_pos.z;
float thrust_diff = height_error * params.height_p - local_pos.vz * params.height_d;
thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition;
/* add attitude component
* F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM
*/
// // TODO problem with attitude
// if (att.R_valid && att.R[2][2] > 0)
// thrust_control = thrust_control / att.R[2][2];
/* set thrust lower limit */
if(thrust_control < params.limit_thrust_lower)
thrust_control = params.limit_thrust_lower;
}
}
/* set thrust upper limit */
if(thrust_control > params.limit_thrust_upper)
thrust_control = params.limit_thrust_upper;
}
/* store actual height for speed estimation */
last_local_pos_z = local_pos.z;
speed_sp.thrust_sp = thrust_control;
speed_sp.timestamp = hrt_absolute_time();
/* publish new speed setpoint */
if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp))
{
if(speed_setpoint_adverted)
{
orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp);
}
else
{
speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp);
speed_setpoint_adverted = true;
}
}
else
{
warnx("NaN in flow position controller!");
}
// TODO remove before flight ;)
debug_value.value = -height_sp;
/* publish new speed setpoint */
if(isfinite(debug_value.value))
{
if(debug_value_adverted)
{
orb_publish(ORB_ID(debug_key_value), debug_value_pub, &debug_value);
}
else
{
debug_value_pub = orb_advertise(ORB_ID(debug_key_value), &debug_value);
debug_value_adverted = true;
}
}
else
{
warnx("NaN in flow position controller!");
}
}
else
{
/* in manual or stabilized state just reset speed and flow sum setpoint */
speed_sp.vx = 0.0f;
speed_sp.vy = 0.0f;
if(isfinite(att.yaw))
{
yaw_sp = att.yaw;
speed_sp.yaw_sp = att.yaw;
}
if(isfinite(manual.throttle))
speed_sp.thrust_sp = manual.throttle;
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
}
}
counter++;
}
int
ETSAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
perf_count(_comms_errors);
return ret;
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
// average
perf_count(_comms_errors);
DEVICE_LOG("zero value from sensor");
return -1;
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa_raw;
}
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub != nullptr && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
IST8310::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} report_buffer;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
const bool sensor_is_external = external();
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.is_external = sensor_is_external;
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = read(ADDR_DATA_OUT_X_LSB, (uint8_t *)&report_buffer, sizeof(report_buffer));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("I2C read error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)report_buffer.x[1]) << 8) | (int16_t)report_buffer.x[0];
report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0];
report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0];
/*
* Check if value makes sense according to the FSR and Resolution of
* this sensor, discarding outliers
*/
if (report.x > IST8310_MAX_VAL_XY || report.x < IST8310_MIN_VAL_XY ||
report.y > IST8310_MAX_VAL_XY || report.y < IST8310_MIN_VAL_XY ||
report.z > IST8310_MAX_VAL_Z || report.z < IST8310_MIN_VAL_Z) {
perf_count(_range_errors);
DEVICE_DEBUG("data/status read error");
goto out;
}
/* temperature measurement is not available on IST8310 */
new_report.temperature = 0;
/*
* raw outputs
*
* Sensor doesn't follow right hand rule, swap x and y to make it obey
* it.
*/
new_report.x_raw = report.y;
new_report.y_raw = report.x;
new_report.z_raw = report.z;
/* scale values for output */
xraw_f = report.y;
yraw_f = report.x;
zraw_f = report.z;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, sensor_is_external ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 128) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
void AttitudePositionEstimatorEKF::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
warnx("OUT OF MEM!");
return;
}
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 9);
/* sets also parameters in the EKF object */
parameters_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _sensor_combined_sub;
fds[1].events = POLLIN;
_gps.vel_n_m_s = 0.0f;
_gps.vel_e_m_s = 0.0f;
_gps.vel_d_m_s = 0.0f;
_task_running = true;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("POLL ERR %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
perf_count(_loop_intvl);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run estimator if gyro updated */
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
vehicle_status_poll();
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
_last_sensor_timestamp = hrt_absolute_time();
_last_run = _last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = _last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
}
/**
* PART ONE: COLLECT ALL DATA
**/
pollData();
/*
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
*/
if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
continue;
}
/**
* PART TWO: EXECUTE THE FILTER
*
* We run the filter only once all data has been fetched
**/
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
// maintain filtered baro and gps altitudes to calculate weather offset
// baro sample rate is ~70Hz and measurement bandwidth is high
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
// maintain heavily filtered values for both baro and gps altitude
// Assume the filtered output should be identical for both sensors
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
// _last_debug_print = hrt_absolute_time();
// perf_print_counter(_perf_baro);
// perf_reset(_perf_baro);
// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
// (double)_baro_gps_offset,
// (double)_baro_alt_filt,
// (double)_gps_alt_filt,
// (double)_global_pos.alt,
// (double)_local_pos.z);
// }
/* Initialize the filter first */
if (!_ekf->statesInitialised) {
// North, East Down position (m)
float initVelNED[3] = {0.0f, 0.0f, 0.0f};
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = 0.0f;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_baro_alt_filt = _baro.altitude;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else {
if (!_gps_initialized && _gpsIsGood) {
initializeGPS();
continue;
}
// Check if on ground - status is used by covariance prediction
_ekf->setOnGround(_landDetector.landed);
// We're apparently initialized in this case now
// check (and reset the filter as needed)
int check = check_filter_state();
if (check) {
// Let the system re-initialize itself
continue;
}
//Run EKF data fusion steps
updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
//Publish attitude estimations
publishAttitude();
//Publish Local Position estimations
publishLocalPosition();
//Publish Global Position, but only if it's any good
if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
publishGlobalPosition();
}
//Publish wind estimates
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
publishWindEstimate();
}
}
}
}
perf_end(_loop_perf);
}
_task_running = false;
_estimator_task = -1;
return;
}
int AttitudePositionEstimatorEKF::check_filter_state()
{
/*
* CHECK IF THE INPUT DATA IS SANE
*/
struct ekf_status_report ekf_report;
int check = _ekf->CheckAndBound(&ekf_report);
const char *const feedback[] = { 0,
"NaN in states, resetting",
"stale sensor data, resetting",
"got initial position lock",
"excessive gyro offsets",
"velocity diverted, check accel config",
"excessive covariances",
"unknown condition, resetting"
};
// Print out error condition
if (check) {
unsigned warn_index = static_cast<unsigned>(check);
unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));
if (max_warn_index < warn_index) {
warn_index = max_warn_index;
}
// Do not warn about accel offset if we have no position updates
if (!(warn_index == 5 && _ekf->staticMode)) {
warnx("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
}
}
struct estimator_status_s rep;
memset(&rep, 0, sizeof(rep));
// If error flag is set, we got a filter reset
if (check && ekf_report.error) {
// Count the reset condition
perf_count(_perf_reset);
} else if (_ekf_logging) {
_ekf->GetFilterState(&ekf_report);
}
if (_ekf_logging || check) {
rep.timestamp = hrt_absolute_time();
rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0);
rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1);
rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2);
rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3);
rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4);
rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5);
rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6);
rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7);
rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0);
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2);
rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout) << 3);
if (_debug > 10) {
if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s",
((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
}
if (rep.timeout_flags) {
warnx("timeout: %s%s%s%s",
((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
}
}
// Copy all states or at least all that we can fit
size_t ekf_n_states = ekf_report.n_states;
size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
for (size_t i = 0; i < rep.n_states; i++) {
rep.states[i] = ekf_report.states[i];
}
if (_estimator_status_pub != nullptr) {
orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
} else {
_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
}
}
return check;
}
void
L3GD20::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
int8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_report;
#pragma pack(pop)
gyro_report report;
/* start the performance counter */
perf_begin(_sample_perf);
check_registers();
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
perf_end(_sample_perf);
return;
}
#endif
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3GD20_USE_DRDY
if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_bad_registers);
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */
report.x_raw = raw_report.x;
report.y_raw = raw_report.y;
break;
case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */
report.x_raw = raw_report.y;
report.y_raw = raw_report.x;
break;
case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */
report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
break;
case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */
report.x_raw = raw_report.y;
report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break;
}
report.z_raw = raw_report.z;
report.temperature_raw = raw_report.temp;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = _gyro_filter_x.apply(report.x);
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
/* stop the perf counter */
perf_end(_sample_perf);
}
/**
check for extreme accelerometer values and log to a file on the SD card
*/
void
LSM303D::check_extremes(const accel_report *arb)
{
const float extreme_threshold = 30;
static bool boot_ok = false;
bool is_extreme = (fabsf(arb->x) > extreme_threshold &&
fabsf(arb->y) > extreme_threshold &&
fabsf(arb->z) > extreme_threshold);
if (is_extreme) {
perf_count(_extreme_values);
// force accel logging on if we see extreme values
_accel_logging_enabled = true;
} else {
boot_ok = true;
}
if (! _accel_logging_enabled) {
// logging has been disabled by user, close
if (_accel_log_fd != -1) {
::close(_accel_log_fd);
_accel_log_fd = -1;
}
return;
}
if (_accel_log_fd == -1) {
// keep last 10 logs
::unlink(ACCEL_LOGFILE ".9");
for (uint8_t i=8; i>0; i--) {
uint8_t len = strlen(ACCEL_LOGFILE)+3;
char log1[len], log2[len];
snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
::rename(log1, log2);
}
::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
// open the new logfile
_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
if (_accel_log_fd == -1) {
return;
}
}
uint64_t now = hrt_absolute_time();
// log accels at 1Hz
if (_last_log_us == 0 ||
now - _last_log_us > 1000*1000) {
_last_log_us = now;
::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
(unsigned long long)arb->timestamp,
(double)arb->x, (double)arb->y, (double)arb->z,
(int)arb->x_raw,
(int)arb->y_raw,
(int)arb->z_raw,
(unsigned)boot_ok);
}
const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1,
ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6,
ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M,
ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A,
ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL,
ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2,
ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC,
ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW,
ADDR_ACT_THS, ADDR_ACT_DUR,
ADDR_OUT_X_L_M, ADDR_OUT_X_H_M,
ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
uint8_t regval[sizeof(reglist)];
for (uint8_t i=0; i<sizeof(reglist); i++) {
regval[i] = read_reg(reglist[i]);
}
// log registers at 10Hz when we have extreme values, or 0.5 Hz without
if (_last_log_reg_us == 0 ||
(is_extreme && (now - _last_log_reg_us > 250*1000)) ||
(now - _last_log_reg_us > 10*1000*1000)) {
_last_log_reg_us = now;
::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
for (uint8_t i=0; i<sizeof(reglist); i++) {
::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
}
::dprintf(_accel_log_fd, "\n");
}
// fsync at 0.1Hz
if (now - _last_log_sync_us > 10*1000*1000) {
_last_log_sync_us = now;
::fsync(_accel_log_fd);
}
// play alarm every 10s if we have had an extreme value
if (perf_event_count(_extreme_values) != 0 &&
(now - _last_log_alarm_us > 10*1000*1000)) {
_last_log_alarm_us = now;
int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
if (tfd != -1) {
uint8_t tone = 3;
if (!is_extreme) {
tone = 3;
} else if (boot_ok) {
tone = 4;
} else {
tone = 5;
}
::ioctl(tfd, TONE_SET_ALARM, tone);
::close(tfd);
}
}
}
void
FixedwingAttitudeControl::task_main()
{
/*
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* do not limit the attitude updates in order to minimize latency.
* actuator outputs are still limited by the individual drivers
* properly to not saturate IO or physical limitations */
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].events = POLLIN;
_task_running = true;
while (!_task_should_exit) {
static int loop_counter = 0;
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
* Additionally, in order to get the correct sign of the pitch, we need to multiply
* the new x axis of the rotation matrix with -1
*
* original: modified:
*
* Rxx Ryx Rzx -Rzx Ryx Rxx
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
/* fill in new attitude data */
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
vehicle_status_poll();
/* lock integrator until control is started */
bool lock_integrator;
if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
lock_integrator = false;
} else {
lock_integrator = true;
}
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[7] = 1.0f;
//warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
_actuators_airframe.control[7] = 0.0f;
//warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
continue;
}
/* default flaps to center */
float flaps_control = 0.0f;
/* map flaps by default to manual if valid */
if (isfinite(_manual.flaps)) {
flaps_control = _manual.flaps;
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
float yaw_manual = 0.0f;
float throttle_sp = 0.0f;
/* Read attitude setpoint from uorb if
* - velocity control or position control is enabled (pos controller is running)
* - manual control is disabled (another app may send the setpoint, but it should
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_auto_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
+ _parameters.rollsp_offset_rad;
}
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
/*
* Velocity should be controlled and manual is enabled.
*/
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do around 45 degrees, the mapping is
* -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
/* allow manual control of rudder deflection */
yaw_manual = _manual.r;
throttle_sp = _manual.z;
/*
* in manual mode no external source should / does emit attitude setpoints.
* emit the manual setpoint here to allow attitude controller tuning
* in attitude control mode.
*/
struct vehicle_attitude_setpoint_s att_sp;
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = roll_sp;
att_sp.pitch_body = pitch_sp;
att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
}
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
control_input.acc_body_x = _accel.x;
control_input.acc_body_y = _accel.y;
control_input.acc_body_z = _accel.z;
control_input.roll_setpoint = roll_sp;
control_input.pitch_setpoint = pitch_sp;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[2] += yaw_manual;
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_rate();
_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
_rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
} else if (_rates_sp_id) {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
}
} else {
/* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll;
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch;
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control;
_actuators.control[5] = _manual.aux1;
_actuators.control[6] = _manual.aux2;
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled)
{
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub != nullptr) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}
}
}
loop_counter++;
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_task_running = false;
_exit(0);
}
void
PX4IO_serial_f7::_do_interrupt()
{
uint32_t sr = rISR; /* get UART status register */
if (sr & USART_ISR_RXNE) {
(void)rRDR; /* read DR to clear RXNE */
}
rICR = sr & rISR_ERR_FLAGS_MASK; /* clear flags */
if (sr & (USART_ISR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
USART_ISR_NF | /* noise error - we have lost a byte due to noise */
USART_ISR_FE)) { /* framing error - start/stop bit lost or line break */
/*
* If we are in the process of listening for something, these are all fatal;
* abort the DMA with an error.
*/
if (_rx_dma_status == _dma_status_waiting) {
_abort_dma();
perf_count(_pc_uerrs);
/* complete DMA as though in error */
_do_rx_dma_callback(DMA_STATUS_TEIF);
return;
}
/* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */
/* don't attempt to handle IDLE if it's set - things went bad */
return;
}
if (sr & USART_ISR_IDLE) {
/* if there is DMA reception going on, this is a short packet */
if (_rx_dma_status == _dma_status_waiting) {
/* Invalidate _current_packet, so we get fresh data from RAM */
arch_invalidate_dcache((uintptr_t)_current_packet,
(uintptr_t)_current_packet + ALIGNED_IO_BUFFER_SIZE);
/* verify that the received packet is complete */
size_t length = sizeof(*_current_packet) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(*_current_packet))) {
perf_count(_pc_badidle);
/* stop the receive DMA */
stm32_dmastop(_rx_dma);
/* complete the short reception */
_do_rx_dma_callback(DMA_STATUS_TEIF);
return;
}
perf_count(_pc_idle);
/* stop the receive DMA */
stm32_dmastop(_rx_dma);
/* complete the short reception */
_do_rx_dma_callback(DMA_STATUS_TCIF);
}
}
}
int
SF0X::collect()
{
int ret;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
DEVICE_DEBUG("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
return ret;
} else {
return -EAGAIN;
}
} else if (ret == 0) {
return -EAGAIN;
}
_last_read = hrt_absolute_time();
float distance_m = -1.0f;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
valid = true;
}
}
if (!valid) {
return -EAGAIN;
}
DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
FXOS8700CQ::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd[2];
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
int16_t mx;
int16_t my;
int16_t mz;
} raw_accel_mag_report;
#pragma pack(pop)
accel_report accel_report;
/* start the performance counter */
perf_begin(_accel_sample_perf);
check_registers();
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
_register_wait--;
perf_end(_accel_sample_perf);
return;
}
/* fetch data from the sensor */
memset(&raw_accel_mag_report, 0, sizeof(raw_accel_mag_report));
raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8700CQ_DR_STATUS);
raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8700CQ_DR_STATUS);
transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report));
if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) {
perf_end(_accel_sample_perf);
perf_count(_accel_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
accel_report.timestamp = hrt_absolute_time();
/*
* Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
* Temperature data is only valid between –40 °C and 125 °C. The temperature sensor
* output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the
* temperature sensor is uncalibrated and its output for a given temperature will vary from
* one device to the next
*/
write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_A | M_CTRL_REG1_OS(7));
_last_temperature = (read_reg(FXOS8700CQ_TEMP)) * 0.96f;
write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7));
accel_report.temperature = _last_temperature;
// report the error count as the sum of the number of bad
// register reads and bad values. This allows the higher level
// code to decide if it should use this sensor based on
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x);
accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y);
accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z);
/* Save off the Mag readings todo: revist integrating theses */
_last_raw_mag_x = swap16(raw_accel_mag_report.mx);
_last_raw_mag_y = swap16(raw_accel_mag_report.my);
_last_raw_mag_z = swap16(raw_accel_mag_report.mz);
float xraw_f = accel_report.x_raw;
float yraw_f = accel_report.y_raw;
float zraw_f = accel_report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
/*
we have logs where the accelerometers get stuck at a fixed
large value. We want to detect this and mark the sensor as
being faulty
*/
if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
fabsf(_last_accel[1] - y_in_new) < 0.001f &&
fabsf(_last_accel[2] - z_in_new) < 0.001f &&
fabsf(x_in_new) > 20 &&
fabsf(y_in_new) > 20 &&
fabsf(z_in_new) > 20) {
_constant_accel_count += 1;
} else {
_constant_accel_count = 0;
}
if (_constant_accel_count > 100) {
// we've had 100 constant accel readings with large
// values. The sensor is almost certainly dead. We
// will raise the error_count so that the top level
// flight code will know to avoid this sensor, but
// we'll still give the data so that it can be logged
// and viewed
perf_count(_bad_values);
_constant_accel_count = 0;
}
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
accel_report.x_integral = aval_integrated(0);
accel_report.y_integral = aval_integrated(1);
accel_report.z_integral = aval_integrated(2);
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
/* return device ID */
accel_report.device_id = _device_id.devid;
_accel_reports->force(&accel_report);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
}
_accel_read++;
/* stop the perf counter */
perf_end(_accel_sample_perf);
}
void DataFlash_File::_io_timer(void)
{
uint16_t _tail;
if (_write_fd == -1 || !_initialised || _open_error) {
return;
}
uint16_t nbytes = BUF_AVAILABLE(_writebuf);
if (nbytes == 0) {
return;
}
uint32_t tnow = hal.scheduler->micros();
if (nbytes < _writebuf_chunk &&
tnow - _last_write_time < 2000000UL) {
// write in 512 byte chunks, but always write at least once
// per 2 seconds if data is available
return;
}
perf_begin(_perf_write);
_last_write_time = tnow;
if (nbytes > _writebuf_chunk) {
// be kind to the FAT PX4 filesystem
nbytes = _writebuf_chunk;
}
if (_writebuf_head > _tail) {
// only write to the end of the buffer
nbytes = min(nbytes, _writebuf_size - _writebuf_head);
}
// try to align writes on a 512 byte boundary to avoid filesystem
// reads
if ((nbytes + _write_offset) % 512 != 0) {
uint32_t ofs = (nbytes + _write_offset) % 512;
if (ofs < nbytes) {
nbytes -= ofs;
}
}
assert(_writebuf_head+nbytes <= _writebuf_size);
ssize_t nwritten = ::write(_write_fd, &_writebuf[_writebuf_head], nbytes);
if (nwritten <= 0) {
perf_count(_perf_errors);
close(_write_fd);
_write_fd = -1;
_initialised = false;
} else {
_write_offset += nwritten;
/*
the best strategy for minimising corruption on microSD cards
seems to be to write in 4k chunks and fsync the file on each
chunk, ensuring the directory entry is updated after each
write.
*/
BUF_ADVANCEHEAD(_writebuf, nwritten);
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
::fsync(_write_fd);
#endif
}
perf_end(_perf_write);
}
int
PX4IO_serial_f4::_bus_exchange(IOPacket *_packet)
{
_current_packet = _packet;
/* clear any lingering error status */
(void)rSR;
(void)rDR;
/* start RX DMA */
perf_begin(_pc_txns);
perf_begin(_pc_dmasetup);
/* DMA setup time ~3µs */
_rx_dma_status = _dma_status_waiting;
/*
* Note that we enable circular buffer mode as a workaround for
* there being no API to disable the DMA FIFO. We need direct mode
* because otherwise when the line idle interrupt fires there
* will be packet bytes still in the DMA FIFO, and we will assume
* that the idle was spurious.
*
* XXX this should be fixed with a NuttX change.
*/
stm32_dmasetup(
_rx_dma,
PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
reinterpret_cast<uint32_t>(_current_packet),
sizeof(*_current_packet),
DMA_SCR_CIRC | /* XXX see note above */
DMA_SCR_DIR_P2M |
DMA_SCR_MINC |
DMA_SCR_PSIZE_8BITS |
DMA_SCR_MSIZE_8BITS |
DMA_SCR_PBURST_SINGLE |
DMA_SCR_MBURST_SINGLE);
stm32_dmastart(_rx_dma, _dma_callback, this, false);
rCR3 |= USART_CR3_DMAR;
/* start TX DMA - no callback if we also expect a reply */
/* DMA setup time ~3µs */
stm32_dmasetup(
_tx_dma,
PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
reinterpret_cast<uint32_t>(_current_packet),
PKT_SIZE(*_current_packet),
DMA_SCR_DIR_M2P |
DMA_SCR_MINC |
DMA_SCR_PSIZE_8BITS |
DMA_SCR_MSIZE_8BITS |
DMA_SCR_PBURST_SINGLE |
DMA_SCR_MBURST_SINGLE);
stm32_dmastart(_tx_dma, nullptr, nullptr, false);
//rCR1 &= ~USART_CR1_TE;
//rCR1 |= USART_CR1_TE;
rCR3 |= USART_CR3_DMAT;
perf_end(_pc_dmasetup);
/* compute the deadline for a 10ms timeout */
struct timespec abstime;
clock_gettime(CLOCK_REALTIME, &abstime);
abstime.tv_nsec += 10 * 1000 * 1000;
if (abstime.tv_nsec >= 1000 * 1000 * 1000) {
abstime.tv_sec++;
abstime.tv_nsec -= 1000 * 1000 * 1000;
}
/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
int ret;
for (;;) {
ret = sem_timedwait(&_completion_semaphore, &abstime);
if (ret == OK) {
/* check for DMA errors */
if (_rx_dma_status & DMA_STATUS_TEIF) {
perf_count(_pc_dmaerrs);
ret = -EIO;
break;
}
/* check packet CRC - corrupt packet errors mean IO receive CRC error */
uint8_t crc = _current_packet->crc;
_current_packet->crc = 0;
if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) {
perf_count(_pc_crcerrs);
ret = -EIO;
break;
}
/* successful txn (may still be reporting an error) */
break;
}
if (errno == ETIMEDOUT) {
/* something has broken - clear out any partial DMA state and reconfigure */
_abort_dma();
perf_count(_pc_timeouts);
perf_cancel(_pc_txns); /* don't count this as a transaction */
break;
}
/* we might? see this for EINTR */
syslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno);
}
/* reset DMA status */
_rx_dma_status = _dma_status_inactive;
/* update counters */
perf_end(_pc_txns);
return ret;
}
int
MEASAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[4] = {0, 0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint8_t status = (val[0] & 0xC0) >> 6;
switch (status) {
case 0:
break;
case 1:
/* fallthrough */
case 2:
/* fallthrough */
case 3:
perf_count(_comms_errors);
perf_end(_sample_perf);
return -EAGAIN;
}
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
/* mask the used bits */
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200.0f * dT_raw) / 2047) - 50;
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative
const float P_min = -1.0f;
const float P_max = 1.0f;
const float PSI_to_Pa = 6894.757f;
/*
this equation is an inversion of the equation in the
pressure transfer function figure on page 4 of the datasheet
We negate the result so that positive differential pressures
are generated when the bottom port is used as the static
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
/*
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa_raw;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub != nullptr && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(_err_perf);
return;
}
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// set dt for all child blocks
setDt(dt);
// auto-detect connected rangefinders while not armed
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
detectDistanceSensors();
}
// reset pos, vel, and terrain on arming
// XXX this will be re-enabled for indoor use cases using a
// selection param, but is really not helping outdoors
// right now.
// if (!_lastArmedState && armedState) {
// // we just armed, we are at origin on the ground
// _x(X_x) = 0;
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
// _x(X_vz) = 0;
// // assume we are on the ground, so terrain alt is local alt
// _x(X_tz) = _x(X_z);
// // reset lowpass filter as well
// _xLowPass.setState(_x);
// _aglLowPass.setState(0);
// }
_lastArmedState = armedState;
// see which updates are available
bool flowUpdated = _sub_flow.updated();
bool paramsUpdated = _sub_param_update.updated();
bool baroUpdated = _sub_sensor.updated();
bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
bool mocapUpdated = _sub_mocap.updated();
bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
bool landUpdated = (
(_sub_land.get().landed ||
((!_sub_armed.get().armed) && (!_sub_land.get().freefall)))
&& (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized))
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE));
// get new data
updateSubscriptions();
// update parameters
if (paramsUpdated) {
updateParams();
updateSSParams();
}
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
vxy_stddev_ok = true;
}
if (_validXY) {
// if valid and gps has timed out, set to not valid
if (!vxy_stddev_ok && !_gpsInitialized) {
_validXY = false;
}
} else {
if (vxy_stddev_ok) {
if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
_validXY = true;
}
}
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
if (_validZ) {
// if valid and baro has timed out, set to not valid
if (!z_stddev_ok && !_baroInitialized) {
_validZ = false;
}
} else {
if (z_stddev_ok) {
_validZ = true;
}
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_validTZ) {
if (!tz_stddev_ok) {
_validTZ = false;
}
} else {
if (tz_stddev_ok) {
_validTZ = true;
}
}
// timeouts
if (_validXY) {
_time_last_xy = _timeStamp;
}
if (_validZ) {
_time_last_z = _timeStamp;
}
if (_validTZ) {
_time_last_tz = _timeStamp;
}
// check timeouts
checkTimeouts();
// if we have no lat, lon initialize projection at 0,0
if (_validXY && !_map_ref.init_done) {
map_projection_init(&_map_ref,
_init_origin_lat.get(),
_init_origin_lon.get());
}
// reinitialize x if necessary
bool reinit_x = false;
for (int i = 0; i < n_x; i++) {
// should we do a reinit
// of sensors here?
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
break;
}
}
if (reinit_x) {
for (int i = 0; i < n_x; i++) {
_x(i) = 0;
}
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
}
// force P symmetry and reinitialize P if necessary
bool reinit_P = false;
for (int i = 0; i < n_x; i++) {
for (int j = 0; j <= i; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit_P = true;
}
if (i == j) {
// make sure diagonal elements are positive
if (_P(i, i) <= 0) {
reinit_P = true;
}
} else {
// copy elememnt from upper triangle to force
// symmetry
_P(j, i) = _P(i, j);
}
if (reinit_P) { break; }
}
if (reinit_P) { break; }
}
if (reinit_P) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
initP();
}
// do prediction
predict();
// sensor corrections/ initializations
if (gpsUpdated) {
if (!_gpsInitialized) {
gpsInit();
} else {
gpsCorrect();
}
}
if (baroUpdated) {
if (!_baroInitialized) {
baroInit();
} else {
baroCorrect();
}
}
if (lidarUpdated) {
if (!_lidarInitialized) {
lidarInit();
} else {
lidarCorrect();
}
}
if (sonarUpdated) {
if (!_sonarInitialized) {
sonarInit();
} else {
sonarCorrect();
}
}
if (flowUpdated) {
if (!_flowInitialized) {
flowInit();
} else {
perf_begin(_loop_perf);// TODO
flowCorrect();
//perf_count(_interval_perf);
perf_end(_loop_perf);
}
}
if (visionUpdated) {
if (!_visionInitialized) {
visionInit();
} else {
visionCorrect();
}
}
if (mocapUpdated) {
if (!_mocapInitialized) {
mocapInit();
} else {
mocapCorrect();
}
}
if (landUpdated) {
if (!_landInitialized) {
landInit();
} else {
landCorrect();
}
}
if (_altOriginInitialized) {
// update all publications if possible
publishLocalPos();
publishEstimatorStatus();
_pub_innov.update();
if (_validXY) {
publishGlobalPos();
}
}
// propagate delayed state, no matter what
// if state is frozen, delayed state still
// needs to be propagated with frozen state
float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);
if (_time_last_hist == 0 ||
(dt_hist > HIST_STEP)) {
_tDelay.update(Scalar<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
}
int
user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
while (true) {
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}
void
Mavlink::count_txerr()
{
perf_count(_txerr_perf);
}
void
FixedwingAttitudeControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
orb_set_interval(_att_sub, 17);
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].events = POLLIN;
_task_running = true;
while (!_task_should_exit) {
static int loop_counter = 0;
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
vehicle_status_poll();
/* lock integrator until control is started */
bool lock_integrator;
if (_vcontrol_mode.flag_control_attitude_enabled) {
lock_integrator = false;
} else {
lock_integrator = true;
}
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[1] = 1.0f;
// warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
_actuators_airframe.control[1] = 0.0f;
// warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
float throttle_sp = 0.0f;
/* Read attitude setpoint from uorb if
* - velocity control or position control is enabled (pos controller is running)
* - manual control is disabled (another app may send the setpoint, but it should
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_velocity_enabled ||
_vcontrol_mode.flag_control_position_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do around 45 degrees, the mapping is
* -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+ _parameters.pitchsp_offset_rad;
throttle_sp = _manual.z;
_actuators.control[4] = _manual.flaps;
/*
* in manual mode no external source should / does emit attitude setpoints.
* emit the manual setpoint here to allow attitude controller tuning
* in attitude control mode.
*/
struct vehicle_attitude_setpoint_s att_sp;
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = roll_sp;
att_sp.pitch_body = pitch_sp;
att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_status.condition_landed) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
_roll_ctrl.control_attitude(roll_sp, _att.roll);
_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
speed_body_u, speed_body_v, speed_body_w,
_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
_att.rollspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_pitch_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = _yaw_ctrl.get_desired_rate();
rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
}
} else {
/* manual/direct control */
_actuators.control[0] = _manual.y;
_actuators.control[1] = -_manual.x;
_actuators.control[2] = _manual.r;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
}
_actuators.control[5] = _manual.aux1;
_actuators.control[6] = _manual.aux2;
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
/* advertise and publish */
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
if (_actuators_1_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
} else {
/* advertise and publish */
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
}
}
loop_counter++;
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_task_running = false;
_exit(0);
}
void
AirspeedSim::new_report(const differential_pressure_s &report)
{
if (!_reports->force(&report))
perf_count(_buffer_overflows);
}
int
SF0X::collect()
{
int ret;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
debug("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
return ret;
} else {
return -EAGAIN;
}
} else if (ret == 0) {
return -EAGAIN;
}
_last_read = hrt_absolute_time();
float si_units;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
valid = true;
}
}
if (!valid) {
return -EAGAIN;
}
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
/* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
}
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
if (_coordinated_method == COORD_METHOD_CLOSEACC) {
//XXX: filtering of acceleration?
_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
}
/* Transform estimation to body angular rates (jacobian) */
float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
ctl_data.scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
return math::constrain(_last_output, -1.0f, 1.0f);
}
