From 9d5ea4bcebcb574ba55543ae0fe68c290dd0df10 Mon Sep 17 00:00:00 2001 From: mswingtra Date: Fri, 28 Aug 2015 11:06:12 +0200 Subject: [PATCH 01/20] px4_param_def to QGC fix --- Tools/px4params/srcparser.py | 16 +++++++++------- Tools/px4params/srcscanner.py | 10 ++++++++-- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 918040bbf5..c69699cad7 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -1,5 +1,7 @@ import sys import re +global default_var +default_var = {} class ParameterGroup(object): """ @@ -98,6 +100,7 @@ class SourceParser(object): re_comment_end = re.compile(r'(.*?)\s*\*\/') re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;') + re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*') re_cut_type_specifier = re.compile(r'[a-z]+$') re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') @@ -114,13 +117,6 @@ class SourceParser(object): def __init__(self): self.param_groups = {} - def GetSupportedExtensions(self): - """ - Returns list of supported file extensions that can be parsed by this - parser. - """ - return [".cpp", ".c"] - def Parse(self, contents): """ Incrementally parse program contents and append all found parameters @@ -193,6 +189,10 @@ class SourceParser(object): name = None defval = "" # Non-empty line outside the comment + m = self.re_px4_param_default_definition.match(line) + if m: + name_m, defval_m = m.group(1,2) + default_var[name_m] = defval_m m = self.re_parameter_definition.match(line) if m: tp, name, defval = m.group(1, 2, 3) @@ -200,6 +200,8 @@ class SourceParser(object): m = self.re_px4_parameter_definition.match(line) if m: tp, name = m.group(1, 2) + if default_var.has_key(name+'_DEFAULT'): + defval = default_var[name+'_DEFAULT'] if tp is not None: # Remove trailing type specifier from numbers: 0.1f => 0.1 if defval != "" and self.re_is_a_number.match(defval): diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index a49039c719..4db5698992 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -13,10 +13,16 @@ class SourceScanner(object): Scans provided path and passes all found contents to the parser using parser.Parse method. """ - extensions = tuple(parser.GetSupportedExtensions()) + extensions1 = tuple([".h"]) + extensions2 = tuple([".cpp", ".c"]) for dirname, dirnames, filenames in os.walk(srcdir): for filename in filenames: - if filename.endswith(extensions): + if filename.endswith(extensions1): + path = os.path.join(dirname, filename) + if not self.ScanFile(path, parser): + return False + for filename in filenames: + if filename.endswith(extensions2): path = os.path.join(dirname, filename) if not self.ScanFile(path, parser): return False From d0dd0fc4d489a8773073daca50a6addcf9cb5a2e Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 23 Sep 2015 16:19:14 +0200 Subject: [PATCH 02/20] add control state topic --- msg/control_state.msg | 18 ++++++++++++++++++ src/modules/uORB/objects_common.cpp | 3 +++ src/platforms/px4_includes.h | 1 + 3 files changed, 22 insertions(+) create mode 100644 msg/control_state.msg diff --git a/msg/control_state.msg b/msg/control_state.msg new file mode 100644 index 0000000000..d976fbae8d --- /dev/null +++ b/msg/control_state.msg @@ -0,0 +1,18 @@ +# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */ +uint64 timestamp # in microseconds since system start +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 frame +float32 y_pos # Y position in local frame +float32 z_pos # z position in local frame +float32 airspeed # Airspeed, estimated +float32[3] vel_variance # Variance in body velocity estimate +float32[3] pos_variance # Variance in local position estimate +float32[4] q # Attitude Quaternion +float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down) +float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down) +float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ac98048771..8fe70d7604 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s); #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); +#include "topics/control_state.h" +ORB_DEFINE(control_state, struct control_state_s); + #include "topics/sensor_combined.h" ORB_DEFINE(sensor_combined, struct sensor_combined_s); diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 83bb60d796..7437a9405a 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -87,6 +87,7 @@ #include #include #include +#include #include #include #include From ce70803d250435ae1fc6f103066759b0c49a576b Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 23 Sep 2015 16:19:48 +0200 Subject: [PATCH 03/20] publish control state from ekf - missing:airspeed --- .../AttitudePositionEstimatorEKF.h | 9 +++ .../ekf_att_pos_estimator_main.cpp | 78 ++++++++++++++++++- 2 files changed, 83 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index e4d924396a..f88c274a59 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -152,12 +153,14 @@ 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; @@ -316,6 +319,12 @@ private: **/ void publishAttitude(); + /** + * @brief + * Publish the system state for control modules + **/ + void publishControlState(); + /** * @brief * Publish local position relative to reference point where filter was initialized diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d23dd1b9e9..8fcaf29f47 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -140,12 +140,14 @@ 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{}, @@ -821,9 +823,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; @@ -832,7 +834,7 @@ void AttitudePositionEstimatorEKF::publishAttitude() /* lazily publish the attitude only once available */ if (_att_pub != nullptr) { - /* publish the attitude setpoint */ + /* publish the attitude */ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); } else { @@ -841,6 +843,74 @@ 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; + + /* 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; + //_local_pos.z_stable = _ekf->states[9]; + + _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]; + + /* 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; + + /* 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; From 9c26e71ef6014bb983556778899c912f472083c4 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 11:53:37 +0200 Subject: [PATCH 04/20] Added robust airspeed measurement --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 6 ++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8fcaf29f47..88ba758cb3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -870,7 +870,6 @@ void AttitudePositionEstimatorEKF::publishControlState() // XXX need to announce change of Z reference somehow elegantly _ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset; - //_local_pos.z_stable = _ekf->states[9]; _ctrl_state.pos_variance[0] = _ekf->P[7][7]; _ctrl_state.pos_variance[1] = _ekf->P[8][8]; @@ -883,6 +882,9 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[2] = _ekf->states[2]; _ctrl_state.q[3] = _ekf->states[3]; + /* Airspeed (Groundspeed - Windspeed) */ + _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + /* 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; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b920cb9458..839918728c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -49,6 +49,8 @@ #define M_PI_F static_cast(M_PI) #endif +#define MIN_AIRSPEED_MEAS 5.0f + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -1687,7 +1689,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the predicted airspeed VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS)) { // Calculate observation jacobians SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); @@ -2595,7 +2597,7 @@ void AttPosEKF::CovarianceInit() P[13][13] = sq(0.2f*dtIMU); //Wind velocities - P[14][14] = 0.0f; + P[14][14] = 0.01f; P[15][15] = P[14][14]; //Earth magnetic field From 2f4afa2da7ac24ccd38a3f532b749e1e46680e76 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 21:19:51 +0200 Subject: [PATCH 05/20] added control state to logging --- src/modules/sdlog2/sdlog2.c | 18 +++++++ src/modules/sdlog2/sdlog2_messages.h | 81 ++++++++++++++++------------ 2 files changed, 65 insertions(+), 34 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 253949b26e..8ee40f1661 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -1108,6 +1109,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; + struct control_state_s ctrl_state; struct vehicle_attitude_setpoint_s att_sp; struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; @@ -1158,6 +1160,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; struct log_VTOL_s log_VTOL; + struct log_CTS_s log_CTS; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1199,6 +1202,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int vtol_status_sub; int sensor_sub; int att_sub; + int ctrl_state_sub; int att_sp_sub; int rates_sp_sub; int act_outputs_sub; @@ -1236,6 +1240,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.gps_pos_sub = -1; subs.sensor_sub = -1; subs.att_sub = -1; + subs.ctrl_state_sub = -1; subs.att_sp_sub = -1; subs.rates_sp_sub = -1; subs.act_outputs_sub = -1; @@ -1714,6 +1719,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(FLOW); } + /* --- 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); + } + /* --- RC CHANNELS --- */ if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7f77a58e7b..16fafd3165 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -185,8 +185,20 @@ struct log_STAT_s { float load; }; +/* --- CONTROL STATE --- */ +#define LOG_CTS_MSG 11 +struct log_CTS_s { + float vx_body; + float vy_body; + float vz_body; + float airspeed; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; uint8_t channel_count; @@ -194,13 +206,13 @@ struct log_RC_s { }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; @@ -208,7 +220,7 @@ struct log_AIRS_s { }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; @@ -216,7 +228,7 @@ struct log_ARSP_s { }; /* --- FLOW - OPTICAL FLOW --- */ -#define LOG_FLOW_MSG 15 +#define LOG_FLOW_MSG 16 struct log_FLOW_s { uint8_t sensor_id; float pixel_flow_x_integral; @@ -233,7 +245,7 @@ struct log_FLOW_s { }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ -#define LOG_GPOS_MSG 16 +#define LOG_GPOS_MSG 17 struct log_GPOS_s { int32_t lat; int32_t lon; @@ -247,7 +259,7 @@ struct log_GPOS_s { }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ -#define LOG_GPSP_MSG 17 +#define LOG_GPSP_MSG 18 struct log_GPSP_s { uint8_t nav_state; int32_t lat; @@ -261,7 +273,7 @@ struct log_GPSP_s { }; /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 18 +#define LOG_ESC_MSG 19 struct log_ESC_s { uint16_t counter; uint8_t esc_count; @@ -278,7 +290,7 @@ struct log_ESC_s { }; /* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ -#define LOG_GVSP_MSG 19 +#define LOG_GVSP_MSG 20 struct log_GVSP_s { float vx; float vy; @@ -286,7 +298,7 @@ struct log_GVSP_s { }; /* --- BATT - BATTERY --- */ -#define LOG_BATT_MSG 20 +#define LOG_BATT_MSG 21 struct log_BATT_s { float voltage; float voltage_filtered; @@ -295,7 +307,7 @@ struct log_BATT_s { }; /* --- DIST - RANGE SENSOR DISTANCE --- */ -#define LOG_DIST_MSG 21 +#define LOG_DIST_MSG 22 struct log_DIST_s { uint8_t id; uint8_t type; @@ -304,11 +316,11 @@ struct log_DIST_s { float covariance; }; -/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ +/* LOG IMU1 and IMU2 MSGs consume IDs 23 and 24 */ /* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 24 +#define LOG_PWR_MSG 25 struct log_PWR_s { float peripherals_5v; float servo_rail_5v; @@ -321,7 +333,7 @@ struct log_PWR_s { }; /* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ -#define LOG_MOCP_MSG 25 +#define LOG_MOCP_MSG 26 struct log_MOCP_s { float qw; float qx; @@ -333,31 +345,31 @@ struct log_MOCP_s { }; /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ -#define LOG_GS0A_MSG 26 +#define LOG_GS0A_MSG 27 struct log_GS0A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS0B - GPS SNR #0, SAT GROUP B --- */ -#define LOG_GS0B_MSG 27 +#define LOG_GS0B_MSG 28 struct log_GS0B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1A - GPS SNR #1, SAT GROUP A --- */ -#define LOG_GS1A_MSG 28 +#define LOG_GS1A_MSG 29 struct log_GS1A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1B - GPS SNR #1, SAT GROUP B --- */ -#define LOG_GS1B_MSG 29 +#define LOG_GS1B_MSG 30 struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- TECS - TECS STATUS --- */ -#define LOG_TECS_MSG 30 +#define LOG_TECS_MSG 31 struct log_TECS_s { float altitudeSp; float altitudeFiltered; @@ -378,7 +390,7 @@ struct log_TECS_s { }; /* --- WIND - WIND ESTIMATE --- */ -#define LOG_WIND_MSG 31 +#define LOG_WIND_MSG 32 struct log_WIND_s { float x; float y; @@ -387,7 +399,7 @@ struct log_WIND_s { }; /* --- EST0 - ESTIMATOR STATUS --- */ -#define LOG_EST0_MSG 32 +#define LOG_EST0_MSG 33 struct log_EST0_s { float s[12]; uint8_t n_states; @@ -397,28 +409,28 @@ struct log_EST0_s { }; /* --- EST1 - ESTIMATOR STATUS --- */ -#define LOG_EST1_MSG 33 +#define LOG_EST1_MSG 34 struct log_EST1_s { float s[16]; }; /* --- EST2 - ESTIMATOR STATUS --- */ -#define LOG_EST2_MSG 34 +#define LOG_EST2_MSG 35 struct log_EST2_s { float cov[12]; }; /* --- EST3 - ESTIMATOR STATUS --- */ -#define LOG_EST3_MSG 35 +#define LOG_EST3_MSG 36 struct log_EST3_s { float cov[16]; }; /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 36 -#define LOG_TEL1_MSG 37 -#define LOG_TEL2_MSG 38 -#define LOG_TEL3_MSG 39 +#define LOG_TEL0_MSG 37 +#define LOG_TEL1_MSG 38 +#define LOG_TEL2_MSG 39 +#define LOG_TEL3_MSG 40 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -431,7 +443,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 40 +#define LOG_VISN_MSG 41 struct log_VISN_s { float x; float y; @@ -446,7 +458,7 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 41 +#define LOG_ENCD_MSG 42 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -455,22 +467,22 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 42 +#define LOG_AIR1_MSG 43 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 43 +#define LOG_VTOL_MSG 44 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 44 +#define LOG_TSYN_MSG 45 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 45 +#define LOG_MACS_MSG 47 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; @@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), + LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), From 78f5a35d1214f3d60878e4c0104428b98826d919 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 22:22:11 +0200 Subject: [PATCH 06/20] integrated control state in fw l1 controller --- .../fw_pos_control_l1_main.cpp | 77 +++++++++++-------- 1 file changed, 43 insertions(+), 34 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c3816f5374..9bfebe5f8b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,7 @@ private: int _global_pos_sub; int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _airspeed_sub; /**< airspeed subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ @@ -165,6 +167,7 @@ private: orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ @@ -364,6 +367,11 @@ private: */ void vehicle_attitude_poll(); + /** + * Check for changes in control state. + */ + void control_state_poll(); + /** * Check for accel updates. */ @@ -481,6 +489,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos_sub(-1), _pos_sp_triplet_sub(-1), _att_sub(-1), + _ctrl_state_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), @@ -495,6 +504,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* states */ _att(), + _ctrl_state(), _att_sp(), _nav_capabilities(), _manual(), @@ -748,32 +758,6 @@ FixedwingPositionControl::vehicle_status_poll() } } -bool -FixedwingPositionControl::vehicle_airspeed_poll() -{ - /* check if there is an airspeed update or if it timed out */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - _airspeed_valid = true; - _airspeed_last_valid = hrt_absolute_time(); - - } else { - - /* no airspeed updates for one second */ - if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { - _airspeed_valid = false; - } - } - - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - - return airspeed_updated; -} - bool FixedwingPositionControl::vehicle_manual_control_setpoint_poll() { @@ -806,6 +790,30 @@ FixedwingPositionControl::vehicle_attitude_poll() } } +void +FixedwingPositionControl::control_state_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 = true; + _airspeed_last_valid = hrt_absolute_time(); + + } else { + + /* no airspeed updates for one second */ + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + _airspeed_valid = false; + } + } + + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); +} + void FixedwingPositionControl::vehicle_sensor_combined_poll() { @@ -852,7 +860,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) float airspeed; if (_airspeed_valid) { - airspeed = _airspeed.true_airspeed_m_s; + airspeed = _ctrl_state.airspeed; } else { airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; @@ -1083,7 +1091,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update TECS filters */ if (!_mTecs.getEnabled()) { - _tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, + _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } @@ -1116,7 +1124,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } } _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -1475,7 +1483,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } } _control_mode_current = FW_POSCTRL_MODE_POSITION; @@ -1586,7 +1594,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } } _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; @@ -1698,6 +1706,7 @@ FixedwingPositionControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -1779,9 +1788,9 @@ FixedwingPositionControl::task_main() _global_pos_valid = true; vehicle_attitude_poll(); + control_state_poll(); vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); - vehicle_airspeed_poll(); vehicle_manual_control_setpoint_poll(); // vehicle_baro_poll(); @@ -1887,7 +1896,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* use pitch max set by MT param */ limitOverride.disablePitchMaxOverride(); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, limitOverride); } else { if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { @@ -1901,7 +1910,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, + _ctrl_state.airspeed, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); From 6dbf4e45733c551d67038ba85f704ce937fbae3c Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 22:43:00 +0200 Subject: [PATCH 07/20] integrated ctrl state rates in mc_att_control --- .../mc_att_control/mc_att_control_main.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 830f48dff8..360362f75f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -128,6 +129,7 @@ private: int _control_task; /**< task handle */ int _v_att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state 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 */ @@ -147,6 +149,7 @@ private: bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ 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 */ @@ -304,6 +307,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _v_att_sub(-1), + _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), @@ -326,6 +330,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : { memset(&_v_att, 0, sizeof(_v_att)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); @@ -708,9 +713,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* current body angular rates */ math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; + rates(0) = _ctrl_state.roll_rate; + rates(1) = _ctrl_state.pitch_rate; + rates(2) = _ctrl_state.yaw_rate; /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; @@ -749,6 +754,7 @@ MulticopterAttitudeControl::task_main() _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _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)); @@ -798,8 +804,9 @@ MulticopterAttitudeControl::task_main() dt = 0.02f; } - /* copy attitude topic */ + /* copy attitude and control state topics */ orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); From 8d756055b8517a153667ca78a0c00f73cf09158d Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 23:00:44 +0200 Subject: [PATCH 08/20] integrated control state in fw_att_control --- .../fw_att_control/fw_att_control_main.cpp | 51 ++++++------------- 1 file changed, 15 insertions(+), 36 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d06e9b9a74..3c326aec6f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -68,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -126,10 +126,10 @@ private: int _control_task; /**< task handle */ int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ @@ -145,11 +145,11 @@ private: orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ @@ -269,12 +269,6 @@ private: */ void vehicle_manual_poll(); - - /** - * Check for airspeed updates. - */ - void vehicle_airspeed_poll(); - /** * Check for accel updates. */ @@ -327,8 +321,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* subscriptions */ _att_sub(-1), + _ctrl_state_sub(-1), _accel_sub(-1), - _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -354,11 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : { /* safely initialize structs */ _att = {}; + _ctrl_state = {}; _accel = {}; _att_sp = {}; _rates_sp = {}; _manual = {}; - _airspeed = {}; _vcontrol_mode = {}; _actuators = {}; _actuators_airframe = {}; @@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -void -FixedwingAttitudeControl::vehicle_airspeed_poll() -{ - /* check if there is a new position */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - } -} - void FixedwingAttitudeControl::vehicle_accel_poll() { @@ -624,8 +606,8 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -641,7 +623,6 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -700,6 +681,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { @@ -756,13 +738,11 @@ FixedwingAttitudeControl::task_main() PX4_R(_att.R, 2, 2) = R_adapted(2, 2); /* lastly, roll- and yawspeed have to be swaped */ - float helper = _att.rollspeed; - _att.rollspeed = -_att.yawspeed; - _att.yawspeed = helper; + float helper = _ctrl_state.roll_rate; + _ctrl_state.roll_rate = -_ctrl_state.yaw_rate; + _ctrl_state.yaw_rate = helper; } - vehicle_airspeed_poll(); - vehicle_setpoint_poll(); vehicle_accel_poll(); @@ -813,15 +793,14 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); } } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ - airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + airspeed = math::max(0.5f, _ctrl_state.airspeed); } /* @@ -975,9 +954,9 @@ FixedwingAttitudeControl::task_main() control_input.roll = _att.roll; control_input.pitch = _att.pitch; control_input.yaw = _att.yaw; - control_input.roll_rate = _att.rollspeed; - control_input.pitch_rate = _att.pitchspeed; - control_input.yaw_rate = _att.yawspeed; + control_input.roll_rate = _ctrl_state.roll_rate; + control_input.pitch_rate = _ctrl_state.pitch_rate; + control_input.yaw_rate = _ctrl_state.yaw_rate; control_input.speed_body_u = speed_body_u; control_input.speed_body_v = speed_body_v; control_input.speed_body_w = speed_body_w; From 5ee9bb5363517f6e15bfd6f436202123b4023f71 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 9 Oct 2015 09:06:02 +0200 Subject: [PATCH 09/20] clean up fw_pos_control_l1 --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9bfebe5f8b..48edca2b52 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include @@ -155,7 +154,6 @@ private: int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -171,7 +169,6 @@ private: struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< control mode */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ @@ -357,11 +354,6 @@ private: */ bool vehicle_manual_control_setpoint_poll(); - /** - * Check for airspeed updates. - */ - bool vehicle_airspeed_poll(); - /** * Check for position updates. */ @@ -490,7 +482,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _pos_sp_triplet_sub(-1), _att_sub(-1), _ctrl_state_sub(-1), - _airspeed_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), _params_sub(-1), @@ -508,7 +499,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _att_sp(), _nav_capabilities(), _manual(), - _airspeed(), _control_mode(), _vehicle_status(), _global_pos(), @@ -1710,7 +1700,6 @@ FixedwingPositionControl::task_main() _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); From 65837bdc98e67504f11db73b3d6f36aaa8c9e551 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Sat, 10 Oct 2015 18:16:17 +0200 Subject: [PATCH 10/20] publish control state from attitude_estimator_q --- .../attitude_estimator_q_main.cpp | 33 +++++++++++++++---- 1 file changed, 27 insertions(+), 6 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 9b60701413..a3fbefb13b 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -53,6 +53,7 @@ #include #include #include + #include #include #include #include @@ -116,6 +117,7 @@ private: int _params_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; + orb_advert_t _ctrl_state_pub = nullptr; struct { param_t w_acc; @@ -417,11 +419,8 @@ void AttitudeEstimatorQ::task_main() att.pitch = euler(1); att.yaw = euler(2); - /* the complimentary filter should reflect the true system - * state, but we need smoother outputs for the control system - */ - att.rollspeed = _lp_roll_rate.apply(_rates(0)); - att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); + att.rollspeed = _rates(0); + att.pitchspeed = _rates(1); att.yawspeed = _rates(2); for (int i = 0; i < 3; i++) { @@ -446,6 +445,29 @@ void AttitudeEstimatorQ::task_main() } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } + + 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); + + /* Attitude rates for control state */ + ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + ctrl_state.roll_rate = _rates(2); + + /* Publish to control state topic */ + if (_ctrl_state_pub == nullptr) + { + _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + + } else { + orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state); + } } } @@ -571,7 +593,6 @@ bool AttitudeEstimatorQ::update(float dt) { return true; } - int attitude_estimator_q_main(int argc, char *argv[]) { if (argc < 1) { warnx("usage: attitude_estimator_q {start|stop|status}"); From 035216fc9ccb86df18d7d1ddd57d91e32ed89ad3 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Tue, 13 Oct 2015 17:54:28 +0200 Subject: [PATCH 11/20] fully replaced vehicle_attitude with control_state in mc_att_control --- .../mc_att_control/mc_att_control_main.cpp | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 360362f75f..8a097cbe50 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -75,7 +75,6 @@ #include #include #include -#include #include #include #include @@ -128,7 +127,6 @@ private: bool _task_should_exit; /**< if true, task_main() should exit */ int _control_task; /**< task handle */ - int _v_att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ @@ -148,7 +146,6 @@ private: bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ @@ -306,7 +303,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _v_att_sub(-1), _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -329,7 +325,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { - memset(&_v_att, 0, sizeof(_v_att)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); @@ -619,9 +614,9 @@ MulticopterAttitudeControl::control_attitude(float dt) math::Matrix<3, 3> R_sp; R_sp.set(_v_att_sp.R_body); - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); + /* 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::Matrix<3, 3> R = q_att.to_dcm(); /* all input data is ready, run controller itself */ @@ -753,7 +748,6 @@ MulticopterAttitudeControl::task_main() */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _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)); @@ -768,7 +762,7 @@ MulticopterAttitudeControl::task_main() /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; - fds[0].fd = _v_att_sub; + fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -805,7 +799,6 @@ MulticopterAttitudeControl::task_main() } /* copy attitude and control state topics */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ @@ -873,7 +866,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _v_att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); From 5092bcae70a5e81254ed0fdba509e9190d01a6e6 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 14 Oct 2015 09:33:07 +0200 Subject: [PATCH 12/20] fully ported control state attitude into fw_att_control --- .../fw_att_control/fw_att_control_main.cpp | 69 ++++++++++--------- 1 file changed, 36 insertions(+), 33 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3c326aec6f..83b5caaf1f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -242,6 +242,11 @@ private: } _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; ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; @@ -684,6 +689,16 @@ FixedwingAttitudeControl::task_main() orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + /* 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]); + _R = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * @@ -701,41 +716,29 @@ FixedwingAttitudeControl::task_main() * Rxy Ryy Rzy -Rzy Ryy Rxy * Rxz Ryz Rzz -Rzz Ryz Rxz * */ - math::Matrix<3, 3> R; //original rotation matrix - math::Matrix<3, 3> R_adapted; //modified rotation matrix - R.set(_att.R); - R_adapted.set(_att.R); + math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix /* move z to x */ - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); + R_adapted(0, 0) = _R(0, 2); + R_adapted(1, 0) = _R(1, 2); + R_adapted(2, 0) = _R(2, 2); /* move x to z */ - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); + R_adapted(0, 2) = _R(0, 0); + R_adapted(1, 2) = _R(1, 0); + R_adapted(2, 2) = _R(2, 0); /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); - math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation - euler_angles = R_adapted.to_euler(); + euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation /* fill in new attitude data */ - _att.roll = euler_angles(0); - _att.pitch = euler_angles(1); - _att.yaw = euler_angles(2); - PX4_R(_att.R, 0, 0) = R_adapted(0, 0); - PX4_R(_att.R, 0, 1) = R_adapted(0, 1); - PX4_R(_att.R, 0, 2) = R_adapted(0, 2); - PX4_R(_att.R, 1, 0) = R_adapted(1, 0); - PX4_R(_att.R, 1, 1) = R_adapted(1, 1); - PX4_R(_att.R, 1, 2) = R_adapted(1, 2); - PX4_R(_att.R, 2, 0) = R_adapted(2, 0); - PX4_R(_att.R, 2, 1) = R_adapted(2, 1); - PX4_R(_att.R, 2, 2) = R_adapted(2, 2); + _R = R_adapted; + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); /* lastly, roll- and yawspeed have to be swaped */ float helper = _ctrl_state.roll_rate; @@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main() /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ - if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max) @@ -940,9 +943,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; - speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; - speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; + speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; + speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; + speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); @@ -951,9 +954,9 @@ FixedwingAttitudeControl::task_main() /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; - control_input.roll = _att.roll; - control_input.pitch = _att.pitch; - control_input.yaw = _att.yaw; + control_input.roll = _roll; + control_input.pitch = _pitch; + control_input.yaw = _yaw; control_input.roll_rate = _ctrl_state.roll_rate; control_input.pitch_rate = _ctrl_state.pitch_rate; control_input.yaw_rate = _ctrl_state.yaw_rate; @@ -1079,9 +1082,9 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); - _actuators_airframe.timestamp_sample = _att.timestamp; + _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || From e72e72cd652d6d266b90db45545b2a54a20d8942 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 14 Oct 2015 10:53:19 +0200 Subject: [PATCH 13/20] removed vehicle_attitude subscription from fw_att_control --- .../fw_att_control/fw_att_control_main.cpp | 24 ++++--------------- 1 file changed, 4 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 83b5caaf1f..8127d0ac55 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -66,7 +66,6 @@ #include #include #include -#include #include #include #include @@ -125,7 +124,6 @@ private: bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle */ - int _att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ @@ -144,7 +142,6 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure - struct vehicle_attitude_s _att; /**< vehicle attitude */ struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -325,7 +322,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), _ctrl_state_sub(-1), _accel_sub(-1), _vcontrol_mode_sub(-1), @@ -352,7 +348,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _debug(false) { /* safely initialize structs */ - _att = {}; _ctrl_state = {}; _accel = {}; _att_sp = {}; @@ -610,7 +605,6 @@ FixedwingAttitudeControl::task_main() * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -640,7 +634,7 @@ FixedwingAttitudeControl::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; - fds[1].fd = _att_sub; + fds[1].fd = _ctrl_state_sub; fds[1].events = POLLIN; _task_running = true; @@ -685,7 +679,6 @@ FixedwingAttitudeControl::task_main() deltaT = 0.01f; /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); @@ -939,18 +932,9 @@ FixedwingAttitudeControl::task_main() } /* Prepare speed_body_u and speed_body_w */ - float speed_body_u = 0.0f; - float speed_body_v = 0.0f; - float speed_body_w = 0.0f; - if(_att.R_valid) { - speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; - speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; - speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; - } else { - if (_debug && loop_counter % 10 == 0) { - warnx("Did not get a valid R\n"); - } - } + float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; + float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; + float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; From 1ec96d87d77da91325ad2822bef4a60e8857adf4 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 14 Oct 2015 10:58:58 +0200 Subject: [PATCH 14/20] fully ported control state into fw_pos_control_l1 --- .../fw_pos_control_l1_main.cpp | 58 +++++++------------ 1 file changed, 21 insertions(+), 37 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 48edca2b52..5ba05f7184 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -74,7 +74,6 @@ #include #include #include -#include #include #include #include @@ -152,7 +151,6 @@ private: int _global_pos_sub; int _pos_sp_triplet_sub; - int _att_sub; /**< vehicle attitude subscription */ int _ctrl_state_sub; /**< control state subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ @@ -164,7 +162,6 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ @@ -221,6 +218,9 @@ private: float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid math::Matrix<3, 3> _R_nb; ///< current attitude + float _roll; + float _pitch; + float _yaw; ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -354,11 +354,6 @@ private: */ bool vehicle_manual_control_setpoint_poll(); - /** - * Check for position updates. - */ - void vehicle_attitude_poll(); - /** * Check for changes in control state. */ @@ -480,7 +475,6 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), _pos_sp_triplet_sub(-1), - _att_sub(-1), _ctrl_state_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), @@ -494,7 +488,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _nav_capabilities_pub(nullptr), /* states */ - _att(), _ctrl_state(), _att_sp(), _nav_capabilities(), @@ -763,23 +756,6 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll() return manual_updated; } - -void -FixedwingPositionControl::vehicle_attitude_poll() -{ - /* check if there is a new position */ - bool att_updated; - orb_check(_att_sub, &att_updated); - - if (att_updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _R_nb(i, j) = PX4_R(_att.R, i, j); - } -} - void FixedwingPositionControl::control_state_poll() { @@ -800,6 +776,16 @@ FixedwingPositionControl::control_state_poll() } } + /* set rotation matrix and euler angles */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R_nb = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R_nb.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); } @@ -1122,7 +1108,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset hold altitude */ _hold_alt = _global_pos.alt; /* reset hold yaw */ - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1216,14 +1202,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; } else { - target_bearing = _att.yaw; + target_bearing = _yaw; } mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); } -// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); + _l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d); land_noreturn_horizontal = true; @@ -1464,7 +1450,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; } @@ -1517,7 +1503,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ - if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading _yaw_lock_engaged = true; @@ -1536,7 +1522,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* just switched back from non heading-hold to heading hold */ if (!_hdg_hold_enabled) { _hdg_hold_enabled = true; - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); } @@ -1695,7 +1681,6 @@ FixedwingPositionControl::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1776,7 +1761,6 @@ FixedwingPositionControl::task_main() // XXX add timestamp check _global_pos_valid = true; - vehicle_attitude_poll(); control_state_poll(); vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); @@ -1898,7 +1882,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, _ctrl_state.airspeed, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, From efe82194af9655e5e14a733a5ab8fc9dd784c126 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 16 Oct 2015 16:03:25 +0200 Subject: [PATCH 15/20] ported control state attitude into mc_pos_ctrl --- .../mc_pos_control/mc_pos_control_main.cpp | 46 ++++++++++++------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 36d87b9383..95ba66361a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ private: int _mavlink_fd; /**< mavlink fd */ int _vehicle_status_sub; /**< vehicle status subscription */ - int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ @@ -142,7 +142,7 @@ private: orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< 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 */ @@ -215,6 +215,9 @@ private: math::Vector<3> _vel_ff; math::Vector<3> _sp_move_rate; + math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ + float _yaw; /**< yaw angle (euler) */ + /** * Update our local parameter cache. */ @@ -304,7 +307,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _mavlink_fd(-1), /* subscriptions */ - _att_sub(-1), + _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), _params_sub(-1), @@ -325,10 +328,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_pos_sp(true), _reset_alt_sp(true), - _mode_auto(false) + _mode_auto(false), + _yaw(0.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); - memset(&_att, 0, sizeof(_att)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); @@ -356,6 +360,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_ff.zero(); _sp_move_rate.zero(); + _R.identity(); + _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); _params_handles.z_p = param_find("MPC_Z_P"); @@ -493,10 +499,10 @@ MulticopterPositionControl::poll_subscriptions() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); } - orb_check(_att_sub, &updated); + orb_check(_ctrl_state_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); } orb_check(_att_sp_sub, &updated); @@ -923,7 +929,7 @@ MulticopterPositionControl::task_main() * do subscriptions */ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -978,6 +984,14 @@ MulticopterPositionControl::task_main() } poll_subscriptions(); + + /* 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]); + _R = q_att.to_dcm(); + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _yaw = euler_angles(2); + parameters_update(false); hrt_abstime t = hrt_absolute_time(); @@ -1046,7 +1060,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); @@ -1216,11 +1230,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { - att_comp = 1.0f / PX4_R(_att.R, 2, 2); + if (_R(2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / _R(2, 2); - } else if (PX4_R(_att.R, 2, 2) > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; + } else if (_R(2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f; saturation_z = true; } else { @@ -1407,7 +1421,7 @@ MulticopterPositionControl::task_main() /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; } /* do not move yaw while arming */ @@ -1417,7 +1431,7 @@ MulticopterPositionControl::task_main() _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); - float yaw_offs = _wrap_pi(yaw_target - _att.yaw); + float yaw_offs = _wrap_pi(yaw_target - _yaw); // If the yaw offset became too big for the system to track stop // shifting it From 560efcbc5bc6de5b1e3aae9214cef4039f10a9aa Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 21 Oct 2015 13:57:26 +0200 Subject: [PATCH 16/20] added all platforms to px4_includes --- src/platforms/px4_includes.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 7437a9405a..0286705968 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -121,6 +122,7 @@ #include #include #include +#include #include #include #include @@ -151,6 +153,7 @@ #include #include #include +#include #include #include #include From 34ff90c62483bd4c983ae8fcf4ed6bede9792ce0 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Tue, 27 Oct 2015 09:41:40 +0100 Subject: [PATCH 17/20] fixed logging --- src/modules/sdlog2/sdlog2.c | 34 +++++----- src/modules/sdlog2/sdlog2_messages.h | 92 ++++++++++++++-------------- 2 files changed, 63 insertions(+), 63 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fa3ede9dc6..408fa396bf 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1060,7 +1060,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; - struct control_state_s ctrl_state; struct vehicle_attitude_setpoint_s att_sp; struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; @@ -1091,6 +1090,7 @@ 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; } buf; memset(&buf, 0, sizeof(buf)); @@ -1111,7 +1111,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; struct log_VTOL_s log_VTOL; - struct log_CTS_s log_CTS; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1140,6 +1139,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ENCD_s log_ENCD; struct log_TSYN_s log_TSYN; struct log_MACS_s log_MACS; + struct log_CTS_s log_CTS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1153,7 +1153,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int vtol_status_sub; int sensor_sub; int att_sub; - int ctrl_state_sub; int att_sp_sub; int rates_sp_sub; int act_outputs_sub; @@ -1183,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int encoders_sub; int tsync_sub; int mc_att_ctrl_status_sub; + int ctrl_state_sub; } subs; subs.cmd_sub = -1; @@ -1191,7 +1191,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.gps_pos_sub = -1; subs.sensor_sub = -1; subs.att_sub = -1; - subs.ctrl_state_sub = -1; subs.att_sp_sub = -1; subs.rates_sp_sub = -1; subs.act_outputs_sub = -1; @@ -1217,6 +1216,7 @@ 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.encoders_sub = -1; /* add new topics HERE */ @@ -1670,19 +1670,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(FLOW); } - /* --- 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); - } - /* --- RC CHANNELS --- */ if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; @@ -1874,6 +1861,19 @@ int sdlog2_thread_main(int argc, char *argv[]) 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); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 30efd95e26..462b753f98 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -185,20 +185,8 @@ struct log_STAT_s { float load; }; -/* --- CONTROL STATE --- */ -#define LOG_CTS_MSG 11 -struct log_CTS_s { - float vx_body; - float vy_body; - float vz_body; - float airspeed; - float roll_rate; - float pitch_rate; - float yaw_rate; -}; - /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 12 +#define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; uint8_t channel_count; @@ -206,13 +194,13 @@ struct log_RC_s { }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 13 +#define LOG_OUT0_MSG 12 struct log_OUT0_s { float output[8]; }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 14 +#define LOG_AIRS_MSG 13 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; @@ -220,7 +208,7 @@ struct log_AIRS_s { }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 15 +#define LOG_ARSP_MSG 14 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; @@ -228,7 +216,7 @@ struct log_ARSP_s { }; /* --- FLOW - OPTICAL FLOW --- */ -#define LOG_FLOW_MSG 16 +#define LOG_FLOW_MSG 15 struct log_FLOW_s { uint8_t sensor_id; float pixel_flow_x_integral; @@ -245,7 +233,7 @@ struct log_FLOW_s { }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ -#define LOG_GPOS_MSG 17 +#define LOG_GPOS_MSG 16 struct log_GPOS_s { int32_t lat; int32_t lon; @@ -259,7 +247,7 @@ struct log_GPOS_s { }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ -#define LOG_GPSP_MSG 18 +#define LOG_GPSP_MSG 17 struct log_GPSP_s { uint8_t nav_state; int32_t lat; @@ -273,7 +261,7 @@ struct log_GPSP_s { }; /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 19 +#define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; uint8_t esc_count; @@ -290,7 +278,7 @@ struct log_ESC_s { }; /* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ -#define LOG_GVSP_MSG 20 +#define LOG_GVSP_MSG 19 struct log_GVSP_s { float vx; float vy; @@ -298,7 +286,7 @@ struct log_GVSP_s { }; /* --- BATT - BATTERY --- */ -#define LOG_BATT_MSG 21 +#define LOG_BATT_MSG 20 struct log_BATT_s { float voltage; float voltage_filtered; @@ -307,7 +295,7 @@ struct log_BATT_s { }; /* --- DIST - RANGE SENSOR DISTANCE --- */ -#define LOG_DIST_MSG 22 +#define LOG_DIST_MSG 21 struct log_DIST_s { uint8_t id; uint8_t type; @@ -316,11 +304,11 @@ struct log_DIST_s { float covariance; }; -/* LOG IMU1 and IMU2 MSGs consume IDs 23 and 24 */ +/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ /* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 25 +#define LOG_PWR_MSG 24 struct log_PWR_s { float peripherals_5v; float servo_rail_5v; @@ -333,7 +321,7 @@ struct log_PWR_s { }; /* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ -#define LOG_MOCP_MSG 26 +#define LOG_MOCP_MSG 25 struct log_MOCP_s { float qw; float qx; @@ -345,31 +333,31 @@ struct log_MOCP_s { }; /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ -#define LOG_GS0A_MSG 27 +#define LOG_GS0A_MSG 26 struct log_GS0A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS0B - GPS SNR #0, SAT GROUP B --- */ -#define LOG_GS0B_MSG 28 +#define LOG_GS0B_MSG 27 struct log_GS0B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1A - GPS SNR #1, SAT GROUP A --- */ -#define LOG_GS1A_MSG 29 +#define LOG_GS1A_MSG 28 struct log_GS1A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1B - GPS SNR #1, SAT GROUP B --- */ -#define LOG_GS1B_MSG 30 +#define LOG_GS1B_MSG 29 struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- TECS - TECS STATUS --- */ -#define LOG_TECS_MSG 31 +#define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; float altitudeFiltered; @@ -390,7 +378,7 @@ struct log_TECS_s { }; /* --- WIND - WIND ESTIMATE --- */ -#define LOG_WIND_MSG 32 +#define LOG_WIND_MSG 31 struct log_WIND_s { float x; float y; @@ -399,7 +387,7 @@ struct log_WIND_s { }; /* --- EST0 - ESTIMATOR STATUS --- */ -#define LOG_EST0_MSG 33 +#define LOG_EST0_MSG 32 struct log_EST0_s { float s[12]; uint8_t n_states; @@ -409,28 +397,28 @@ struct log_EST0_s { }; /* --- EST1 - ESTIMATOR STATUS --- */ -#define LOG_EST1_MSG 34 +#define LOG_EST1_MSG 33 struct log_EST1_s { float s[16]; }; /* --- EST2 - ESTIMATOR STATUS --- */ -#define LOG_EST2_MSG 35 +#define LOG_EST2_MSG 34 struct log_EST2_s { float cov[12]; }; /* --- EST3 - ESTIMATOR STATUS --- */ -#define LOG_EST3_MSG 36 +#define LOG_EST3_MSG 35 struct log_EST3_s { float cov[16]; }; /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 37 -#define LOG_TEL1_MSG 38 -#define LOG_TEL2_MSG 39 -#define LOG_TEL3_MSG 40 +#define LOG_TEL0_MSG 36 +#define LOG_TEL1_MSG 37 +#define LOG_TEL2_MSG 38 +#define LOG_TEL3_MSG 39 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -443,7 +431,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 41 +#define LOG_VISN_MSG 40 struct log_VISN_s { float x; float y; @@ -458,7 +446,7 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 42 +#define LOG_ENCD_MSG 41 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -467,28 +455,40 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 43 +#define LOG_AIR1_MSG 42 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 44 +#define LOG_VTOL_MSG 43 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 45 +#define LOG_TSYN_MSG 44 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 47 +#define LOG_MACS_MSG 45 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; float yaw_rate_integ; }; +/* --- CONTROL STATE --- */ +#define LOG_CTS_MSG 47 +struct log_CTS_s { + float vx_body; + float vy_body; + float vz_body; + float airspeed; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + /* WARNING: ID 46 is already in use for ATTC1 */ /********** SYSTEM MESSAGES, ID > 0x80 **********/ From 318eeb04945c21ebfa478bb12656ebb1fed3e141 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 6 Nov 2015 13:11:02 +0100 Subject: [PATCH 18/20] fixed ros build --- cmake/ros-CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt index aa42fb6ff4..4e814f4114 100644 --- a/cmake/ros-CMakeLists.txt +++ b/cmake/ros-CMakeLists.txt @@ -81,6 +81,7 @@ add_message_files( offboard_control_mode.msg vehicle_force_setpoint.msg distance_sensor.msg + control_state.msg ) ## Generate services in the 'srv' folder From 556a5dbd1d3a0c478dc7835e7600d91711bf64e1 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Fri, 6 Nov 2015 13:48:50 +0100 Subject: [PATCH 19/20] astyle fix --- .../attitude_estimator_q_main.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a094772b37..f56e3ad243 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -54,7 +54,7 @@ #include #include #include - #include +#include #include #include #include @@ -527,22 +527,27 @@ void AttitudeEstimatorQ::task_main() } 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); /* Attitude rates for control state */ ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + ctrl_state.roll_rate = _rates(2); /* Publish to control state topic */ - if (_ctrl_state_pub == nullptr) - { + if (_ctrl_state_pub == nullptr) { _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else { From e1924c5d666f44f71e521bd50da5690de90861a2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 6 Nov 2015 16:46:32 +0100 Subject: [PATCH 20/20] ekf: publish control state --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 82b96c4043..f4fcf78e25 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -700,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main() // Publish attitude estimations publishAttitude(); + // publish control state + publishControlState(); + // Publish Local Position estimations publishLocalPosition();