aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.cpp78
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.hpp11
-rw-r--r--src/modules/commander/airspeed_calibration.cpp44
-rw-r--r--src/modules/commander/commander.cpp17
-rw-r--r--src/modules/commander/mag_calibration.cpp29
-rw-r--r--src/modules/fixedwing_backside/fixedwing.cpp23
-rw-r--r--src/modules/fixedwing_backside/fixedwing.hpp14
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp67
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp65
-rw-r--r--src/modules/mavlink/missionlib.c1
-rw-r--r--src/modules/mavlink/orb_listener.c28
-rw-r--r--src/modules/multirotor_att_control/multirotor_rate_control.c52
-rw-r--r--src/modules/multirotor_pos_control/multirotor_pos_control.c5
-rw-r--r--src/modules/sensors/sensors.cpp1
-rw-r--r--src/modules/systemlib/airspeed.c2
-rw-r--r--src/modules/uORB/topics/differential_pressure.h4
-rw-r--r--src/modules/uORB/uORB.cpp4
17 files changed, 271 insertions, 174 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 33879892e..ecca04dd7 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -34,13 +34,14 @@
/**
* @file KalmanNav.cpp
*
- * kalman filter navigation code
+ * Kalman filter navigation code
*/
#include <poll.h>
#include "KalmanNav.hpp"
#include <systemlib/err.h>
+#include <geo/geo.h>
// constants
// Titterton pg. 52
@@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// attitude representations
C_nb(),
q(),
- _accel_sub(-1),
- _gyro_sub(-1),
- _mag_sub(-1),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
+ _localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
@@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
+ lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
@@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
lon = 0.0f;
alt = 0.0f;
- // gyro, accel and mag subscriptions
- _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
- _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
- _mag_sub = orb_subscribe(ORB_ID(sensor_mag));
-
- struct accel_report accel;
- orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
-
- struct mag_report mag;
- orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
-
// initialize rotation quaternion with a single raw sensor measurement
- q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
+ _sensors.update();
+ q = init(
+ _sensors.accelerometer_m_s2[0],
+ _sensors.accelerometer_m_s2[1],
+ _sensors.accelerometer_m_s2[2],
+ _sensors.magnetometer_ga[0],
+ _sensors.magnetometer_ga[1],
+ _sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = Dcm(q);
@@ -228,10 +224,7 @@ void KalmanNav::update()
updateSubscriptions();
// initialize attitude when sensors online
- if (!_attitudeInitialized && sensorsUpdate &&
- _sensors.accelerometer_counter > 10 &&
- _sensors.gyro_counter > 10 &&
- _sensors.magnetometer_counter > 10) {
+ if (!_attitudeInitialized && sensorsUpdate) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
@@ -255,11 +248,21 @@ void KalmanNav::update()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
+ // set reference position for
+ // local position
+ lat0 = lat;
+ lon0 = lon;
+ alt0 = alt;
+ // XXX map_projection has internal global
+ // states that multiple things could change,
+ // should make map_projection take reference
+ // lat/lon and not have init
+ map_projection_init(lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS\n");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
- lat, lon, alt);
+ lat, lon, double(alt));
}
// prediction step
@@ -314,7 +317,7 @@ void KalmanNav::updatePublications()
{
using namespace math;
- // position publication
+ // global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.valid = true;
@@ -327,6 +330,31 @@ void KalmanNav::updatePublications()
_pos.vz = vD;
_pos.yaw = psi;
+ // local position publication
+ float x;
+ float y;
+ bool landed = alt < (alt0 + 0.1); // XXX improve?
+ map_projection_project(lat, lon, &x, &y);
+ _localPos.timestamp = _pubTimeStamp;
+ _localPos.xy_valid = true;
+ _localPos.z_valid = true;
+ _localPos.v_xy_valid = true;
+ _localPos.v_z_valid = true;
+ _localPos.x = x;
+ _localPos.y = y;
+ _localPos.z = alt0 - alt;
+ _localPos.vx = vN;
+ _localPos.vy = vE;
+ _localPos.vz = vD;
+ _localPos.yaw = psi;
+ _localPos.xy_global = true;
+ _localPos.z_global = true;
+ _localPos.ref_timestamp = _pubTimeStamp;
+ _localPos.ref_lat = getLatDegE7();
+ _localPos.ref_lon = getLonDegE7();
+ _localPos.ref_alt = 0;
+ _localPos.landed = landed;
+
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
@@ -350,8 +378,10 @@ void KalmanNav::updatePublications()
// selectively update publications,
// do NOT call superblock do-all method
- if (_positionInitialized)
+ if (_positionInitialized) {
_pos.update();
+ _localPos.update();
+ }
if (_attitudeInitialized)
_att.update();
@@ -643,7 +673,7 @@ int KalmanNav::correctAtt()
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
- warnx("y:\n"); y.print();
+ warnx("y:"); y.print();
}
// update quaternions from euler
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 799fc2fb9..a69bde1a6 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
@@ -142,12 +143,8 @@ protected:
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
+ control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
-
- int _accel_sub; /**< Accelerometer subscription */
- int _gyro_sub; /**< Gyroscope subscription */
- int _mag_sub; /**< Magnetometer subscription */
-
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
@@ -164,8 +161,10 @@ protected:
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon; /**< lat, lon radians */
- float alt; /**< altitude, meters */
// parameters
+ float alt; /**< altitude, meters */
+ double lat0, lon0; /**< reference latitude and longitude */
+ float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 248eb4a66..1809f9688 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -37,12 +37,15 @@
*/
#include "airspeed_calibration.h"
+#include "calibration_messages.h"
#include "commander_helper.h"
#include <stdio.h>
+#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
@@ -55,10 +58,13 @@
#endif
static const int ERROR = -1;
+static const char *sensor_name = "dpress";
+
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
- mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+ mavlink_log_info(mavlink_fd, "don't move system");
const int calibration_count = 2500;
@@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
int calibration_counter = 0;
float diff_pres_offset = 0.0f;
+ /* Reset sensor parameters */
+ struct airspeed_scale airscale = {
+ 0.0f,
+ 1.0f,
+ };
+
+ bool paramreset_successful = false;
+ int fd = open(AIRSPEED_DEVICE_PATH, 0);
+ if (fd > 0) {
+ if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+ paramreset_successful = true;
+ }
+ close(fd);
+ }
+
+ if (!paramreset_successful) {
+ warn("WARNING: failed to set scale / offsets for airspeed sensor");
+ mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+ mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+ return ERROR;
+ }
+
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
+ if (calibration_counter % (calibration_count / 20) == 0)
+ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
- mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+ mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
@@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
- mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+ mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
@@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
- mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+ mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
- mavlink_log_info(mavlink_fd, "airspeed calibration done");
+ mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+ tune_neutral();
close(diff_pres_sub);
return OK;
} else {
- mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+ mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ed090271c..ace13bb78 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -116,6 +116,8 @@ extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+#define MAVLINK_OPEN_INTERVAL 50000
+
#define STICK_ON_OFF_LIMIT 0.75f
#define STICK_THRUST_RANGE 1.0f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
@@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
- if (mavlink_fd < 0) {
- /* try again later */
- usleep(20000);
- mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
- if (mavlink_fd < 0) {
- warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
- }
- }
-
/* Main state machine */
/* make sure we are in preflight state */
memset(&status, 0, sizeof(status));
@@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
+ if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
+ /* try to open the mavlink log device every once in a while */
+ mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+ }
+
/* update parameters */
orb_check(param_changed_sub, &updated);
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 09f4104f8..4ebf266f4 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd)
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
- unsigned int calibration_counter = 0;
+ unsigned int calibration_counter;
struct mag_scale mscale_null = {
0.0f,
@@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd)
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
- mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
+ mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
+ /* this is non-fatal - mark it accordingly */
+ res = OK;
}
}
close(fd);
- float *x;
- float *y;
- float *z;
+ float *x = NULL;
+ float *y = NULL;
+ float *z = NULL;
if (res == OK) {
/* allocate memory */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
- x = (float *)malloc(sizeof(float) * calibration_maxcount);
- y = (float *)malloc(sizeof(float) * calibration_maxcount);
- z = (float *)malloc(sizeof(float) * calibration_maxcount);
+ x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+ y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+ z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
res = ERROR;
+ return res;
}
+ } else {
+ /* exit */
+ return ERROR;
}
if (res == OK) {
@@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
+ calibration_counter = 0;
+
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
@@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd)
float sphere_radius;
if (res == OK) {
+
/* sphere fit */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
@@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd)
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
-
- return res;
}
+
+ return res;
}
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index d65045d68..6dc19df41 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
- if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here?
+ if (_status.main_state == MAIN_STATE_AUTO) {
+ // TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
@@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position
// handle autopilot modes
- if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
- _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
+ if (_status.main_state == MAIN_STATE_AUTO) {
- // update guidance
- _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
-
- // calculate velocity, XXX should be airspeed, but using ground speed for now
- // for the purpose of control we will limit the velocity feedback between
+ // calculate velocity, XXX should be airspeed,
+ // but using ground speed for now for the purpose
+ // of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
@@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update()
// a first binary release can be targeted.
// This is not a hack, but a design choice.
- /* do not limit in HIL */
+ // do not limit in HIL
if (_status.hil_state != HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
- } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here?
+ } else if (_status.main_state == MAIN_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
- } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
+
+ } else if (_status.main_state == MAIN_STATE_SEATBELT ||
+ _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
+
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index 3876e4630..567efeb35 100644
--- a/src/modules/fixedwing_backside/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -255,13 +255,13 @@ private:
BlockWaypointGuidance _guide;
// block params
- BlockParam<float> _trimAil;
- BlockParam<float> _trimElv;
- BlockParam<float> _trimRdr;
- BlockParam<float> _trimThr;
- BlockParam<float> _trimV;
- BlockParam<float> _vCmd;
- BlockParam<float> _crMax;
+ BlockParamFloat _trimAil;
+ BlockParamFloat _trimElv;
+ BlockParamFloat _trimRdr;
+ BlockParamFloat _trimThr;
+ BlockParamFloat _trimV;
+ BlockParamFloat _vCmd;
+ BlockParamFloat _crMax;
struct pollfd _attPoll;
vehicle_global_position_set_triplet_s _lastPosCmd;
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index eb67874db..00a0dcd61 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_i = param_find("FW_P_I");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
- _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max");
+ _parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_R_P");
_parameter_handles.r_d = param_find("FW_R_D");
_parameter_handles.r_i = param_find("FW_R_I");
- _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
+ _parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_Y_P");
_parameter_handles.y_i = param_find("FW_Y_I");
_parameter_handles.y_d = param_find("FW_Y_D");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
- _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
+ _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main()
}
}
- float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
- airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
- _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+ if (isfinite(roll_sp) && isfinite(pitch_sp)) {
- float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
- lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
- _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+ float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
+ airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+ _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
- float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
- _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
- _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+ float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
+ lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+ _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
- /* throttle passed through */
- _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+ float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
+ _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+ _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
- // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
- // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
- // _actuators.control[2], _actuators.control[3]);
+ /* throttle passed through */
+ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
- /*
- * 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 = 0.0f; // XXX not yet implemented
+ // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
+ // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
+ // _actuators.control[2], _actuators.control[3]);
- rates_sp.timestamp = hrt_absolute_time();
+ /*
+ * 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 = 0.0f; // XXX not yet implemented
- if (_rate_sp_pub > 0) {
- /* publish the attitude setpoint */
- orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+ rates_sp.timestamp = hrt_absolute_time();
- } else {
- /* advertise and publish */
- _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+ 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 {
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c51a6de08..bfccd5fb0 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -72,6 +72,7 @@
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
#include <commander/px4_custom_mode.h>
+#include <geo/geo.h>
__BEGIN_DECLS
@@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd;
static struct offboard_control_setpoint_s offboard_control_sp;
struct vehicle_global_position_s hil_global_pos;
+struct vehicle_local_position_s hil_local_pos;
struct vehicle_attitude_s hil_attitude;
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
struct battery_status_s hil_battery_status;
static orb_advert_t pub_hil_global_pos = -1;
+static orb_advert_t pub_hil_local_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
static orb_advert_t pub_hil_sensors = -1;
@@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1;
static orb_advert_t vicon_position_pub = -1;
static orb_advert_t telemetry_status_pub = -1;
+// variables for HIL reference position
+static int32_t lat0 = 0;
+static int32_t lon0 = 0;
+static double alt0 = 0;
+
static void
handle_message(mavlink_message_t *msg)
{
@@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
- hil_global_pos.valid = true;
- hil_global_pos.lat = hil_state.lat;
- hil_global_pos.lon = hil_state.lon;
- hil_global_pos.alt = hil_state.alt / 1000.0f;
- hil_global_pos.vx = hil_state.vx / 100.0f;
- hil_global_pos.vy = hil_state.vy / 100.0f;
- hil_global_pos.vz = hil_state.vz / 100.0f;
-
- /* set timestamp and notify processes (broadcast) */
- hil_global_pos.timestamp = hrt_absolute_time();
+ uint64_t timestamp = hrt_absolute_time();
+ // publish global position
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+ // global position packet
+ hil_global_pos.timestamp = timestamp;
+ hil_global_pos.valid = true;
+ hil_global_pos.lat = hil_state.lat;
+ hil_global_pos.lon = hil_state.lon;
+ hil_global_pos.alt = hil_state.alt / 1000.0f;
+ hil_global_pos.vx = hil_state.vx / 100.0f;
+ hil_global_pos.vy = hil_state.vy / 100.0f;
+ hil_global_pos.vz = hil_state.vz / 100.0f;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
}
+ // publish local position
+ if (pub_hil_local_pos > 0) {
+ float x;
+ float y;
+ bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
+ double lat = hil_state.lat*1e-7;
+ double lon = hil_state.lon*1e-7;
+ map_projection_project(lat, lon, &x, &y);
+ hil_local_pos.timestamp = timestamp;
+ hil_local_pos.xy_valid = true;
+ hil_local_pos.z_valid = true;
+ hil_local_pos.v_xy_valid = true;
+ hil_local_pos.v_z_valid = true;
+ hil_local_pos.x = x;
+ hil_local_pos.y = y;
+ hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
+ hil_local_pos.vx = hil_state.vx/100.0f;
+ hil_local_pos.vy = hil_state.vy/100.0f;
+ hil_local_pos.vz = hil_state.vz/100.0f;
+ hil_local_pos.yaw = hil_attitude.yaw;
+ hil_local_pos.xy_global = true;
+ hil_local_pos.z_global = true;
+ hil_local_pos.ref_timestamp = timestamp;
+ hil_local_pos.ref_lat = hil_state.lat;
+ hil_local_pos.ref_lon = hil_state.lon;
+ hil_local_pos.ref_alt = alt0;
+ hil_local_pos.landed = landed;
+ orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
+ } else {
+ pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+ lat0 = hil_state.lat;
+ lon0 = hil_state.lon;
+ alt0 = hil_state.alt / 1000.0f;
+ map_projection_init(hil_state.lat, hil_state.lon);
+ }
+
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index d37b18a19..fa23f996f 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -64,6 +64,7 @@
#include <systemlib/systemlib.h>
#include <mavlink/mavlink_log.h>
+#include "geo/geo.h"
#include "waypoints.h"
#include "orb_topics.h"
#include "missionlib.h"
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index f6860930c..abc91d34f 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -75,6 +75,7 @@ struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
+struct airspeed_s airspeed;
struct mavlink_subscriptions mavlink_subs;
@@ -92,6 +93,8 @@ static unsigned int gps_counter;
*/
static uint64_t last_sensor_timestamp;
+static hrt_abstime last_sent_vfr = 0;
+
static void *uorb_receive_thread(void *arg);
struct listener {
@@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l)
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
- if (gcs_link)
+ if (gcs_link) {
/* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
@@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l)
att.pitchspeed,
att.yawspeed);
+ /* limit VFR message rate to 10Hz */
+ hrt_abstime t = hrt_absolute_time();
+ if (t >= last_sent_vfr + 100000) {
+ last_sent_vfr = t;
+ float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+ uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
+ float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+ mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
+ }
+ }
+
attitude_counter++;
}
@@ -681,17 +695,7 @@ l_home(const struct listener *l)
void
l_airspeed(const struct listener *l)
{
- struct airspeed_s airspeed;
-
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
-
- float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
- uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
- float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
- float alt = global_pos.relative_alt;
- float climb = -global_pos.vz;
-
- mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
}
void
@@ -849,7 +853,7 @@ uorb_receive_start(void)
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
- /* --- AIRSPEED / VFR / HUD --- */
+ /* --- AIRSPEED --- */
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index adb63186c..86ac0e4ff 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -59,31 +59,23 @@
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
- //float yawrate_awu;
- //float yawrate_lim;
float attrate_p;
float attrate_d;
float attrate_i;
- //float attrate_awu;
- //float attrate_lim;
float rate_lim;
};
@@ -93,14 +85,10 @@ struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
- //param_t yawrate_awu;
- //param_t yawrate_lim;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
- //param_t attrate_awu;
- //param_t attrate_lim;
};
/**
@@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
- //h->yawrate_awu = param_find("MC_YAWRATE_AWU");
- //h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
- //h->attrate_awu = param_find("MC_ATTRATE_AWU");
- //h->attrate_lim = param_find("MC_ATTRATE_LIM");
return OK;
}
@@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
- //param_get(h->yawrate_awu, &(p->yawrate_awu));
- //param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
- //param_get(h->attrate_awu, &(p->attrate_awu));
- //param_get(h->attrate_lim, &(p->attrate_lim));
return OK;
}
@@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
+ static PID_t yaw_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
@@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
-
+ pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
@@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
+ pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
- // TODO pid_reset_integral(&yaw_rate_controller);
+ pid_reset_integral(&yaw_rate_controller);
}
- /* control pitch (forward) output */
- float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
- rates[1], 0.0f, deltaT);
-
- /* control roll (left/right) output */
- float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
- rates[0], 0.0f, deltaT);
-
- /* control yaw rate */ //XXX use library here
- float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
-
- /* increase resilience to faulty control inputs */
- if (!isfinite(yaw_rate_control)) {
- yaw_rate_control = 0.0f;
- warnx("rej. NaN ctrl yaw");
- }
+ /* run pitch, roll and yaw controllers */
+ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
+ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
+ float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
- actuators->control[2] = yaw_rate_control;
+ actuators->control[2] = yaw_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 36dd370fb..3d23d0c09 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
- att_sp.yaw_body = global_pos_sp.yaw;
+ /* update yaw setpoint only if value is valid */
+ if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
+ att_sp.yaw_body = global_pos_sp.yaw;
+ }
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index da0c11372..6d38b98ec 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced)
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
warn("WARNING: failed to set scale / offsets for airspeed sensor");
+ close(fd);
}
#if 0
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index 310fbf60f..1a479c205 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -59,7 +59,7 @@
float calc_indicated_airspeed(float differential_pressure)
{
- if (differential_pressure > 0) {
+ if (differential_pressure > 0.0f) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index e4d2c92ce..5d94d4288 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -54,8 +54,8 @@
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
- uint16_t differential_pressure_pa; /**< Differential pressure reading */
- uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
+ float differential_pressure_pa; /**< Differential pressure reading */
+ float max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor */
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 7abbf42ae..149b8f6d2 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp)
} else {
SubscriberData *sd = filp_to_sd(filp);
- if (sd != nullptr)
+ if (sd != nullptr) {
+ hrt_cancel(&sd->update_call);
delete sd;
+ }
}
return CDev::close(filp);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 13:46:33 +0000