From c38c58291e9db5425414b1466e5ed2e1254067b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:15:10 +0200 Subject: [PATCH 01/15] GPS: silence verbose boot messages --- src/drivers/gps/ubx.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 064ce20e5f..81b580892b 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -83,7 +83,7 @@ /**** Warning macros, disable to save memory */ #define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);} - +#define UBX_DEBUG(s, ...) {/*warnx(s, ## __VA_ARGS__);*/} UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : _fd(fd), @@ -212,7 +212,7 @@ UBX::configure(unsigned &baudrate) } else { _use_nav_pvt = true; } - UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); + UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); if (!_use_nav_pvt) { configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); @@ -271,9 +271,9 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) ret = 0; // ACK received ok } else if (report) { if (_ack_state == UBX_ACK_GOT_NAK) { - UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); + UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); } else { - UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); + UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } } @@ -566,7 +566,7 @@ UBX::payload_rx_init() break; case UBX_RXMSG_DISABLE: // disable unexpected messages - UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); + UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); { hrt_abstime t = hrt_absolute_time(); @@ -574,7 +574,7 @@ UBX::payload_rx_init() if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { /* don't attempt for every message to disable, some might not be disabled */ _disable_cmd_last = t; - UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); + UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); configure_message_rate(_rx_msg, 0); } } @@ -677,16 +677,16 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT); _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version); - UBX_WARN("VER hash 0x%08x", _ubx_version); - UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); - UBX_WARN("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); + UBX_DEBUG("VER hash 0x%08x", _ubx_version); + UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); + UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); } // fill Part 2 buffer unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); _buf.raw[buf_index] = b; if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); + UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); } } From 3679c2512501a99e14d5610191ed9236d2694b00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:15:25 +0200 Subject: [PATCH 02/15] PX4 FLOW: Silence alarming messages on a normal boot --- src/drivers/px4flow/px4flow.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 8ee365d8df..ddc5c61094 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -602,7 +602,7 @@ bool start_in_progress = false; const int START_RETRY_COUNT = 5; const int START_RETRY_TIMEOUT = 1000; -void start(); +int start(); void stop(); void test(); void reset(); @@ -611,22 +611,26 @@ void info(); /** * Start the driver. */ -void +int start() { int fd; /* entry check: */ if (start_in_progress) { - errx(1, "start in progress"); + warnx("start already in progress"); + return 1; } start_in_progress = true; if (g_dev != nullptr) { start_in_progress = false; - errx(1, "already started"); + warnx("already started"); + return 1; } + warnx("scanning I2C buses for device.."); + int retry_nr = 0; while (1) { const int busses_to_try[] = { @@ -684,11 +688,12 @@ start() /* success! */ start_in_progress = false; - exit(0); + return 0; } if (retry_nr < START_RETRY_COUNT) { - warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); + /* lets not be too verbose */ + // warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); usleep(START_RETRY_TIMEOUT * 1000); retry_nr++; } else { @@ -702,7 +707,7 @@ start() } start_in_progress = false; - errx(1, "PX4FLOW could not be started over I2C"); + return 1; } /** @@ -852,7 +857,7 @@ px4flow_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(argv[1], "start")) { - px4flow::start(); + return px4flow::start(); } /* From a446cb6b2f7736c1f38032c8a72cd5027c6799fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:15:38 +0200 Subject: [PATCH 03/15] FMU driver: Disable debug mode --- src/drivers/px4fmu/fmu.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c55289fc54..f2ec0285be 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -295,7 +295,8 @@ PX4FMU::PX4FMU() : memset(_controls, 0, sizeof(_controls)); memset(_poll_fds, 0, sizeof(_poll_fds)); - _debug_enabled = true; + /* only enable this during development */ + _debug_enabled = false; } PX4FMU::~PX4FMU() @@ -342,9 +343,9 @@ PX4FMU::init() _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { - log("default PWM output device"); + /* lets not be too verbose */ } else if (_class_instance < 0) { - log("FAILED registering class device"); + warnx("FAILED registering class device"); } /* reset GPIOs */ @@ -536,11 +537,11 @@ PX4FMU::subscribe() _poll_fds_num = 0; for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { - warnx("subscribe to actuator_controls_%d", i); + debug("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } if (unsub_groups & (1 << i)) { - warnx("unsubscribe from actuator_controls_%d", i); + debug("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); _control_subs[i] = -1; } From 283bca90d6dcf4ea7a92ed8ba2ac6836bb670bcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:32:31 +0200 Subject: [PATCH 04/15] MAVLink: Remove leftover debug comment --- src/modules/mavlink/mavlink_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5df0c47272..b4474f96dc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1268,7 +1268,6 @@ Mavlink::update_rate_mult() /* scale down if we have a TX err rate suggesting link congestion */ if (_rate_txerr > 0.0f && !radio_critical) { hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr); - warnx("RTS limiting"); } else if (radio_found && tstatus.timestamp != _last_hw_rate_timestamp) { if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { From c00b9885b586b906ca59d60d8ed9468cbbb9b11a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Aug 2015 17:46:28 +0200 Subject: [PATCH 05/15] Enable RC gamepad input in all HIL configs by default --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 3 +++ ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 3 +++ ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 3 +++ ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 3 +++ ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 3 +++ 5 files changed, 15 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 7938c47bae..4100cdb4a3 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -39,5 +39,8 @@ then param set FW_RR_P 0.3 fi +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set HIL yes set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index e1f2cdfe8d..52c512ed8c 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -11,4 +11,7 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_x +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 7aa888c763..c590f9ac86 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -11,4 +11,7 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_+ +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 57bcd24d02..0369da5980 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -11,4 +11,7 @@ sh /etc/init.d/rc.fw_defaults set HIL yes +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 5e0b6cd746..278df193aa 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -36,5 +36,8 @@ fi set HIL yes +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + # Set the AERT mixer for HIL (even if the malolo is a flying wing) set MIXER AERT From 3d8c628efa50b7495d499523702ffcc757df51b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 00:41:58 +0200 Subject: [PATCH 06/15] FW pos control: Comment and clean up altitude setpoint handling --- .../fw_pos_control_l1_main.cpp | 49 +++++++++++++------ 1 file changed, 33 insertions(+), 16 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 33a78f3141..4f3fdac91c 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 @@ -179,6 +179,8 @@ private: float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ + float _althold_epv; /**< the position estimate accuracy when engaging alt hold */ + bool _was_in_deadband; /**< wether the last stick input was in althold deadband */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH); + /* pitching down */ + float pitch = -(_manual.x - deadBand) / factor; + _hold_alt += (_parameters.max_sink_rate * dt) * pitch; + _was_in_deadband = false; } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - was_in_deadband = false; - } else if (!was_in_deadband) { + /* pitching up */ + float pitch = -(_manual.x + deadBand) / factor; + _hold_alt += (_parameters.max_climb_rate * dt) * pitch; + _was_in_deadband = false; + climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); + } else if (!_was_in_deadband) { /* store altitude at which manual.x was inside deadBand * The aircraft should immediately try to fly at this altitude * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; _althold_epv = _global_pos.epv; - was_in_deadband = true; + _was_in_deadband = true; } return climbout_mode; From f3ae231dad5b6c8f4415056c44ea711b3b2de99b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 01:23:00 +0200 Subject: [PATCH 07/15] TECS: Fix manual climbout --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 4f3fdac91c..ceea71fca9 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 @@ -1502,7 +1502,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - pitch_limit_min, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); @@ -1612,7 +1612,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - pitch_limit_min, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); From 0d9d5d1fa5d443cb397a14751105213304b173ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jul 2015 11:44:34 +0200 Subject: [PATCH 08/15] TECS: Limit rate demands --- src/lib/external_lgpl/tecs/tecs.cpp | 117 +++++++++++++++------------- 1 file changed, 64 insertions(+), 53 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 53380d75da..e80c418b14 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -48,8 +48,6 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< bool reset_altitude = false; - _in_air = in_air; - if (_update_50hz_last_usec == 0 || DT > DT_MAX) { DT = 0.02f; // when first starting TECS, use a // small time constant @@ -69,6 +67,8 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< _update_50hz_last_usec = now; _EAS = airspeed; + _in_air = in_air; + // Get height acceleration float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G); // Perform filter calculation using backwards Euler integration @@ -108,7 +108,9 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< _vel_dot = 0.0f; } - _states_initalized = true; + if (!_in_air) { + _states_initalized = false; + } } @@ -179,40 +181,38 @@ void TECS::_update_speed_demand(void) // calculate velocity rate limits based on physical performance limits // provision to use a different rate limit if bad descent or underspeed condition exists - // Use 50% of maximum energy rate to allow margin for total energy contgroller -// float velRateMax; -// float velRateMin; -// -// if ((_badDescent) || (_underspeed)) { -// velRateMax = 0.5f * _STEdot_max / _integ5_state; -// velRateMin = 0.5f * _STEdot_min / _integ5_state; -// -// } else { -// velRateMax = 0.5f * _STEdot_max / _integ5_state; -// velRateMin = 0.5f * _STEdot_min / _integ5_state; -// } -// + // Use 50% of maximum energy rate to allow margin for total energy controller + float velRateMax; + float velRateMin; + + if ((_badDescent) || (_underspeed)) { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + + } else { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + } + // // Apply rate limit -// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { -// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; +// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * _DT)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMax * _DT; // _TAS_rate_dem = velRateMax; // -// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { -// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; +// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * _DT)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMin * _DT; // _TAS_rate_dem = velRateMin; // // } else { // _TAS_dem_adj = _TAS_dem; // // -// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; +// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / _DT; // } - _TAS_dem_adj = _TAS_dem; - _TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now + _TAS_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax);; + _TAS_rate_dem = constrain((_TAS_dem_adj - _integ5_state) * _speedrate_p, velRateMin, velRateMax); //xxx: using a p loop for now - // Constrain speed demand again to protect against bad values on initialisation. - _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); // _TAS_dem_last = _TAS_dem; // warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f", @@ -223,23 +223,29 @@ void TECS::_update_speed_demand(void) void TECS::_update_height_demand(float demand, float state) { -// // Apply 2 point moving average to demanded height -// // This is required because height demand is only updated at 5Hz -// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); -// _hgt_dem_in_old = _hgt_dem; -// -// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, -// // _maxClimbRate); -// -// // Limit height rate of change -// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { -// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; -// -// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { -// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; -// } -// -// _hgt_dem_prev = _hgt_dem; + // Handle initialization + if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { + _hgt_dem_in_old = demand; + } + // Apply 2 point moving average to demanded height + // This is required because height demand is updated in steps + if (isfinite(demand)) { + _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); + } else { + _hgt_dem = _hgt_dem_in_old; + } + _hgt_dem_in_old = _hgt_dem; + + // Limit height demand + // this is important to avoid a windup + if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * _DT)) { + _hgt_dem = _hgt_dem_prev + _maxClimbRate * _DT; + + } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * _DT)) { + _hgt_dem = _hgt_dem_prev - _maxSinkRate * _DT; + } + + _hgt_dem_prev = _hgt_dem; // // // Apply first order lag to height demand // _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; @@ -249,9 +255,9 @@ void TECS::_update_height_demand(float demand, float state) // // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, // // _hgt_rate_dem); - _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; + _hgt_dem_adj = _hgt_dem; //0.025f * _hgt_dem + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; + _hgt_rate_dem = (_hgt_dem_adj - state) * _heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last) / _DT; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { @@ -485,7 +491,7 @@ void TECS::_update_pitch(void) void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) { // Initialise states and variables if DT > 1 second or in climbout - if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air) { + if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air || !_states_initalized) { _integ6_state = 0.0f; _integ7_state = 0.0f; _last_throttle_dem = throttle_cruise; @@ -498,8 +504,10 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt _TAS_dem_adj = _TAS_dem; _underspeed = false; _badDescent = false; - _DT = DT_MIN; // when first starting TECS, use a - // small time constant + + if (_DT > 1.0f || _DT < 0.001f) { + _DT = DT_MIN; + } } else if (_climbOutDem) { _PITCHminf = ptchMinCO_rad; @@ -512,6 +520,8 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt _underspeed = false; _badDescent = false; } + + _states_initalized = true; } void TECS::_update_STE_rate_lim(void) @@ -526,9 +536,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { - if (!_states_initalized) { - return; - } // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); @@ -537,9 +544,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n", // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); - // Update the speed estimate using a 2nd order complementary filter - _update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS); - // Convert inputs _THRmaxf = throttle_max; _THRminf = throttle_min; @@ -550,6 +554,13 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // initialise selected states and variables if DT > 1 second or in climbout _initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO); + if (!_in_air) { + return; + } + + // Update the speed estimate using a 2nd order complementary filter + _update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS); + // Calculate Specific Total Energy Rate Limits _update_STE_rate_lim(); From e443a3f3be42ed9ebfcf0ca588d7b0a4d359f582 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 01:34:00 +0200 Subject: [PATCH 09/15] Harmonize FW default gains, increase TECS height rate default gain considerably --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 ---- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index f1b92112a3..408707ec0b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,10 +11,6 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACC_RAD 50 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 param set PE_VELNE_NOISE 0.3 param set PE_VELD_NOISE 0.35 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 78d66afe1e..98ce1fa1de 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -327,7 +327,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * * @group Fixed Wing TECS */ -PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); /** * Speed <--> Altitude priority From c0d2b22d00b88206110f6445354fad2c97a1a8c1 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 2 Aug 2015 06:35:37 -0600 Subject: [PATCH 10/15] cosmetic: fix typos in comments --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index a855e4092f..cfba54d452 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -204,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight mode = tecs_status_s::TECS_MODE_UNDERSPEED; } - /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ + /* Set special output limiters if we are not in TECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); if (mode == tecs_status_s::TECS_MODE_TAKEOFF) { @@ -221,7 +221,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } - /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by + /* Apply override given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); @@ -253,7 +253,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)accelerationLongitudinalSp, (double)airspeedDerivative); } - /* publish status messge */ + /* publish status message */ _status.update(); /* clean up */ From b066a79ca6057294babe244a3155a6cc099b70af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 14:50:36 +0200 Subject: [PATCH 11/15] TECS lib: Low-pass height demand to avoid using the motor as a mechanical low-pass filter --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index e80c418b14..6f049d4f52 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -255,7 +255,7 @@ void TECS::_update_height_demand(float demand, float state) // // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, // // _hgt_rate_dem); - _hgt_dem_adj = _hgt_dem; //0.025f * _hgt_dem + 0.975f * _hgt_dem_adj_last; + _hgt_dem_adj = 0.1f * _hgt_dem + 0.9f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; _hgt_rate_dem = (_hgt_dem_adj - state) * _heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last) / _DT; From fbc1b78b3819beeec9f5327ef40fc3de1d599808 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 14:51:09 +0200 Subject: [PATCH 12/15] Attitude control: Enforce rate limits by default to prevent jittery control outputs / steps --- src/modules/fw_att_control/fw_att_control_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index aa8787b623..7975a3b3c3 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.01f); * @max 90.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); /** * Maximum negative / down pitch rate. @@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); * @max 90.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); /** * Pitch rate integrator limit @@ -185,7 +185,7 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); * @max 90.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f); +PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f); /** * Yaw rate proportional gain From 4e1faafc9bd5643a638c202ea42596e1cbff8e4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 14:51:36 +0200 Subject: [PATCH 13/15] Remove unused variables --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ---- 1 file changed, 4 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 9cbbe80cc3..43c44aaeac 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -163,7 +163,6 @@ private: struct { float tconst; float p_p; - float p_d; float p_i; float p_ff; float p_rmax_pos; @@ -171,7 +170,6 @@ private: float p_integrator_max; float p_roll_feedforward; float r_p; - float r_d; float r_i; float r_ff; float r_integrator_max; @@ -208,7 +206,6 @@ private: param_t tconst; param_t p_p; - param_t p_d; param_t p_i; param_t p_ff; param_t p_rmax_pos; @@ -216,7 +213,6 @@ private: param_t p_integrator_max; param_t p_roll_feedforward; param_t r_p; - param_t r_d; param_t r_i; param_t r_ff; param_t r_integrator_max; From 90e4fa8a33f234bfdf8e5294203f2537e416751f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 14:59:40 +0200 Subject: [PATCH 14/15] TECS: Weight down speed gain --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 98ce1fa1de..ee982b8b8d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -378,7 +378,7 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); * * @group Fixed Wing TECS */ -PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f); /** * Landing slope angle From 5b3449e75bec1d74737d72b39a6481a84894de04 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Aug 2015 15:00:05 +0200 Subject: [PATCH 15/15] FW Att control: Crank up pitch default gains --- src/modules/fw_att_control/fw_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 7975a3b3c3..62cd5b1556 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); * @max 0.5 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_I, 0.01f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f); /** * Maximum positive / up pitch rate. @@ -258,7 +258,7 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); * @max 10.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); /** * Yaw rate feed forward