dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

delete control_state and cleanup vehicle_attitude (#7882)

This commit is contained in:
Daniel Agar 2017-09-21 16:24:53 -04:00 committed by GitHub
parent 5bea264a5f
commit b4755297ec
42 changed files with 776 additions and 1061 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
msg/CMakeLists.txt
View File

@ -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

28
msg/control_state.msg
View File

@ -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

30
msg/sensor_bias.msg Normal file
View File

@ -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)

12
msg/vehicle_attitude.msg
View File

@ -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

14
msg/vehicle_global_position.msg
View File

@ -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

24
msg/vehicle_local_position.msg
View File

@ -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)

9
msg/wind_estimate.msg
View File

@ -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

31
src/drivers/frsky_telemetry/frsky_data.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
* 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 <px4@sradonia.net>
*
* 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);

13
src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -49,7 +49,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
@ -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

113
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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) {

4
src/examples/segway/CMakeLists.txt
View File

@ -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

12
src/examples/uuv_example_app/uuv_example_app.cpp
View File

@ -64,7 +64,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h> // this topics hold the acceleration data
#include <uORB/topics/actuator_controls.h> // this topic gives the actuators control input
#include <uORB/topics/control_state.h> // this topic holds the orientation of the hippocampus
#include <uORB/topics/vehicle_attitude.h> // 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

2
src/lib/geo/geo.h
View File

@ -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 {

104
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -55,7 +55,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/att_pos_mocap.h>
@ -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);
}
}

771
src/modules/ekf2/ekf2_main.cpp
View File

File diff suppressed because it is too large Load Diff

12
src/modules/ekf2/ekf2_params.c
View File

@ -32,14 +32,13 @@
****************************************************************************/
/**
* @file parameters.c
* @file ekf2_params.c
* Parameter definition for ekf2.
*
* @author Roman Bast <bapstroman@gmail.com>
*
*/
/**
* 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
*

91
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -55,11 +55,13 @@
#include <mathlib/mathlib.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
@ -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<airspeed_s> _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 ||

9
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -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);

84
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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);

48
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -72,12 +72,15 @@
#include <mathlib/mathlib.h>
#include <runway_takeoff/RunwayTakeoff.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/control_state.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
@ -87,15 +90,22 @@
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
#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<airspeed_s> _sub_airspeed;
Subscription<sensor_bias_s> _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();

12
src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
View File

@ -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 ||

6
src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp
View File

@ -53,9 +53,9 @@
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -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 */

31
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
View File

@ -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> &current_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);

12
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp
View File

@ -54,11 +54,13 @@
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/pid/pid.h>
#include <uORB/topics/control_state.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
@ -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<vehicle_attitude_s> _sub_attitude;
Subscription<sensor_bias_s> _sub_sensors;
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
@ -180,7 +185,6 @@ private:
int parameters_update();
void manual_control_setpoint_poll();
void control_state_poll();
void position_setpoint_triplet_poll();
void vehicle_control_mode_poll();

29
src/modules/land_detector/FixedwingLandDetector.cpp
View File

@ -39,12 +39,12 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_config.h>
#include <px4_defines.h>
#include "FixedwingLandDetector.h"
#include <cmath>
#include "FixedwingLandDetector.h"
#include <px4_config.h>
#include <px4_defines.h>
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

9
src/modules/land_detector/FixedwingLandDetector.h
View File

@ -42,9 +42,10 @@
#pragma once
#include <uORB/topics/control_state.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_local_position.h>
#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};

18
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -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

19
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -42,19 +42,22 @@
#pragma once
#include "LandDetector.h"
#include <systemlib/param/param.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include "LandDetector.h"
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
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;

4
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -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;

28
src/modules/mavlink/mavlink_messages.cpp
View File

@ -77,6 +77,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
@ -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 = {};

73
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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)

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -77,10 +77,8 @@
#include <uORB/topics/follow_target.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/collision_report.h>
#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;

70
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -71,13 +71,14 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -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) {

29
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -56,11 +56,11 @@
#include <drivers/drv_hrt.h>
#include <systemlib/hysteresis/hysteresis.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -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));

1
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -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);

21
src/modules/sdlog2/sdlog2.c
View File

@ -66,7 +66,6 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
@ -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 --- */

4
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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;

23
src/modules/vtol_att_control/standard.cpp
View File

@ -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");
}

4
src/modules/vtol_att_control/standard.h
View File

@ -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;

13
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -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;

10
src/modules/vtol_att_control/tiltrotor.h
View File

@ -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;

1
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -67,7 +67,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/tecs_status.h>