From a94a8c5f5163ba91452f34a0a1008b4d8841427d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jun 2015 08:56:36 +0200 Subject: [PATCH 01/10] sdlog2: Flow: Remove unused field --- src/modules/sdlog2/sdlog2_messages.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abdf518c51..e75b6ca256 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -218,7 +218,6 @@ struct log_ARSP_s { /* --- FLOW - OPTICAL FLOW --- */ #define LOG_FLOW_MSG 15 struct log_FLOW_s { - uint64_t timestamp; uint8_t sensor_id; float pixel_flow_x_integral; float pixel_flow_y_integral; @@ -508,7 +507,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(FLOW, "QBffffffLLHhB", "IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), + LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), From e1c050df096721a1e49cdebaa9df8118bbaff0ca Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 2 Jul 2015 14:46:37 -0700 Subject: [PATCH 02/10] Generic AETR and AERT airframes Bixler converted to generic AERT --- .../init.d/{2101_hk_bixler => 2101_fw_AERT} | 0 ROMFS/px4fmu_common/init.d/2104_fw_AETR | 5 ++ ROMFS/px4fmu_common/init.d/rc.autostart | 8 +- ROMFS/px4fmu_common/mixers/AETR.main.mix | 84 +++++++++++++++++++ 4 files changed, 96 insertions(+), 1 deletion(-) rename ROMFS/px4fmu_common/init.d/{2101_hk_bixler => 2101_fw_AERT} (100%) create mode 100644 ROMFS/px4fmu_common/init.d/2104_fw_AETR create mode 100644 ROMFS/px4fmu_common/mixers/AETR.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_fw_AERT similarity index 100% rename from ROMFS/px4fmu_common/init.d/2101_hk_bixler rename to ROMFS/px4fmu_common/init.d/2101_fw_AERT diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR new file mode 100644 index 0000000000..bb4390b1d3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -0,0 +1,5 @@ +#!nsh + +sh /etc/init.d/rc.fw_defaults + +set MIXER AETR diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index a45ceeb276..ec405accac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -65,7 +65,7 @@ fi if param compare SYS_AUTOSTART 2101 101 then - sh /etc/init.d/2101_hk_bixler + sh /etc/init.d/2101_fw_AERT set MODE custom fi @@ -81,6 +81,12 @@ then set MODE custom fi +if param compare SYS_AUTOSTART 2104 +then + sh /etc/init.d/2104_fw_AETR + set MODE custom +fi + # # Flying wing # diff --git a/ROMFS/px4fmu_common/mixers/AETR.main.mix b/ROMFS/px4fmu_common/mixers/AETR.main.mix new file mode 100644 index 0000000000..8bd3613128 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AETR.main.mix @@ -0,0 +1,84 @@ +Aileron/Elevator/Throttle/Rudder mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the throttle to output 2 and the rudder to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 From 88d200e3a471716a8b3e98a3c28131ff18191ad6 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 3 Jul 2015 14:36:55 +0200 Subject: [PATCH 03/10] set altitude control flag for velocity control --- src/modules/commander/commander.cpp | 3 ++- src/platforms/ros/nodes/commander/commander.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5ac8e9ca8b..6c0f1da8cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2637,7 +2637,8 @@ set_control_mode() control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position; - control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; + control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; break; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index abaa6fc60d..54086cfd4b 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -135,7 +135,8 @@ void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboar msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; - msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position; + msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; } void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, From 9451d2285026de60e47f726dcfa7d39c886721c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 14:54:25 +0200 Subject: [PATCH 04/10] Aerocore: Retire attitude-only EKF --- makefiles/config_aerocore_default.mk | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index c906d54189..0ce01d499d 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -48,11 +48,12 @@ MODULES += modules/navigator MODULES += modules/mavlink # -# Estimation modules (EKF/ SO3 / other filters) +# Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 +# Too high RAM usage due to static allocations +#MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_q MODULES += modules/position_estimator_inav # From ecaa25ba1a9d6f1ac0f1df2de5b20355a6fb4b95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 14:55:53 +0200 Subject: [PATCH 05/10] FMUv2: Retire attitude only EKF --- makefiles/config_px4fmu-v2_default.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 7884b94cb0..0998ebf9f8 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -76,7 +76,8 @@ MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) # -MODULES += modules/attitude_estimator_ekf +# Too high RAM usage due to static allocations +#MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_q MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav From e23459e8500ca2ea712cc756060019176bec17a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:17:07 +0200 Subject: [PATCH 06/10] Commander: Fix dynamic battery scaling, proposed by @orangelynx. Fixes #2523. --- src/modules/commander/commander_helper.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 362a707c03..0d5abef9ab 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -382,8 +382,12 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); - float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + /* the range from full to empty is the same for batteries under load and without load, + * since the voltage drop applies to both the full and empty state + */ + float voltage_range = (bat_v_full - bat_v_empty) + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ From f8f412fc61108a1b1962bb096b18092ed564c175 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:50:47 +0200 Subject: [PATCH 07/10] Commander: Compile fix --- src/modules/commander/commander_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0d5abef9ab..68949eec1d 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -386,7 +386,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */ - float voltage_range = (bat_v_full - bat_v_empty) + float voltage_range = (bat_v_full - bat_v_empty); float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range); if (bat_capacity > 0.0f) { From 615affdef9cb84d8c50103bb910265cf41c619c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jul 2015 23:51:45 +0200 Subject: [PATCH 08/10] S.BUS Output: deliver the disarmed PWM values --- src/modules/px4iofirmware/mixer.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b5d93daea7..0106fa1eb7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -287,15 +287,18 @@ mixer_tick(void) } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); + } /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) - sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } } } From b27b864cf0981274ec96ece16fd3969803c91ffc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 10:45:01 +0200 Subject: [PATCH 09/10] Commander: Only copy global position is valid. This is because the app assumed that it only gets published once valid. --- src/modules/commander/commander.cpp | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c0f1da8cb..f6fa5e6813 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1479,7 +1479,22 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + vehicle_global_position_s gpos; + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos); + + /* copy to global struct if valid, with hysteresis */ + + // XXX consolidate this with local position handling and timeouts after release + // but we want a low-risk change now. + if (status.condition_global_position_valid) { + if (gpos.eph < eph_threshold * 2.5f) { + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + } else { + if (gpos.eph < eph_threshold) { + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + } } /* update local position estimate */ @@ -1492,17 +1507,16 @@ int commander_thread_main(int argc, char *argv[]) //update condition_global_position_valid //Global positions are only published by the estimators if they are valid - if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { + if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { //We have had no good fix for POSITION_TIMEOUT amount of time - if(status.condition_global_position_valid) { + if (status.condition_global_position_valid) { set_tune_override(TONE_GPS_WARNING_TUNE); status_changed = true; status.condition_global_position_valid = false; } - } - else if(global_position.timestamp != 0) { - //Got good global position estimate - if(!status.condition_global_position_valid) { + } else if (global_position.timestamp != 0) { + // Got good global position estimate + if (!status.condition_global_position_valid) { status_changed = true; status.condition_global_position_valid = true; } From 8f4b9c02f0f68ddc69b11c5045dac672ccb886b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 10:45:30 +0200 Subject: [PATCH 10/10] EKF: Fix for the GPS timeout logic --- .../ekf_att_pos_estimator_main.cpp | 16 +++++++++++++--- 1 file changed, 13 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 877bff6585..60be85b2cd 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 @@ -74,6 +74,8 @@ static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we si static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) +static constexpr float EPH_LARGE_VALUE = 1000.0f; +static constexpr float EPV_LARGE_VALUE = 1000.0f; /** * estimator app start / stop handling function @@ -924,8 +926,16 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; + + if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _global_pos.eph = EPH_LARGE_VALUE; + _global_pos.epv = EPV_LARGE_VALUE; + } else { + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + } if (!isfinite(_global_pos.lat) || !isfinite(_global_pos.lon) || @@ -1424,7 +1434,7 @@ void AttitudePositionEstimatorEKF::pollData() // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {