forked from Archive/PX4-Autopilot
commit
95fc701376
|
@ -554,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
|
||||||
// Calculate pitch demand
|
// Calculate pitch demand
|
||||||
_update_pitch();
|
_update_pitch();
|
||||||
|
|
||||||
// // Write internal variables to the log_tuning structure. This
|
_tecs_state.timestamp = now;
|
||||||
// // structure will be logged in dataflash at 10Hz
|
|
||||||
// log_tuning.hgt_dem = _hgt_dem_adj;
|
if (_underspeed) {
|
||||||
// log_tuning.hgt = _integ3_state;
|
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
|
||||||
// log_tuning.dhgt_dem = _hgt_rate_dem;
|
} else if (_badDescent) {
|
||||||
// log_tuning.dhgt = _integ2_state;
|
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
|
||||||
// log_tuning.spd_dem = _TAS_dem_adj;
|
} else if (_climbOutDem) {
|
||||||
// log_tuning.spd = _integ5_state;
|
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
|
||||||
// log_tuning.dspd = _vel_dot;
|
} else {
|
||||||
// log_tuning.ithr = _integ6_state;
|
// If no error flag applies, conclude normal
|
||||||
// log_tuning.iptch = _integ7_state;
|
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
|
||||||
// log_tuning.thr = _throttle_dem;
|
}
|
||||||
// log_tuning.ptch = _pitch_dem;
|
|
||||||
// log_tuning.dspd_dem = _TAS_rate_dem;
|
_tecs_state.hgt_dem = _hgt_dem_adj;
|
||||||
|
_tecs_state.hgt = _integ3_state;
|
||||||
|
_tecs_state.dhgt_dem = _hgt_rate_dem;
|
||||||
|
_tecs_state.dhgt = _integ2_state;
|
||||||
|
_tecs_state.spd_dem = _TAS_dem_adj;
|
||||||
|
_tecs_state.spd = _integ5_state;
|
||||||
|
_tecs_state.dspd = _vel_dot;
|
||||||
|
_tecs_state.ithr = _integ6_state;
|
||||||
|
_tecs_state.iptch = _integ7_state;
|
||||||
|
_tecs_state.thr = _throttle_dem;
|
||||||
|
_tecs_state.ptch = _pitch_dem;
|
||||||
|
_tecs_state.dspd_dem = _TAS_rate_dem;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -28,6 +28,7 @@ class __EXPORT TECS
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
TECS() :
|
TECS() :
|
||||||
|
_tecs_state {},
|
||||||
_update_50hz_last_usec(0),
|
_update_50hz_last_usec(0),
|
||||||
_update_speed_last_usec(0),
|
_update_speed_last_usec(0),
|
||||||
_update_pitch_throttle_last_usec(0),
|
_update_pitch_throttle_last_usec(0),
|
||||||
|
@ -120,24 +121,33 @@ public:
|
||||||
return _spdWeight;
|
return _spdWeight;
|
||||||
}
|
}
|
||||||
|
|
||||||
// log data on internal state of the controller. Called at 10Hz
|
enum ECL_TECS_MODE {
|
||||||
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
|
ECL_TECS_MODE_NORMAL = 0,
|
||||||
|
ECL_TECS_MODE_UNDERSPEED,
|
||||||
|
ECL_TECS_MODE_BAD_DESCENT,
|
||||||
|
ECL_TECS_MODE_CLIMBOUT
|
||||||
|
};
|
||||||
|
|
||||||
// struct PACKED log_TECS_Tuning {
|
struct tecs_state {
|
||||||
// LOG_PACKET_HEADER;
|
uint64_t timestamp;
|
||||||
// float hgt;
|
float hgt;
|
||||||
// float dhgt;
|
float dhgt;
|
||||||
// float hgt_dem;
|
float hgt_dem;
|
||||||
// float dhgt_dem;
|
float dhgt_dem;
|
||||||
// float spd_dem;
|
float spd_dem;
|
||||||
// float spd;
|
float spd;
|
||||||
// float dspd;
|
float dspd;
|
||||||
// float ithr;
|
float ithr;
|
||||||
// float iptch;
|
float iptch;
|
||||||
// float thr;
|
float thr;
|
||||||
// float ptch;
|
float ptch;
|
||||||
// float dspd_dem;
|
float dspd_dem;
|
||||||
// } log_tuning;
|
enum ECL_TECS_MODE mode;
|
||||||
|
};
|
||||||
|
|
||||||
|
void get_tecs_state(struct tecs_state& state) {
|
||||||
|
state = _tecs_state;
|
||||||
|
}
|
||||||
|
|
||||||
void set_time_const(float time_const) {
|
void set_time_const(float time_const) {
|
||||||
_timeConst = time_const;
|
_timeConst = time_const;
|
||||||
|
@ -212,6 +222,9 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
struct tecs_state _tecs_state;
|
||||||
|
|
||||||
// Last time update_50Hz was called
|
// Last time update_50Hz was called
|
||||||
uint64_t _update_50hz_last_usec;
|
uint64_t _update_50hz_last_usec;
|
||||||
|
|
||||||
|
|
|
@ -146,6 +146,7 @@ private:
|
||||||
int _range_finder_sub; /**< range finder subscription */
|
int _range_finder_sub; /**< range finder subscription */
|
||||||
|
|
||||||
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
|
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
|
||||||
|
orb_advert_t _tecs_status_pub; /**< TECS status publication */
|
||||||
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
|
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
|
||||||
|
|
||||||
struct vehicle_attitude_s _att; /**< vehicle attitude */
|
struct vehicle_attitude_s _att; /**< vehicle attitude */
|
||||||
|
@ -414,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_attitude_sp_pub(-1),
|
_attitude_sp_pub(-1),
|
||||||
|
_tecs_status_pub(-1),
|
||||||
_nav_capabilities_pub(-1),
|
_nav_capabilities_pub(-1),
|
||||||
|
|
||||||
/* states */
|
/* states */
|
||||||
|
@ -1384,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
|
||||||
climbout_mode, climbout_pitch_min_rad,
|
climbout_mode, climbout_pitch_min_rad,
|
||||||
throttle_min, throttle_max, throttle_cruise,
|
throttle_min, throttle_max, throttle_cruise,
|
||||||
pitch_min_rad, pitch_max_rad);
|
pitch_min_rad, pitch_max_rad);
|
||||||
|
|
||||||
|
struct TECS::tecs_state s;
|
||||||
|
_tecs.get_tecs_state(s);
|
||||||
|
|
||||||
|
struct tecs_status_s t;
|
||||||
|
|
||||||
|
t.timestamp = s.timestamp;
|
||||||
|
|
||||||
|
switch (s.mode) {
|
||||||
|
case TECS::ECL_TECS_MODE_NORMAL:
|
||||||
|
t.mode = TECS_MODE_NORMAL;
|
||||||
|
break;
|
||||||
|
case TECS::ECL_TECS_MODE_UNDERSPEED:
|
||||||
|
t.mode = TECS_MODE_UNDERSPEED;
|
||||||
|
break;
|
||||||
|
case TECS::ECL_TECS_MODE_BAD_DESCENT:
|
||||||
|
t.mode = TECS_MODE_BAD_DESCENT;
|
||||||
|
break;
|
||||||
|
case TECS::ECL_TECS_MODE_CLIMBOUT:
|
||||||
|
t.mode = TECS_MODE_CLIMBOUT;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
t.altitudeSp = s.hgt_dem;
|
||||||
|
t.altitude_filtered = s.hgt;
|
||||||
|
t.airspeedSp = s.spd_dem;
|
||||||
|
t.airspeed_filtered = s.spd;
|
||||||
|
|
||||||
|
t.flightPathAngleSp = s.dhgt_dem;
|
||||||
|
t.flightPathAngle = s.dhgt;
|
||||||
|
t.flightPathAngleFiltered = s.dhgt;
|
||||||
|
|
||||||
|
t.airspeedDerivativeSp = s.dspd_dem;
|
||||||
|
t.airspeedDerivative = s.dspd;
|
||||||
|
|
||||||
|
t.totalEnergyRateSp = s.thr;
|
||||||
|
t.totalEnergyRate = s.ithr;
|
||||||
|
t.energyDistributionRateSp = s.ptch;
|
||||||
|
t.energyDistributionRate = s.iptch;
|
||||||
|
|
||||||
|
if (_tecs_status_pub > 0) {
|
||||||
|
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
|
||||||
|
} else {
|
||||||
|
_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
|
||||||
|
|
||||||
/* Write part of the status message */
|
/* Write part of the status message */
|
||||||
_status.altitudeSp = altitudeSp;
|
_status.altitudeSp = altitudeSp;
|
||||||
_status.altitude = altitude;
|
_status.altitude_filtered = altitudeFiltered;
|
||||||
_status.altitudeFiltered = altitudeFiltered;
|
|
||||||
|
|
||||||
|
|
||||||
/* use flightpath angle setpoint for total energy control */
|
/* use flightpath angle setpoint for total energy control */
|
||||||
|
@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
|
||||||
|
|
||||||
/* Write part of the status message */
|
/* Write part of the status message */
|
||||||
_status.airspeedSp = airspeedSp;
|
_status.airspeedSp = airspeedSp;
|
||||||
_status.airspeed = airspeed;
|
_status.airspeed_filtered = airspeedFiltered;
|
||||||
_status.airspeedFiltered = airspeedFiltered;
|
|
||||||
|
|
||||||
|
|
||||||
/* use longitudinal acceleration setpoint for total energy control */
|
/* use longitudinal acceleration setpoint for total energy control */
|
||||||
|
|
|
@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
|
if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
|
||||||
log_msg.msg_type = LOG_TECS_MSG;
|
log_msg.msg_type = LOG_TECS_MSG;
|
||||||
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
|
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
|
||||||
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
|
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
|
||||||
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
|
|
||||||
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
|
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
|
||||||
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
|
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
|
||||||
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
|
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
|
||||||
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
|
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
|
||||||
log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
|
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
|
||||||
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
|
|
||||||
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
|
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
|
||||||
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
|
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
|
||||||
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
|
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
|
||||||
|
|
|
@ -333,13 +333,11 @@ struct log_GS1B_s {
|
||||||
#define LOG_TECS_MSG 30
|
#define LOG_TECS_MSG 30
|
||||||
struct log_TECS_s {
|
struct log_TECS_s {
|
||||||
float altitudeSp;
|
float altitudeSp;
|
||||||
float altitude;
|
|
||||||
float altitudeFiltered;
|
float altitudeFiltered;
|
||||||
float flightPathAngleSp;
|
float flightPathAngleSp;
|
||||||
float flightPathAngle;
|
float flightPathAngle;
|
||||||
float flightPathAngleFiltered;
|
float flightPathAngleFiltered;
|
||||||
float airspeedSp;
|
float airspeedSp;
|
||||||
float airspeed;
|
|
||||||
float airspeedFiltered;
|
float airspeedFiltered;
|
||||||
float airspeedDerivativeSp;
|
float airspeedDerivativeSp;
|
||||||
float airspeedDerivative;
|
float airspeedDerivative;
|
||||||
|
@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = {
|
||||||
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||||
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||||
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||||
LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
|
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
|
||||||
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
|
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
|
||||||
|
|
||||||
/* system-level messages, ID >= 0x80 */
|
/* system-level messages, ID >= 0x80 */
|
||||||
|
|
|
@ -51,11 +51,13 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
TECS_MODE_NORMAL,
|
TECS_MODE_NORMAL = 0,
|
||||||
TECS_MODE_UNDERSPEED,
|
TECS_MODE_UNDERSPEED,
|
||||||
TECS_MODE_TAKEOFF,
|
TECS_MODE_TAKEOFF,
|
||||||
TECS_MODE_LAND,
|
TECS_MODE_LAND,
|
||||||
TECS_MODE_LAND_THROTTLELIM
|
TECS_MODE_LAND_THROTTLELIM,
|
||||||
|
TECS_MODE_BAD_DESCENT,
|
||||||
|
TECS_MODE_CLIMBOUT
|
||||||
} tecs_mode;
|
} tecs_mode;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -65,14 +67,12 @@ struct tecs_status_s {
|
||||||
uint64_t timestamp; /**< timestamp, in microseconds since system start */
|
uint64_t timestamp; /**< timestamp, in microseconds since system start */
|
||||||
|
|
||||||
float altitudeSp;
|
float altitudeSp;
|
||||||
float altitude;
|
float altitude_filtered;
|
||||||
float altitudeFiltered;
|
|
||||||
float flightPathAngleSp;
|
float flightPathAngleSp;
|
||||||
float flightPathAngle;
|
float flightPathAngle;
|
||||||
float flightPathAngleFiltered;
|
float flightPathAngleFiltered;
|
||||||
float airspeedSp;
|
float airspeedSp;
|
||||||
float airspeed;
|
float airspeed_filtered;
|
||||||
float airspeedFiltered;
|
|
||||||
float airspeedDerivativeSp;
|
float airspeedDerivativeSp;
|
||||||
float airspeedDerivative;
|
float airspeedDerivative;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue