diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 189cd5aff4..40a34dd8b5 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -40,11 +40,10 @@ set(msg_file_names airspeed.msg att_pos_mocap.msg battery_status.msg - camera_trigger.msg camera_capture.msg + camera_trigger.msg collision_report.msg commander_state.msg - control_state.msg cpuload.msg debug_key_value.msg debug_value.msg @@ -89,12 +88,13 @@ set(msg_file_names satellite_info.msg sensor_accel.msg sensor_baro.msg + sensor_bias.msg sensor_combined.msg sensor_correction.msg - sensor_selection.msg sensor_gyro.msg sensor_mag.msg sensor_preflight.msg + sensor_selection.msg servorail_status.msg subsystem_info.msg system_power.msg diff --git a/msg/control_state.msg b/msg/control_state.msg deleted file mode 100644 index be4ffd7b1c..0000000000 --- a/msg/control_state.msg +++ /dev/null @@ -1,28 +0,0 @@ -# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */ -uint8 AIRSPD_MODE_MEAS = 0 # airspeed is measured airspeed from sensor -uint8 AIRSPD_MODE_EST = 1 # airspeed is estimated by body velocity -uint8 AIRSPD_MODE_DISABLED = 2 # airspeed is disabled - -float32 x_acc # X acceleration in body frame -float32 y_acc # Y acceleration in body frame -float32 z_acc # Z acceleration in body frame -float32 x_vel # X velocity in body frame -float32 y_vel # Y velocity in body frame -float32 z_vel # Z velocity in body frame -float32 x_pos # X position in local earth frame -float32 y_pos # Y position in local earth frame -float32 z_pos # z position in local earth frame -float32 airspeed # Airspeed, estimated -bool airspeed_valid # False: Non-finite values or non-updating sensor -float32[3] vel_variance # Variance in body velocity estimate -float32[3] pos_variance # Variance in local position estimate -float32[4] q # Attitude Quaternion -float32[4] delta_q_reset # Amount by which quaternion has changed during last reset -uint8 quat_reset_counter # Quaternion reset counter -float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down) - corrected for bias -float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down) - corrected for bias -float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down) - corrected for bias -float32 horz_acc_mag # low pass filtered magnitude of the horizontal acceleration -float32 roll_rate_bias # Roll body angular rate bias (rad/s, x forward) - subtract from uncorrected gyro data -float32 pitch_rate_bias # Pitch body angular rate bias (rad/s, y right) - subtract from uncorrected gyro data -float32 yaw_rate_bias # Yaw body angular rate bias (rad/s, z down) - subtract from uncorrected gyro data diff --git a/msg/sensor_bias.msg b/msg/sensor_bias.msg new file mode 100644 index 0000000000..9197dc87bf --- /dev/null +++ b/msg/sensor_bias.msg @@ -0,0 +1,30 @@ +# +# Sensor readings and in-run biases in SI-unit form. Sensor readings are compensated for static offsets, +# scale errors, in-run bias and thermal drift (if thermal compensation is enabled and available). +# + +float32 gyro_x # Bias corrected angular velocity about X body axis (roll) (in rad/s) +float32 gyro_y # Bias corrected angular velocity about Y body axis (pitch) (in rad/s) +float32 gyro_z # Bias corrected angular velocity about Z body axis (yaw) (in rad/s) + +float32 accel_x # Bias corrected acceleration in body X axis (in rad/s) +float32 accel_y # Bias corrected acceleration in body Y axis (in rad/s) +float32 accel_z # Bias corrected acceleration in body Z axis (in rad/s) + +float32 mag_x # Bias corrected magnetometer reading in body X axis (in Gauss) +float32 mag_y # Bias corrected magnetometer reading in body Y axis (in Gauss) +float32 mag_z # Bias corrected magnetometer reading in body Z axis (in Gauss) + +# In-run bias estimates (subtract from uncorrected data) + +float32 gyro_x_bias # X gyroscope in-run bias in body frame (rad/s, x forward) +float32 gyro_y_bias # Y gyroscope in-run bias in body frame (rad/s, y right) +float32 gyro_z_bias # Z gyroscope in-run bias in body frame (rad/s, z down) + +float32 accel_x_bias # X accelerometer in-run bias in body frame (m/s^2, x forward) +float32 accel_y_bias # Y accelerometer in-run bias in body frame (m/s^2, y right) +float32 accel_z_bias # Z accelerometer in-run bias in body frame (m/s^2, z down) + +float32 mag_x_bias # X magnetometer in-run bias in body frame (Gauss, x forward) +float32 mag_y_bias # Y magnetometer in-run bias in body frame (Gauss, y right) +float32 mag_z_bias # Z magnetometer in-run bias in body frame (Gauss, z down) \ No newline at end of file diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 0f3827c027..4504da967d 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -1,7 +1,11 @@ # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use -float32 rollspeed # Angular velocity about body north axis (x) in rad/s -float32 pitchspeed # Angular velocity about body east axis (y) in rad/s -float32 yawspeed # Angular velocity about body down axis (z) in rad/s -float32[4] q # Quaternion (NED) + +float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s +float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s +float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s + +float32[4] q # Quaternion rotation from NED earth frame to XYZ body frame +float32[4] delta_q_reset # Amount by which quaternion has changed during last reset +uint8 quat_reset_counter # Quaternion reset counter # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index e1f4278cde..707e87882c 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -4,25 +4,33 @@ # estimator, which will take more sources of information into account than just GPS, # e.g. control inputs of the vehicle in a Kalman-filter implementation. # -uint64 time_utc_usec # GPS UTC timestamp, (microseconds) + float64 lat # Latitude, (degrees) float64 lon # Longitude, (degrees) float32 alt # Altitude AMSL, (meters) + float32 delta_alt # Reset delta for altitude uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates -uint8 alt_reset_counter # Counter for reset events on altitude +uint8 alt_reset_counter # Counter for reset events on altitude + float32 vel_n # North velocity in NED earth-fixed frame, (metres/sec) float32 vel_e # East velocity in NED earth-fixed frame, (metres/sec) float32 vel_d # Down velocity in NED earth-fixed frame, (metres/sec) + float32 pos_d_deriv # Down position time derivative in NED earth-fixed frame, (metres/sec) + float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) + float32 eph # Standard deviation of horizontal position error, (metres) float32 epv # Standard deviation of vertical position error, (metres) float32 evh # Standard deviation of horizontal velocity error, (metres/sec) float32 evv # Standard deviation of horizontal velocity error, (metres/sec) + float32 terrain_alt # Terrain altitude WGS84, (metres) bool terrain_alt_valid # Terrain altitude estimate is valid -bool dead_reckoning # True if this position is estimated through dead-reckoning + float32 pressure_alt # Pressure altitude AMSL, (metres) +bool dead_reckoning # True if this position is estimated through dead-reckoning + # TOPICS vehicle_global_position vehicle_global_position_groundtruth diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 40b7ab08a2..2deb285a9c 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,13 +1,5 @@ # Fused local position in NED. -uint8 ESTIMATOR_TYPE_NAIVE = 1 # Estimator without real covariance feedback -uint8 ESTIMATOR_TYPE_VISION = 2 # Computer vision based estimate. Might be up to scale. -uint8 ESTIMATOR_TYPE_VIO = 3 # Visual-Inertial estimator. -uint8 ESTIMATOR_TYPE_GPS = 4 # Plain GPS estimate. -uint8 ESTIMATOR_TYPE_GPS_INS = 5 # Estimator integrating GPS and inertial sensing. - -uint8 estimator_type # type of estimator according to above types - bool xy_valid # true if x and y are valid bool z_valid # true if z is valid bool v_xy_valid # true if vy and vy are valid @@ -20,7 +12,10 @@ float32 z # Down position (negative altitude) in NED earth-fixed frame, (metr # Position reset delta float32[2] delta_xy +uint8 xy_reset_counter + float32 delta_z +uint8 z_reset_counter # Velocity in NED frame float32 vx # North velocity in NED earth-fixed frame, (metres/sec) @@ -30,17 +25,10 @@ float32 z_deriv # Down position time derivative in NED earth-fixed frame, (me # Velocity reset delta float32[2] delta_vxy -float32 delta_vz - -uint8 xy_reset_counter -uint8 z_reset_counter uint8 vxy_reset_counter -uint8 vz_reset_counter -# Acceleration in NED frame -float32 ax # North acceleration in NED earth-fixed frame, (metres/sec^2) -float32 ay # East acceleration in NED earth-fixed frame, (metres/sec^2) -float32 az # Down acceleration in NED earth-fixed frame, (metres/sec^2) +float32 delta_vz +uint8 vz_reset_counter # Heading float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) @@ -56,8 +44,8 @@ float32 ref_alt # Reference altitude AMSL, MUST be set to current (not at ref # Distance to surface float32 dist_bottom # Distance from from bottom surface to ground, (metres) float32 dist_bottom_rate # Rate of change of distance from bottom surface to ground, (metres/sec) -uint64 surface_bottom_timestamp # Time when new bottom surface found since system start, (microseconds) bool dist_bottom_valid # true if distance to bottom surface is valid + float32 eph # Standard deviation of horizontal position error, (metres) float32 epv # Standard deviation of vertical position error, (metres) float32 evh # Standard deviation of horizontal velocity error, (metres/sec) diff --git a/msg/wind_estimate.msg b/msg/wind_estimate.msg index a61df3252a..b8e47a8875 100644 --- a/msg/wind_estimate.msg +++ b/msg/wind_estimate.msg @@ -1,4 +1,5 @@ -float32 windspeed_north # Wind component in north / X direction -float32 windspeed_east # Wind component in east / Y direction -float32 covariance_north # Uncertainty - set to zero (no uncertainty) if not estimated -float32 covariance_east # Uncertainty - set to zero (no uncertainty) if not estimated +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 3fb3c169b4..dd2b83ef98 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Stefan Rado + * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +33,7 @@ /** * @file frsky_data.c + * * @author Stefan Rado * * FrSky telemetry implementation. @@ -160,6 +160,7 @@ void frsky_update_topics() { struct frsky_subscription_data_s *subs = subscription_data; bool updated; + /* get a local copy of the current sensor values */ orb_check(subs->sensor_sub, &updated); @@ -167,12 +168,7 @@ void frsky_update_topics() orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &subs->sensor_combined); } - orb_check(subs->vehicle_gps_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_gps_position), subs->vehicle_gps_position_sub, &subs->vehicle_gps_position); - } - + /* get a local copy of the vehicle status */ orb_check(subs->vehicle_status_sub, &updated); if (updated) { @@ -194,6 +190,13 @@ void frsky_update_topics() if (updated) { orb_copy(ORB_ID(vehicle_global_position), subs->vehicle_global_position_sub, &subs->global_pos); } + + /* get a local copy of the raw GPS data */ + orb_check(subs->vehicle_gps_position_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_gps_position), subs->vehicle_gps_position_sub, &subs->vehicle_gps_position); + } } /** @@ -245,15 +248,13 @@ void frsky_send_frame2(int uart) { struct vehicle_global_position_s *global_pos = &subscription_data->global_pos; struct battery_status_s *battery_status = &subscription_data->battery_status; + struct vehicle_gps_position_s *gps = &subscription_data->vehicle_gps_position; /* send formatted frame */ float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; if (global_pos->timestamp != 0 && hrt_absolute_time() < global_pos->timestamp + 20000) { - time_t time_gps = global_pos->time_utc_usec / 1000000ULL; - struct tm *tm_gps = gmtime(&time_gps); - course = global_pos->yaw / M_PI_F * 180.0f; if (course < 0.f) { // course is in range [0, 360], 0=north, CW @@ -267,6 +268,12 @@ void frsky_send_frame2(int uart) speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e) * 25.0f / 46.0f; alt = global_pos->alt; + } + + if (gps->timestamp != 0 && hrt_absolute_time() < gps->timestamp + 20000) { + time_t time_gps = gps->time_utc_usec / 1000000ULL; + struct tm *tm_gps = gmtime(&time_gps); + sec = tm_gps->tm_sec; } @@ -302,7 +309,7 @@ void frsky_send_frame2(int uart) void frsky_send_frame3(int uart) { /* send formatted frame */ - time_t time_gps = subscription_data->global_pos.time_utc_usec / 1000000ULL; + time_t time_gps = subscription_data->vehicle_gps_position.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); diff --git a/src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index daba828139..ab997e08bf 100644 --- a/src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -155,14 +154,12 @@ private: int _armedSub; orb_advert_t _att_pub; /**< vehicle attitude */ - orb_advert_t _ctrl_state_pub; /**< control state */ orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct control_state_s _ctrl_state; /**< control state */ struct gyro_report _gyro; struct accel_report _accel; struct mag_report _mag; @@ -244,7 +241,7 @@ private: float magb_pnoise; float eas_noise; float pos_stddev_threshold; - int32_t airspeed_mode; + int32_t airspeed_disabled; } _parameters; /**< local copies of interesting parameters */ struct { @@ -266,7 +263,7 @@ private: param_t magb_pnoise; param_t eas_noise; param_t pos_stddev_threshold; - param_t airspeed_mode; + param_t airspeed_disabled; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -323,12 +320,6 @@ private: **/ void publishAttitude(); - /** - * @brief - * Publish the system state for control modules - **/ - void publishControlState(); - /** * @brief * Publish local position relative to reference point where filter was initialized diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 056964633c..24523db8e1 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -142,14 +142,12 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : /* publications */ _att_pub(nullptr), - _ctrl_state_pub(nullptr), _global_pos_pub(nullptr), _local_pos_pub(nullptr), _estimator_status_pub(nullptr), _wind_pub(nullptr), _att{}, - _ctrl_state{}, _gyro{}, _accel{}, _mag{}, @@ -248,7 +246,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); - _parameter_handles.airspeed_mode = param_find("FW_AIRSPD_MODE"); + _parameter_handles.airspeed_disabled = param_find("FW_AIRSPD_MODE"); /* indicate consumers that the current position data is not valid */ _gps.eph = 10000.0f; @@ -316,7 +314,7 @@ int AttitudePositionEstimatorEKF::parameters_update() param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold)); - param_get(_parameter_handles.airspeed_mode, &_parameters.airspeed_mode); + param_get(_parameter_handles.airspeed_disabled, &_parameters.airspeed_disabled); if (_ekf) { // _ekf->yawVarScale = 1.0f; @@ -715,9 +713,6 @@ void AttitudePositionEstimatorEKF::task_main() // Publish attitude estimations publishAttitude(); - // publish control state - publishControlState(); - // Publish Local Position estimations publishLocalPosition(); @@ -841,100 +836,6 @@ void AttitudePositionEstimatorEKF::publishAttitude() } } -void AttitudePositionEstimatorEKF::publishControlState() -{ - /* Accelerations in Body Frame */ - _ctrl_state.x_acc = _ekf->accel.x; - _ctrl_state.y_acc = _ekf->accel.y; - _ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13]; - - _ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal(); - - /* Velocity in Body Frame */ - Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); - Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]); - Vector3f v_b = _ekf->Tnb * v_n; - Vector3f v_b_var = _ekf->Tnb * v_n_var; - - _ctrl_state.x_vel = v_b.x; - _ctrl_state.y_vel = v_b.y; - _ctrl_state.z_vel = v_b.z; - - _ctrl_state.vel_variance[0] = v_b_var.x; - _ctrl_state.vel_variance[1] = v_b_var.y; - _ctrl_state.vel_variance[2] = v_b_var.z; - - /* Local Position */ - _ctrl_state.x_pos = _ekf->states[7]; - _ctrl_state.y_pos = _ekf->states[8]; - - // XXX need to announce change of Z reference somehow elegantly - _ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset; - - _ctrl_state.pos_variance[0] = _ekf->P[7][7]; - _ctrl_state.pos_variance[1] = _ekf->P[8][8]; - _ctrl_state.pos_variance[2] = _ekf->P[9][9]; - - /* Attitude */ - _ctrl_state.timestamp = _last_sensor_timestamp; - _ctrl_state.q[0] = _ekf->states[0]; - _ctrl_state.q[1] = _ekf->states[1]; - _ctrl_state.q[2] = _ekf->states[2]; - _ctrl_state.q[3] = _ekf->states[3]; - - // use estimated velocity for airspeed estimate - if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) { - // use measured airspeed - if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 - && _airspeed.timestamp > 0) { - _ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; - _ctrl_state.airspeed_valid = true; - } - - } else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_EST) { - if (_local_pos.v_xy_valid && _local_pos.v_z_valid) { - _ctrl_state.airspeed = sqrtf(_ekf->states[4] * _ekf->states[4] - + _ekf->states[5] * _ekf->states[5] + _ekf->states[6] * _ekf->states[6]); - _ctrl_state.airspeed_valid = true; - } - - } else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) { - // do nothing, airspeed has been declared as non-valid above, controllers - // will handle this assuming always trim airspeed - } - - /* Attitude Rates */ - _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; - _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; - _ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; - - /* Gyro bias estimates */ - _ctrl_state.roll_rate_bias = _ekf->states[10] / _ekf->dtIMUfilt; - _ctrl_state.pitch_rate_bias = _ekf->states[11] / _ekf->dtIMUfilt; - _ctrl_state.yaw_rate_bias = _ekf->states[12] / _ekf->dtIMUfilt; - - /* Guard from bad data */ - if (!PX4_ISFINITE(_ctrl_state.x_vel) || - !PX4_ISFINITE(_ctrl_state.y_vel) || - !PX4_ISFINITE(_ctrl_state.z_vel) || - !PX4_ISFINITE(_ctrl_state.x_pos) || - !PX4_ISFINITE(_ctrl_state.y_pos) || - !PX4_ISFINITE(_ctrl_state.z_pos)) { - // bad data, abort publication - return; - } - - /* lazily publish the control state only once available */ - if (_ctrl_state_pub != nullptr) { - /* publish the control state */ - orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state); - - } else { - /* advertise and publish */ - _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state); - } -} - void AttitudePositionEstimatorEKF::publishLocalPosition() { _local_pos.timestamp = _last_sensor_timestamp; @@ -998,12 +899,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; - _global_pos.time_utc_usec = _gps.time_utc_usec; } else { _global_pos.lat = 0.0; _global_pos.lon = 0.0; - _global_pos.time_utc_usec = 0; } if (_local_pos.v_xy_valid) { @@ -1087,10 +986,10 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; - // XXX we need to do something smart about the covariance here - // but we default to the estimate covariance for now - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; + // XXX we need to do something smart about the variance here + // but we default to the estimated variance for now + _wind.variance_north = _ekf->P[14][14]; + _wind.variance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub != nullptr) { diff --git a/src/examples/segway/CMakeLists.txt b/src/examples/segway/CMakeLists.txt index 9508503da3..4bc0533eaa 100644 --- a/src/examples/segway/CMakeLists.txt +++ b/src/examples/segway/CMakeLists.txt @@ -33,8 +33,8 @@ px4_add_module( MODULE examples__segway MAIN segway - STACK_MAIN 1300 - STACK_MAX 1300 + STACK_MAIN 1400 + STACK_MAX 1400 SRCS blocks.cpp segway_main.cpp diff --git a/src/examples/uuv_example_app/uuv_example_app.cpp b/src/examples/uuv_example_app/uuv_example_app.cpp index 7a3a763181..18c52cf5f0 100644 --- a/src/examples/uuv_example_app/uuv_example_app.cpp +++ b/src/examples/uuv_example_app/uuv_example_app.cpp @@ -64,7 +64,7 @@ #include #include // this topics hold the acceleration data #include // this topic gives the actuators control input -#include // this topic holds the orientation of the hippocampus +#include // this topic holds the orientation of the hippocampus extern "C" __EXPORT int uuv_example_app_main(int argc, char *argv[]); @@ -78,9 +78,9 @@ int uuv_example_app_main(int argc, char *argv[]) orb_set_interval(sensor_sub_fd, 200); /* subscribe to control_state topic */ - int ctrl_state_sub_fd = orb_subscribe(ORB_ID(control_state)); + int vehicle_attitude_sub_fd = orb_subscribe(ORB_ID(vehicle_attitude)); /* limit the update rate to 5 Hz */ - orb_set_interval(ctrl_state_sub_fd, 200); + orb_set_interval(vehicle_attitude_sub_fd, 200); /* advertise to actuator_control topic */ struct actuator_controls_s act; @@ -90,7 +90,7 @@ int uuv_example_app_main(int argc, char *argv[]) /* one could wait for multiple topics with this technique, just using one here */ px4_pollfd_struct_t fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, - { .fd = ctrl_state_sub_fd, .events = POLLIN }, + { .fd = vehicle_attitude_sub_fd, .events = POLLIN }, }; int error_counter = 0; @@ -127,9 +127,9 @@ int uuv_example_app_main(int argc, char *argv[]) (double)raw_sensor.accelerometer_m_s2[2]); /* obtained data for the third file descriptor */ - struct control_state_s raw_ctrl_state; + struct vehicle_attitude_s raw_ctrl_state; /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(control_state), ctrl_state_sub_fd, &raw_ctrl_state); + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub_fd, &raw_ctrl_state); // get current rotation matrix from control state quaternions, the quaternions are generated by the // attitude_estimator_q application using the sensor data diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index b266ee0ed5..d077e00524 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -65,12 +65,12 @@ struct crosstrack_error_s { /* lat/lon are in radians */ struct map_projection_reference_s { + uint64_t timestamp; double lat_rad; double lon_rad; double sin_lat; double cos_lat; bool init_done; - uint64_t timestamp; }; struct globallocal_converter_reference_s { diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d9c3b90803..8136e4e4c2 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -55,7 +55,6 @@ #include #include #include -#include #include #include #include @@ -125,7 +124,6 @@ private: int _airspeed_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; - orb_advert_t _ctrl_state_pub = nullptr; orb_advert_t _est_state_pub = nullptr; struct { @@ -138,7 +136,7 @@ private: param_t acc_comp; param_t bias_max; param_t ext_hdg_mode; - param_t airspeed_mode; + param_t airspeed_disabled; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; @@ -150,7 +148,7 @@ private: bool _acc_comp = false; float _bias_max = 0.0f; int _ext_hdg_mode = 0; - int _airspeed_mode = 0; + int _airspeed_disabled = 0; Vector<3> _gyro; Vector<3> _accel; @@ -176,9 +174,6 @@ private: math::LowPassFilter2p _lp_accel_x; math::LowPassFilter2p _lp_accel_y; math::LowPassFilter2p _lp_accel_z; - math::LowPassFilter2p _lp_gyro_x; - math::LowPassFilter2p _lp_gyro_y; - math::LowPassFilter2p _lp_gyro_z; hrt_abstime _vel_prev_t = 0; @@ -206,10 +201,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _pos_acc(0, 0, 0), _lp_accel_x(250.0f, 30.0f), _lp_accel_y(250.0f, 30.0f), - _lp_accel_z(250.0f, 30.0f), - _lp_gyro_x(250.0f, 30.0f), - _lp_gyro_y(250.0f, 30.0f), - _lp_gyro_z(250.0f, 30.0f) + _lp_accel_z(250.0f, 30.0f) { _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); @@ -220,7 +212,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); _params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M"); - _params_handles.airspeed_mode = param_find("FW_ARSP_MODE"); + _params_handles.airspeed_disabled = param_find("FW_ARSP_MODE"); } /** @@ -330,14 +322,13 @@ void AttitudeEstimatorQ::task_main() // Feed validator with recent sensor data if (sensors.timestamp > 0) { - // Filter gyro signal since it is not fildered in the drivers. - _gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]); - _gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]); - _gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]); + _gyro(0) = sensors.gyro_rad[0]; + _gyro(1) = sensors.gyro_rad[1]; + _gyro(2) = sensors.gyro_rad[2]; } if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { - // Filter accel signal since it is not fildered in the drivers. + // Filter accel signal since it is not filtered in the drivers. _accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]); _accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]); _accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]); @@ -459,73 +450,20 @@ void AttitudeEstimatorQ::task_main() continue; } - { - vehicle_attitude_s att = { - .timestamp = sensors.timestamp, - .rollspeed = _rates(0), - .pitchspeed = _rates(1), - .yawspeed = _rates(2), - .q = {_q(0), _q(1), _q(2), _q(3)} - }; + vehicle_attitude_s att = { + .timestamp = sensors.timestamp, + .rollspeed = _rates(0), + .pitchspeed = _rates(1), + .yawspeed = _rates(2), - /* the instance count is not used here */ - int att_inst; - orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); - } + .q = {_q(0), _q(1), _q(2), _q(3)}, + .delta_q_reset = {}, + .quat_reset_counter = 0, + }; - { - struct control_state_s ctrl_state = {}; - - ctrl_state.timestamp = sensors.timestamp; - - /* attitude quaternions for control state */ - ctrl_state.q[0] = _q(0); - ctrl_state.q[1] = _q(1); - ctrl_state.q[2] = _q(2); - ctrl_state.q[3] = _q(3); - - ctrl_state.x_acc = _accel(0); - ctrl_state.y_acc = _accel(1); - ctrl_state.z_acc = _accel(2); - - /* attitude rates for control state */ - ctrl_state.roll_rate = _rates(0); - ctrl_state.pitch_rate = _rates(1); - ctrl_state.yaw_rate = _rates(2); - - /* TODO get bias estimates from estimator */ - ctrl_state.roll_rate_bias = 0.0f; - ctrl_state.pitch_rate_bias = 0.0f; - ctrl_state.yaw_rate_bias = 0.0f; - - ctrl_state.airspeed_valid = false; - - if (_airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) { - // use measured airspeed - if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 - && _airspeed.timestamp > 0) { - ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; - ctrl_state.airspeed_valid = true; - } - } - - else if (_airspeed_mode == control_state_s::AIRSPD_MODE_EST) { - // use estimated body velocity as airspeed estimate - if (hrt_absolute_time() - _gpos.timestamp < 1e6) { - ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d); - ctrl_state.airspeed_valid = true; - } - - } else if (_airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) { - // do nothing, airspeed has been declared as non-valid above, controllers - // will handle this assuming always trim airspeed - } - - /* the instance count is not used here */ - int ctrl_inst; - /* publish to control state topic */ - orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH); - } + /* the instance count is not used here */ + int att_inst; + orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); { //struct estimator_status_s est = {}; @@ -582,7 +520,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); - param_get(_params_handles.airspeed_mode, &_airspeed_mode); + param_get(_params_handles.airspeed_disabled, &_airspeed_disabled); } } diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 8ea0c24319..2aec5f1ea9 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -38,48 +38,48 @@ * @author Roman Bapst */ -#include +#include + +#include +#include +#include +#include +#include #include #include #include #include #include - -#include - #include -#include -#include -#include -#include -#include - -#include -#include #include -#include -#include -#include -#include -#include -#include +#include #include #include +#include #include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include +using control::BlockParamFloat; +using control::BlockParamExtFloat; +using control::BlockParamInt; +using control::BlockParamExtInt; +using math::constrain; extern "C" __EXPORT int ekf2_main(int argc, char *argv[]); - -class Ekf2 : public control::SuperBlock, public ModuleBase +class Ekf2 final : public control::SuperBlock, public ModuleBase { public: Ekf2(); @@ -100,7 +100,7 @@ public: /** @see ModuleBase::run() */ void run() override; - void set_replay_mode(bool replay) { _replay_mode = replay; } + void set_replay_mode(bool replay) { _replay_mode = replay; } static void task_main_trampoline(int argc, char *argv[]); @@ -120,21 +120,19 @@ private: uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected // Used to down sample magnetometer data - float _mag_data_sum[3]; ///< summed magnetometer readings (Gauss) + float _mag_data_sum[3] = {}; ///< summed magnetometer readings (Gauss) uint64_t _mag_time_sum_ms = 0; ///< summed magnetoemter time stamps (mSec) uint8_t _mag_sample_count = 0; ///< number of magnetometer measurements summed during downsampling uint32_t _mag_time_ms_last_used = 0; ///< time stamp of the last averaged magnetometer measurement sent to the EKF (mSec) // Used to down sample barometer data - float _balt_data_sum; ///< summed pressure altitude readings (m) + float _balt_data_sum = 0.0f; ///< summed pressure altitude readings (m) uint64_t _balt_time_sum_ms = 0; ///< summed pressure altitude time stamps (mSec) uint8_t _balt_sample_count = 0; ///< number of barometric altitude measurements summed uint32_t _balt_time_ms_last_used = 0; ///< time stamp of the last averaged barometric altitude measurement sent to the EKF (mSec) - float _acc_hor_filt = 0.0f; ///< low-pass filtered horizontal acceleration (m/sec**2) - // Used to check, save and use learned magnetometer biases hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save @@ -156,21 +154,16 @@ private: const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; ///< preflight velocity innovation spike limit (m/sec) const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; ///< preflight position innovation spike limit (m) - orb_advert_t _att_pub; - orb_advert_t _control_state_pub; - orb_advert_t _wind_pub; - orb_advert_t _estimator_status_pub; - orb_advert_t _estimator_innovations_pub; - orb_advert_t _ekf2_timestamps_pub; + orb_advert_t _att_pub{nullptr}; + orb_advert_t _wind_pub{nullptr}; + orb_advert_t _estimator_status_pub{nullptr}; + orb_advert_t _estimator_innovations_pub{nullptr}; + orb_advert_t _ekf2_timestamps_pub{nullptr}; + orb_advert_t _sensor_bias_pub{nullptr}; uORB::Publication _vehicle_local_position_pub; uORB::Publication _vehicle_global_position_pub; - /* Low pass filter for attitude rates */ - math::LowPassFilter2p _lp_roll_rate; ///< Low pass filter applied to roll rates published on the control_state message - math::LowPassFilter2p _lp_pitch_rate; ///< Low pass filter applied to pitch rates published on the control_state message - math::LowPassFilter2p _lp_yaw_rate; ///< Low pass filter applied to yaw rates published on the control_state message - // Used to correct baro data for positional errors Vector3f _vel_body_wind = {}; // XYZ velocity relative to wind in body frame (m/s) @@ -178,270 +171,253 @@ private: parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) - control::BlockParamExtInt + BlockParamExtInt _obs_dt_min_ms; ///< Maximmum time delay of any sensor used to increse buffer length to handle large timing jitter (mSec) - control::BlockParamExtFloat _mag_delay_ms; ///< magnetometer measurement delay relative to the IMU (mSec) - control::BlockParamExtFloat _baro_delay_ms; ///< barometer height measurement delay relative to the IMU (mSec) - control::BlockParamExtFloat _gps_delay_ms; ///< GPS measurement delay relative to the IMU (mSec) - control::BlockParamExtFloat + BlockParamExtFloat _mag_delay_ms; ///< magnetometer measurement delay relative to the IMU (mSec) + BlockParamExtFloat _baro_delay_ms; ///< barometer height measurement delay relative to the IMU (mSec) + BlockParamExtFloat _gps_delay_ms; ///< GPS measurement delay relative to the IMU (mSec) + BlockParamExtFloat _flow_delay_ms; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval - control::BlockParamExtFloat _rng_delay_ms; ///< range finder measurement delay relative to the IMU (mSec) - control::BlockParamExtFloat _airspeed_delay_ms; ///< airspeed measurement delay relative to the IMU (mSec) - control::BlockParamExtFloat _ev_delay_ms; ///< off-board vision measurement delay relative to the IMU (mSec) + BlockParamExtFloat _rng_delay_ms; ///< range finder measurement delay relative to the IMU (mSec) + BlockParamExtFloat _airspeed_delay_ms; ///< airspeed measurement delay relative to the IMU (mSec) + BlockParamExtFloat _ev_delay_ms; ///< off-board vision measurement delay relative to the IMU (mSec) - control::BlockParamExtFloat _gyro_noise; ///< IMU angular rate noise used for covariance prediction (rad/sec) - control::BlockParamExtFloat _accel_noise; ///< IMU acceleration noise use for covariance prediction (m/sec**2) + BlockParamExtFloat _gyro_noise; ///< IMU angular rate noise used for covariance prediction (rad/sec) + BlockParamExtFloat _accel_noise; ///< IMU acceleration noise use for covariance prediction (m/sec**2) // process noise - control::BlockParamExtFloat _gyro_bias_p_noise; ///< process noise for IMU rate gyro bias prediction (rad/sec**2) - control::BlockParamExtFloat _accel_bias_p_noise;///< process noise for IMU accelerometer bias prediction (m/sec**3) - control::BlockParamExtFloat _mage_p_noise; ///< process noise for earth magnetic field prediction (Gauss/sec) - control::BlockParamExtFloat _magb_p_noise; ///< process noise for body magnetic field prediction (Gauss/sec) - control::BlockParamExtFloat _wind_vel_p_noise; ///< process noise for wind velocity prediction (m/sec**2) - control::BlockParamExtFloat _terrain_p_noise; ///< process noise for terrain offset (m/sec) - control::BlockParamExtFloat + BlockParamExtFloat _gyro_bias_p_noise; ///< process noise for IMU rate gyro bias prediction (rad/sec**2) + BlockParamExtFloat _accel_bias_p_noise;///< process noise for IMU accelerometer bias prediction (m/sec**3) + BlockParamExtFloat _mage_p_noise; ///< process noise for earth magnetic field prediction (Gauss/sec) + BlockParamExtFloat _magb_p_noise; ///< process noise for body magnetic field prediction (Gauss/sec) + BlockParamExtFloat _wind_vel_p_noise; ///< process noise for wind velocity prediction (m/sec**2) + BlockParamExtFloat _terrain_p_noise; ///< process noise for terrain offset (m/sec) + BlockParamExtFloat _terrain_gradient; ///< gradient of terrain used to estimate process noise due to changing position (m/m) - control::BlockParamExtFloat _gps_vel_noise; ///< minimum allowed observation noise for gps velocity fusion (m/sec) - control::BlockParamExtFloat _gps_pos_noise; ///< minimum allowed observation noise for gps position fusion (m) - control::BlockParamExtFloat _pos_noaid_noise; ///< observation noise for non-aiding position fusion (m) - control::BlockParamExtFloat _baro_noise; ///< observation noise for barometric height fusion (m) - control::BlockParamExtFloat _baro_innov_gate; ///< barometric height innovation consistency gate size (STD) - control::BlockParamExtFloat _posNE_innov_gate; ///< GPS horizontal position innovation consistency gate size (STD) - control::BlockParamExtFloat _vel_innov_gate; ///< GPS velocity innovation consistency gate size (STD) - control::BlockParamExtFloat _tas_innov_gate; ///< True Airspeed innovation consistency gate size (STD) + BlockParamExtFloat _gps_vel_noise; ///< minimum allowed observation noise for gps velocity fusion (m/sec) + BlockParamExtFloat _gps_pos_noise; ///< minimum allowed observation noise for gps position fusion (m) + BlockParamExtFloat _pos_noaid_noise; ///< observation noise for non-aiding position fusion (m) + BlockParamExtFloat _baro_noise; ///< observation noise for barometric height fusion (m) + BlockParamExtFloat _baro_innov_gate; ///< barometric height innovation consistency gate size (STD) + BlockParamExtFloat _posNE_innov_gate; ///< GPS horizontal position innovation consistency gate size (STD) + BlockParamExtFloat _vel_innov_gate; ///< GPS velocity innovation consistency gate size (STD) + BlockParamExtFloat _tas_innov_gate; ///< True Airspeed innovation consistency gate size (STD) // control of magnetometer fusion - control::BlockParamExtFloat _mag_heading_noise; ///< measurement noise used for simple heading fusion (rad) - control::BlockParamExtFloat _mag_noise; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) - control::BlockParamExtFloat _eas_noise; ///< measurement noise used for airspeed fusion (m/sec) - control::BlockParamExtFloat _beta_noise; ///< synthetic sideslip noise (rad) - control::BlockParamExtFloat _mag_declination_deg;///< magnetic declination (degrees) - control::BlockParamExtFloat _heading_innov_gate;///< heading fusion innovation consistency gate size (STD) - control::BlockParamExtFloat _mag_innov_gate; ///< magnetometer fusion innovation consistency gate size (STD) - control::BlockParamExtInt _mag_decl_source; ///< bitmask used to control the handling of declination data - control::BlockParamExtInt _mag_fuse_type; ///< integer used to specify the type of magnetometer fusion used - control::BlockParamExtFloat _mag_acc_gate; ///< integer used to specify the type of magnetometer fusion used - control::BlockParamExtFloat _mag_yaw_rate_gate; ///< yaw rate threshold used by mode select logic (rad/sec) + BlockParamExtFloat _mag_heading_noise; ///< measurement noise used for simple heading fusion (rad) + BlockParamExtFloat _mag_noise; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) + BlockParamExtFloat _eas_noise; ///< measurement noise used for airspeed fusion (m/sec) + BlockParamExtFloat _beta_noise; ///< synthetic sideslip noise (rad) + BlockParamExtFloat _mag_declination_deg;///< magnetic declination (degrees) + BlockParamExtFloat _heading_innov_gate;///< heading fusion innovation consistency gate size (STD) + BlockParamExtFloat _mag_innov_gate; ///< magnetometer fusion innovation consistency gate size (STD) + BlockParamExtInt _mag_decl_source; ///< bitmask used to control the handling of declination data + BlockParamExtInt _mag_fuse_type; ///< integer used to specify the type of magnetometer fusion used + BlockParamExtFloat _mag_acc_gate; ///< integer used to specify the type of magnetometer fusion used + BlockParamExtFloat _mag_yaw_rate_gate; ///< yaw rate threshold used by mode select logic (rad/sec) - control::BlockParamExtInt _gps_check_mask; ///< bitmask used to control which GPS quality checks are used - control::BlockParamExtFloat _requiredEph; ///< maximum acceptable horiz position error (m) - control::BlockParamExtFloat _requiredEpv; ///< maximum acceptable vert position error (m) - control::BlockParamExtFloat _requiredSacc; ///< maximum acceptable speed error (m/s) - control::BlockParamExtInt _requiredNsats; ///< minimum acceptable satellite count - control::BlockParamExtFloat _requiredGDoP; ///< maximum acceptable geometric dilution of precision - control::BlockParamExtFloat _requiredHdrift; ///< maximum acceptable horizontal drift speed (m/s) - control::BlockParamExtFloat _requiredVdrift; ///< maximum acceptable vertical drift speed (m/s) + BlockParamExtInt _gps_check_mask; ///< bitmask used to control which GPS quality checks are used + BlockParamExtFloat _requiredEph; ///< maximum acceptable horiz position error (m) + BlockParamExtFloat _requiredEpv; ///< maximum acceptable vert position error (m) + BlockParamExtFloat _requiredSacc; ///< maximum acceptable speed error (m/s) + BlockParamExtInt _requiredNsats; ///< minimum acceptable satellite count + BlockParamExtFloat _requiredGDoP; ///< maximum acceptable geometric dilution of precision + BlockParamExtFloat _requiredHdrift; ///< maximum acceptable horizontal drift speed (m/s) + BlockParamExtFloat _requiredVdrift; ///< maximum acceptable vertical drift speed (m/s) // measurement source control - control::BlockParamExtInt + BlockParamExtInt _fusion_mode; ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used - control::BlockParamExtInt _vdist_sensor_type; ///< selects the primary source for height data + BlockParamExtInt _vdist_sensor_type; ///< selects the primary source for height data // range finder fusion - control::BlockParamExtFloat _range_noise; ///< observation noise for range finder measurements (m) - control::BlockParamExtFloat _range_noise_scaler; ///< scale factor from range to range noise (m/m) - control::BlockParamExtFloat _range_innov_gate; ///< range finder fusion innovation consistency gate size (STD) - control::BlockParamExtFloat _rng_gnd_clearance; ///< minimum valid value for range when on ground (m) - control::BlockParamExtFloat _rng_pitch_offset; ///< range sensor pitch offset (rad) - control::BlockParamExtInt - _rng_aid; ///< enables use of a range finder even if primary height source is not range finder (EKF2_HGT_MODE != 2) - control::BlockParamExtFloat _rng_aid_hor_vel_max; ///< maximum allowed horizontal velocity for range aid (m/s) - control::BlockParamExtFloat _rng_aid_height_max; ///< maximum allowed absolute altitude (AGL) for range aid (m) - control::BlockParamExtFloat - _rng_aid_innov_gate; ///< gate size used for innovation consistency checks for range aid fusion (STD) + BlockParamExtFloat _range_noise; ///< observation noise for range finder measurements (m) + BlockParamExtFloat _range_noise_scaler; ///< scale factor from range to range noise (m/m) + BlockParamExtFloat _range_innov_gate; ///< range finder fusion innovation consistency gate size (STD) + BlockParamExtFloat _rng_gnd_clearance; ///< minimum valid value for range when on ground (m) + BlockParamExtFloat _rng_pitch_offset; ///< range sensor pitch offset (rad) + BlockParamExtInt _rng_aid; ///< enables use of a range finder even if primary height source is not range finder + BlockParamExtFloat _rng_aid_hor_vel_max; ///< maximum allowed horizontal velocity for range aid (m/s) + BlockParamExtFloat _rng_aid_height_max; ///< maximum allowed absolute altitude (AGL) for range aid (m) + BlockParamExtFloat _rng_aid_innov_gate; ///< gate size used for innovation consistency checks for range aid fusion (STD) // vision estimate fusion - control::BlockParamFloat _ev_pos_noise; ///< default position observation noise for exernal vision measurements (m) - control::BlockParamFloat _ev_ang_noise; ///< default angular observation noise for exernal vision measurements (rad) - control::BlockParamExtFloat _ev_innov_gate; ///< external vision position innovation consistency gate size (STD) + BlockParamFloat _ev_pos_noise; ///< default position observation noise for exernal vision measurements (m) + BlockParamFloat _ev_ang_noise; ///< default angular observation noise for exernal vision measurements (rad) + BlockParamExtFloat _ev_innov_gate; ///< external vision position innovation consistency gate size (STD) // optical flow fusion - control::BlockParamExtFloat - _flow_noise; ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) - control::BlockParamExtFloat + BlockParamExtFloat _flow_noise; ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) + BlockParamExtFloat _flow_noise_qual_min; ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) - control::BlockParamExtInt _flow_qual_min; ///< minimum acceptable quality integer from the flow sensor - control::BlockParamExtFloat _flow_innov_gate; ///< optical flow fusion innovation consistency gate size (STD) - control::BlockParamExtFloat _flow_rate_max; ///< maximum valid optical flow rate (rad/sec) + BlockParamExtInt _flow_qual_min; ///< minimum acceptable quality integer from the flow sensor + BlockParamExtFloat _flow_innov_gate; ///< optical flow fusion innovation consistency gate size (STD) + BlockParamExtFloat _flow_rate_max; ///< maximum valid optical flow rate (rad/sec) // sensor positions in body frame - control::BlockParamExtFloat _imu_pos_x; ///< X position of IMU in body frame (m) - control::BlockParamExtFloat _imu_pos_y; ///< Y position of IMU in body frame (m) - control::BlockParamExtFloat _imu_pos_z; ///< Z position of IMU in body frame (m) - control::BlockParamExtFloat _gps_pos_x; ///< X position of GPS antenna in body frame (m) - control::BlockParamExtFloat _gps_pos_y; ///< Y position of GPS antenna in body frame (m) - control::BlockParamExtFloat _gps_pos_z; ///< Z position of GPS antenna in body frame (m) - control::BlockParamExtFloat _rng_pos_x; ///< X position of range finder in body frame (m) - control::BlockParamExtFloat _rng_pos_y; ///< Y position of range finder in body frame (m) - control::BlockParamExtFloat _rng_pos_z; ///< Z position of range finder in body frame (m) - control::BlockParamExtFloat _flow_pos_x; ///< X position of optical flow sensor focal point in body frame (m) - control::BlockParamExtFloat _flow_pos_y; ///< Y position of optical flow sensor focal point in body frame (m) - control::BlockParamExtFloat _flow_pos_z; ///< Z position of optical flow sensor focal point in body frame (m) - control::BlockParamExtFloat _ev_pos_x; ///< X position of VI sensor focal point in body frame (m) - control::BlockParamExtFloat _ev_pos_y; ///< Y position of VI sensor focal point in body frame (m) - control::BlockParamExtFloat _ev_pos_z; ///< Z position of VI sensor focal point in body frame (m) + BlockParamExtFloat _imu_pos_x; ///< X position of IMU in body frame (m) + BlockParamExtFloat _imu_pos_y; ///< Y position of IMU in body frame (m) + BlockParamExtFloat _imu_pos_z; ///< Z position of IMU in body frame (m) + BlockParamExtFloat _gps_pos_x; ///< X position of GPS antenna in body frame (m) + BlockParamExtFloat _gps_pos_y; ///< Y position of GPS antenna in body frame (m) + BlockParamExtFloat _gps_pos_z; ///< Z position of GPS antenna in body frame (m) + BlockParamExtFloat _rng_pos_x; ///< X position of range finder in body frame (m) + BlockParamExtFloat _rng_pos_y; ///< Y position of range finder in body frame (m) + BlockParamExtFloat _rng_pos_z; ///< Z position of range finder in body frame (m) + BlockParamExtFloat _flow_pos_x; ///< X position of optical flow sensor focal point in body frame (m) + BlockParamExtFloat _flow_pos_y; ///< Y position of optical flow sensor focal point in body frame (m) + BlockParamExtFloat _flow_pos_z; ///< Z position of optical flow sensor focal point in body frame (m) + BlockParamExtFloat _ev_pos_x; ///< X position of VI sensor focal point in body frame (m) + BlockParamExtFloat _ev_pos_y; ///< Y position of VI sensor focal point in body frame (m) + BlockParamExtFloat _ev_pos_z; ///< Z position of VI sensor focal point in body frame (m) // control of airspeed and sideslip fusion - control::BlockParamFloat + BlockParamFloat _arspFusionThreshold; ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) - control::BlockParamInt _fuseBeta; ///< Controls synthetic sideslip fusion, 0 disables, 1 enables + BlockParamInt _fuseBeta; ///< Controls synthetic sideslip fusion, 0 disables, 1 enables // output predictor filter time constants - control::BlockParamExtFloat _tau_vel; ///< time constant used by the output velocity complementary filter (sec) - control::BlockParamExtFloat _tau_pos; ///< time constant used by the output position complementary filter (sec) + BlockParamExtFloat _tau_vel; ///< time constant used by the output velocity complementary filter (sec) + BlockParamExtFloat _tau_pos; ///< time constant used by the output position complementary filter (sec) // IMU switch on bias paameters - control::BlockParamExtFloat _gyr_bias_init; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) - control::BlockParamExtFloat _acc_bias_init; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) - control::BlockParamExtFloat _ang_err_init; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) - - // airspeed mode parameter - control::BlockParamInt _airspeed_mode; + BlockParamExtFloat _gyr_bias_init; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) + BlockParamExtFloat _acc_bias_init; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) + BlockParamExtFloat _ang_err_init; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) // EKF saved XYZ magnetometer bias values - control::BlockParamFloat _mag_bias_x; ///< X magnetometer bias (mGauss) - control::BlockParamFloat _mag_bias_y; ///< Y magnetometer bias (mGauss) - control::BlockParamFloat _mag_bias_z; ///< Z magnetometer bias (mGauss) - control::BlockParamInt _mag_bias_id; ///< ID of the magnetometer sensor used to learn the bias values - control::BlockParamFloat + BlockParamFloat _mag_bias_x; ///< X magnetometer bias (mGauss) + BlockParamFloat _mag_bias_y; ///< Y magnetometer bias (mGauss) + BlockParamFloat _mag_bias_z; ///< Z magnetometer bias (mGauss) + BlockParamInt _mag_bias_id; ///< ID of the magnetometer sensor used to learn the bias values + BlockParamFloat _mag_bias_saved_variance; ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2) - control::BlockParamFloat - _mag_bias_alpha; ///< maximum fraction of the learned magnetometer bias that is saved at each disarm + BlockParamFloat _mag_bias_alpha; ///< maximum fraction of the learned magnetometer bias that is saved at each disarm // Multi-rotor drag specific force fusion - control::BlockParamExtFloat - _drag_noise; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 - control::BlockParamExtFloat _bcoef_x; ///< ballistic coefficient along the X-axis (kg/m**2) - control::BlockParamExtFloat _bcoef_y; ///< ballistic coefficient along the Y-axis (kg/m**2) + BlockParamExtFloat _drag_noise; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 + BlockParamExtFloat _bcoef_x; ///< ballistic coefficient along the X-axis (kg/m**2) + BlockParamExtFloat _bcoef_y; ///< ballistic coefficient along the Y-axis (kg/m**2) // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 - control::BlockParamFloat _aspd_max; ///< upper limit on airspeed used for correction (m/s**2) - control::BlockParamFloat - _K_pstatic_coef_xp; ///< static pressure position error coefficient along the positive X body axis - control::BlockParamFloat - _K_pstatic_coef_xn; ///< static pressure position error coefficient along the negative X body axis - control::BlockParamFloat _K_pstatic_coef_y; ///< static pressure position error coefficient along the Y body axis - control::BlockParamFloat _K_pstatic_coef_z; ///< static pressure position error coefficient along the Z body axis + BlockParamFloat _aspd_max; ///< upper limit on airspeed used for correction (m/s**2) + BlockParamFloat _K_pstatic_coef_xp; ///< static pressure position error coefficient along the positive X body axis + BlockParamFloat _K_pstatic_coef_xn; ///< static pressure position error coefficient along the negative X body axis + BlockParamFloat _K_pstatic_coef_y; ///< static pressure position error coefficient along the Y body axis + BlockParamFloat _K_pstatic_coef_z; ///< static pressure position error coefficient along the Z body axis + // airspeed mode parameter + BlockParamInt _airspeed_disabled; }; Ekf2::Ekf2(): - SuperBlock(nullptr, "EKF"), - _replay_mode(false), - _att_pub(nullptr), - _control_state_pub(nullptr), - _wind_pub(nullptr), - _estimator_status_pub(nullptr), - _estimator_innovations_pub(nullptr), - _ekf2_timestamps_pub(nullptr), + SuperBlock(nullptr, "EKF2"), _vehicle_local_position_pub(ORB_ID(vehicle_local_position), -1, &getPublications()), _vehicle_global_position_pub(ORB_ID(vehicle_global_position), -1, &getPublications()), - _lp_roll_rate(250.0f, 30.0f), - _lp_pitch_rate(250.0f, 30.0f), - _lp_yaw_rate(250.0f, 20.0f), _params(_ekf.getParamHandle()), - _obs_dt_min_ms(this, "EKF2_MIN_OBS_DT", false, _params->sensor_interval_min_ms), - _mag_delay_ms(this, "EKF2_MAG_DELAY", false, _params->mag_delay_ms), - _baro_delay_ms(this, "EKF2_BARO_DELAY", false, _params->baro_delay_ms), - _gps_delay_ms(this, "EKF2_GPS_DELAY", false, _params->gps_delay_ms), - _flow_delay_ms(this, "EKF2_OF_DELAY", false, _params->flow_delay_ms), - _rng_delay_ms(this, "EKF2_RNG_DELAY", false, _params->range_delay_ms), - _airspeed_delay_ms(this, "EKF2_ASP_DELAY", false, _params->airspeed_delay_ms), - _ev_delay_ms(this, "EKF2_EV_DELAY", false, _params->ev_delay_ms), - _gyro_noise(this, "EKF2_GYR_NOISE", false, _params->gyro_noise), - _accel_noise(this, "EKF2_ACC_NOISE", false, _params->accel_noise), - _gyro_bias_p_noise(this, "EKF2_GYR_B_NOISE", false, _params->gyro_bias_p_noise), - _accel_bias_p_noise(this, "EKF2_ACC_B_NOISE", false, _params->accel_bias_p_noise), - _mage_p_noise(this, "EKF2_MAG_E_NOISE", false, _params->mage_p_noise), - _magb_p_noise(this, "EKF2_MAG_B_NOISE", false, _params->magb_p_noise), - _wind_vel_p_noise(this, "EKF2_WIND_NOISE", false, _params->wind_vel_p_noise), - _terrain_p_noise(this, "EKF2_TERR_NOISE", false, _params->terrain_p_noise), - _terrain_gradient(this, "EKF2_TERR_GRAD", false, _params->terrain_gradient), - _gps_vel_noise(this, "EKF2_GPS_V_NOISE", false, _params->gps_vel_noise), - _gps_pos_noise(this, "EKF2_GPS_P_NOISE", false, _params->gps_pos_noise), - _pos_noaid_noise(this, "EKF2_NOAID_NOISE", false, _params->pos_noaid_noise), - _baro_noise(this, "EKF2_BARO_NOISE", false, _params->baro_noise), - _baro_innov_gate(this, "EKF2_BARO_GATE", false, _params->baro_innov_gate), - _posNE_innov_gate(this, "EKF2_GPS_P_GATE", false, _params->posNE_innov_gate), - _vel_innov_gate(this, "EKF2_GPS_V_GATE", false, _params->vel_innov_gate), - _tas_innov_gate(this, "EKF2_TAS_GATE", false, _params->tas_innov_gate), - _mag_heading_noise(this, "EKF2_HEAD_NOISE", false, _params->mag_heading_noise), - _mag_noise(this, "EKF2_MAG_NOISE", false, _params->mag_noise), - _eas_noise(this, "EKF2_EAS_NOISE", false, _params->eas_noise), - _beta_noise(this, "EKF2_BETA_NOISE", false, _params->beta_noise), - _mag_declination_deg(this, "EKF2_MAG_DECL", false, _params->mag_declination_deg), - _heading_innov_gate(this, "EKF2_HDG_GATE", false, _params->heading_innov_gate), - _mag_innov_gate(this, "EKF2_MAG_GATE", false, _params->mag_innov_gate), - _mag_decl_source(this, "EKF2_DECL_TYPE", false, _params->mag_declination_source), - _mag_fuse_type(this, "EKF2_MAG_TYPE", false, _params->mag_fusion_type), - _mag_acc_gate(this, "EKF2_MAG_ACCLIM", false, _params->mag_acc_gate), - _mag_yaw_rate_gate(this, "EKF2_MAG_YAWLIM", false, _params->mag_yaw_rate_gate), - _gps_check_mask(this, "EKF2_GPS_CHECK", false, _params->gps_check_mask), - _requiredEph(this, "EKF2_REQ_EPH", false, _params->req_hacc), - _requiredEpv(this, "EKF2_REQ_EPV", false, _params->req_vacc), - _requiredSacc(this, "EKF2_REQ_SACC", false, _params->req_sacc), - _requiredNsats(this, "EKF2_REQ_NSATS", false, _params->req_nsats), - _requiredGDoP(this, "EKF2_REQ_GDOP", false, _params->req_gdop), - _requiredHdrift(this, "EKF2_REQ_HDRIFT", false, _params->req_hdrift), - _requiredVdrift(this, "EKF2_REQ_VDRIFT", false, _params->req_vdrift), - _fusion_mode(this, "EKF2_AID_MASK", false, _params->fusion_mode), - _vdist_sensor_type(this, "EKF2_HGT_MODE", false, _params->vdist_sensor_type), - _range_noise(this, "EKF2_RNG_NOISE", false, _params->range_noise), - _range_noise_scaler(this, "EKF2_RNG_SFE", false, _params->range_noise_scaler), - _range_innov_gate(this, "EKF2_RNG_GATE", false, _params->range_innov_gate), - _rng_gnd_clearance(this, "EKF2_MIN_RNG", false, _params->rng_gnd_clearance), - _rng_pitch_offset(this, "EKF2_RNG_PITCH", false, _params->rng_sens_pitch), - _rng_aid(this, "EKF2_RNG_AID", false, _params->range_aid), - _rng_aid_hor_vel_max(this, "EKF2_RNG_A_VMAX", false, _params->max_vel_for_range_aid), - _rng_aid_height_max(this, "EKF2_RNG_A_HMAX", false, _params->max_hagl_for_range_aid), - _rng_aid_innov_gate(this, "EKF2_RNG_A_IGATE", false, _params->range_aid_innov_gate), - _ev_pos_noise(this, "EKF2_EVP_NOISE", false), - _ev_ang_noise(this, "EKF2_EVA_NOISE", false), - _ev_innov_gate(this, "EKF2_EV_GATE", false, _params->ev_innov_gate), - _flow_noise(this, "EKF2_OF_N_MIN", false, _params->flow_noise), - _flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, _params->flow_noise_qual_min), - _flow_qual_min(this, "EKF2_OF_QMIN", false, _params->flow_qual_min), - _flow_innov_gate(this, "EKF2_OF_GATE", false, _params->flow_innov_gate), - _flow_rate_max(this, "EKF2_OF_RMAX", false, _params->flow_rate_max), - _imu_pos_x(this, "EKF2_IMU_POS_X", false, _params->imu_pos_body(0)), - _imu_pos_y(this, "EKF2_IMU_POS_Y", false, _params->imu_pos_body(1)), - _imu_pos_z(this, "EKF2_IMU_POS_Z", false, _params->imu_pos_body(2)), - _gps_pos_x(this, "EKF2_GPS_POS_X", false, _params->gps_pos_body(0)), - _gps_pos_y(this, "EKF2_GPS_POS_Y", false, _params->gps_pos_body(1)), - _gps_pos_z(this, "EKF2_GPS_POS_Z", false, _params->gps_pos_body(2)), - _rng_pos_x(this, "EKF2_RNG_POS_X", false, _params->rng_pos_body(0)), - _rng_pos_y(this, "EKF2_RNG_POS_Y", false, _params->rng_pos_body(1)), - _rng_pos_z(this, "EKF2_RNG_POS_Z", false, _params->rng_pos_body(2)), - _flow_pos_x(this, "EKF2_OF_POS_X", false, _params->flow_pos_body(0)), - _flow_pos_y(this, "EKF2_OF_POS_Y", false, _params->flow_pos_body(1)), - _flow_pos_z(this, "EKF2_OF_POS_Z", false, _params->flow_pos_body(2)), - _ev_pos_x(this, "EKF2_EV_POS_X", false, _params->ev_pos_body(0)), - _ev_pos_y(this, "EKF2_EV_POS_Y", false, _params->ev_pos_body(1)), - _ev_pos_z(this, "EKF2_EV_POS_Z", false, _params->ev_pos_body(2)), - _arspFusionThreshold(this, "EKF2_ARSP_THR", false), - _fuseBeta(this, "EKF2_FUSE_BETA", false), - _tau_vel(this, "EKF2_TAU_VEL", false, _params->vel_Tau), - _tau_pos(this, "EKF2_TAU_POS", false, _params->pos_Tau), - _gyr_bias_init(this, "EKF2_GBIAS_INIT", false, _params->switch_on_gyro_bias), - _acc_bias_init(this, "EKF2_ABIAS_INIT", false, _params->switch_on_accel_bias), - _ang_err_init(this, "EKF2_ANGERR_INIT", false, _params->initial_tilt_err), - _airspeed_mode(this, "FW_ARSP_MODE", false), - _mag_bias_x(this, "EKF2_MAGBIAS_X", false), - _mag_bias_y(this, "EKF2_MAGBIAS_Y", false), - _mag_bias_z(this, "EKF2_MAGBIAS_Z", false), - _mag_bias_id(this, "EKF2_MAGBIAS_ID", false), - _mag_bias_saved_variance(this, "EKF2_MAGB_VREF", false), - _mag_bias_alpha(this, "EKF2_MAGB_K", false), - _drag_noise(this, "EKF2_DRAG_NOISE", false, _params->drag_noise), - _bcoef_x(this, "EKF2_BCOEF_X", false, _params->bcoef_x), - _bcoef_y(this, "EKF2_BCOEF_Y", false, _params->bcoef_y), - _aspd_max(this, "EKF2_ASPD_MAX", false), - _K_pstatic_coef_xp(this, "EKF2_PCOEF_XP", false), - _K_pstatic_coef_xn(this, "EKF2_PCOEF_XN", false), - _K_pstatic_coef_y(this, "EKF2_PCOEF_Y", false), - _K_pstatic_coef_z(this, "EKF2_PCOEF_Z", false) + _obs_dt_min_ms(this, "MIN_OBS_DT", true, _params->sensor_interval_min_ms), + _mag_delay_ms(this, "MAG_DELAY", true, _params->mag_delay_ms), + _baro_delay_ms(this, "BARO_DELAY", true, _params->baro_delay_ms), + _gps_delay_ms(this, "GPS_DELAY", true, _params->gps_delay_ms), + _flow_delay_ms(this, "OF_DELAY", true, _params->flow_delay_ms), + _rng_delay_ms(this, "RNG_DELAY", true, _params->range_delay_ms), + _airspeed_delay_ms(this, "ASP_DELAY", true, _params->airspeed_delay_ms), + _ev_delay_ms(this, "EV_DELAY", true, _params->ev_delay_ms), + _gyro_noise(this, "GYR_NOISE", true, _params->gyro_noise), + _accel_noise(this, "ACC_NOISE", true, _params->accel_noise), + _gyro_bias_p_noise(this, "GYR_B_NOISE", true, _params->gyro_bias_p_noise), + _accel_bias_p_noise(this, "ACC_B_NOISE", true, _params->accel_bias_p_noise), + _mage_p_noise(this, "MAG_E_NOISE", true, _params->mage_p_noise), + _magb_p_noise(this, "MAG_B_NOISE", true, _params->magb_p_noise), + _wind_vel_p_noise(this, "WIND_NOISE", true, _params->wind_vel_p_noise), + _terrain_p_noise(this, "TERR_NOISE", true, _params->terrain_p_noise), + _terrain_gradient(this, "TERR_GRAD", true, _params->terrain_gradient), + _gps_vel_noise(this, "GPS_V_NOISE", true, _params->gps_vel_noise), + _gps_pos_noise(this, "GPS_P_NOISE", true, _params->gps_pos_noise), + _pos_noaid_noise(this, "NOAID_NOISE", true, _params->pos_noaid_noise), + _baro_noise(this, "BARO_NOISE", true, _params->baro_noise), + _baro_innov_gate(this, "BARO_GATE", true, _params->baro_innov_gate), + _posNE_innov_gate(this, "GPS_P_GATE", true, _params->posNE_innov_gate), + _vel_innov_gate(this, "GPS_V_GATE", true, _params->vel_innov_gate), + _tas_innov_gate(this, "TAS_GATE", true, _params->tas_innov_gate), + _mag_heading_noise(this, "HEAD_NOISE", true, _params->mag_heading_noise), + _mag_noise(this, "MAG_NOISE", true, _params->mag_noise), + _eas_noise(this, "EAS_NOISE", true, _params->eas_noise), + _beta_noise(this, "BETA_NOISE", true, _params->beta_noise), + _mag_declination_deg(this, "MAG_DECL", true, _params->mag_declination_deg), + _heading_innov_gate(this, "HDG_GATE", true, _params->heading_innov_gate), + _mag_innov_gate(this, "MAG_GATE", true, _params->mag_innov_gate), + _mag_decl_source(this, "DECL_TYPE", true, _params->mag_declination_source), + _mag_fuse_type(this, "MAG_TYPE", true, _params->mag_fusion_type), + _mag_acc_gate(this, "MAG_ACCLIM", true, _params->mag_acc_gate), + _mag_yaw_rate_gate(this, "MAG_YAWLIM", true, _params->mag_yaw_rate_gate), + _gps_check_mask(this, "GPS_CHECK", true, _params->gps_check_mask), + _requiredEph(this, "REQ_EPH", true, _params->req_hacc), + _requiredEpv(this, "REQ_EPV", true, _params->req_vacc), + _requiredSacc(this, "REQ_SACC", true, _params->req_sacc), + _requiredNsats(this, "REQ_NSATS", true, _params->req_nsats), + _requiredGDoP(this, "REQ_GDOP", true, _params->req_gdop), + _requiredHdrift(this, "REQ_HDRIFT", true, _params->req_hdrift), + _requiredVdrift(this, "REQ_VDRIFT", true, _params->req_vdrift), + _fusion_mode(this, "AID_MASK", true, _params->fusion_mode), + _vdist_sensor_type(this, "HGT_MODE", true, _params->vdist_sensor_type), + _range_noise(this, "RNG_NOISE", true, _params->range_noise), + _range_noise_scaler(this, "RNG_SFE", true, _params->range_noise_scaler), + _range_innov_gate(this, "RNG_GATE", true, _params->range_innov_gate), + _rng_gnd_clearance(this, "MIN_RNG", true, _params->rng_gnd_clearance), + _rng_pitch_offset(this, "RNG_PITCH", true, _params->rng_sens_pitch), + _rng_aid(this, "RNG_AID", true, _params->range_aid), + _rng_aid_hor_vel_max(this, "RNG_A_VMAX", true, _params->max_vel_for_range_aid), + _rng_aid_height_max(this, "RNG_A_HMAX", true, _params->max_hagl_for_range_aid), + _rng_aid_innov_gate(this, "RNG_A_IGATE", true, _params->range_aid_innov_gate), + _ev_pos_noise(this, "EVP_NOISE"), + _ev_ang_noise(this, "EVA_NOISE"), + _ev_innov_gate(this, "EV_GATE", true, _params->ev_innov_gate), + _flow_noise(this, "OF_N_MIN", true, _params->flow_noise), + _flow_noise_qual_min(this, "OF_N_MAX", true, _params->flow_noise_qual_min), + _flow_qual_min(this, "OF_QMIN", true, _params->flow_qual_min), + _flow_innov_gate(this, "OF_GATE", true, _params->flow_innov_gate), + _flow_rate_max(this, "OF_RMAX", true, _params->flow_rate_max), + _imu_pos_x(this, "IMU_POS_X", true, _params->imu_pos_body(0)), + _imu_pos_y(this, "IMU_POS_Y", true, _params->imu_pos_body(1)), + _imu_pos_z(this, "IMU_POS_Z", true, _params->imu_pos_body(2)), + _gps_pos_x(this, "GPS_POS_X", true, _params->gps_pos_body(0)), + _gps_pos_y(this, "GPS_POS_Y", true, _params->gps_pos_body(1)), + _gps_pos_z(this, "GPS_POS_Z", true, _params->gps_pos_body(2)), + _rng_pos_x(this, "RNG_POS_X", true, _params->rng_pos_body(0)), + _rng_pos_y(this, "RNG_POS_Y", true, _params->rng_pos_body(1)), + _rng_pos_z(this, "RNG_POS_Z", true, _params->rng_pos_body(2)), + _flow_pos_x(this, "OF_POS_X", true, _params->flow_pos_body(0)), + _flow_pos_y(this, "OF_POS_Y", true, _params->flow_pos_body(1)), + _flow_pos_z(this, "OF_POS_Z", true, _params->flow_pos_body(2)), + _ev_pos_x(this, "EV_POS_X", true, _params->ev_pos_body(0)), + _ev_pos_y(this, "EV_POS_Y", true, _params->ev_pos_body(1)), + _ev_pos_z(this, "EV_POS_Z", true, _params->ev_pos_body(2)), + _arspFusionThreshold(this, "ARSP_THR"), + _fuseBeta(this, "FUSE_BETA"), + _tau_vel(this, "TAU_VEL", true, _params->vel_Tau), + _tau_pos(this, "TAU_POS", true, _params->pos_Tau), + _gyr_bias_init(this, "GBIAS_INIT", true, _params->switch_on_gyro_bias), + _acc_bias_init(this, "ABIAS_INIT", true, _params->switch_on_accel_bias), + _ang_err_init(this, "ANGERR_INIT", true, _params->initial_tilt_err), + _mag_bias_x(this, "MAGBIAS_X"), + _mag_bias_y(this, "MAGBIAS_Y"), + _mag_bias_z(this, "MAGBIAS_Z"), + _mag_bias_id(this, "MAGBIAS_ID"), + _mag_bias_saved_variance(this, "MAGB_VREF"), + _mag_bias_alpha(this, "MAGB_K"), + _drag_noise(this, "DRAG_NOISE", true, _params->drag_noise), + _bcoef_x(this, "BCOEF_X", true, _params->bcoef_x), + _bcoef_y(this, "BCOEF_Y", true, _params->bcoef_y), + _aspd_max(this, "ASPD_MAX"), + _K_pstatic_coef_xp(this, "PCOEF_XP"), + _K_pstatic_coef_xn(this, "PCOEF_XN"), + _K_pstatic_coef_y(this, "PCOEF_Y"), + _K_pstatic_coef_z(this, "PCOEF_Z"), + // non EKF2 parameters + _airspeed_disabled(this, "FW_ARSP_MODE", false) { } @@ -546,7 +522,10 @@ void Ekf2::run() orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); } - orb_check(airspeed_sub, &airspeed_updated); + // Do not attempt to use airspeed if use has been disabled by the user. + if (_airspeed_disabled.get() == 0) { + orb_check(airspeed_sub, &airspeed_updated); + } if (airspeed_updated) { orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); @@ -653,6 +632,7 @@ void Ekf2::run() _mag_bias_z.commit_no_notification(); _mag_bias_id.set(sensor_selection.mag_device_id); _mag_bias_id.commit(); + _invalid_mag_id_count = 0; PX4_INFO("Mag sensor ID changed to %i", _mag_bias_id.get()); @@ -681,7 +661,6 @@ void Ekf2::run() _mag_data_sum[0] = 0.0f; _mag_data_sum[1] = 0.0f; _mag_data_sum[2] = 0.0f; - } } } @@ -737,7 +716,6 @@ void Ekf2::run() _balt_time_sum_ms = 0; _balt_sample_count = 0; _balt_data_sum = 0.0f; - } } } @@ -759,11 +737,10 @@ void Ekf2::run() gps_msg.vel_ned[2] = gps.vel_d_m_s; gps_msg.vel_ned_valid = gps.vel_ned_valid; gps_msg.nsats = gps.satellites_used; - //TODO add gdop to gps topic + //TODO: add gdop to gps topic gps_msg.gdop = 0.0f; _ekf.setGpsData(gps.timestamp, &gps_msg); - } // only set airspeed data if condition for airspeed fusion are met @@ -844,131 +821,21 @@ void Ekf2::run() matrix::Quatf q; _ekf.copy_quaternion(q.data()); - float velocity[3]; - _ekf.get_velocity(velocity); - - float pos_d_deriv; - _ekf.get_pos_d_deriv(&pos_d_deriv); - - float gyro_rad[3]; - - { - // generate control state data - control_state_s ctrl_state = {}; - float gyro_bias[3] = {}; - _ekf.get_gyro_bias(gyro_bias); - ctrl_state.timestamp = now; - gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0]; - gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1]; - gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2]; - ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]); - ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]); - ctrl_state.roll_rate_bias = gyro_bias[0]; - ctrl_state.pitch_rate_bias = gyro_bias[1]; - ctrl_state.yaw_rate_bias = gyro_bias[2]; - - // Velocity in body frame - Vector3f v_n(velocity); - matrix::Dcm R_to_body(q.inversed()); - Vector3f v_b = R_to_body * v_n; - ctrl_state.x_vel = v_b(0); - ctrl_state.y_vel = v_b(1); - ctrl_state.z_vel = v_b(2); - - // Calculate velocity relative to wind in body frame - float velNE_wind[2] = {}; - _ekf.get_wind_velocity(velNE_wind); - v_n(0) -= velNE_wind[0]; - v_n(1) -= velNE_wind[1]; - _vel_body_wind = R_to_body * v_n; - - // Local Position NED - float position[3]; - _ekf.get_position(position); - ctrl_state.x_pos = position[0]; - ctrl_state.y_pos = position[1]; - ctrl_state.z_pos = position[2]; - - // Attitude quaternion - q.copyTo(ctrl_state.q); - - _ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter); - - // Acceleration data - matrix::Vector acceleration(sensors.accelerometer_m_s2); - - float accel_bias[3]; - _ekf.get_accel_bias(accel_bias); - ctrl_state.x_acc = acceleration(0) - accel_bias[0]; - ctrl_state.y_acc = acceleration(1) - accel_bias[1]; - ctrl_state.z_acc = acceleration(2) - accel_bias[2]; - - // compute lowpass filtered horizontal acceleration - acceleration = R_to_body.transpose() * acceleration; - _acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) + - acceleration(1) * acceleration(1)); - ctrl_state.horz_acc_mag = _acc_hor_filt; - - ctrl_state.airspeed_valid = false; - - // use estimated velocity for airspeed estimate - // TODO move this out of the estimators and put it into a dedicated air data consolidation algorithm - if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) { - // use measured airspeed - if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && now - airspeed.timestamp < 1e6 - && airspeed.timestamp > 0) { - ctrl_state.airspeed = airspeed.indicated_airspeed_m_s; - ctrl_state.airspeed_valid = true; - - } else { - // This airspeed mode requires a measurement which we no longer have, so wind relative speed - // is used as a surrogate and the validity is set to false. - ctrl_state.airspeed = sqrtf(v_n(0) * v_n(0) + v_n(1) * v_n(1) + v_n(2) * v_n(2)); - ctrl_state.airspeed_valid = false; - - } - - } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) { - if (_ekf.local_position_is_valid()) { - // This airspeed mode uses an estimate which is calculated from the wind relative speed - // TODO modify the ecl EKF to provide a boolean validity with the wind speed estimate and - // use that to set the validity of the estimated airspeed. - ctrl_state.airspeed = sqrtf(v_n(0) * v_n(0) + v_n(1) * v_n(1) + v_n(2) * v_n(2)); - ctrl_state.airspeed_valid = true; - - } - - } else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) { - // This airspeed mode has disabled airspeed use and controllers will handle this. - // We still return wind relative speed as a surrogate and set the validity to zero. - if (_ekf.local_position_is_valid()) { - ctrl_state.airspeed = sqrtf(v_n(0) * v_n(0) + v_n(1) * v_n(1) + v_n(2) * v_n(2)); - ctrl_state.airspeed_valid = false; - - } - } - - // publish control state data - if (_control_state_pub == nullptr) { - _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); - - } else { - orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); - } - } - + // In-run bias estimates + float gyro_bias[3]; + _ekf.get_gyro_bias(gyro_bias); { // generate vehicle attitude quaternion data - struct vehicle_attitude_s att = {}; + vehicle_attitude_s att; att.timestamp = now; q.copyTo(att.q); + _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); - att.rollspeed = gyro_rad[0]; - att.pitchspeed = gyro_rad[1]; - att.yawspeed = gyro_rad[2]; + att.rollspeed = sensors.gyro_rad[0] - gyro_bias[0]; + att.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1]; + att.yawspeed = sensors.gyro_rad[2] - gyro_bias[2]; // publish vehicle attitude data if (_att_pub == nullptr) { @@ -982,22 +849,27 @@ void Ekf2::run() // generate vehicle local position data vehicle_local_position_s &lpos = _vehicle_local_position_pub.get(); - float pos[3] = {}; - lpos.timestamp = now; // Position of body origin in local NED frame - _ekf.get_position(pos); + float position[3]; + _ekf.get_position(position); const float lpos_x_prev = lpos.x; const float lpos_y_prev = lpos.y; - lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f; - lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f; - lpos.z = pos[2]; + lpos.x = (_ekf.local_position_is_valid()) ? position[0] : 0.0f; + lpos.y = (_ekf.local_position_is_valid()) ? position[1] : 0.0f; + lpos.z = position[2]; // Velocity of body origin in local NED frame (m/s) + float velocity[3]; + _ekf.get_velocity(velocity); + lpos.vx = velocity[0]; lpos.vy = velocity[1]; lpos.vz = velocity[2]; + + float pos_d_deriv; + _ekf.get_pos_d_deriv(&pos_d_deriv); lpos.z_deriv = pos_d_deriv; // vertical position time derivative (m/s) // TODO: better status reporting @@ -1025,10 +897,11 @@ void Ekf2::run() matrix::Eulerf euler(q); lpos.yaw = euler.psi(); - float terrain_vpos; lpos.dist_bottom_valid = _ekf.get_terrain_valid(); + + float terrain_vpos; _ekf.get_terrain_vert_pos(&terrain_vpos); - lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters + lpos.dist_bottom = terrain_vpos - position[2]; // Distance to bottom surface (ground) in meters // constrain the distance to ground to _params->rng_gnd_clearance if (lpos.dist_bottom < _params->rng_gnd_clearance) { @@ -1036,7 +909,6 @@ void Ekf2::run() } lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate - lpos.surface_bottom_timestamp = now; // Time when new bottom surface found bool dead_reckoning; _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv, &dead_reckoning); @@ -1056,7 +928,6 @@ void Ekf2::run() vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); global_pos.timestamp = now; - global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) { map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &global_pos.lat, &global_pos.lon); @@ -1064,7 +935,7 @@ void Ekf2::run() global_pos.lat_lon_reset_counter = lpos.xy_reset_counter; - global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters + global_pos.alt = -position[2] + lpos.ref_alt; // Altitude AMSL in meters _ekf.get_posD_reset(&global_pos.delta_alt, &global_pos.alt_reset_counter); // global altitude has opposite sign of local down position global_pos.delta_alt *= -1.0f; @@ -1080,24 +951,65 @@ void Ekf2::run() global_pos.evh = lpos.evh; global_pos.evv = lpos.evv; - if (lpos.dist_bottom_valid) { + global_pos.terrain_alt_valid = lpos.dist_bottom_valid; + + if (global_pos.terrain_alt_valid) { global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 - global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid } else { global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 - global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid } - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning - global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m) + global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning + _vehicle_global_position_pub.update(); } - // publish estimator status { + // publish all corrected sensor readings and bias estimates after mag calibration is updated above + float accel_bias[3]; + _ekf.get_accel_bias(accel_bias); + + sensor_bias_s bias; + + bias.timestamp = now; + + bias.gyro_x = sensors.gyro_rad[0] - gyro_bias[0]; + bias.gyro_y = sensors.gyro_rad[1] - gyro_bias[1]; + bias.gyro_z = sensors.gyro_rad[2] - gyro_bias[2]; + + bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0]; + bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1]; + bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2]; + + bias.mag_x = sensors.magnetometer_ga[0] - (_last_valid_mag_cal[0] / 1000.0f); // mGauss -> Gauss + bias.mag_y = sensors.magnetometer_ga[1] - (_last_valid_mag_cal[1] / 1000.0f); // mGauss -> Gauss + bias.mag_z = sensors.magnetometer_ga[2] - (_last_valid_mag_cal[2] / 1000.0f); // mGauss -> Gauss + + bias.gyro_x_bias = gyro_bias[0]; + bias.gyro_y_bias = gyro_bias[1]; + bias.gyro_z_bias = gyro_bias[2]; + + bias.accel_x_bias = accel_bias[0]; + bias.accel_y_bias = accel_bias[1]; + bias.accel_z_bias = accel_bias[2]; + + bias.mag_x_bias = _last_valid_mag_cal[0]; + bias.mag_y_bias = _last_valid_mag_cal[1]; + bias.mag_z_bias = _last_valid_mag_cal[2]; + + if (_sensor_bias_pub == nullptr) { + _sensor_bias_pub = orb_advertise(ORB_ID(sensor_bias), &bias); + + } else { + orb_publish(ORB_ID(sensor_bias), _sensor_bias_pub, &bias); + } + } + + { + // publish estimator status estimator_status_s status; status.timestamp = now; _ekf.get_state_delayed(status.states); @@ -1184,14 +1096,14 @@ void Ekf2::run() if ((vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status.filter_fault_flags == 0) && (sensor_selection.mag_device_id == _mag_bias_id.get())) { - control::BlockParamFloat *mag_biases[] = { &_mag_bias_x, &_mag_bias_y, &_mag_bias_z }; + BlockParamFloat *mag_biases[] = { &_mag_bias_x, &_mag_bias_y, &_mag_bias_z }; for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) { if (_valid_cal_available[axis_index]) { // calculate weighting using ratio of variances and update stored bias values float weighting = _mag_bias_saved_variance.get() / (_mag_bias_saved_variance.get() + _last_valid_variance[axis_index]); - weighting = math::constrain(weighting, 0.0f, _mag_bias_alpha.get()); + weighting = constrain(weighting, 0.0f, _mag_bias_alpha.get()); float mag_bias_saved = mag_biases[axis_index]->get(); _last_valid_mag_cal[axis_index] = weighting * _last_valid_mag_cal[axis_index] + mag_bias_saved; mag_biases[axis_index]->set(_last_valid_mag_cal[axis_index]); @@ -1205,24 +1117,36 @@ void Ekf2::run() _total_cal_time_us = 0; } - // Publish wind estimate - wind_estimate_s wind_estimate; - wind_estimate.timestamp = now; - wind_estimate.windspeed_north = status.states[22]; - wind_estimate.windspeed_east = status.states[23]; - wind_estimate.covariance_north = status.covariances[22]; - wind_estimate.covariance_east = status.covariances[23]; + { + float velNE_wind[2]; + _ekf.get_wind_velocity(velNE_wind); - if (_wind_pub == nullptr) { - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); + // Calculate wind-compensated velocity in body frame + Vector3f v_wind_comp(velocity); + matrix::Dcmf R_to_body(q.inversed()); + v_wind_comp(0) -= velNE_wind[0]; + v_wind_comp(1) -= velNE_wind[1]; + _vel_body_wind = R_to_body * v_wind_comp; // TODO: move this elsewhere - } else { - orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); + // Publish wind estimate + wind_estimate_s wind_estimate; + wind_estimate.timestamp = now; + wind_estimate.windspeed_north = velNE_wind[0]; + wind_estimate.windspeed_east = velNE_wind[1]; + wind_estimate.variance_north = status.covariances[22]; + wind_estimate.variance_east = status.covariances[23]; + + if (_wind_pub == nullptr) { + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); + + } else { + orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); + } } } - // publish estimator innovation data { + // publish estimator innovation data ekf2_innovations_s innovations; innovations.timestamp = now; _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); @@ -1247,15 +1171,15 @@ void Ekf2::run() // calculate noise filtered velocity innovations which are used for pre-flight checking if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - float alpha = math::constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f); + float alpha = constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f); float beta = 1.0f - alpha; - _vel_innov_lpf_ned(0) = beta * _vel_innov_lpf_ned(0) + alpha * math::constrain(innovations.vel_pos_innov[0], + _vel_innov_lpf_ned(0) = beta * _vel_innov_lpf_ned(0) + alpha * constrain(innovations.vel_pos_innov[0], -_vel_innov_spike_lim, _vel_innov_spike_lim); - _vel_innov_lpf_ned(1) = beta * _vel_innov_lpf_ned(1) + alpha * math::constrain(innovations.vel_pos_innov[1], + _vel_innov_lpf_ned(1) = beta * _vel_innov_lpf_ned(1) + alpha * constrain(innovations.vel_pos_innov[1], -_vel_innov_spike_lim, _vel_innov_spike_lim); - _vel_innov_lpf_ned(2) = beta * _vel_innov_lpf_ned(2) + alpha * math::constrain(innovations.vel_pos_innov[2], + _vel_innov_lpf_ned(2) = beta * _vel_innov_lpf_ned(2) + alpha * constrain(innovations.vel_pos_innov[2], -_vel_innov_spike_lim, _vel_innov_spike_lim); - _hgt_innov_lpf = beta * _hgt_innov_lpf + alpha * math::constrain(innovations.vel_pos_innov[5], -_hgt_innov_spike_lim, + _hgt_innov_lpf = beta * _hgt_innov_lpf + alpha * constrain(innovations.vel_pos_innov[5], -_hgt_innov_spike_lim, _hgt_innov_spike_lim); _vel_innov_preflt_fail = ((_vel_innov_lpf_ned.norm() > _vel_innov_test_lim) || (fabsf(_hgt_innov_lpf) > _hgt_innov_test_lim)); @@ -1272,13 +1196,12 @@ void Ekf2::run() } else { orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); } - } } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp - struct vehicle_attitude_s att = {}; + vehicle_attitude_s att; att.timestamp = now; if (_att_pub == nullptr) { @@ -1289,8 +1212,8 @@ void Ekf2::run() } } - // publish ekf2_timestamps (using 0.1 ms relative timestamps) { + // publish ekf2_timestamps (using 0.1 ms relative timestamps) ekf2_timestamps_s ekf2_timestamps; ekf2_timestamps.timestamp = sensors.timestamp; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index db52fffd8a..ce2962f44b 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -32,14 +32,13 @@ ****************************************************************************/ /** - * @file parameters.c + * @file ekf2_params.c * Parameter definition for ekf2. * * @author Roman Bast * */ - /** * Minimum time of arrival delta between non-IMU observations before data is downsampled. * Baro and Magnetometer data will be averaged before downsampling, other data will be point sampled resulting in loss of information. @@ -631,15 +630,6 @@ PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f); */ PARAM_DEFINE_FLOAT(EKF2_EV_GATE, 5.0f); -/** - * Minimum valid range for the vision estimate - * - * @group EKF2 - * @min 0.01 - * @unit m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MIN_EV, 0.01f); /** * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum * 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 02e0ed670a..eb2563edbe 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -55,11 +55,13 @@ #include #include #include +#include #include +#include #include -#include #include #include +#include #include #include #include @@ -72,6 +74,8 @@ using matrix::Eulerf; using matrix::Quatf; +using uORB::Subscription; + /** * Fixedwing attitude control app start / stop handling function * @@ -112,9 +116,9 @@ private: bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle */ + int _att_sub; /**< vehicle attitude */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _battery_status_sub; /**< battery status subscription */ - int _ctrl_state_sub; /**< control state subscription */ int _global_pos_sub; /**< global position subscription */ int _manual_sub; /**< notification of manual control updates */ int _params_sub; /**< notification of parameter updates */ @@ -134,8 +138,8 @@ private: struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ struct battery_status_s _battery_status; /**< battery status */ - struct control_state_s _ctrl_state; /**< control state */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_attitude_s _att; /**< vehicle attitude setpoint */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global position */ @@ -143,17 +147,17 @@ private: struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ + Subscription _sub_airspeed; + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _debug; /**< if set to true, print debug output */ float _flaps_applied; float _flaperons_applied; - struct { float p_tc; float p_p; @@ -275,31 +279,24 @@ private: param_t bat_scale_en; - } _parameter_handles{}; /**< handles for interesting parameters */ + } _parameter_handles{}; /**< handles for interesting parameters */ // Rotation matrix and euler angles to extract from control state math::Matrix<3, 3> _R; - float _roll; - float _pitch; - float _yaw; + float _roll{0.0f}; + float _pitch{0.0f}; + float _yaw{0.0f}; ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; ECL_WheelController _wheel_ctrl; - /** * Update our local parameter cache. */ int parameters_update(); - /** - * Update control outputs - * - */ - void control_update(); - /** * Check for changes in vehicle control mode. */ @@ -359,9 +356,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _control_task(-1), /* subscriptions */ + _att_sub(-1), _att_sp_sub(-1), _battery_status_sub(-1), - _ctrl_state_sub(-1), _global_pos_sub(-1), _manual_sub(-1), _params_sub(-1), @@ -379,17 +376,13 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators_id(nullptr), _attitude_setpoint_id(nullptr), + _sub_airspeed(ORB_ID(airspeed), 0, 0, nullptr), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")), -#if 0 _nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano")), -#else - _nonfinite_input_perf(nullptr), - _nonfinite_output_perf(nullptr), -#endif /* states */ - _setpoint_valid(false), _debug(false), _flaps_applied(0), _flaperons_applied(0), @@ -400,9 +393,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* safely initialize structs */ _actuators = {}; _actuators_airframe = {}; + _att = {}; _att_sp = {}; _battery_status = {}; - _ctrl_state = {}; _global_pos = {}; _manual = {}; _rates_sp = {}; @@ -651,7 +644,6 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() if (att_sp_updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); - _setpoint_valid = true; } } @@ -729,8 +721,8 @@ FixedwingAttitudeControl::task_main() /* * do subscriptions */ + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _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)); @@ -748,12 +740,13 @@ FixedwingAttitudeControl::task_main() vehicle_status_poll(); vehicle_land_detected_poll(); battery_status_poll(); + _sub_airspeed.update(); /* wakeup source */ px4_pollfd_struct_t fds[1]; /* Setup of loop */ - fds[0].fd = _ctrl_state_sub; + fds[0].fd = _att_sub; fds[0].events = POLLIN; _task_running = true; @@ -802,11 +795,10 @@ FixedwingAttitudeControl::task_main() } /* load local copies */ - orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); /* get current rotation matrix and euler angles from control state quaternions */ - math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Quaternion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]); _R = q_att.to_dcm(); math::Vector<3> euler_angles; @@ -857,23 +849,18 @@ FixedwingAttitudeControl::task_main() _yaw = euler_angles(2); /* lastly, roll- and yawspeed have to be swaped */ - float helper = _ctrl_state.roll_rate; - _ctrl_state.roll_rate = -_ctrl_state.yaw_rate; - _ctrl_state.yaw_rate = helper; + float helper = _att.rollspeed; + _att.rollspeed = -_att.yawspeed; + _att.yawspeed = helper; } + _sub_airspeed.update(); vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - global_pos_poll(); - vehicle_status_poll(); - vehicle_land_detected_poll(); - battery_status_poll(); // the position controller will not emit attitude setpoints in some modes @@ -953,19 +940,17 @@ FixedwingAttitudeControl::task_main() /* scale around tuning airspeed */ float airspeed; - bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed); - /* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */ - if (nonfinite || !_ctrl_state.airspeed_valid) { - airspeed = _parameters.airspeed_trim; + const bool airspeed_valid = PX4_ISFINITE(_sub_airspeed.get().indicated_airspeed_m_s) + && ((_sub_airspeed.get().timestamp - hrt_absolute_time()) < 1e6); - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } + if (airspeed_valid) { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _sub_airspeed.get().indicated_airspeed_m_s); } else { - /* prevent numerical drama by requiring 0.5 m/s minimal speed */ - airspeed = math::max(0.5f, _ctrl_state.airspeed); + airspeed = _parameters.airspeed_trim; + perf_count(_nonfinite_input_perf); } /* @@ -1041,9 +1026,9 @@ FixedwingAttitudeControl::task_main() control_input.roll = _roll; control_input.pitch = _pitch; control_input.yaw = _yaw; - control_input.body_x_rate = _ctrl_state.roll_rate; - control_input.body_y_rate = _ctrl_state.pitch_rate; - control_input.body_z_rate = _ctrl_state.yaw_rate; + control_input.body_x_rate = _att.rollspeed; + control_input.body_y_rate = _att.pitchspeed; + control_input.body_z_rate = _att.yawspeed; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; control_input.yaw_setpoint = yaw_sp; @@ -1227,9 +1212,9 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _ctrl_state.timestamp; + _actuators.timestamp_sample = _att.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); - _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; + _actuators_airframe.timestamp_sample = _att.timestamp; /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_rates_enabled || diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 13543b9e73..434ef3c6aa 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -528,17 +528,12 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); /** - * Airspeed mode + * Disable airspeed sensor * - * The param value sets the method used to publish the control state airspeed. * For small wings or VTOL without airspeed sensor this parameter can be used to * enable flying without an airspeed reading * - * @min 0 - * @max 2 - * @value 0 use measured airspeed - * @value 1 use vehicle ground velocity as airspeed - * @value 2 declare airspeed invalid + * @boolean * @group FW Attitude Control */ PARAM_DEFINE_INT32(FW_ARSP_MODE, 0); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 86c9f968be..0f91efe9a3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -39,6 +39,8 @@ FixedwingPositionControl *l1_control::g_control; static int _control_task = -1; ///< task handle for sensor task */ FixedwingPositionControl::FixedwingPositionControl() : + _sub_airspeed(ORB_ID(airspeed), 0, 0, nullptr), + _sub_sensors(ORB_ID(sensor_bias), 0, 0, nullptr), _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")) { _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); @@ -48,7 +50,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); - _parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE"); + _parameter_handles.airspeed_disabled = param_find("FW_ARSP_MODE"); _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); @@ -134,7 +136,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); param_get(_parameter_handles.airspeed_trans, &(_parameters.airspeed_trans)); - param_get(_parameter_handles.airspeed_mode, &(_parameters.airspeed_mode)); + param_get(_parameter_handles.airspeed_disabled, &(_parameters.airspeed_disabled)); param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max)); @@ -319,27 +321,47 @@ FixedwingPositionControl::manual_control_setpoint_poll() } void -FixedwingPositionControl::control_state_poll() +FixedwingPositionControl::airspeed_poll() { - /* check if there is a new position */ - bool ctrl_state_updated; - orb_check(_ctrl_state_sub, &ctrl_state_updated); - - if (ctrl_state_updated) { - orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - _airspeed_valid = _ctrl_state.airspeed_valid; + if (!_parameters.airspeed_disabled && _sub_airspeed.updated()) { + _sub_airspeed.update(); + _airspeed_valid = PX4_ISFINITE(_sub_airspeed.get().indicated_airspeed_m_s) + && PX4_ISFINITE(_sub_airspeed.get().true_airspeed_m_s); _airspeed_last_received = hrt_absolute_time(); + _airspeed = _sub_airspeed.get().indicated_airspeed_m_s; + + if (_sub_airspeed.get().indicated_airspeed_m_s > 0.0f + && _sub_airspeed.get().true_airspeed_m_s > _sub_airspeed.get().indicated_airspeed_m_s) { + _eas2tas = max(_sub_airspeed.get().true_airspeed_m_s / _sub_airspeed.get().indicated_airspeed_m_s, 1.0f); + + } else { + _eas2tas = 1.0f; + } } else { - /* no airspeed updates for one second */ if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_received) > 1e6) { _airspeed_valid = false; } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); +} + +void +FixedwingPositionControl::vehicle_attitude_poll() +{ + /* check if there is a new position */ + bool updated; + orb_check(_vehicle_attitude_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); + } + /* set rotation matrix and euler angles */ - math::Quaternion q_att(_ctrl_state.q); + math::Quaternion q_att(_att.q); _R_nb = q_att.to_dcm(); math::Vector<3> euler_angles; @@ -347,9 +369,6 @@ FixedwingPositionControl::control_state_poll() _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); - - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); } void @@ -650,7 +669,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _att_sp.apply_flaps = false; // by default we don't use flaps /* filter speed and altitude for controller */ - math::Vector<3> accel_body(_ctrl_state.x_acc, _ctrl_state.y_acc, _ctrl_state.z_acc); + math::Vector<3> accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z); // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame @@ -669,14 +688,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _control_mode.flag_control_altitude_enabled)); /* update TECS filters */ - _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, + _tecs.update_state(_global_pos.alt, _airspeed, _R_nb, accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); // l1 navigation logic breaks down when wind speed exceeds max airspeed // compute 2D groundspeed from airspeed-heading projection - math::Vector<2> air_speed_2d{_ctrl_state.airspeed * cosf(_yaw), _ctrl_state.airspeed * sinf(_yaw)}; + math::Vector<2> air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)}; math::Vector<2> nav_speed_2d{0.0f, 0.0f}; // angle between air_speed_2d and ground_speed @@ -1063,8 +1082,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - Eulerf euler(Quatf(_ctrl_state.q)); - _runway_takeoff.init(euler.psi(), _global_pos.lat, _global_pos.lon); + _runway_takeoff.init(_yaw, _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ @@ -1076,7 +1094,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper - _runway_takeoff.update(_ctrl_state.airspeed, _global_pos.alt - terrain_alt, + _runway_takeoff.update(_airspeed, _global_pos.alt - terrain_alt, _global_pos.lat, _global_pos.lon, &_mavlink_log_pub); /* @@ -1124,8 +1142,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _launch_detection_notify = hrt_absolute_time(); } - /* Detect launch */ - _launchDetector.update(_ctrl_state.x_acc); + /* Detect launch using body X (forward) acceleration */ + _launchDetector.update(_sub_sensors.get().accel_x); /* update our copy of the launch detection state */ _launch_detection_state = _launchDetector.getLaunchDetected(); @@ -1271,7 +1289,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ - if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading _yaw_lock_engaged = true; @@ -1497,8 +1515,8 @@ FixedwingPositionControl::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); @@ -1550,6 +1568,7 @@ FixedwingPositionControl::task_main() vehicle_command_poll(); vehicle_land_detected_poll(); vehicle_status_poll(); + _sub_sensors.update(); /* only update parameters if they changed */ bool params_updated = false; @@ -1592,7 +1611,8 @@ FixedwingPositionControl::task_main() _alt_reset_counter = _global_pos.alt_reset_counter; _pos_reset_counter = _global_pos.lat_lon_reset_counter; - control_state_poll(); + airspeed_poll(); + vehicle_attitude_poll(); manual_control_setpoint_poll(); position_setpoint_triplet_poll(); @@ -1741,12 +1761,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee _was_in_transition = true; // set this to transition airspeed to init tecs correctly - if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) { + if (_parameters.airspeed_disabled) { // some vtols fly without airspeed sensor _asp_after_transition = _parameters.airspeed_trans; } else { - _asp_after_transition = _ctrl_state.airspeed; + _asp_after_transition = _airspeed; } _asp_after_transition = constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max); @@ -1755,8 +1775,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee // after transition we ramp up desired airspeed from the speed we had coming out of the transition _asp_after_transition += dt * 2; // increase 2m/s - if (_asp_after_transition < airspeed_sp && _ctrl_state.airspeed < airspeed_sp) { - airspeed_sp = max(_asp_after_transition, _ctrl_state.airspeed); + if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) { + airspeed_sp = max(_asp_after_transition, _airspeed); } else { _was_in_transition = false; @@ -1801,11 +1821,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee pitch_for_tecs = euler(1); } - float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - _tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, _global_pos.alt, alt_sp, - airspeed_sp, _ctrl_state.airspeed, eas2tas, + airspeed_sp, _airspeed, _eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 2c1ee9cec4..b247409623 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -72,12 +72,15 @@ #include #include #include -#include +#include +#include #include #include #include #include +#include #include +#include #include #include #include @@ -87,15 +90,22 @@ #include #include -#define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode -#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode -#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane -#define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode -#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll/yaw input from user which does not change the locked heading -#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid -#define THROTTLE_THRESH 0.05f ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f ///< a throttle / pitch input above this value leads to the system switching to climbout mode -#define ALTHOLD_EPV_RESET_THRESH 5.0f +static constexpr float HDG_HOLD_DIST_NEXT = + 3000.0f; // initial distance of waypoint in front of plane in heading hold mode +static constexpr float HDG_HOLD_REACHED_DIST = + 1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode +static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane +static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode +static constexpr float HDG_HOLD_MAN_INPUT_THRESH = + 0.01f; // max manual roll/yaw input from user which does not change the locked heading + +static constexpr hrt_abstime T_ALT_TIMEOUT = 1; // time after which we abort landing if terrain estimate is not valid + +static constexpr float THROTTLE_THRESH = + 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = + 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; using math::constrain; using math::max; @@ -108,6 +118,8 @@ using matrix::Quatf; using matrix::Vector2f; using matrix::Vector3f; +using uORB::Subscription; + using namespace launchdetection; using namespace runwaytakeoff; @@ -141,8 +153,8 @@ private: int _global_pos_sub{-1}; int _pos_sp_triplet_sub{-1}; - int _ctrl_state_sub{-1}; ///< control state subscription */ int _control_mode_sub{-1}; ///< control mode subscription */ + int _vehicle_attitude_sub{-1}; ///< vehicle attitude subscription */ int _vehicle_command_sub{-1}; ///< vehicle command subscription */ int _vehicle_status_sub{-1}; ///< vehicle status subscription */ int _vehicle_land_detected_sub{-1}; ///< vehicle land detected subscription */ @@ -155,10 +167,10 @@ private: orb_id_t _attitude_setpoint_id{nullptr}; - control_state_s _ctrl_state {}; ///< control state */ fw_pos_ctrl_status_s _fw_pos_ctrl_status {}; ///< navigation capabilities */ manual_control_setpoint_s _manual {}; ///< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items */ + vehicle_attitude_s _att {}; ///< vehicle attitude setpoint */ vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint */ vehicle_command_s _vehicle_command {}; ///< vehicle commands */ vehicle_control_mode_s _control_mode {}; ///< control mode */ @@ -166,6 +178,9 @@ private: vehicle_land_detected_s _vehicle_land_detected {}; ///< vehicle land detected */ vehicle_status_s _vehicle_status {}; ///< vehicle status */ + Subscription _sub_airspeed; + Subscription _sub_sensors; + perf_counter_t _loop_perf; ///< loop performance counter */ float _hold_alt{0.0f}; ///< hold altitude for altitude mode */ @@ -214,6 +229,8 @@ private: /* throttle and airspeed states */ bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists hrt_abstime _airspeed_last_received{0}; ///< last time airspeed was received. Used to detect timeouts. + float _airspeed{0.0f}; + float _eas2tas{1.0f}; float _groundspeed_undershoot{0.0f}; ///< ground speed error to min. speed in m/s @@ -269,7 +286,7 @@ private: float airspeed_trim; float airspeed_max; float airspeed_trans; - int32_t airspeed_mode; + int32_t airspeed_disabled; float pitch_limit_min; float pitch_limit_max; @@ -327,7 +344,7 @@ private: param_t airspeed_trim; param_t airspeed_max; param_t airspeed_trans; - param_t airspeed_mode; + param_t airspeed_disabled; param_t pitch_limit_min; param_t pitch_limit_max; @@ -366,10 +383,11 @@ private: int parameters_update(); // Update subscriptions - void control_state_poll(); + void airspeed_poll(); void control_update(); void manual_control_setpoint_poll(); void position_setpoint_triplet_poll(); + void vehicle_attitude_poll(); void vehicle_command_poll(); void vehicle_control_mode_poll(); void vehicle_land_detected_poll(); diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp index 14bdc58c14..cafe2c3041 100644 --- a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp +++ b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp @@ -184,7 +184,7 @@ void GroundRoverAttitudeControl::task_main() { _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -204,7 +204,7 @@ GroundRoverAttitudeControl::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; - fds[1].fd = _ctrl_state_sub; + fds[1].fd = _att_sub; fds[1].events = POLLIN; _task_running = true; @@ -252,7 +252,7 @@ GroundRoverAttitudeControl::task_main() } /* load local copies */ - orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); vehicle_attitude_setpoint_poll(); vehicle_control_mode_poll(); @@ -264,10 +264,10 @@ GroundRoverAttitudeControl::task_main() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled) { - Eulerf euler_angles(matrix::Quatf(_ctrl_state.q)); + Eulerf euler_angles(matrix::Quatf(_att.q)); /* Calculate the control output for the steering as yaw */ - float yaw_u = pid_calculate(&_steering_ctrl, _att_sp.yaw_body, euler_angles.psi(), _ctrl_state.yaw_rate, deltaT); + float yaw_u = pid_calculate(&_steering_ctrl, _att_sp.yaw_body, euler_angles.psi(), _att.yawspeed, deltaT); float angle_diff = 0.0f; @@ -322,7 +322,7 @@ GroundRoverAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _ctrl_state.timestamp; + _actuators.timestamp_sample = _att.timestamp; /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_attitude_enabled || diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp index c61e2a740c..918f0e5ec3 100644 --- a/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp +++ b/src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp @@ -53,9 +53,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -81,7 +81,7 @@ private: int _att_sp_sub{-1}; /**< vehicle attitude setpoint */ int _battery_status_sub{-1}; /**< battery status subscription */ - int _ctrl_state_sub{-1}; /**< control state subscription */ + int _att_sub{-1}; /**< control state subscription */ int _manual_sub{-1}; /**< notification of manual control updates */ int _params_sub{-1}; /**< notification of parameter updates */ int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */ @@ -90,8 +90,8 @@ private: actuator_controls_s _actuators {}; /**< actuator control inputs */ battery_status_s _battery_status {}; /**< battery status */ - control_state_s _ctrl_state {}; /**< control state */ manual_control_setpoint_s _manual {}; /**< r/c channel data */ + vehicle_attitude_s _att {}; /**< control state */ vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */ vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index b750286342..279aab4faa 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -48,6 +48,7 @@ static int _control_task = -1; /**< task handle for sensor task */ using matrix::Eulerf; using matrix::Quatf; +using matrix::Vector3f; /** * L1 control app start / stop handling function @@ -64,7 +65,9 @@ GroundRoverPositionControl *g_control = nullptr; GroundRoverPositionControl::GroundRoverPositionControl() : /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")) + _sub_attitude(ORB_ID(vehicle_attitude), 0, 0, nullptr), + _sub_sensors(ORB_ID(sensor_bias), 0, 0, nullptr), + _loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters { _parameter_handles.l1_period = param_find("GND_L1_PERIOD"); _parameter_handles.l1_damping = param_find("GND_L1_DAMPING"); @@ -178,17 +181,6 @@ GroundRoverPositionControl::manual_control_setpoint_poll() } } -void -GroundRoverPositionControl::control_state_poll() -{ - bool ctrl_state_updated; - orb_check(_ctrl_state_sub, &ctrl_state_updated); - - if (ctrl_state_updated) { - orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); - } -} - void GroundRoverPositionControl::position_setpoint_triplet_poll() { @@ -260,9 +252,16 @@ GroundRoverPositionControl::control_position(const math::Vector<2> ¤t_posi mission_target_speed = _pos_sp_triplet.current.cruising_speed; } - //Compute airspeed control out and just scale it as a constant + // Velocity in body frame + const Dcmf R_to_body(Quatf(_sub_attitude.get().q).inversed()); + const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); + + const float x_vel = vel(0); + const float x_acc = _sub_sensors.get().accel_x; + + // Compute airspeed control out and just scale it as a constant mission_throttle = _parameters.throttle_speed_scaler - * pid_calculate(&_speed_ctrl, mission_target_speed, _ctrl_state.x_vel, _ctrl_state.x_acc, dt); + * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt); // Constrain throttle between min and max mission_throttle = math::constrain(mission_throttle, _parameters.throttle_min, _parameters.throttle_max); @@ -331,7 +330,6 @@ void GroundRoverPositionControl::task_main() { _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -409,9 +407,10 @@ GroundRoverPositionControl::task_main() // update the reset counters in any case _pos_reset_counter = _global_pos.lat_lon_reset_counter; - control_state_poll(); manual_control_setpoint_poll(); position_setpoint_triplet_poll(); + _sub_attitude.update(); + _sub_sensors.update(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp index 901cf9cf43..8498f8d17b 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp @@ -54,11 +54,13 @@ #include #include #include -#include +#include #include #include #include #include +#include +#include #include #include #include @@ -66,6 +68,8 @@ using matrix::Dcmf; +using uORB::Subscription; + class GroundRoverPositionControl { public: @@ -96,13 +100,11 @@ private: bool _task_running{false}; /**< if true, task is running in its mainloop */ int _control_mode_sub{-1}; /**< control mode subscription */ - int _ctrl_state_sub{-1}; /**< control state subscription */ int _global_pos_sub{-1}; int _manual_control_sub{-1}; /**< notification of manual control updates */ int _params_sub{-1}; /**< notification of parameter updates */ int _pos_sp_triplet_sub{-1}; - control_state_s _ctrl_state{}; /**< control state */ fw_pos_ctrl_status_s _gnd_pos_ctrl_status{}; /**< navigation capabilities */ manual_control_setpoint_s _manual{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ @@ -110,6 +112,9 @@ private: vehicle_control_mode_s _control_mode{}; /**< control mode */ vehicle_global_position_s _global_pos{}; /**< global vehicle position */ + Subscription _sub_attitude; + Subscription _sub_sensors; + perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /** */ -#include -#include +#include "FixedwingLandDetector.h" #include -#include "FixedwingLandDetector.h" +#include +#include namespace land_detector { @@ -62,16 +62,18 @@ FixedwingLandDetector::FixedwingLandDetector() void FixedwingLandDetector::_initialize_topics() { - _controlStateSub = orb_subscribe(ORB_ID(control_state)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); } void FixedwingLandDetector::_update_topics() { - _orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); + _orb_update(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); } void FixedwingLandDetector::_update_params() @@ -87,7 +89,7 @@ float FixedwingLandDetector::_get_max_altitude() // TODO // This means no altitude limit as the limit // is always current position plus 1000 meters - return roundf(-_controlState.z_pos + 1000); + return roundf(-_local_pos.z + 1000); } bool FixedwingLandDetector::_get_freefall_state() @@ -117,15 +119,18 @@ bool FixedwingLandDetector::_get_landed_state() bool landDetected = false; - if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) { - float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel * - _controlState.x_vel + _controlState.y_vel * _controlState.y_vel); + if (hrt_elapsed_time(&_local_pos.timestamp) < 500 * 1000) { + + // horizontal velocity + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * + _local_pos.vy); if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } - val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel); + // vertical velocity + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_local_pos.vz); if (PX4_ISFINITE(val)) { _velocity_z_filtered = val; @@ -135,7 +140,9 @@ bool FixedwingLandDetector::_get_landed_state() // a leaking lowpass prevents biases from building up, but // gives a mostly correct response for short impulses - _accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f; + const float acc_hor = sqrtf(_sensors.accel_x * _sensors.accel_x + + _sensors.accel_y * _sensors.accel_y); + _accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f; // crude land detector for fixedwing landDetected = _velocity_xy_filtered < _params.maxVelocity diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 1835506134..20e1ac31d8 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,9 +42,10 @@ #pragma once -#include #include #include +#include +#include #include "LandDetector.h" @@ -85,13 +86,15 @@ private: float maxIntVelocity; } _params{}; - int _controlStateSub{-1}; int _armingSub{-1}; int _airspeedSub{-1}; + int _sensor_bias_sub{-1}; + int _local_pos_sub{-1}; - control_state_s _controlState{}; actuator_armed_s _arming{}; airspeed_s _airspeed{}; + sensor_bias_s _sensors{}; + vehicle_local_position_s _local_pos{}; float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index d9d6ba9c9a..7419e53eec 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -80,7 +80,7 @@ MulticopterLandDetector::MulticopterLandDetector() : _armingSub(-1), _attitudeSub(-1), _manualSub(-1), - _ctrl_state_sub(-1), + _sensor_bias_sub(-1), _vehicle_control_mode_sub(-1), _battery_sub(-1), _vehicleLocalPosition{}, @@ -89,7 +89,7 @@ MulticopterLandDetector::MulticopterLandDetector() : _arming{}, _vehicleAttitude{}, _manual{}, - _ctrl_state{}, + _sensors{}, _control_mode{}, _battery{}, _min_trust_start(0), @@ -125,7 +125,7 @@ void MulticopterLandDetector::_initialize_topics() _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); + _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _battery_sub = orb_subscribe(ORB_ID(battery_status)); } @@ -138,7 +138,7 @@ void MulticopterLandDetector::_update_topics() _orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); _orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); - _orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); _orb_update(ORB_ID(battery_status), _battery_sub, &_battery); } @@ -170,14 +170,14 @@ bool MulticopterLandDetector::_get_freefall_state() return false; } - if (_ctrl_state.timestamp == 0) { - // _ctrl_state is not valid yet, we have to assume we're not falling. + if (_sensors.timestamp == 0) { + // _sensors is not valid yet, we have to assume we're not falling. return false; } - float acc_norm = _ctrl_state.x_acc * _ctrl_state.x_acc - + _ctrl_state.y_acc * _ctrl_state.y_acc - + _ctrl_state.z_acc * _ctrl_state.z_acc; + float acc_norm = _sensors.accel_x * _sensors.accel_x + + _sensors.accel_y * _sensors.accel_y + + _sensors.accel_z * _sensors.accel_z; acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed. return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 7bbec9bf1d..22a9bfa976 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -42,19 +42,22 @@ #pragma once +#include "LandDetector.h" + #include #include #include #include #include #include -#include -#include -#include -#include +#include #include - -#include "LandDetector.h" +#include +#include +#include +#include +#include +#include namespace land_detector { @@ -135,7 +138,7 @@ private: int _armingSub; int _attitudeSub; int _manualSub; - int _ctrl_state_sub; + int _sensor_bias_sub; int _vehicle_control_mode_sub; int _battery_sub; @@ -145,7 +148,7 @@ private: struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; struct manual_control_setpoint_s _manual; - struct control_state_s _ctrl_state; + struct sensor_bias_s _sensors; struct vehicle_control_mode_s _control_mode; struct battery_status_s _battery; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index cfb9749821..5822d25d6a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -623,7 +623,6 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().ref_alt = _altOrigin; _pub_lpos.get().dist_bottom = _aglLowPass.getState(); _pub_lpos.get().dist_bottom_rate = - xLP(X_vz); - _pub_lpos.get().surface_bottom_timestamp = _timeStamp; // we estimate agl even when we don't have terrain info // if you are in terrain following mode this is important // so that if terrain estimation fails there isn't a @@ -687,7 +686,6 @@ void BlockLocalPositionEstimator::publishGlobalPos() PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && PX4_ISFINITE(xLP(X_vz))) { _pub_gpos.get().timestamp = _timeStamp; - _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; @@ -701,10 +699,10 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().yaw = _eul(2); _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; + _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ; _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY); - _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.update(); // TODO provide calculated values for these _pub_gpos.get().evh = 0.0f; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7624e5e733..6ab40b3a25 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -632,6 +633,9 @@ private: MavlinkOrbSubscription *_sensor_sub; uint64_t _sensor_time; + MavlinkOrbSubscription *_bias_sub; + uint64_t _bias_time; + MavlinkOrbSubscription *_differential_pressure_sub; uint64_t _differential_pressure_time; @@ -648,6 +652,8 @@ protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), _sensor_time(0), + _bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))), + _bias_time(0), _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), _differential_pressure_time(0), _accel_timestamp(0), @@ -659,6 +665,7 @@ protected: bool send(const hrt_abstime t) { struct sensor_combined_s sensor = {}; + struct sensor_bias_s bias = {}; struct differential_pressure_s differential_pressure = {}; if (_sensor_sub->update(&_sensor_time, &sensor)) { @@ -688,20 +695,21 @@ protected: _baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative; } + _bias_sub->update(&_bias_time, &bias); _differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); mavlink_highres_imu_t msg = {}; msg.time_usec = sensor.timestamp; - msg.xacc = sensor.accelerometer_m_s2[0]; - msg.yacc = sensor.accelerometer_m_s2[1]; - msg.zacc = sensor.accelerometer_m_s2[2]; - msg.xgyro = sensor.gyro_rad[0]; - msg.ygyro = sensor.gyro_rad[1]; - msg.zgyro = sensor.gyro_rad[2]; - msg.xmag = sensor.magnetometer_ga[0]; - msg.ymag = sensor.magnetometer_ga[1]; - msg.zmag = sensor.magnetometer_ga[2]; + msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_x_bias; + msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_y_bias; + msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_z_bias; + msg.xgyro = sensor.gyro_rad[0] - bias.gyro_x_bias; + msg.ygyro = sensor.gyro_rad[1] - bias.gyro_y_bias; + msg.zgyro = sensor.gyro_rad[2] - bias.gyro_z_bias; + msg.xmag = sensor.magnetometer_ga[0] - bias.mag_x_bias; + msg.ymag = sensor.magnetometer_ga[1] - bias.mag_y_bias; + msg.zmag = sensor.magnetometer_ga[2] - bias.mag_z_bias; msg.abs_pressure = 0; msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; msg.pressure_alt = sensor.baro_alt_meter; @@ -3878,7 +3886,7 @@ protected: msg.wind_y = wind_estimate.windspeed_east; msg.wind_z = 0.0f; - msg.var_horiz = wind_estimate.covariance_north + wind_estimate.covariance_east; + msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east; msg.var_vert = 0.0f; struct vehicle_global_position_s global_pos = {}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 16b9899e26..511c5dd10e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -137,7 +137,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _follow_target_pub(nullptr), _transponder_report_pub(nullptr), _collision_report_pub(nullptr), - _control_state_pub(nullptr), _debug_key_value_pub(nullptr), _debug_value_pub(nullptr), _debug_vect_pub(nullptr), @@ -1188,9 +1187,6 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg) vision_position.timestamp = sync_stamp(pos.time_usec); - // Use the estimator type to identify the external estimate - vision_position.estimator_type = pos.estimator_type; - vision_position.xy_valid = true; vision_position.z_valid = true; vision_position.v_xy_valid = true; @@ -1204,10 +1200,6 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg) vision_position.vy = pos.vy; vision_position.vz = pos.vz; - vision_position.ax = pos.ax; - vision_position.ay = pos.ay; - vision_position.az = pos.az; - // Low risk change for now. TODO : full covariance matrix vision_position.eph = sqrtf(fmaxf(pos.covariance[0], pos.covariance[9])); vision_position.epv = sqrtf(pos.covariance[17]); @@ -1232,9 +1224,6 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) struct vehicle_local_position_s vision_position = {}; - // Use the estimator type to identify the simple vision estimate - vision_position.estimator_type = MAV_ESTIMATOR_TYPE_VISION; - vision_position.timestamp = sync_stamp(pos.usec); vision_position.x = pos.x; vision_position.y = pos.y; @@ -2220,7 +2209,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* global position */ { struct vehicle_global_position_s hil_global_pos = {}; - matrix::Eulerf euler = matrix::Quatf(hil_attitude.q); hil_global_pos.timestamp = timestamp; hil_global_pos.lat = hil_state.lat / ((double)1e7); @@ -2229,7 +2217,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_n = hil_state.vx / 100.0f; hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; - hil_global_pos.yaw = euler.psi(); hil_global_pos.eph = 2.0f; hil_global_pos.epv = 4.0f; @@ -2338,66 +2325,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } - - /* control state */ - control_state_s ctrl_state = {}; - matrix::Quatf q(hil_state.attitude_quaternion); - matrix::Dcmf R_to_body(q.inversed()); - - //Time - ctrl_state.timestamp = hrt_absolute_time(); - - //Roll Rates: - //ctrl_state: body angular rate (rad/s, x forward/y right/z down) - //hil_state : body frame angular speed (rad/s) - ctrl_state.roll_rate = hil_state.rollspeed; - ctrl_state.pitch_rate = hil_state.pitchspeed; - ctrl_state.yaw_rate = hil_state.yawspeed; - - // Local Position NED: - //ctrl_state: position in local earth frame - //hil_state : Latitude/Longitude expressed as * 1E7 - float x = 0.0f; - float y = 0.0f; - double lat = hil_state.lat * 1e-7; - double lon = hil_state.lon * 1e-7; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); - ctrl_state.x_pos = x; - ctrl_state.y_pos = y; - ctrl_state.z_pos = hil_state.alt / 1000.0f; - - // Attitude quaternion - q.copyTo(ctrl_state.q); - - // Velocity - //ctrl_state: velocity in body frame (x forward/y right/z down) - //hil_state : Ground Speed in NED expressed as m/s * 100 - matrix::Vector3f v_n(hil_state.vx, hil_state.vy, hil_state.vz); - matrix::Vector3f v_b = R_to_body * v_n; - ctrl_state.x_vel = v_b(0) / 100.0f; - ctrl_state.y_vel = v_b(1) / 100.0f; - ctrl_state.z_vel = v_b(2) / 100.0f; - - // Acceleration - //ctrl_state: acceleration in body frame - //hil_state : acceleration in body frame - ctrl_state.x_acc = hil_state.xacc; - ctrl_state.y_acc = hil_state.yacc; - ctrl_state.z_acc = hil_state.zacc; - - static float _acc_hor_filt = 0; - _acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(ctrl_state.x_acc * ctrl_state.x_acc + ctrl_state.y_acc * - ctrl_state.y_acc); - ctrl_state.horz_acc_mag = _acc_hor_filt; - ctrl_state.airspeed_valid = false; - - // publish control state data - if (_control_state_pub == nullptr) { - _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); - - } else { - orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); - } } void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3265ffe91a..0b92ef9b31 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -77,10 +77,8 @@ #include #include #include -#include #include - #include "mavlink_mission.h" #include "mavlink_parameters.h" #include "mavlink_ftp.h" @@ -243,7 +241,6 @@ private: orb_advert_t _follow_target_pub; orb_advert_t _transponder_report_pub; orb_advert_t _collision_report_pub; - orb_advert_t _control_state_pub; orb_advert_t _debug_key_value_pub; orb_advert_t _debug_value_pub; orb_advert_t _debug_vect_pub; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 31df082c0f..9a080779b2 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -71,13 +71,14 @@ #include #include #include -#include #include #include #include #include +#include #include #include +#include #include #include #include @@ -129,7 +130,7 @@ private: bool _task_should_exit; /**< if true, task_main() should exit */ int _control_task; /**< task handle */ - int _ctrl_state_sub; /**< control state subscription */ + int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ int _v_control_mode_sub; /**< vehicle control mode subscription */ @@ -137,10 +138,11 @@ private: int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ - int _motor_limits_sub; /**< motor limits subscription */ - int _battery_status_sub; /**< battery status subscription */ - int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ - int _sensor_correction_sub; /**< sensor thermal correction subscription */ + int _motor_limits_sub; /**< motor limits subscription */ + int _battery_status_sub; /**< battery status subscription */ + int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ + int _sensor_correction_sub; /**< sensor thermal correction subscription */ + int _sensor_bias_sub; /**< sensor in-run bias correction subscription */ unsigned _gyro_count; int _selected_gyro; @@ -154,7 +156,7 @@ private: bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct control_state_s _ctrl_state; /**< control state */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ @@ -166,6 +168,7 @@ private: struct battery_status_s _battery_status; /**< battery status */ struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */ struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */ + struct sensor_bias_s _sensor_bias; /**< sensor in-run bias corrections */ MultirotorMixer::saturation_status _saturation_status{}; @@ -339,13 +342,18 @@ private: /** * Check for control state updates. */ - void control_state_poll(); + void vehicle_attitude_poll(); /** * Check for sensor thermal correction updates. */ void sensor_correction_poll(); + /** + * Check for sensor in-run bias correction updates. + */ + void sensor_bias_poll(); + /** * Shim for calling task_main from task_create. */ @@ -369,7 +377,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _ctrl_state_sub(-1), + _v_att_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), @@ -379,6 +387,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _motor_limits_sub(-1), _battery_status_sub(-1), _sensor_correction_sub(-1), + _sensor_bias_sub(-1), /* gyro selection */ _gyro_count(1), @@ -393,7 +402,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_circuit_breaker_enabled(false), - _ctrl_state{}, + _v_att{}, _v_att_sp{}, _v_rates_sp{}, _manual_control_sp{}, @@ -405,6 +414,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _battery_status{}, _sensor_gyro{}, _sensor_correction{}, + _sensor_bias{}, + _saturation_status{}, /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), @@ -787,14 +798,14 @@ MulticopterAttitudeControl::battery_status_poll() } void -MulticopterAttitudeControl::control_state_poll() +MulticopterAttitudeControl::vehicle_attitude_poll() { /* check if there is a new message */ bool updated; - orb_check(_ctrl_state_sub, &updated); + orb_check(_v_att_sub, &updated); if (updated) { - orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); } } @@ -815,6 +826,19 @@ MulticopterAttitudeControl::sensor_correction_poll() } } +void +MulticopterAttitudeControl::sensor_bias_poll() +{ + /* check if there is a new message */ + bool updated; + orb_check(_sensor_bias_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias); + } + +} + /** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) @@ -832,7 +856,7 @@ MulticopterAttitudeControl::control_attitude(float dt) math::Matrix<3, 3> R_sp = q_sp.to_dcm(); /* get current rotation matrix from control state quaternions */ - math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Quaternion q_att(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]); math::Matrix<3, 3> R = q_att.to_dcm(); /* all input data is ready, run controller itself */ @@ -995,9 +1019,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) rates = _board_rotation * rates; // correct for in-run bias errors - rates(0) -= _ctrl_state.roll_rate_bias; - rates(1) -= _ctrl_state.pitch_rate_bias; - rates(2) -= _ctrl_state.yaw_rate_bias; + rates(0) -= _sensor_bias.gyro_x_bias; + rates(1) -= _sensor_bias.gyro_y_bias; + rates(2) -= _sensor_bias.gyro_z_bias; math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p)); //math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i)); @@ -1071,9 +1095,9 @@ MulticopterAttitudeControl::task_main() /* * do subscriptions */ + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -1093,6 +1117,7 @@ MulticopterAttitudeControl::task_main() } _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); + _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); /* initialize parameters cache */ parameters_update(); @@ -1148,8 +1173,9 @@ MulticopterAttitudeControl::task_main() vehicle_status_poll(); vehicle_motor_limits_poll(); battery_status_poll(); - control_state_poll(); + vehicle_attitude_poll(); sensor_correction_poll(); + sensor_bias_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't @@ -1171,7 +1197,7 @@ MulticopterAttitudeControl::task_main() } else { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; - math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Quaternion q(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); @@ -1240,7 +1266,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[7] = _v_att_sp.landing_gear; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _ctrl_state.timestamp; + _actuators.timestamp_sample = _v_att.timestamp; /* scale effort by battery status */ if (_params.bat_scale_en && _battery_status.scale > 0.0f) { @@ -1290,7 +1316,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = 0.0f; _actuators.control[3] = 0.0f; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _ctrl_state.timestamp; + _actuators.timestamp_sample = _v_att.timestamp; if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4c5da2ba2f..d2d4c2f290 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -56,11 +56,11 @@ #include #include -#include #include #include #include #include +#include #include #include #include @@ -138,13 +138,14 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ int _vehicle_land_detected_sub; /**< vehicle land detected subscription */ - int _ctrl_state_sub; /**< control state subscription */ + int _vehicle_attitude_sub; /**< control state subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ int _home_pos_sub; /**< home position */ + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -152,7 +153,7 @@ private: struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */ - struct control_state_s _ctrl_state; /**< vehicle attitude */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -406,7 +407,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _mavlink_log_pub(nullptr), /* subscriptions */ - _ctrl_state_sub(-1), + _vehicle_attitude_sub(-1), _control_mode_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -420,7 +421,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _attitude_setpoint_id(nullptr), _vehicle_status{}, _vehicle_land_detected{}, - _ctrl_state{}, + _att{}, _att_sp{}, _manual{}, _control_mode{}, @@ -471,7 +472,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _heading_reset_counter(0) { // Make the quaternion valid for control state - _ctrl_state.q[0] = 1.0f; + _att.q[0] = 1.0f; _ref_pos = {}; @@ -725,23 +726,23 @@ MulticopterPositionControl::poll_subscriptions() orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); } - orb_check(_ctrl_state_sub, &updated); + orb_check(_vehicle_attitude_sub, &updated); if (updated) { - orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); /* get current rotation matrix and euler angles from control state quaternions */ - math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Quaternion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]); _R = q_att.to_dcm(); math::Vector<3> euler_angles; euler_angles = _R.to_euler(); _yaw = euler_angles(2); if (_control_mode.flag_control_manual_enabled) { - if (_heading_reset_counter != _ctrl_state.quat_reset_counter) { - _heading_reset_counter = _ctrl_state.quat_reset_counter; - math::Quaternion delta_q(_ctrl_state.delta_q_reset[0], _ctrl_state.delta_q_reset[1], _ctrl_state.delta_q_reset[2], - _ctrl_state.delta_q_reset[3]); + if (_heading_reset_counter != _att.quat_reset_counter) { + _heading_reset_counter = _att.quat_reset_counter; + math::Quaternion delta_q(_att.delta_q_reset[0], _att.delta_q_reset[1], _att.delta_q_reset[2], + _att.delta_q_reset[3]); // we only extract the heading change from the delta quaternion math::Vector<3> delta_euler = delta_q.to_euler(); @@ -2938,7 +2939,7 @@ MulticopterPositionControl::task_main() */ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); + _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _control_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)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 33bf97fb67..8234111eec 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1362,7 +1362,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; - global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b6f9ecdc26..242826f37d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -66,7 +66,6 @@ #include #include #include -#include #include #include #include @@ -1187,7 +1186,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; struct mc_att_ctrl_status_s mc_att_ctrl_status; - struct control_state_s ctrl_state; struct ekf2_innovations_s innovations; struct camera_trigger_s camera_trigger; struct vehicle_land_detected_s land_detected; @@ -1293,7 +1291,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int wind_sub; int tsync_sub; int mc_att_ctrl_status_sub; - int ctrl_state_sub; int innov_sub; int cam_trig_sub; int land_detected_sub; @@ -1336,7 +1333,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = -1; subs.tsync_sub = -1; subs.mc_att_ctrl_status_sub = -1; - subs.ctrl_state_sub = -1; subs.innov_sub = -1; subs.cam_trig_sub = -1; subs.land_detected_sub = -1; @@ -2071,8 +2067,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_WIND_MSG; log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; - log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north; - log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east; + log_msg.body.log_WIND.cov_x = buf.wind_estimate.variance_north; + log_msg.body.log_WIND.cov_y = buf.wind_estimate.variance_east; LOGBUFFER_WRITE_AND_COUNT(WIND); } @@ -2091,19 +2087,6 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_MACS.yaw_rate_integ = buf.mc_att_ctrl_status.yaw_rate_integ; LOGBUFFER_WRITE_AND_COUNT(MACS); } - - /* --- CONTROL STATE --- */ - if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) { - log_msg.msg_type = LOG_CTS_MSG; - log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel; - log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel; - log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel; - log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed; - log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate; - log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate; - log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate; - LOGBUFFER_WRITE_AND_COUNT(CTS); - } } /* --- ATTITUDE --- */ diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ea54239637..e53492f92c 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -462,7 +462,6 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) { hil_gpos.timestamp = timestamp; - hil_gpos.time_utc_usec = timestamp; hil_gpos.lat = hil_state.lat / 1E7;//1E7 hil_gpos.lon = hil_state.lon / 1E7;//1E7 hil_gpos.alt = hil_state.alt / 1E3;//1E3 @@ -1126,9 +1125,6 @@ int Simulator::publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink) struct vehicle_local_position_s vision_position = {}; - // Use the estimator type to identify the simple vision estimate - vision_position.estimator_type = MAV_ESTIMATOR_TYPE_VISION; - vision_position.timestamp = timestamp; vision_position.x = ev_mavlink->x; vision_position.y = ev_mavlink->y; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index cf90f1bb5f..4d82922a20 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -72,7 +72,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); _params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); _params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); - _params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE"); + _params_handles_standard.airspeed_disabled = param_find("FW_ARSP_MODE"); _params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF"); _params_handles_standard.reverse_output = param_find("VT_B_REV_OUT"); _params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL"); @@ -131,8 +131,8 @@ Standard::parameters_update() param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale); /* airspeed mode */ - param_get(_params_handles_standard.airspeed_mode, &i); - _params_standard.airspeed_mode = math::constrain(i, 0, 2); + param_get(_params_handles_standard.airspeed_disabled, &i); + _params_standard.airspeed_disabled = math::constrain(i, 0, 1); /* pitch setpoint offset */ param_get(_params_handles_standard.pitch_setpoint_offset, &v); @@ -240,7 +240,7 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED || + if (((_params_standard.airspeed_disabled == 1 || _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)) || @@ -308,14 +308,13 @@ void Standard::update_transition_state() // time based blending when no airspeed sensor is set - } else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED && - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f) && - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * - 1000000.0f) + } else if (_params_standard.airspeed_disabled && + hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1e6f) && + hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * 1e6f) ) { - float weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - (( - _params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) / - ((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f)); + float weight = 1.0f - ((hrt_elapsed_time(&_vtol_schedule.transition_start) - (( + _params_standard.front_trans_time_min / 2.0f) * 1e6f)) / + ((_params_standard.front_trans_time_min / 2.0f) * 1e6f)); weight = math::constrain(weight, 0.0f, 1.0f); @@ -340,7 +339,7 @@ void Standard::update_transition_state() // check front transition timeout if (_params_standard.front_trans_timeout > FLT_EPSILON) { - if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) { + if (hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1e6f)) { // transition timeout occured, abort transition _attc->abort_front_transition("Transition timeout"); } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 8488c50a0d..d1b1b631ce 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -77,7 +77,7 @@ private: float front_trans_time_min; float down_pitch_max; float forward_thrust_scale; - int airspeed_mode; + int32_t airspeed_disabled; float pitch_setpoint_offset; float reverse_output; float reverse_delay; @@ -96,7 +96,7 @@ private: param_t front_trans_time_min; param_t down_pitch_max; param_t forward_thrust_scale; - param_t airspeed_mode; + param_t airspeed_disabled; param_t pitch_setpoint_offset; param_t reverse_output; param_t reverse_delay; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 96be33bb24..4ef89ced8e 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -69,7 +69,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID"); - _params_handles_tiltrotor.airspeed_mode = param_find("FW_ARSP_MODE"); + _params_handles_tiltrotor.airspeed_disabled = param_find("FW_ARSP_MODE"); _params_handles_tiltrotor.diff_thrust = param_find("VT_FW_DIFTHR_EN"); _params_handles_tiltrotor.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); } @@ -89,7 +89,6 @@ Tiltrotor::parameters_update() param_get(_params_handles_tiltrotor.fw_motors_off, &l); _params_tiltrotor.fw_motors_off = get_motor_off_channels(l); - /* vtol duration of a front transition */ param_get(_params_handles_tiltrotor.front_trans_dur, &v); _params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f); @@ -134,8 +133,8 @@ Tiltrotor::parameters_update() } /* airspeed mode */ - param_get(_params_handles_tiltrotor.airspeed_mode, &l); - _params_tiltrotor.airspeed_mode = math::constrain(l, 0, 2); + param_get(_params_handles_tiltrotor.airspeed_disabled, &l); + _params_tiltrotor.airspeed_disabled = math::constrain(l, 0, 1); param_get(_params_handles_tiltrotor.diff_thrust, &_params_tiltrotor.diff_thrust); @@ -219,12 +218,12 @@ void Tiltrotor::update_vtol_state() bool transition_to_p2 = can_transition_on_ground(); // check if we have reached airspeed to switch to fw mode - transition_to_p2 |= _params_tiltrotor.airspeed_mode != control_state_s::AIRSPD_MODE_DISABLED && + transition_to_p2 |= !_params_tiltrotor.airspeed_disabled && _airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_min * 1e6f); // check if airspeed is invalid and transition by time - transition_to_p2 |= _params_tiltrotor.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED && + transition_to_p2 |= _params_tiltrotor.airspeed_disabled && _tilt_control > _params_tiltrotor.tilt_transition && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_openloop * 1e6f); @@ -334,7 +333,7 @@ void Tiltrotor::update_transition_state() &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } - bool use_airspeed = _params_tiltrotor.airspeed_mode != control_state_s::AIRSPD_MODE_DISABLED; + bool use_airspeed = !_params_tiltrotor.airspeed_disabled; // at low speeds give full weight to MC _mc_roll_weight = 1.0f; diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 3d791db8ea..8f39b20137 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -69,11 +69,11 @@ private: float tilt_fw; /**< actuator value corresponding to fw tilt */ float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ - int elevons_mc_lock; /**< lock elevons in multicopter mode */ + int32_t elevons_mc_lock; /**< lock elevons in multicopter mode */ float front_trans_dur_p2; - int fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ - int airspeed_mode; - int diff_thrust; + int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ + int32_t airspeed_disabled; + int32_t diff_thrust; float diff_thrust_scale; } _params_tiltrotor; @@ -88,7 +88,7 @@ private: param_t elevons_mc_lock; param_t front_trans_dur_p2; param_t fw_motors_off; - param_t airspeed_mode; + param_t airspeed_disabled; param_t diff_thrust; param_t diff_thrust_scale; } _params_handles_tiltrotor; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2219a80a76..891eebc1b6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -67,7 +67,6 @@ #include #include #include -#include #include #include #include