diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml
index 6cf856017c..2f164ee52b 100644
--- a/Tools/parameters_injected.xml
+++ b/Tools/parameters_injected.xml
@@ -89,9 +89,8 @@
specification sheet; accuracy will help control performance but
some deviation from the specified value is acceptable.
RPM/v
- 0
0
- 100
+ 4000
READ ONLY: Motor inductance in henries.
@@ -116,7 +115,7 @@
Ohms
3
-
+
Acceleration limit (V)
Acceleration limit (V)
Volts
diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index b4617e40cf..8767f9f841 100755
--- a/Tools/sitl_run.sh
+++ b/Tools/sitl_run.sh
@@ -56,8 +56,8 @@ then
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build
# Set the model path so Gazebo finds the airframes
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
- # Disable online model lookup since this is quite experimental and unstable
- export GAZEBO_MODEL_DATABASE_URI=""
+ # The next line would disable online model lookup, can be commented in, in case of unstable behaviour.
+ # export GAZEBO_MODEL_DATABASE_URI=""
export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
mkdir -p Tools/sitl_gazebo/Build
cd Tools/sitl_gazebo/Build
diff --git a/msg/control_state.msg b/msg/control_state.msg
index d976fbae8d..4f130353d3 100644
--- a/msg/control_state.msg
+++ b/msg/control_state.msg
@@ -16,3 +16,4 @@ 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)
+float32 horz_acc_mag # Magnitude of the horizontal acceleration
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index e575b9d5e3..c91672c0be 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -120,6 +120,8 @@ bool in_transition_mode
bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
+bool condition_system_prearm_error_reported # true if errors have already been reported
+bool condition_system_hotplug_timeout # true if the hotplug sensor search is over
bool condition_system_returned_to_home
bool condition_auto_mission_available
bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
@@ -139,6 +141,7 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded
bool rc_input_blocked # set if RC input should be ignored temporarily
uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.
+bool data_link_found_new # new datalink to GCS found
bool data_link_lost # datalink to GCS lost
bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events
@@ -179,3 +182,4 @@ bool circuit_breaker_engaged_power_check
bool circuit_breaker_engaged_airspd_check
bool circuit_breaker_engaged_enginefailure_check
bool circuit_breaker_engaged_gpsfailure_check
+bool cb_usb
diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
index 0b52ca48ff..155173db5f 100644
--- a/src/drivers/boards/aerocore/board_config.h
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -82,6 +82,9 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
+/* SPI4--Ramtron */
+#define PX4_SPI_BUS_RAMTRON 4
+
/* Nominal chip selects for devices on SPI bus #3 */
#define PX4_SPIDEV_ACCEL_MAG 0
#define PX4_SPIDEV_GYRO 1
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 0f1f2fd4a2..ea60df9100 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -113,6 +113,7 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define PX4_SPI_BUS_SENSORS 1
+#define PX4_SPI_BUS_RAMTRON 2
#define PX4_SPI_BUS_EXT 4
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4d82ef9e08..64d9dced4a 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1469,6 +1469,10 @@ PX4FMU::sensor_reset(int ms)
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+ stm32_configgpio(GPIO_SPI1_SCK);
+ stm32_configgpio(GPIO_SPI1_MISO);
+ stm32_configgpio(GPIO_SPI1_MOSI);
+
// // XXX bring up the EXTI pins again
// stm32_configgpio(GPIO_GYRO_DRDY);
// stm32_configgpio(GPIO_MAG_DRDY);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 719bacb41f..2cabe98a23 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1780,8 +1780,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
/* get RSSI from input channel */
if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
- int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) /
- ((_rssi_pwm_max - _rssi_pwm_min) / 100);
+ int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) /
+ (_rssi_pwm_max - _rssi_pwm_min);
rssi = rssi > 100 ? 100 : rssi;
rssi = rssi < 0 ? 0 : rssi;
input_rc.rssi = rssi;
diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp
index 737d9ea1a3..299811ea43 100644
--- a/src/lib/ecl/validation/data_validator.cpp
+++ b/src/lib/ecl/validation/data_validator.cpp
@@ -43,6 +43,7 @@
#include
DataValidator::DataValidator(DataValidator *prev_sibling) :
+ _error_mask(ERROR_FLAG_NO_ERROR),
_time_last(0),
_timeout_interval(20000),
_event_count(0),
@@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in
float
DataValidator::confidence(uint64_t timestamp)
{
+ float ret = 1.0f;
+
/* check if we have any data */
if (_time_last == 0) {
- return 0.0f;
+ _error_mask |= ERROR_FLAG_NO_DATA;
+ ret = 0.0f;
}
-
- /* check error count limit */
- if (_error_count > NORETURN_ERRCOUNT) {
- return 0.0f;
+
+ /* timed out - that's it */
+ if (timestamp - _time_last > _timeout_interval) {
+ _error_mask |= ERROR_FLAG_TIMEOUT;
+ ret = 0.0f;
}
/* we got the exact same sensor value N times in a row */
if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
- return 0.0f;
+ _error_mask |= ERROR_FLAG_STALE_DATA;
+ ret = 0.0f;
}
-
- /* timed out - that's it */
- if (timestamp - _time_last > _timeout_interval) {
- return 0.0f;
+
+ /* check error count limit */
+ if (_error_count > NORETURN_ERRCOUNT) {
+ _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
+ ret = 0.0f;
}
/* cap error density counter at window size */
if (_error_density > ERROR_DENSITY_WINDOW) {
+ _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
_error_density = ERROR_DENSITY_WINDOW;
}
-
- /* return local error density for last N measurements */
- return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
-}
-
-int
-DataValidator::priority()
-{
- return _priority;
+
+ /* no critical errors */
+ if(ret > 0.0f) {
+ /* return local error density for last N measurements */
+ ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
+ }
+
+ return ret;
}
void
diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index dde9cb51aa..31f6d9d989 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -91,7 +91,18 @@ public:
* Get the priority of this validator
* @return the stored priority
*/
- int priority();
+ int priority() { return (_priority); }
+
+ /**
+ * Get the error state of this validator
+ * @return the bitmask with the error status
+ */
+ uint32_t state() { return (_error_mask); }
+
+ /**
+ * Reset the error state of this validator
+ */
+ void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
/**
* Get the RMS values of this validator
@@ -111,9 +122,20 @@ public:
* @param timeout_interval_us The timeout interval in microseconds
*/
void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
+
+ /**
+ * Data validator error states
+ */
+ static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U);
+ static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U);
+ static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1);
+ static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2);
+ static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3);
+ static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4);
private:
static const unsigned _dimensions = 3;
+ uint32_t _error_mask; /**< sensor error state */
uint64_t _time_last; /**< last timestamp */
uint64_t _timeout_interval; /**< interval in which the datastream times out in us */
uint64_t _event_count; /**< total data counter */
diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp
index ac4bcc8b9b..8bf060cf65 100644
--- a/src/lib/ecl/validation/data_validator_group.cpp
+++ b/src/lib/ecl/validation/data_validator_group.cpp
@@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
bool true_failsafe = true;
- /* check wether the switch was a failsafe or preferring a higher priority sensor */
+ /* check whether the switch was a failsafe or preferring a higher priority sensor */
if (pre_check_prio != -1 && pre_check_prio < max_priority &&
fabsf(pre_check_confidence - max_confidence) < 0.1f) {
/* this is not a failover */
true_failsafe = false;
+ /* reset error flags, this is likely a hotplug sensor coming online late */
+ best->reset_state();
}
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
@@ -199,17 +201,25 @@ void
DataValidatorGroup::print()
{
/* print the group's state */
- ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+ ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
-
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (next->used()) {
- ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+ uint32_t flags = next->state();
+
+ ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(),
+ ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""),
+ ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""),
+ ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""),
+ ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : ""));
+
next->print();
}
next = next->sibling();
@@ -222,3 +232,35 @@ DataValidatorGroup::failover_count()
{
return _toggle_count;
}
+
+int
+DataValidatorGroup::failover_index()
+{
+ DataValidator *next = _first;
+ unsigned i = 0;
+
+ while (next != nullptr) {
+ if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+ return i;
+ }
+ next = next->sibling();
+ i++;
+ }
+ return -1;
+}
+
+uint32_t
+DataValidatorGroup::failover_state()
+{
+ DataValidator *next = _first;
+ unsigned i = 0;
+
+ while (next != nullptr) {
+ if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+ return next->state();
+ }
+ next = next->sibling();
+ i++;
+ }
+ return DataValidator::ERROR_FLAG_NO_ERROR;
+}
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
index 3756be2638..855c3ed4ae 100644
--- a/src/lib/ecl/validation/data_validator_group.h
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -80,6 +80,20 @@ public:
* @return the number of failovers
*/
unsigned failover_count();
+
+ /**
+ * Get the index of the failed sensor in the group
+ *
+ * @return index of the failed sensor
+ */
+ int failover_index();
+
+ /**
+ * Get the error state of the failed sensor in the group
+ *
+ * @return bitmask with erro states of the failed sensor
+ */
+ uint32_t failover_state();
/**
* Print the validator value
diff --git a/src/lib/matrix b/src/lib/matrix
index 2c7a375e3d..7656385ea1 160000
--- a/src/lib/matrix
+++ b/src/lib/matrix
@@ -1 +1 @@
-Subproject commit 2c7a375e3d7ce143dd1991c9135a5a55952a8977
+Subproject commit 7656385ea1d3f0a374a8146430bc63cf02e66d6b
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 1531e32778..4b4fba54c8 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -348,10 +348,17 @@ void AttitudeEstimatorQ::task_main()
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
}
- _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
- sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
- _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
- sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+ /* ignore empty fields */
+ if (sensors.accelerometer_timestamp[i] > 0) {
+ _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
+ sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
+ }
+
+ /* ignore empty fields */
+ if (sensors.magnetometer_timestamp[i] > 0) {
+ _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
+ sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+ }
}
int best_gyro, best_accel, best_mag;
@@ -369,12 +376,48 @@ void AttitudeEstimatorQ::task_main()
_data_good = true;
- if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
- _voter_accel.failover_count() > 0 ||
- _voter_mag.failover_count() > 0)) {
+ if (!_failsafe) {
+ uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
- _failsafe = true;
- mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+ if (_voter_gyro.failover_count() > 0) {
+ _failsafe = true;
+ flags = _voter_gyro.failover_state();
+ mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
+ _voter_gyro.failover_index(),
+ ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+ ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+ ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+ }
+
+ if (_voter_accel.failover_count() > 0) {
+ _failsafe = true;
+ flags = _voter_accel.failover_state();
+ mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
+ _voter_accel.failover_index(),
+ ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+ ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+ ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+ }
+
+ if (_voter_mag.failover_count() > 0) {
+ _failsafe = true;
+ flags = _voter_mag.failover_state();
+ mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!",
+ _voter_mag.failover_index(),
+ ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+ ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+ ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+ ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+ }
+
+ if (_failsafe) {
+ mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+ }
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index 2a8a0695cf..8093ef56ba 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -114,7 +114,7 @@ int check_calibration(int fd, const char* param_template, int &devid)
return !calibration_found;
}
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -124,7 +124,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (fd < 0) {
if (!optional) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
@@ -134,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
if (ret) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false;
goto out;
@@ -143,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
success = false;
goto out;
@@ -154,7 +154,7 @@ out:
return success;
}
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
@@ -164,7 +164,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (fd < 0) {
if (!optional) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
@@ -174,7 +174,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
if (ret) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false;
goto out;
@@ -183,7 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
success = false;
goto out;
@@ -200,13 +200,15 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
- mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
+ if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
/* this is frickin' fatal */
success = false;
goto out;
}
} else {
- mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+ }
/* this is frickin' fatal */
success = false;
goto out;
@@ -219,7 +221,7 @@ out:
return success;
}
-static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -229,8 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (fd < 0) {
if (!optional) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
+ }
}
return false;
@@ -239,8 +243,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
if (ret) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
+ }
success = false;
goto out;
}
@@ -248,8 +254,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
+ }
success = false;
goto out;
}
@@ -259,7 +267,7 @@ out:
return success;
}
-static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -269,8 +277,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (fd < 0) {
if (!optional) {
- mavlink_and_console_log_critical(mavlink_fd,
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
+ }
}
return false;
@@ -294,7 +304,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return success;
}
-static bool airspeedCheck(int mavlink_fd, bool optional)
+static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
{
bool success = true;
int ret;
@@ -304,13 +314,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
- mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+ }
success = false;
goto out;
}
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
- mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+ }
// XXX do not make this fatal yet
}
@@ -319,7 +333,7 @@ out:
return success;
}
-static bool gnssCheck(int mavlink_fd)
+static bool gnssCheck(int mavlink_fd, bool report_fail)
{
bool success = true;
@@ -341,8 +355,10 @@ static bool gnssCheck(int mavlink_fd)
}
//Report failure to detect module
- if(!success) {
- mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+ if (!success) {
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+ }
}
px4_close(gpsSub);
@@ -350,7 +366,7 @@ static bool gnssCheck(int mavlink_fd)
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
- bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
+ bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
bool failed = false;
@@ -365,7 +381,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
- if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
+ if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -376,7 +392,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
- mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
+ if (reportFailures) {
+ mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
+ }
failed = true;
}
}
@@ -392,7 +410,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
- if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
+ if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
failed = true;
}
@@ -403,7 +421,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
- mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
+ if (reportFailures) {
+ mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
+ }
failed = true;
}
}
@@ -419,7 +439,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
- if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
+ if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -430,7 +450,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
- mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
+ if (reportFailures) {
+ mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
+ }
failed = true;
}
}
@@ -446,7 +468,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
- if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
+ if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -458,29 +480,33 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
- mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
+ if (reportFailures) {
+ mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
+ }
failed = true;
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
- if (!airspeedCheck(mavlink_fd, true)) {
+ if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
- if (rc_calibration_check(mavlink_fd) != OK) {
- mavlink_log_critical(mavlink_fd, "RC calibration check failed");
+ if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
+ if (reportFailures) {
+ mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
+ }
failed = true;
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
- if(!gnssCheck(mavlink_fd)) {
+ if(!gnssCheck(mavlink_fd, reportFailures)) {
failed = true;
}
}
diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h
index b6423a4d9a..058d6b8956 100644
--- a/src/modules/commander/PreflightCheck.h
+++ b/src/modules/commander/PreflightCheck.h
@@ -67,7 +67,7 @@ namespace Commander
* true if the GNSS receiver should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
- bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
+ bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false);
const unsigned max_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3;
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index eb2bcd650e..4139a949e0 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -68,7 +68,7 @@ static const char *sensor_name = "dpress";
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
- mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+ mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(int mavlink_fd)
@@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd)
const unsigned maxcount = 2400;
/* give directions */
- mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
@@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true;
} else {
- mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
}
px4_close(fd);
@@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
- mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
- mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
}
- mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
@@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
- mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
@@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd)
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
- mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
- mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
@@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
- mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
@@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd)
goto error_return;
}
- mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
- mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0;
@@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
- mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
+ mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
- mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
+ mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
- mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
+ mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
- mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
/* save */
- mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
goto error_return;
} else {
- mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
+ mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
@@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd)
goto error_return;
}
- mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
- mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index fc755437da..83e79b77fc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
+#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 10 seconds */
+
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 10000000
@@ -360,6 +362,8 @@ int commander_main(int argc, char *argv[])
calib_ret = do_level_calibration(mavlink_fd);
} else if (!strcmp(argv[2], "esc")) {
calib_ret = do_esc_calibration(mavlink_fd, &armed);
+ } else if (!strcmp(argv[2], "airspeed")) {
+ calib_ret = do_airspeed_calibration(mavlink_fd);
} else {
warnx("argument %s unsupported.", argv[2]);
}
@@ -377,23 +381,31 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "check")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
- int checkres = prearm_check(&status, mavlink_fd_local);
+ int checkres = 0;
+ checkres = preflight_check(&status, mavlink_fd_local, false);
+ warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
+ checkres = preflight_check(&status, mavlink_fd_local, true);
+ warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
px4_close(mavlink_fd_local);
- warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
return 0;
}
if (!strcmp(argv[1], "arm")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
- arm_disarm(true, mavlink_fd_local, "command line");
- warnx("note: not updating home position on commandline arming!");
+ if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) {
+ warnx("note: not updating home position on commandline arming!");
+ } else {
+ warnx("arming failed");
+ }
px4_close(mavlink_fd_local);
return 0;
}
if (!strcmp(argv[1], "disarm")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
- arm_disarm(false, mavlink_fd_local, "command line");
+ if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) {
+ warnx("rejected disarm");
+ }
px4_close(mavlink_fd_local);
return 0;
}
@@ -891,6 +903,7 @@ int commander_thread_main(int argc, char *argv[])
/* not yet initialized */
commander_initialized = false;
+ bool sensor_fail_tune_played = false;
bool arm_tune_played = false;
bool was_landed = true;
bool was_armed = false;
@@ -1020,6 +1033,9 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
+
+ status.condition_system_prearm_error_reported = true;
+ status.condition_system_hotplug_timeout = false;
status.counter++;
status.timestamp = hrt_absolute_time();
@@ -1101,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[])
bool updated = false;
- rc_calibration_check(mavlink_fd);
+ rc_calibration_check(mavlink_fd, true);
/* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -1243,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
// Run preflight check
int32_t rc_in_off = 0;
+ bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
param_get(_param_autostart_id, &autostart_id);
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off;
@@ -1251,14 +1268,10 @@ int commander_thread_main(int argc, char *argv[])
status.condition_system_sensors_initialized = false;
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else {
- status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
- checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check);
- if (!status.condition_system_sensors_initialized) {
- set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
- }
- else {
+ // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
+ status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
+ checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false);
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
- }
}
commander_boot_timestamp = hrt_absolute_time();
@@ -1354,11 +1367,11 @@ int commander_thread_main(int argc, char *argv[])
if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
- !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+ !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
/* re-check RC calibration */
- rc_calibration_check(mavlink_fd);
+ rc_calibration_check(mavlink_fd, true);
}
/* Safety parameters */
@@ -1446,6 +1459,8 @@ int commander_thread_main(int argc, char *argv[])
!armed.armed) {
bool chAirspeed = false;
+ bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@@ -1456,11 +1471,11 @@ int commander_thread_main(int argc, char *argv[])
if (is_hil_setup(autostart_id)) {
/* HIL configuration: check only RC input */
(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
- !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
+ !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true);
} else {
/* check sensors also */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
- !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+ !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
}
@@ -1686,10 +1701,10 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
if (status.condition_landed) {
- mavlink_log_critical(mavlink_fd, "LANDING DETECTED");
+ mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED");
} else {
- mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
+ mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED");
}
}
@@ -2177,9 +2192,13 @@ int commander_thread_main(int argc, char *argv[])
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
- /* only report a regain */
+ /* report a regain */
if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
+ } else if (telemetry_last_dl_loss[i] == 0){
+ /* do not report a new data link in order to not spam the user */
+ status.data_link_found_new = true;
+ status_changed = true;
}
telemetry_lost[i] = false;
@@ -2189,6 +2208,10 @@ int commander_thread_main(int argc, char *argv[])
/* telemetry was healthy also in last iteration
* we don't have to check a timeout */
have_link = true;
+ if(status.data_link_found_new) {
+ status.data_link_found_new = false;
+ status_changed = true;
+ }
}
} else {
@@ -2414,7 +2437,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
set_tune(TONE_STOP_TUNE);
}
-
+
/* reset arm_tune_played when disarmed */
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
@@ -2425,6 +2448,21 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false;
}
+
+ /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
+ hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+
+ if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
+ set_tune_override(TONE_GPS_WARNING_TUNE);
+ sensor_fail_tune_played = true;
+ status_changed = true;
+ }
+
+ /* update timeout flag */
+ if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
+ status.condition_system_hotplug_timeout = hotplug_timeout;
+ status_changed = true;
+ }
counter++;
@@ -2482,6 +2520,8 @@ get_circuit_breaker_params()
{
status.circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+ status.cb_usb =
+ circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
status.circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
@@ -2508,19 +2548,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
/* driving rgbled */
if (changed) {
bool set_normal_color = false;
-
+ bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+
/* set mode */
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
- } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) {
+ } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
+
+ } else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
+ rgbled_set_mode(RGBLED_MODE_BREATHE);
+ set_normal_color = true;
} else { // STANDBY_ERROR and other states
rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
@@ -3197,6 +3242,7 @@ void *commander_low_prio_loop(void *arg)
// so this would be prone to false negatives.
bool checkAirspeed = false;
+ bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@@ -3204,7 +3250,7 @@ void *commander_low_prio_loop(void *arg)
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
- !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+ !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 21789c9c66..bcdb5d3f9d 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -93,8 +93,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_IN_AIR_RESTORE",
};
-static bool sensor_feedback_provided = false;
-
transition_result_t
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
@@ -122,10 +120,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
*/
int prearm_ret = OK;
- /* only perform the check if we have to */
+ /* only perform the pre-arm check if we have to */
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
- prearm_ret = prearm_check(status, mavlink_fd);
+ prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ );
+ }
+ /* re-run the pre-flight check as long as sensors are failing */
+ if (!status->condition_system_sensors_initialized
+ && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED ||
+ new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
+ && status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+ prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
+ status->condition_system_sensors_initialized = !prearm_ret;
}
/*
@@ -171,7 +177,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
- mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+ mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@@ -196,10 +202,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
- mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+ mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
- mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+ mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
@@ -243,9 +249,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) {
- if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+ if ((!status->condition_system_prearm_error_reported &&
+ status->condition_system_hotplug_timeout) ||
+ (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
- sensor_feedback_provided = true;
+ status->condition_system_prearm_error_reported = true;
}
feedback_provided = true;
valid_transition = false;
@@ -262,7 +270,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* reset feedback state */
if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
valid_transition) {
- sensor_feedback_provided = false;
+ status->condition_system_prearm_error_reported = false;
+ }
+ if(status->data_link_found_new == true)
+ {
+ status->condition_system_prearm_error_reported = false;
}
/* end of atomic state update */
@@ -380,7 +392,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
- mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
+ mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
@@ -453,7 +465,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
closedir(d);
ret = TRANSITION_CHANGED;
- mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+ mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
@@ -490,11 +502,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
}
ret = TRANSITION_CHANGED;
- mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+ mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
#endif
} else {
- mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+ mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
@@ -731,24 +743,26 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
return status->nav_state != nav_state_old;
}
-int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm)
{
- /* at this point this equals the preflight check, but might add additional
- * quantities later.
+ /*
*/
+ bool reportFailures = false;
+ reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout;
+
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
- if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
+ if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
checkAirspeed = true;
}
- bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
-
- if (status->usb_connected) {
- prearm_ok = false;
- mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
+ bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
+
+ if (!status->cb_usb && status->usb_connected && prearm) {
+ preflight_ok = false;
+ if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
}
- return !prearm_ok;
+ return !preflight_ok;
}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 084813f8cb..bdb7b13f28 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
-int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm);
#endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 7c2942f53e..1ef0c57e3e 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -52,7 +52,7 @@
* @unit meters
* @group Mission
*/
-PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
+PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
/**
* Enable persistent onboard mission storage
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 1ddedddff4..6ecf73a606 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -135,7 +135,7 @@ public:
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
- bool home_position_valid() { return _home_position_set; }
+ bool home_position_valid() { return (_home_pos.timestamp > 0); }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
@@ -205,8 +205,6 @@ private:
geofence_result_s _geofence_result;
vehicle_attitude_setpoint_s _att_sp;
- bool _home_position_set;
-
bool _mission_item_valid; /**< flags if the current mission item is valid */
int _mission_instance_count; /**< instance count for the current mission */
@@ -257,7 +255,7 @@ private:
/**
* Retrieve home position
*/
- void home_position_update();
+ void home_position_update(bool force=false);
/**
* Retreive navigation capabilities
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 92f3e2fecb..e44b8d0be5 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -126,7 +126,6 @@ Navigator::Navigator() :
_pos_sp_triplet{},
_mission_result{},
_att_sp{},
- _home_position_set(false),
_mission_item_valid(false),
_mission_instance_count(0),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -206,17 +205,13 @@ Navigator::sensor_combined_update()
}
void
-Navigator::home_position_update()
+Navigator::home_position_update(bool force)
{
bool updated = false;
orb_check(_home_pos_sub, &updated);
- if (updated) {
+ if (updated || force) {
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
-
- if (_home_pos.timestamp > 0) {
- _home_position_set = true;
- }
}
}
@@ -298,7 +293,7 @@ Navigator::task_main()
global_position_update();
gps_position_update();
sensor_combined_update();
- home_position_update();
+ home_position_update(true);
navigation_capabilities_update();
params_update();
@@ -408,7 +403,7 @@ Navigator::task_main()
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
- bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
+ bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
@@ -489,7 +484,7 @@ Navigator::task_main()
}
/* iterate through navigation modes and set active/inactive for each */
- for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index be08851c25..f15ec8969a 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -156,7 +156,9 @@ struct logbuffer_s lb;
static pthread_mutex_t logbuffer_mutex;
static pthread_cond_t logbuffer_cond;
-static char log_dir[32];
+#define LOG_BASE_PATH_LEN 64
+
+static char log_dir[LOG_BASE_PATH_LEN];
/* statistics counters */
static uint64_t start_time = 0;
@@ -423,7 +425,6 @@ int create_log_dir()
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
- warnx("log dir created: %s", log_dir);
break;
} else if (errno != EEXIST) {
@@ -444,15 +445,15 @@ int create_log_dir()
}
/* print logging path, important to find log file later */
- mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
+ mavlink_and_console_log_info(mavlink_fd, "[log] log dir: %s", log_dir);
return 0;
}
int open_log_file()
{
/* string to hold the path to the log */
- char log_file_name[32] = "";
- char log_file_path[64] = "";
+ char log_file_name[64] = "";
+ char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = "";
struct tm tt;
bool time_ok = get_log_time_utc_tt(&tt, false);
@@ -480,7 +481,7 @@ int open_log_file()
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
- mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+ mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
@@ -492,10 +493,10 @@ int open_log_file()
#endif
if (fd < 0) {
- mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
+ mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name);
} else {
- mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name);
+ mavlink_and_console_log_info(mavlink_fd, "[log] start: %s", log_file_name);
}
return fd;
@@ -504,8 +505,8 @@ int open_log_file()
int open_perf_file(const char* str)
{
/* string to hold the path to the log */
- char log_file_name[32] = "";
- char log_file_path[64] = "";
+ char log_file_name[64] = "";
+ char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = "";
struct tm tt;
bool time_ok = get_log_time_utc_tt(&tt, false);
@@ -532,7 +533,7 @@ int open_perf_file(const char* str)
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
- mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+ mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
@@ -544,7 +545,7 @@ int open_perf_file(const char* str)
#endif
if (fd < 0) {
- mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
+ mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name);
}
@@ -671,7 +672,7 @@ void sdlog2_start_log()
/* create log dir if needed */
if (create_log_dir() != 0) {
- mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
+ mavlink_and_console_log_critical(mavlink_fd, "[log] error creating log dir");
return;
}
@@ -761,7 +762,7 @@ void sdlog2_stop_log()
/* free log writer performance counter */
perf_free(perf_write);
- mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped");
+ mavlink_and_console_log_info(mavlink_fd, "[log] logging stopped");
sdlog2_status();
}
@@ -1675,6 +1676,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
+ log_msg.body.log_RC.rssi = buf.rc.rssi;
log_msg.body.log_RC.channel_count = buf.rc.channel_count;
log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
LOGBUFFER_WRITE_AND_COUNT(RC);
@@ -1911,7 +1913,7 @@ void sdlog2_status()
float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
- mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
+ mavlink_log_info(mavlink_fd, "[log] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
}
}
@@ -1936,7 +1938,7 @@ int check_free_space()
/* use a threshold of 50 MiB */
if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
mavlink_and_console_log_critical(mavlink_fd,
- "[sdlog2] no space on MicroSD: %u MiB",
+ "[log] no space on MicroSD: %u MiB",
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
/* we do not need a flag to remember that we sent this warning because we will exit anyway */
return PX4_ERROR;
@@ -1944,7 +1946,7 @@ int check_free_space()
/* use a threshold of 100 MiB to send a warning */
} else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
mavlink_and_console_log_critical(mavlink_fd,
- "[sdlog2] space on MicroSD low: %u MiB",
+ "[log] space on MicroSD low: %u MiB",
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
/* we don't want to flood the user with warnings */
space_warning_sent = true;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 462b753f98..6ccbec2fa9 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -189,6 +189,7 @@ struct log_STAT_s {
#define LOG_RC_MSG 11
struct log_RC_s {
float channel[8];
+ uint8_t rssi;
uint8_t channel_count;
uint8_t signal_lost;
};
@@ -532,7 +533,7 @@ static const struct log_format_s log_formats[] = {
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(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"),
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"),
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index c76e6c37f3..b4d9b5d1c6 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -56,6 +56,7 @@
#define CBRK_FLIGHTTERM_KEY 121212
#define CBRK_ENGINEFAIL_KEY 284953
#define CBRK_GPSFAIL_KEY 240024
+#define CBRK_USB_CHK_KEY 197848
#include
diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c
index 003b73bb62..a3eaebbb33 100644
--- a/src/modules/systemlib/circuit_breaker_params.c
+++ b/src/modules/systemlib/circuit_breaker_params.c
@@ -147,3 +147,16 @@ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_BUZZER, 0);
+
+/**
+ * Circuit breaker for USB link check
+ *
+ * Setting this parameter to 197848 will disable the USB connected
+ * checks in the commander.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 197848
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_USB_CHK, 0);
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index 25dea7a9b0..622a3a0603 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -52,7 +52,7 @@
#define RC_INPUT_MAP_UNMAPPED 0
-int rc_calibration_check(int mavlink_fd)
+int rc_calibration_check(int mavlink_fd, bool report_fail)
{
char nbuf[20];
@@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd)
param_t map_parm = param_find(rc_map_mandatory[j]);
if (map_parm == PARAM_INVALID) {
- mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]);
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
@@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd)
param_get(map_parm, &mapping);
if (mapping > RC_INPUT_MAX_CHANNELS) {
- mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]);
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
}
if (mapping == 0) {
- mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]);
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
@@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd)
/* assert min..center..max ordering */
if (param_min < RC_INPUT_LOWEST_MIN_US) {
count++;
- mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US);
+
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > RC_INPUT_HIGHEST_MAX_US) {
count++;
- mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US);
+
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
- mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min);
+
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
- mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max);
+
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
- mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US);
+ if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); }
+
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
@@ -187,8 +199,13 @@ int rc_calibration_check(int mavlink_fd)
if (channels_failed) {
sleep(2);
- mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count,
- (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
+
+ if (report_fail) {
+ mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.",
+ total_fail_count,
+ (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
+ }
+
usleep(100000);
}
diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h
index 035f63bef4..239c8a6a13 100644
--- a/src/modules/systemlib/rc_check.h
+++ b/src/modules/systemlib/rc_check.h
@@ -47,6 +47,6 @@ __BEGIN_DECLS
* @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed)
*/
-__EXPORT int rc_calibration_check(int mavlink_fd);
+__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail);
__END_DECLS
diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt
index b8c899840a..ce348ec910 100644
--- a/src/modules/uavcan/CMakeLists.txt
+++ b/src/modules/uavcan/CMakeLists.txt
@@ -37,15 +37,12 @@ set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
string(TOUPPER "${OS}" OS_UPPER)
add_definitions(
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
- -DUAVCAN_MAX_NETWORK_SIZE_HINT=16
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
-DUAVCAN_NO_ASSERTIONS
-DUAVCAN_PLATFORM=stm32
-DUAVCAN_STM32_${OS_UPPER}=1
-DUAVCAN_STM32_NUM_IFACES=2
-DUAVCAN_STM32_TIMER_NUMBER=5
- -DUAVCAN_USE_CPP03=ON
- -DUAVCAN_USE_EXTERNAL_SNPRINT
)
add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
diff --git a/src/modules/uavcan/allocator.hpp b/src/modules/uavcan/allocator.hpp
new file mode 100644
index 0000000000..db34aa04fb
--- /dev/null
+++ b/src/modules/uavcan/allocator.hpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step.
+namespace uavcan_node
+{
+
+struct AllocatorSynchronizer
+{
+ const ::irqstate_t state = ::irqsave();
+ ~AllocatorSynchronizer() { ::irqrestore(state); }
+};
+
+struct Allocator : public uavcan::HeapBasedPoolAllocator
+{
+ static constexpr unsigned CapacitySoftLimit = 250;
+ static constexpr unsigned CapacityHardLimit = 500;
+
+ Allocator() :
+ uavcan::HeapBasedPoolAllocator(CapacitySoftLimit, CapacityHardLimit)
+ { }
+
+ ~Allocator()
+ {
+ if (getNumAllocatedBlocks() > 0)
+ {
+ warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
+ getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
+ }
+ }
+};
+
+}
diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan
index 0643879922..ed1d71e639 160000
--- a/src/modules/uavcan/libuavcan
+++ b/src/modules/uavcan/libuavcan
@@ -1 +1 @@
-Subproject commit 0643879922239930cf7482777356f06891c26616
+Subproject commit ed1d71e639543ea5743a839c313c53237ab3a27d
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 694a3988a1..36f28a423c 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -44,7 +44,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
_sub_air_pressure_data(node),
_sub_air_temperature_data(node),
- _reports(nullptr)
+ _reports(2, sizeof(baro_report))
{ }
int UavcanBarometerBridge::init()
@@ -55,13 +55,6 @@ int UavcanBarometerBridge::init()
return res;
}
- /* allocate basic report buffers */
- _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
-
- if (_reports == nullptr) {
- return -1;
- }
-
res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
if (res < 0) {
@@ -91,7 +84,7 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl
}
while (count--) {
- if (_reports->get(baro_buf)) {
+ if (_reports.get(baro_buf)) {
ret += sizeof(*baro_buf);
baro_buf++;
}
@@ -132,7 +125,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
irqstate_t flags = irqsave();
- if (!_reports->resize(arg)) {
+ if (!_reports.resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
@@ -186,7 +179,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// add to the ring buffer
- _reports->force(&report);
+ _reports.force(&report);
publish(msg.getSrcNodeID().get(), &report);
}
diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp
index 38cf6ab89a..ed1e308515 100644
--- a/src/modules/uavcan/sensors/baro.hpp
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -76,8 +76,10 @@ private:
uavcan::Subscriber _sub_air_pressure_data;
uavcan::Subscriber _sub_air_temperature_data;
+
+ ringbuffer::RingBuffer _reports;
+
unsigned _msl_pressure = 101325;
- ringbuffer::RingBuffer *_reports;
float last_temperature_kelvin = 0.0f;
};
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 1ba9d345fb..00671decd2 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -77,7 +77,7 @@
UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
- _node(can_driver, system_clock),
+ _node(can_driver, system_clock, _pool_allocator),
_node_mutex(),
_esc_controller(_node),
_time_sync_master(_node),
@@ -144,10 +144,9 @@ UavcanNode::~UavcanNode()
} while (_task != -1);
}
- /* clean up the alternate device node */
- // unregister_driver(PWM_OUTPUT_DEVICE_PATH);
-
- ::close(_armed_sub);
+ (void)::close(_armed_sub);
+ (void)::close(_test_motor_sub);
+ (void)::close(_actuator_direct_sub);
// Removing the sensor bridges
auto br = _sensor_bridges.getHead();
@@ -166,6 +165,10 @@ UavcanNode::~UavcanNode()
pthread_mutex_destroy(&_node_mutex);
px4_sem_destroy(&_server_command_sem);
+ // Is it allowed to delete it like that?
+ if (_mixers != nullptr) {
+ delete _mixers;
+ }
}
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
@@ -965,6 +968,8 @@ int UavcanNode::run()
}
}
+ (void)::close(busevent_fd);
+
teardown();
warnx("exiting.");
@@ -1119,6 +1124,33 @@ UavcanNode::print_info()
(void)pthread_mutex_lock(&_node_mutex);
+ // Memory status
+ printf("Pool allocator status:\n");
+ printf("\tCapacity hard/soft: %u/%u blocks\n",
+ _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity());
+ printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks());
+ printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
+
+ // UAVCAN node perfcounters
+ printf("UAVCAN node status:\n");
+ printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
+ printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount());
+ printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
+ printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
+
+ // CAN driver status
+ for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
+ printf("CAN%u status:\n", unsigned(i + 1));
+
+ auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
+ printf("\tHW errors: %llu\n", iface->getErrorCount());
+
+ auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
+ printf("\tIO errors: %llu\n", iface_perf_cnt.errors);
+ printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx);
+ printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
+ }
+
// ESC mixer status
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
@@ -1169,13 +1201,20 @@ UavcanNode::print_info()
(void)pthread_mutex_unlock(&_node_mutex);
}
+void UavcanNode::shrink()
+{
+ (void)pthread_mutex_lock(&_node_mutex);
+ _pool_allocator.shrink();
+ (void)pthread_mutex_unlock(&_node_mutex);
+}
+
/*
* App entry point
*/
static void print_usage()
{
warnx("usage: \n"
- "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
+ "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -1251,6 +1290,11 @@ int uavcan_main(int argc, char *argv[])
::exit(0);
}
+ if (!std::strcmp(argv[1], "shrink")) {
+ inst->shrink();
+ ::exit(0);
+ }
+
if (!std::strcmp(argv[1], "arm")) {
inst->arm_actuators(true);
::exit(0);
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 6a1d2f391d..b236946e24 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -36,6 +36,7 @@
#include
#include
+#include
#include
#include
#include
@@ -55,6 +56,7 @@
#include "sensors/sensor_bridge.hpp"
#include "uavcan_servers.hpp"
+#include "allocator.hpp"
/**
* @file uavcan_main.hpp
@@ -78,9 +80,8 @@ class UavcanNode : public device::CDev
static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame;
static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1);
- static constexpr unsigned PollTimeoutMs = 10;
+ static constexpr unsigned PollTimeoutMs = 3;
- static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
/*
* This memory is reserved for uavcan to use for queuing CAN frames.
* At 1Mbit there is approximately one CAN frame every 145 uS.
@@ -97,7 +98,6 @@ class UavcanNode : public device::CDev
static constexpr unsigned StackSize = 1800;
public:
- typedef uavcan::Node Node;
typedef uavcan_stm32::CanInitHelper CanInitHelper;
enum eServerAction {None, Start, Stop, CheckFW , Busy};
@@ -109,7 +109,7 @@ public:
static int start(uavcan::NodeID node_id, uint32_t bitrate);
- Node &get_node() { return _node; }
+ uavcan::Node<> &get_node() { return _node; }
// TODO: move the actuator mixing stuff into the ESC controller class
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
@@ -121,6 +121,8 @@ public:
void print_info();
+ void shrink();
+
static UavcanNode *instance() { return _instance; }
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
@@ -130,8 +132,8 @@ public:
int set_param(int remote_node_id, const char *name, char *value);
int get_param(int remote_node_id, const char *name);
int reset_node(int remote_node_id);
-private:
+private:
void fill_node_info();
int init(uavcan::NodeID node_id);
void node_spin_once();
@@ -167,7 +169,9 @@ private:
static UavcanNode *_instance; ///< singleton pointer
- Node _node; ///< library instance
+ uavcan_node::Allocator _pool_allocator;
+
+ uavcan::Node<> _node; ///< library instance
pthread_mutex_t _node_mutex;
px4_sem_t _server_command_sem;
UavcanEscController _esc_controller;
diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp
index 713a4e00f7..ca5bcede66 100644
--- a/src/modules/uavcan/uavcan_servers.cpp
+++ b/src/modules/uavcan/uavcan_servers.cpp
@@ -85,8 +85,8 @@
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
- _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()),
- _subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
+ _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
+ _subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()),
_main_node(main_node),
_tracer(),
_storage_backend(),
@@ -141,13 +141,14 @@ int UavcanServers::stop()
return -1;
}
- _instance = nullptr;
-
- if (server->_subnode_thread != -1) {
- pthread_cancel(server->_subnode_thread);
- pthread_join(server->_subnode_thread, NULL);
+ if (server->_subnode_thread) {
+ warnx("stopping fw srv thread...");
+ server->_subnode_thread_should_exit = true;
+ (void)pthread_join(server->_subnode_thread, NULL);
}
+ _instance = nullptr;
+
server->_main_node.getDispatcher().removeRxFrameListener();
delete server;
@@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
_esc_enumeration_index = 0;
- while (1) {
+ while (!_subnode_thread_should_exit) {
if (_check_fw == true) {
_check_fw = false;
@@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
}
}
- warnx("exiting.");
+ _subnode_thread_should_exit = false;
+
+ warnx("exiting");
return (pthread_addr_t) 0;
}
diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp
index 0b83baa941..3cf4cbb4da 100644
--- a/src/modules/uavcan/uavcan_servers.hpp
+++ b/src/modules/uavcan/uavcan_servers.hpp
@@ -81,24 +81,10 @@ class UavcanServers
{
static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES
- static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
-
- static constexpr unsigned MaxCanFramesPerTransfer = 63;
-
- /**
- * This number is based on the worst case max number of frames per interface. With
- * MemPoolBlockSize set at 48 this is 6048 Bytes.
- *
- * The servers can be forced to use the primary interface only, this can be achieved simply by passing
- * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
- */
- static constexpr unsigned QueuePoolSize =
- (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
-
static constexpr unsigned StackSize = 6000;
static constexpr unsigned Priority = 120;
- typedef uavcan::SubNode SubNode;
+ static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80;
public:
UavcanServers(uavcan::INode &main_node);
@@ -108,7 +94,7 @@ public:
static int start(uavcan::INode &main_node);
static int stop();
- SubNode &get_node() { return _subnode; }
+ uavcan::SubNode<> &get_node() { return _subnode; }
static UavcanServers *instance() { return _instance; }
@@ -124,6 +110,7 @@ public:
private:
pthread_t _subnode_thread;
pthread_mutex_t _subnode_mutex;
+ volatile bool _subnode_thread_should_exit = false;
int init();
@@ -131,12 +118,10 @@ private:
static UavcanServers *_instance; ///< singleton pointer
- typedef VirtualCanDriver vCanDriver;
+ VirtualCanDriver _vdriver;
- vCanDriver _vdriver;
-
- uavcan::SubNode _subnode; ///< library instance
- uavcan::INode &_main_node; ///< library instance
+ uavcan::SubNode<> _subnode;
+ uavcan::INode &_main_node;
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp
index 1c861e481c..956caf11ee 100644
--- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp
+++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp
@@ -114,6 +114,14 @@ public:
uavcan::IsDynamicallyAllocatable- ::check();
}
+ ~Queue()
+ {
+ while (!isEmpty())
+ {
+ pop();
+ }
+ }
+
bool isEmpty() const { return list_.isEmpty(); }
/**
@@ -329,11 +337,7 @@ public:
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
- * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the
- * memory pool that will be shared across all interfaces for RX/TX queues.
- * Typically this value should be no less than 4K per interface.
*/
-template
class VirtualCanDriver : public uavcan::ICanDriver,
public uavcan::IRxFrameListener,
public ITxQueueInjector,
@@ -400,9 +404,8 @@ class VirtualCanDriver : public uavcan::ICanDriver,
}
};
- Event event_; ///< Used to unblock the select() call when IO happens.
+ Event event_; ///< Used to unblock the select() call when IO happens.
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
- uavcan::PoolAllocator allocator_; ///< Shared across all ifaces
uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces];
const unsigned num_ifaces_;
uavcan::ISystemClock &clock_;
@@ -476,7 +479,10 @@ class VirtualCanDriver : public uavcan::ICanDriver,
}
public:
- VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) :
+ VirtualCanDriver(unsigned arg_num_ifaces,
+ uavcan::ISystemClock &system_clock,
+ uavcan::IPoolAllocator& allocator,
+ unsigned virtual_iface_block_allocation_quota) :
num_ifaces_(arg_num_ifaces),
clock_(system_clock)
{
@@ -485,15 +491,12 @@ public:
assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
- const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_;
- const unsigned quota_per_queue = quota_per_iface; // 2x overcommit
-
- UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
- unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue));
+ const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit
for (unsigned i = 0; i < num_ifaces_; i++) {
- ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue);
+ ifaces_[i].template
+ construct
+ (allocator, clock_, driver_mutex_, quota_per_queue);
}
}
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index c87ca630ac..05c1a525b1 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -154,7 +154,7 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
- if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) {
+ if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) {
_vtol_schedule.flight_mode = FW_MODE;
// we can turn off the multirotor motors now
_flag_enable_mc_motors = false;
diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
index 9e170104c4..270aa065a9 100644
--- a/src/modules/vtol_att_control/tiltrotor.cpp
+++ b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -205,7 +205,8 @@ void Tiltrotor::update_vtol_state()
case TRANSITION_FRONT_P1:
// check if we have reached airspeed to switch to fw mode
- if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
+ // also allow switch if we are not armed for the sake of bench testing
+ if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index e15e950fd1..0248a20727 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -690,6 +690,7 @@ int vtol_att_control_main(int argc, char *argv[])
return 1;
}
+ return 0;
}
if (!strcmp(argv[1], "stop")) {
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index 0f54c9c4c3..b38504c623 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -176,12 +176,9 @@ struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
static void
ramtron_attach(void)
{
- /* find the right spi */
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
- struct spi_dev_s *spi = up_spiinitialize(4);
-#else
- struct spi_dev_s *spi = up_spiinitialize(2);
-#endif
+ /* initialize the right spi */
+ struct spi_dev_s *spi = up_spiinitialize(PX4_SPI_BUS_RAMTRON);
+
/* this resets the spi bus, set correct bus speed again */
SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
SPI_SETBITS(spi, 8);
@@ -269,10 +266,10 @@ mtd_start(char *partition_names[], unsigned n_partitions)
}
if (!attached) {
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
- at24xxx_attach();
-#else
+#ifdef CONFIG_MTD_RAMTRON
ramtron_attach();
+#else
+ at24xxx_attach();
#endif
}