From 3157b06fee99b57fe336f7772b293f8689ff8cdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 15:52:19 +0200 Subject: [PATCH 01/48] Generalized the airspeed check --- .../commander/state_machine_helper.cpp | 29 +++++++------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f6..d894c9db09 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -666,29 +667,21 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || + hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; goto system_eval; } - struct differential_pressure_s diff_pres; - - ret = read(fd, &diff_pres, sizeof(diff_pres)); - - if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); - // XXX do not make this fatal yet - ret = OK; - } - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); - /* this is frickin' fatal */ - ret = ERROR; - goto system_eval; + if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; } } From 687612dd654bcdfb390bfdddf8a004b10119c2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 18:34:33 +0200 Subject: [PATCH 02/48] Do not abort if the sensor reset failed, only abort on no data --- src/modules/commander/airspeed_calibration.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 5d21d89d04..923c1a3fff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -95,10 +95,8 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to set scale / offsets for airspeed"); - mavlink_log_critical(mavlink_fd, "dpress reset failed"); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - return ERROR; + warn("FAILED to reset - assuming analog"); + mavlink_log_critical(mavlink_fd, "assuming analog sensor"); } while (calibration_counter < calibration_count) { From 50414257a8802fe7c51a370f598899c8b554914c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:09:57 +0200 Subject: [PATCH 03/48] Remove voltage field for digital sensors --- src/drivers/ets_airspeed/ets_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 1a7e068fe5..d1a9fa57c8 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -186,7 +186,6 @@ ETSAirspeed::collect() report.differential_pressure_filtered_pa = (float)diff_pres_pa; report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; report.temperature = -1000.0f; - report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { From 56e1fe382b77200b3826b1ba438fc49ebab8f8ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:15 +0200 Subject: [PATCH 04/48] Remove voltage field for MEAS sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c0f3c28e0f..7763f10578 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -265,7 +265,6 @@ MEASAirspeed::collect() } report.differential_pressure_raw_pa = diff_press_pa_raw; - report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { From 3461d3d215843681b1537256d26b053e3f2b78e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:35 +0200 Subject: [PATCH 05/48] Introduce analog scaling parameter for analog airspeed sensor --- src/modules/sensors/sensor_params.c | 10 ++++++++-- src/modules/sensors/sensors.cpp | 18 +++++++++--------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5deb471d69..6b90a12391 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -194,16 +194,22 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); /** * Differential pressure sensor offset * + * The offset (zero-reading) in Pascal + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); /** - * Differential pressure sensor analog enabled + * Differential pressure sensor analog scaling + * + * Pick the appropriate scaling from the datasheet. + * this number defines the (linear) conversion from voltage + * to Pascal (pa). * * @group Sensor Calibration */ -PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); /** diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6e2d906e64..0cad0c0e58 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -248,7 +248,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; - float diff_pres_analog_enabled; + float diff_pres_analog_scale; int board_rotation; int external_mag_rotation; @@ -311,7 +311,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; - param_t diff_pres_analog_enabled; + param_t diff_pres_analog_scale; param_t rc_map_roll; param_t rc_map_pitch; @@ -590,7 +590,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); - _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); @@ -798,7 +798,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); - param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1323,22 +1323,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { - float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; - _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_diff_pres_pub > 0) { From 8d081a8b0dd002cda075ee9d8e087fdf54722ccf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:11:03 +0200 Subject: [PATCH 06/48] uORB: Remove voltage field from airspeed --- src/modules/uORB/topics/differential_pressure.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 01e14cda9e..cd48d3cb2f 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,6 @@ struct differential_pressure_s { float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; From 25f3f6e7f2dd87a831d25a9348a67ed918407d96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:12:31 +0200 Subject: [PATCH 07/48] airspeed calibration improvements for analog sensors --- src/modules/commander/airspeed_calibration.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 923c1a3fff..12e527a681 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -76,7 +76,7 @@ int do_airspeed_calibration(int mavlink_fd) /* Reset sensor parameters */ struct airspeed_scale airscale = { - 0.0f, + diff_pres_offset, 1.0f, }; @@ -97,6 +97,12 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } } while (calibration_counter < calibration_count) { From 899657613e44f0d4bbdb66644b35420d7381ff99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:25 +0200 Subject: [PATCH 08/48] airspeed cal: Improve user feedback --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 12e527a681..7599fc777c 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -96,7 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); From e3afb669cae54b042f453605601fed861c52ee45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:47 +0200 Subject: [PATCH 09/48] sensors: Fix usage of offset for measurements --- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6b90a12391..8f6e7abe60 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -205,7 +205,7 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * * Pick the appropriate scaling from the datasheet. * this number defines the (linear) conversion from voltage - * to Pascal (pa). + * to Pascal (pa). For the MPXV7002DP this is 1000. * * @group Sensor Calibration */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0cad0c0e58..cda79693d2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1330,12 +1330,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0)) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale; + float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa - _parameters.diff_pres_offset_pa; + _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa; _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; _diff_pres.temperature = -1000.0f; From 9c63aba9a72083afe1e5f818c6559760c5b6b1cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:44:54 +0200 Subject: [PATCH 10/48] Ensure NaN check is executed before any parts of the filter are run. Fix GPS velocity reset for case of initialized GPS --- .../ekf_att_pos_estimator_main.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 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 5d768b73dd..bc5f709e58 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 @@ -1140,6 +1140,20 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // Guard before running any parts of the filter + // of NaN / invalid values + if (_ekf->statesInitialised) { + + // We're apparently initialized in this case now + + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1204,16 +1218,6 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - - // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1281,7 +1285,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (_ekf->statesInitialised) { + } else if (!_gps_initialized && _ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; From 2fa4434a03abe057e65a5c8ba29f8cd9cd75ca93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 00:17:50 +0200 Subject: [PATCH 11/48] Limit output of driver to positive range --- src/modules/sensors/sensors.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cda79693d2..1ffd7f02f1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -501,6 +501,7 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_diff_pres, 0, sizeof(_diff_pres)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1332,12 +1333,13 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - float diff_pres_pa = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; - _diff_pres.differential_pressure_raw_pa = diff_pres_pa; - _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ From a0d150332ab67dbc501b10fa8adb97d91a79f23c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 15:39:04 +0200 Subject: [PATCH 12/48] Add note to param about tubes --- src/modules/sensors/sensor_params.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 8f6e7abe60..38b1907612 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -207,6 +207,9 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); * this number defines the (linear) conversion from voltage * to Pascal (pa). For the MPXV7002DP this is 1000. * + * NOTE: If the sensor always registers zero, try switching + * the static and dynamic tubes. + * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); From 45617e9f4385e80846c571b237e220216192d6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 16:09:14 +0200 Subject: [PATCH 13/48] Airspeed calibration improvement, enforce correct pitot order --- .../commander/airspeed_calibration.cpp | 137 ++++++++++++++---- src/modules/ekf_att_pos_estimator/InertialNav | 1 + 2 files changed, 112 insertions(+), 26 deletions(-) create mode 160000 src/modules/ekf_att_pos_estimator/InertialNav diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 7599fc777c..a1a20abc98 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -64,14 +64,12 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2000; + const unsigned calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; - int calibration_counter = 0; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ @@ -105,29 +103,58 @@ int do_airspeed_calibration(int mavlink_fd) } } - while (calibration_counter < calibration_count) { + for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + unsigned calibration_counter = 0; - int poll_ret = poll(fds, 1, 1000); + if (i == 0) { + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); + } - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + float diff_pres_offset = 0.0f; - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + if (i > 0) { + + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + return ERROR; + } + } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; } - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); - close(diff_pres_sub); - return ERROR; } } @@ -151,14 +178,72 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(true); - close(diff_pres_sub); - return OK; - } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } + + /* wait 500 ms to ensure parameter propagated through the system */ + usleep(500 * 1000); + + calibration_counter = 0; + diff_pres_offset = 0.0f; + + /* just take a few samples and make sure pitot tubes are not reversed */ + while (calibration_counter < 50) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + float curr_avg = (diff_pres_offset / calibration_counter); + + if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if (curr_avg < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + + /* 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_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } + + /* save */ + (void)param_save_default(); + + return ERROR; + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + } + + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(true); + close(diff_pres_sub); + return OK; } diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav new file mode 160000 index 0000000000..8b65d755b8 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/InertialNav @@ -0,0 +1 @@ +Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 From 0d1ac4235411e8f05f96bcbe51558d92f0d86cf6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:10:08 +0200 Subject: [PATCH 14/48] airspeed calibration: Update and resolve compile errors --- .../commander/airspeed_calibration.cpp | 75 ++++++++----------- 1 file changed, 33 insertions(+), 42 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1a20abc98..421e53ae02 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -51,6 +51,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -103,58 +104,48 @@ int do_airspeed_calibration(int mavlink_fd) } } - for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { + unsigned calibration_counter = 0; - unsigned calibration_counter = 0; + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); - if (i == 0) { - mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); - usleep(500 * 1000); - } + while (calibration_counter < calibration_count) { - float diff_pres_offset = 0.0f; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; - while (calibration_counter < calibration_count) { + int poll_ret = poll(fds, 1, 1000); - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - int poll_ret = poll(fds, 1, 1000); + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - - if (i > 0) { - - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } - - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - } - - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); - } - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); return ERROR; } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; } } From 7bf0e6f3e26ee9e10073293677563f6862758557 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:47:26 +0200 Subject: [PATCH 15/48] Better airspeed feedback --- src/modules/commander/airspeed_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 421e53ae02..a1b8816ff0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,13 +200,13 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < calc_indicated_airspeed(-5.0f)) { + if (curr_avg < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); From 67d23253c3b3ec04bfd9d5f7e09d3d405ceba7c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:00:50 +0200 Subject: [PATCH 16/48] airspeed cal build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1b8816ff0..529a5b32f5 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,7 +200,7 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } From 4824a4e8ac93d992e1401f768291cca65ba46acc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:18:59 +0200 Subject: [PATCH 17/48] Fix for dynamic / static part of calibration --- .../commander/airspeed_calibration.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 529a5b32f5..01db34be25 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -121,19 +121,6 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } - - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; @@ -197,16 +184,16 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - float curr_avg = (diff_pres_offset / calibration_counter); + float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + if (fabsf(curr_avg) < 10.0f) { mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < -calc_indicated_airspeed(5.0f)) { + if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); From 34abf5c40cda538f204d313cc49715b5a938d168 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:50:56 +0200 Subject: [PATCH 18/48] airspeed cal: Fix up logic --- .../commander/airspeed_calibration.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 01db34be25..47c9c7af55 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -165,11 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - calibration_counter = 0; - diff_pres_offset = 0.0f; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 50) { + while (calibration_counter < 10) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,19 +178,15 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - - if (fabsf(curr_avg) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); - usleep(5000 * 1000); + if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + usleep(3000 * 1000); continue; } - /* do not log negative values in the second go */ - if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { + /* do not allow negative values */ + if (diff_pres.differential_pressure_raw_pa < 0.0f) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); @@ -209,7 +202,11 @@ int do_airspeed_calibration(int mavlink_fd) /* save */ (void)param_save_default(); + close(diff_pres_sub); return ERROR; + } else { + close(diff_pres_sub); + return OK; } } else if (poll_ret == 0) { From 65409ad2c8ba10d95cbcfa088951abc40eb46774 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 23:30:00 +0200 Subject: [PATCH 19/48] Airspeed calibration improvements --- .../commander/airspeed_calibration.cpp | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 47c9c7af55..d52cafb8f3 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -125,7 +125,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -165,6 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + calibration_counter = 0; + /* just take a few samples and make sure pitot tubes are not reversed */ while (calibration_counter < 10) { @@ -178,16 +180,18 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + + if (fabsf(calibrated_pa) < 9.0f) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", + (double)calibrated_pa); usleep(3000 * 1000); continue; } /* do not allow negative values */ - if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + if (calibrated_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -205,8 +209,9 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - close(diff_pres_sub); - return OK; + mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + break; } } else if (poll_ret == 0) { @@ -217,6 +222,8 @@ int do_airspeed_calibration(int mavlink_fd) } } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); close(diff_pres_sub); From 1b6ad3e1990bec84489f949b195adba4412e1c7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:37:10 +0200 Subject: [PATCH 20/48] ekf: Logic cleanup and print code cleanup --- .../ekf_att_pos_estimator_main.cpp | 137 +++++++----------- 1 file changed, 49 insertions(+), 88 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 bc5f709e58..5ce6bdb0ac 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 @@ -559,61 +559,26 @@ FixedwingEstimator::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* ekfname = "att pos estimator: "; + const char* const feedback[] = { 0, + "NaN in states, resetting", + "stale IMU data, resetting", + "got initial position lock", + "excessive gyro offsets", + "GPS velocity divergence", + "excessive covariances", + "unknown condition"}; - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 4: - { - const char* str = "excessive gyro offsets"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 5: - { - const char* str = "GPS velocity divergence"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 6: - { - const char* str = "excessive covariances"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; + // Print out error condition + if (check) { + unsigned warn_index = static_cast(check); + unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0])); + + if (max_warn_index < warn_index) { + warn_index = max_warn_index; } - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); } struct estimator_status_report rep; @@ -1140,20 +1105,6 @@ FixedwingEstimator::task_main() float initVelNED[3]; - // Guard before running any parts of the filter - // of NaN / invalid values - if (_ekf->statesInitialised) { - - // We're apparently initialized in this case now - - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } - } - /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1216,8 +1167,17 @@ FixedwingEstimator::task_main() _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + } else if (_ekf->statesInitialised) { + // We're apparently initialized in this case now + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } + // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 @@ -1285,7 +1245,7 @@ FixedwingEstimator::task_main() // run the fusion step _ekf->FuseVelposNED(); - } else if (!_gps_initialized && _ekf->statesInitialised) { + } else if (!_gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; @@ -1307,7 +1267,7 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newHgtData && _ekf->statesInitialised) { + if (newHgtData) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; @@ -1321,7 +1281,7 @@ FixedwingEstimator::task_main() } // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { + if (newDataMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data @@ -1335,7 +1295,7 @@ FixedwingEstimator::task_main() } // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + if (newAdsData && _ekf->VtasMeas > 7.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); @@ -1403,7 +1363,7 @@ FixedwingEstimator::task_main() _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, @@ -1494,27 +1454,28 @@ FixedwingEstimator::task_main() } + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } + } } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } } } From be5ac5e4127d09187ef0e19a7955cca9b0f1b378 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:38:52 +0200 Subject: [PATCH 21/48] Fix mavlink log header docs. --- src/include/mavlink/mavlink_log.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 0ea655cac3..6d56c546a7 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-2014 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 From 716e20fab6d63f3dc6f80c70f382d6b0f45f9340 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:39:13 +0200 Subject: [PATCH 22/48] Fix accel cal docs. --- src/modules/commander/accelerometer_calibration.cpp | 3 +-- src/modules/commander/accelerometer_calibration.h | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index be465ab766..7a4e7a766d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 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 diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 1cf9c09775..6b7e16d449 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 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 From 259014b0d50426b0fbed3b9eac5a1d34aaa5b211 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:40:09 +0200 Subject: [PATCH 23/48] Documentation only and unused defines only cleanup. --- src/modules/commander/airspeed_calibration.h | 2 +- src/modules/commander/baro_calibration.cpp | 2 +- src/modules/commander/baro_calibration.h | 2 +- src/modules/commander/calibration_messages.h | 3 +-- src/modules/commander/calibration_routines.cpp | 3 +-- src/modules/commander/calibration_routines.h | 3 +-- src/modules/commander/mag_calibration.h | 2 +- src/modules/commander/state_machine_helper.h | 10 ++++------ 8 files changed, 11 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 71c701fc2e..8c6b65ff1e 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 3123c40876..da98644b40 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 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 diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index bc42bc6cfc..ce78ae7005 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 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 diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index fd8b8564df..b1e209efc0 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013 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 diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 43f341ae7a..5796204bfd 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index e3e7fbafdd..3c8e49ee9e 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index a101cd7b1f..c14f948cc1 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index ddc5bf154c..bb1b87e712 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 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 @@ -36,14 +34,14 @@ /** * @file state_machine_helper.h * State machine helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - #include #include #include From ea11bc6c4bf7180ae4e321eebcaf817eebb18e6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:41:39 +0200 Subject: [PATCH 24/48] Command handling: Fix up local variable usage and status prints. --- src/modules/commander/commander.cpp | 67 ++++++++++++++++------------- 1 file changed, 38 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e14dff197a..009e0ac010 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char return arming_res; } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) +bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); @@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } } } @@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints + // for logic state parameters + + if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { + mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); } else { + bool cmd_arms = (static_cast(cmd->param1 + 0.5f) == 1); + // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { + status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { @@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_OVERRIDE_GOTO: { // TODO listen vehicle_command topic directly from navigator (?) - unsigned int mav_goto = cmd->param1; + + // Increase by 0.5f and rely on the integer cast + // implicit floor(). This is the *safest* way to + // convert from floats representing small ints to actual ints. + unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", (double)cmd->param1, (double)cmd->param2, (double)cmd->param3, @@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //XXX: to enable the parachute, a param needs to be set //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { + if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (use_current) { /* use current position */ - if (status->condition_global_position_valid) { + if (status_local->condition_global_position_valid) { home->lat = global_pos->lat; home->lon = global_pos->lon; home->alt = global_pos->alt; @@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } /* mark home position as set */ - status->condition_home_position_valid = true; + status_local->condition_home_position_valid = true; } } break; From 3ec9ffa66166b10101887cc4077d1b02d4a0897b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:42:02 +0200 Subject: [PATCH 25/48] Buzzer and led: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 009e0ac010..85c9ccbaa7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -697,11 +697,11 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); + warnx("ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); + warnx("ERROR: BUZZER INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); From 76f7960d77046c33a771be49b57e32c957e7a2ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:43:56 +0200 Subject: [PATCH 26/48] Mavlink text feedback: Remove now unneeded audio tag for critical messages, make overall printing more efficient. Remove buffers where no buffers are needed. --- src/modules/commander/commander.cpp | 55 +++++++++++++---------------- 1 file changed, 25 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 85c9ccbaa7..76cfd5feb9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,8 +775,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); } else { - warnx("reading mission state failed"); - mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + const char *missionfail = "reading mission state failed"; + warnx("%s", missionfail); + mavlink_log_critical(mavlink_fd, missionfail); /* initialize mission state in dataman */ mission.dataman_id = 0; @@ -789,8 +790,6 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } - mavlink_log_info(mavlink_fd, "[cmd] started"); - int ret; pthread_attr_t commander_low_prio_attr; @@ -1083,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -1187,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: LANDED"); + mavlink_log_critical(mavlink_fd, "LANDED MODE"); } else { - mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); + mavlink_log_critical(mavlink_fd, "IN AIR MODE"); } } } @@ -1270,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[]) /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { @@ -1339,12 +1338,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC signal regained"); status_changed = true; } } @@ -1385,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[]) * the system can be armed in auto if armed via the GCS. */ if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1405,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + mavlink_log_info(mavlink_fd, "DISARMED by RC"); } arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + mavlink_log_critical(mavlink_fd, "arming state transition denied"); tune_negative(true); } @@ -1428,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + mavlink_log_critical(mavlink_fd, "main state transition denied"); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1445,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } have_link = true; } else { if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1467,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status_changed = true; } @@ -2010,15 +2009,13 @@ set_control_mode() } void -print_reject_mode(struct vehicle_status_s *status, const char *msg) +print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: REJECT %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -2033,9 +2030,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; - char s[80]; - sprintf(s, "#audio: %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, msg); tune_negative(true); } } @@ -2048,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command); tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command); tune_negative(true); break; @@ -2064,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command); tune_negative(true); break; From f2b30be92ae946e8e40faade6cb10edfb037fdaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:44:27 +0200 Subject: [PATCH 27/48] commander status leds: Fix -Wshadow warning. --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 76cfd5feb9..e5ea237fc0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1650,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { bool set_normal_color = false; /* set mode */ - if (status->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -1676,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { + if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { - if (status->condition_local_position_valid) { + if (status_local->condition_local_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -1714,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status->load > 0.95f) { + if (status_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } From 66d5bc20c095a4dbd937b21c4f5fc1a67205f2d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:02 +0200 Subject: [PATCH 28/48] commander RC handling: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 42 ++++++++++++++--------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5ea237fc0..0c4d48dead 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1727,16 +1727,16 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) +set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode(status, "OFFBOARD"); + print_reject_mode(status_local, "OFFBOARD"); } else { return res; @@ -1751,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin case SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_ACRO); + res = main_state_transition(status_local, MAIN_STATE_ACRO); } else { - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "POSCTL"); + print_reject_mode(status_local, "POSCTL"); } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } if (sp_man->posctl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTL"); + print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case SWITCH_POS_ON: // AUTO if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_RTL); + res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1830,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } // fallback to POSCTL - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; From afc8908d38ff2eaabc2ca35363ac04697152cb6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:32 +0200 Subject: [PATCH 29/48] commander: More docs-only changes in headers. --- src/modules/commander/commander_helper.cpp | 10 ++++++---- src/modules/commander/commander_helper.h | 9 ++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 80e6861f61..d5fe122cb6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * Copyright (c) 2013, 2014 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 @@ -37,6 +34,11 @@ /** * @file commander_helper.cpp * Commander helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * */ #include diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e75f2592f5..a49c9e263d 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 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 @@ -36,6 +34,9 @@ /** * @file commander_helper.h * Commander helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef COMMANDER_HELPER_H_ @@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), * else use simple estimate based on voltage. * + * @param voltage the current battery voltage + * @param discharged the discharged capacity * @return the estimated remaining capacity in 0..1 */ float battery_remaining_estimate_voltage(float voltage, float discharged); From 7525c290f2cdfd058d54e5e6eb2d2e522d070889 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:46:36 +0200 Subject: [PATCH 30/48] commander: more text feedback improvements. --- .../commander/state_machine_helper.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 363f5e8eb4..87a60e7fda 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -87,7 +87,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char *state_names[ARMING_STATE_MAX] = { +static const char * const state_names[ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -160,7 +160,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -625,7 +625,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); failed = true; goto system_eval; } @@ -633,7 +633,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); failed = true; goto system_eval; } @@ -647,14 +647,14 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) 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_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); - mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); + mavlink_log_critical(mavlink_fd, "hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); /* this is frickin' fatal */ failed = true; goto system_eval; @@ -667,7 +667,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd <= 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; } @@ -678,11 +678,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(diff_pres)) { if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "ARM WARNING: AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED READ"); /* this is frickin' fatal */ failed = true; goto system_eval; From 0d51230d4e3b87dd67819e594895b24a0a8a0306 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:04:43 +0200 Subject: [PATCH 31/48] commander: Final MAVLink text feedback cleanup --- src/modules/commander/state_machine_helper.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87a60e7fda..56c59ad3df 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -171,16 +171,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f)) { + if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f))) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } From a32577377b8d735c77ffaaee15e2bbfa74be703f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:38:43 +0200 Subject: [PATCH 32/48] EKF init improvements --- .../ekf_att_pos_estimator_main.cpp | 8 ++++++++ .../ekf_att_pos_estimator/estimator_23states.cpp | 16 +++++++++++----- 2 files changed, 19 insertions(+), 5 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 5ce6bdb0ac..ee15612801 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 @@ -610,6 +610,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); + // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1246,6 +1250,10 @@ FixedwingEstimator::task_main() _ekf->FuseVelposNED(); } else if (!_gps_initialized) { + + // force static mode + _ekf->staticMode = true; + // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index f33a1d7805..e83c37bbdc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2206,7 +2206,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } @@ -2806,12 +2806,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) current_ekf_state.statesNaN = false; current_ekf_state.velHealth = true; - //current_ekf_state.posHealth = ?; - //current_ekf_state.hgtHealth = ?; + current_ekf_state.posHealth = true; + current_ekf_state.hgtHealth = true; current_ekf_state.velTimeout = false; - //current_ekf_state.posTimeout = ?; - //current_ekf_state.hgtTimeout = ?; + current_ekf_state.posTimeout = false; + current_ekf_state.hgtTimeout = false; + + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; // Fill variables with valid data velNED[0] = initvelNED[0]; From 744eed91dca8a5f2ad8ead7ffa812d25755bcc93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:43:10 +0200 Subject: [PATCH 33/48] Fix calibration counter usage --- .../commander/airspeed_calibration.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index d52cafb8f3..a2f11b14a4 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -168,7 +168,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 10) { + while (calibration_counter < 300) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -182,16 +182,19 @@ int do_airspeed_calibration(int mavlink_fd) float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; - if (fabsf(calibrated_pa) < 9.0f) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", - (double)calibrated_pa); - usleep(3000 * 1000); + if (fabsf(calibrated_pa) < 50.0f) { + if (calibration_counter % 20 == 0) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + (int)calibrated_pa); + } + usleep(100 * 1000); + calibration_counter++; continue; } /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -209,8 +212,8 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + (int)calibrated_pa); break; } From 8590d555b432f206e2b88893ea5198c1398aa99c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:44:57 +0200 Subject: [PATCH 34/48] Fix calibration counter usage, take every sample --- src/modules/commander/airspeed_calibration.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a2f11b14a4..10d04aae93 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -167,8 +167,8 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 300) { + /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ + while (calibration_counter < 1500) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,14 +181,13 @@ int do_airspeed_calibration(int mavlink_fd) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + calibration_counter++; if (fabsf(calibrated_pa) < 50.0f) { - if (calibration_counter % 20 == 0) { + if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", (int)calibrated_pa); } - usleep(100 * 1000); - calibration_counter++; continue; } From d4a867071a2183069d8902292357133856cd2ffd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:44:15 +0200 Subject: [PATCH 35/48] airspeed: Better calibration messages --- src/modules/commander/airspeed_calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 10d04aae93..469f156327 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -193,7 +193,7 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -206,9 +206,12 @@ int do_airspeed_calibration(int mavlink_fd) } /* save */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); close(diff_pres_sub); + + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", From 36c20681621c2b99c1486aa0e3700cdbd86bd42d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:42:34 +0200 Subject: [PATCH 36/48] Removed spurious submodule entry --- src/modules/ekf_att_pos_estimator/InertialNav | 1 - 1 file changed, 1 deletion(-) delete mode 160000 src/modules/ekf_att_pos_estimator/InertialNav diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav deleted file mode 160000 index 8b65d755b8..0000000000 --- a/src/modules/ekf_att_pos_estimator/InertialNav +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 From 1dffa750d8b91af8f600ba4cf1fbcdcc61edf80f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:44:32 +0200 Subject: [PATCH 37/48] added detailed print --- src/modules/commander/airspeed_calibration.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 469f156327..bc12037c3b 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,6 +162,8 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); From 56ad0c708d2695515489802676cc295f040b3606 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:53:50 +0200 Subject: [PATCH 38/48] Fixed compile error --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index bc12037c3b..2e3f89e652 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,7 +162,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); From 7968c6864e1255b4a65427187119aec2c3fc7ae0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:04:02 +0200 Subject: [PATCH 39/48] Force update of offset, do not add offset in final value --- .../commander/airspeed_calibration.cpp | 21 +++++++++++++------ .../commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 2e3f89e652..ea81ac6a64 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -140,6 +140,16 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { + int fd_scale = open(AIRSPEED_DEVICE_PATH, 0); + airscale.offset_pa = diff_pres_offset; + if (fd_scale > 0) { + if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + } + + close(fd_scale); + } + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); @@ -182,20 +192,19 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; calibration_counter++; - if (fabsf(calibrated_pa) < 50.0f) { + if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ - if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); + if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -217,7 +226,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 09ea12c38a..372ba9d7dc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -668,8 +668,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) struct airspeed_s airspeed; - if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || - hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; From 180b83cc6dc6872c5248993b5409ac835a201114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:41:12 +0200 Subject: [PATCH 40/48] Better analog error handling --- src/modules/commander/airspeed_calibration.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index ea81ac6a64..92d50a0251 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -94,9 +94,17 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + /* only warn if analog scaling is zero */ + 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, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + close(diff_pres_sub); + return ERROR; + } + + /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); From ec8d395a2d270bd77e873f616a53ee0771c94165 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:07:30 +0200 Subject: [PATCH 41/48] build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 92d50a0251..423d886828 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -97,7 +97,7 @@ int do_airspeed_calibration(int mavlink_fd) /* only warn if analog scaling is zero */ float analog_scaling = 0.0f; - param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); close(diff_pres_sub); From 5f176d14fed9ace9f5265a40b7ace1e6d00a7690 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:33 +0200 Subject: [PATCH 42/48] ekf: Only return from start handler with all allocations done --- .../ekf_att_pos_estimator_main.cpp | 25 +++++++++++++++++-- 1 file changed, 23 insertions(+), 2 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 a835599e79..247814a7e3 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 @@ -124,6 +124,13 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + /** * Print the current status. */ @@ -151,7 +158,8 @@ public: private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _estimator_task; /**< task handle for sensor task */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _estimator_task; /**< task handle for sensor task */ #ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ @@ -313,12 +321,13 @@ namespace estimator #endif static const int ERROR = -1; -FixedwingEstimator *g_estimator; +FixedwingEstimator *g_estimator = nullptr; } FixedwingEstimator::FixedwingEstimator() : _task_should_exit(false), + _task_running(false), _estimator_task(-1), /* subscriptions */ @@ -772,6 +781,8 @@ FixedwingEstimator::task_main() _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1518,6 +1529,8 @@ FixedwingEstimator::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _estimator_task = -1; @@ -1670,6 +1683,14 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } From 8a0e83c9cd9a54f09c6bc5c2c89917d87a451192 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:11:55 +0200 Subject: [PATCH 43/48] fw att ctrl: Only return from start handler with all allocations done --- .../fw_att_control/fw_att_control_main.cpp | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3cd4ce9285..4cdba735a0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 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 @@ -99,13 +98,21 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _att_sub; /**< vehicle attitude subscription */ @@ -276,6 +283,7 @@ private: * Main sensor collection task. */ void task_main(); + }; namespace att_control @@ -287,12 +295,13 @@ namespace att_control #endif static const int ERROR = -1; -FixedwingAttitudeControl *g_control; +FixedwingAttitudeControl *g_control = nullptr; } FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -598,6 +607,8 @@ FixedwingAttitudeControl::task_main() fds[1].fd = _att_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { static int loop_counter = 0; @@ -921,6 +932,7 @@ FixedwingAttitudeControl::task_main() warnx("exiting.\n"); _control_task = -1; + _task_running = false; _exit(0); } @@ -966,6 +978,14 @@ int fw_att_control_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (att_control::g_control == nullptr || !att_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } From efa5d8f57f303a51f618b7c108f516d5e857c3b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:12:17 +0200 Subject: [PATCH 44/48] fw pos ctrl: Only return from start handler with all allocations done --- .../fw_pos_control_l1_main.cpp | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 98ccd09a5e..08c996ebc0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 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 @@ -120,10 +119,18 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _global_pos_sub; @@ -391,13 +398,14 @@ namespace l1_control #endif static const int ERROR = -1; -FixedwingPositionControl *g_control; +FixedwingPositionControl *g_control = nullptr; } FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ @@ -1290,6 +1298,8 @@ FixedwingPositionControl::task_main() fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1390,6 +1400,8 @@ FixedwingPositionControl::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _control_task = -1; @@ -1478,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } From 9ce7820e419d2ffa379fb7a3cc168f500623fa3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:32:51 +0200 Subject: [PATCH 45/48] Make instructions in commander more obvious for airspeed calibration --- src/modules/commander/airspeed_calibration.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 423d886828..598cfe9e2d 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,15 +180,16 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); calibration_counter = 0; + const int maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ - while (calibration_counter < 1500) { + while (calibration_counter < maxcount) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -204,7 +205,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -212,6 +213,8 @@ 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, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); @@ -246,6 +249,12 @@ int do_airspeed_calibration(int mavlink_fd) } } + if (calibration_counter == maxcount) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); From f3549d775cb049bcde93c3e860c3adbad3763364 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:33:35 +0200 Subject: [PATCH 46/48] Airspeed driver: Use the known sensor offset for raw value as well --- src/drivers/ets_airspeed/ets_airspeed.cpp | 3 +++ src/drivers/meas_airspeed/meas_airspeed.cpp | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d1a9fa57c8..c15a0cee43 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -172,6 +172,9 @@ ETSAirspeed::collect() diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; } + // The raw value still should be compensated for the known offset + diff_pres_pa_raw -= _diff_pres_offset; + // Track maximum differential pressure measured (so we can work out top speed). if (diff_pres_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_pres_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7763f10578..07611f9039 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -225,7 +225,10 @@ MEASAirspeed::collect() // correct for 5V rail voltage if possible voltage_correction(diff_press_pa_raw, temperature); - float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + // the raw value still should be compensated for the known offset + diff_press_pa_raw -= _diff_pres_offset; + + float diff_press_pa = fabsf(diff_press_pa_raw); /* note that we return both the absolute value with offset From 83652794c7709ee8058587a99e81826d4959ed49 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 15 Jul 2014 00:06:24 +0200 Subject: [PATCH 47/48] Add a mixer for the Phantom so it can be uniquely adjusted for this airframe. --- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/mixers/phantom.mix | 69 +++++++++++++++++++++++++ 2 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/phantom.mix diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index d05c3174f9..654b0bdab1 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -36,7 +36,7 @@ then param set FW_T_TIME_CONST 5 fi -set MIXER FMU_Q +set MIXER phantom # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 diff --git a/ROMFS/px4fmu_common/mixers/phantom.mix b/ROMFS/px4fmu_common/mixers/phantom.mix new file mode 100644 index 0000000000..7fe6dc0551 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/phantom.mix @@ -0,0 +1,69 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for the Phantom FX-61 + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4/Pixhawk. The configuration assumes the elevon servos are connected to +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor are set so that pitch will have more travel than roll. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 6500 6500 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 -6500 -6500 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +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 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 9f837267de861cfd8313984f2ec64b8482a72161 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 15 Jul 2014 00:10:08 +0200 Subject: [PATCH 48/48] Updated the comments. --- ROMFS/px4fmu_common/mixers/phantom.mix | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/phantom.mix b/ROMFS/px4fmu_common/mixers/phantom.mix index 7fe6dc0551..00c37a16c1 100644 --- a/ROMFS/px4fmu_common/mixers/phantom.mix +++ b/ROMFS/px4fmu_common/mixers/phantom.mix @@ -1,7 +1,5 @@ -Delta-wing mixer for PX4FMU -=========================== - -Designed for the Phantom FX-61 +Phantom FX-61 mixer +=================== This file defines mixers suitable for controlling a delta wing aircraft using PX4/Pixhawk. The configuration assumes the elevon servos are connected to