From 2c2c65d446f991b273ee1f275fd2bc8440f74844 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Feb 2013 22:26:04 +0100 Subject: corrected some wrong units (used in airspeed calculation) --- apps/systemlib/conversions.c | 2 +- apps/systemlib/conversions.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) (limited to 'apps/systemlib') diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 2b8003e45..ac94252c5 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9]) float get_air_density(float static_pressure, float temperature_celsius) { - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN)); + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); } diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index c2987709b..5d485b01f 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,10 +44,10 @@ #include #include -#define CONSTANTS_ONE_G 9.80665f -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f -#define CONSTANTS_AIR_GAS_CONST 8.31432f -#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f +#define CONSTANTS_ONE_G 9.80665f // m/s^2 +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 +#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C __BEGIN_DECLS -- cgit v1.2.3 From 2707d2c1dde65dfb9ba48258994badb4b57f9627 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Feb 2013 16:01:08 +0100 Subject: more fixes for the airspeed readout --- apps/sensors/sensors.cpp | 25 ++++++------ apps/systemlib/airspeed.c | 40 ++++++++++++------ apps/systemlib/airspeed.h | 69 ++++++++++++++++---------------- apps/uORB/topics/differential_pressure.h | 4 +- 4 files changed, 77 insertions(+), 61 deletions(-) (limited to 'apps/systemlib') diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9..c29910dcc 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -993,7 +993,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* look for battery channel */ for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - + if (ret >= (int)sizeof(buf_adc[0])) { if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1025,8 +1025,7 @@ 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 = buf_adc[i].am_data / 4096.0f; + 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 @@ -1034,21 +1033,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f) { - float pres_raw = fabsf(voltage - (3.3f / 2.0f)); - float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; +// float pres_raw = fabsf(voltage - (3.3f / 2.0f)); +// float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; + //XXX depends on sensor used..., where are the above numbers from? + float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration - float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); + float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, + _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa // XXX HACK - true temperature is much less than indicated temperature in baro, // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); - // XXX HACK + float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); + + //printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true); _differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure.static_pressure_mbar = _barometer.pressure; - _differential_pressure.differential_pressure_mbar = pres_mbar; + _differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f; _differential_pressure.temperature_celcius = _barometer.temperature; _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; _differential_pressure.true_airspeed_m_s = airspeed_true; @@ -1064,7 +1065,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } _last_adc = hrt_absolute_time(); - break; + //break; } } } diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 5c68f8ea5..32943b2f5 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -40,14 +40,25 @@ * */ -#include "math.h" +#include +#include #include "conversions.h" #include "airspeed.h" -float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature) +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param differential_pressure total_ pressure - static pressure + * @return indicated airspeed in m/s + */ +float calc_indicated_airspeed(float differential_pressure) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } /** @@ -55,14 +66,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param speed current indicated airspeed + * @param speed_indicated current indicated airspeed * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature) +float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) { - return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); } /** @@ -70,12 +81,17 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param total_pressure pressure inside the pitot/prandl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature) +float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature)); + float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || isnan(density)) { + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + printf("Invalid air density, using density at sea level\n"); + } + return sqrtf((2.0f*(total_pressure - static_pressure)) / density); } diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index b1beb79ae..088498943 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -48,43 +48,42 @@ __BEGIN_DECLS -/** - * Calculate indicated airspeed. - * - * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return indicated airspeed in m/s - */ -__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ + __EXPORT float calc_indicated_airspeed(float differential_pressure); -/** - * Calculate true airspeed from indicated airspeed. - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param speed current indicated airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature); + /** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); -/** - * Directly calculate true airspeed - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); __END_DECLS diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index fd7670cbc..78a37fdf4 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -49,7 +49,7 @@ */ /** - * Battery voltages and status + * Differential pressure and airspeed */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ @@ -67,4 +67,4 @@ struct differential_pressure_s { /* register this as object request broker structure */ ORB_DECLARE(differential_pressure); -#endif \ No newline at end of file +#endif -- cgit v1.2.3 From c0a852dab48e55aa12c995adc6dc0c32aa9a7ac3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Feb 2013 21:57:38 +0100 Subject: airspeed (pitot) offset calibration --- apps/commander/commander.c | 96 ++++++++++++++++++++++++++++++++ apps/sensors/sensor_params.c | 2 + apps/sensors/sensors.cpp | 13 ++++- apps/systemlib/airspeed.c | 20 ++++++- apps/uORB/topics/differential_pressure.h | 5 +- apps/uORB/topics/vehicle_status.h | 1 + 6 files changed, 130 insertions(+), 7 deletions(-) (limited to 'apps/systemlib') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 42ca10f55..41f4e5674 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -785,6 +786,79 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } +void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it still"); + /* set to accel calibration mode */ + status->flag_preflight_airspeed_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + + int calibration_counter = 0; + float airspeed_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); + airspeed_offset += differential_pressure.voltage; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + airspeed_offset = airspeed_offset / calibration_count; + + if (isfinite(airspeed_offset)) { + + if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + /* exit airspeed calibration mode */ + status->flag_preflight_airspeed_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_differential_pressure); +} + void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) @@ -980,6 +1054,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta handled = true; } + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + /* none found */ if (!handled) { //warnx("refusing unsupported calibration request\n"); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 9f23ebbba..a1ef9d136 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -64,6 +64,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index c29910dcc..f77bc9b8b 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -187,6 +187,7 @@ private: float mag_scale[3]; float accel_offset[3]; float accel_scale[3]; + float airspeed_offset; int rc_type; @@ -235,6 +236,7 @@ private: param_t accel_scale[3]; param_t mag_offset[3]; param_t mag_scale[3]; + param_t airspeed_offset; param_t rc_map_roll; param_t rc_map_pitch; @@ -480,6 +482,9 @@ Sensors::Sensors() : _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + /*Airspeed offset */ + _parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF"); + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); /* fetch initial parameter values */ @@ -687,6 +692,9 @@ Sensors::parameters_update() param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1])); param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); + /* Airspeed offset */ + param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset)); + /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("Failed updating voltage scaling param"); @@ -1036,7 +1044,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) // float pres_raw = fabsf(voltage - (3.3f / 2.0f)); // float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; //XXX depends on sensor used..., where are the above numbers from? - float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration + float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa @@ -1045,7 +1053,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); - //printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true); + //printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true); _differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure.static_pressure_mbar = _barometer.pressure; @@ -1053,6 +1061,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _differential_pressure.temperature_celcius = _barometer.temperature; _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; _differential_pressure.true_airspeed_m_s = airspeed_true; + _differential_pressure.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 32943b2f5..382df2ee4 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -58,7 +58,13 @@ */ float calc_indicated_airspeed(float differential_pressure) { - return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + + if (differential_pressure > 0) { + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { + return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } + } /** @@ -89,9 +95,17 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); - if (density < 0.0001f || isnan(density)) { + if (density < 0.0001f || !isfinite(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; printf("Invalid air density, using density at sea level\n"); } - return sqrtf((2.0f*(total_pressure - static_pressure)) / density); + + float pressure_difference = total_pressure - static_pressure; + + if(pressure_difference > 0) { + return sqrtf((2.0f*(pressure_difference)) / density); + } else + { + return -sqrtf((2.0f*fabs(pressure_difference)) / density); + } } diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index 78a37fdf4..d5e4bf37e 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -52,12 +52,13 @@ * Differential pressure and airspeed */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float static_pressure_mbar; /**< Static / environment pressure */ float differential_pressure_mbar; /**< Differential pressure reading */ float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */ }; /** diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5..ae18ba050 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -173,6 +173,7 @@ struct vehicle_status_s bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; + bool flag_preflight_airspeed_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ -- cgit v1.2.3 From 3f674ba78cc8af2be8b8062f9366629b6c2a34be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Feb 2013 14:34:53 +0100 Subject: fixed a typo --- apps/systemlib/airspeed.c | 2 +- apps/systemlib/airspeed.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'apps/systemlib') diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 382df2ee4..d7096d597 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -87,7 +87,7 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param total_pressure pressure inside the pitot/prandl tube + * @param total_pressure pressure inside the pitot/prandtl tube * @param static_pressure pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index 088498943..def53f0c1 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -78,7 +78,7 @@ * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param total_pressure pressure inside the pitot/prandl tube + * @param total_pressure pressure inside the pitot/prandtl tube * @param static_pressure pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s -- cgit v1.2.3 From 8e3b09bd50fed9280328ecdaf2dbc7bd789bbbf9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Feb 2013 13:38:48 +0100 Subject: small printf change --- apps/systemlib/airspeed.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps/systemlib') diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index d7096d597..264287b10 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -97,7 +97,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp float density = get_air_density(static_pressure, temperature_celsius); if (density < 0.0001f || !isfinite(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; - printf("Invalid air density, using density at sea level\n"); + printf("[airspeed] Invalid air density, using density at sea level\n"); } float pressure_difference = total_pressure - static_pressure; -- cgit v1.2.3 From ebac51cad8e144b64938e6726e26bdc23aaf45e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 19:47:43 +0100 Subject: Working on restart resilience, hunting down multi-load mixer issue (still present) --- apps/drivers/px4io/px4io.cpp | 38 +++++++++++++++++++++++++----------- apps/px4io/mixer.cpp | 16 ++++++++++++--- apps/px4io/registers.c | 7 +++++++ apps/systemlib/mixer/mixer.cpp | 1 + apps/systemlib/mixer/mixer_group.cpp | 1 + 5 files changed, 49 insertions(+), 14 deletions(-) (limited to 'apps/systemlib') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 8fb53295f..882ca9fed 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1556,7 +1556,7 @@ px4io_main(int argc, char *argv[]) errx(1, "already loaded"); /* create the driver - it will set g_dev */ - (void)new PX4IO; + (void)new PX4IO(); if (g_dev == nullptr) errx(1, "driver alloc failed"); @@ -1567,7 +1567,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if (argc > 2 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { if (argc > 2 + 1) { #warning implement this } else { @@ -1579,16 +1579,31 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); } + exit(0); + } if (!strcmp(argv[1], "status")) { @@ -1613,8 +1628,9 @@ px4io_main(int argc, char *argv[]) exit(1); } uint8_t level = atoi(argv[2]); - // we can cheat and call the driver directly, as it - // doesn't reference filp in ioctl() + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 0fba2cbe5..54584e685 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -108,9 +108,11 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - /* don't actually mix anything - we already have raw PWM values */ + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ source = MIX_NONE; } else { @@ -239,6 +241,11 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { + /* do not allow a mixer change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + return; + } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -252,9 +259,12 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); + + /* FIRST mark the mixer as invalid */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index d97fd8d86..511a47f8d 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_PAGE_RC_CONFIG: { + + /* do not allow a RC config change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + break; + } + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90..df0dfe838 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff225..1dbc512cd 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } } -- cgit v1.2.3 From 802d0ae2faa101b2a9eaef75f4019160faf250fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 21:07:29 +0100 Subject: Made dtors virtual, tested on IO and FMU --- apps/drivers/blinkm/blinkm.cpp | 2 +- apps/drivers/bma180/bma180.cpp | 2 +- apps/drivers/gps/gps.cpp | 2 +- apps/drivers/hil/hil.cpp | 2 +- apps/drivers/hmc5883/hmc5883.cpp | 2 +- apps/drivers/l3gd20/l3gd20.cpp | 2 +- apps/drivers/led/led.cpp | 2 +- apps/drivers/mb12xx/mb12xx.cpp | 2 +- apps/drivers/mpu6000/mpu6000.cpp | 2 +- apps/drivers/ms5611/ms5611.cpp | 2 +- apps/drivers/px4fmu/fmu.cpp | 2 +- apps/drivers/px4io/px4io.cpp | 2 +- apps/drivers/px4io/uploader.h | 2 +- apps/mavlink_onboard/mavlink.c | 2 +- apps/systemlib/mixer/mixer.h | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) (limited to 'apps/systemlib') diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index fc929284c..54c7d4266 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -127,7 +127,7 @@ class BlinkM : public device::I2C { public: BlinkM(int bus, int blinkm); - ~BlinkM(); + virtual ~BlinkM(); virtual int init(); diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index 32eb5333e..4409a8a9c 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -126,7 +126,7 @@ class BMA180 : public device::SPI { public: BMA180(int bus, spi_dev_e device); - ~BMA180(); + virtual ~BMA180(); virtual int init(); diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 135010653..e35bdb944 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -86,7 +86,7 @@ class GPS : public device::CDev { public: GPS(const char* uart_path); - ~GPS(); + virtual ~GPS(); virtual int init(); diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index fe9b281f6..861ed7924 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -91,7 +91,7 @@ public: MODE_NONE }; HIL(); - ~HIL(); + virtual ~HIL(); virtual int ioctl(file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 4a201b98c..8ab568282 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -130,7 +130,7 @@ class HMC5883 : public device::I2C { public: HMC5883(int bus); - ~HMC5883(); + virtual ~HMC5883(); virtual int init(); diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index f2f585f42..6227df72a 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -152,7 +152,7 @@ class L3GD20 : public device::SPI { public: L3GD20(int bus, const char* path, spi_dev_e device); - ~L3GD20(); + virtual ~L3GD20(); virtual int init(); diff --git a/apps/drivers/led/led.cpp b/apps/drivers/led/led.cpp index 12d864be2..c7c45525e 100644 --- a/apps/drivers/led/led.cpp +++ b/apps/drivers/led/led.cpp @@ -53,7 +53,7 @@ class LED : device::CDev { public: LED(); - ~LED(); + virtual ~LED(); virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp index 406c726fb..9d0f6bddc 100644 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -100,7 +100,7 @@ class MB12XX : public device::I2C { public: MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); - ~MB12XX(); + virtual ~MB12XX(); virtual int init(); diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 27e200e40..ce7062046 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -151,7 +151,7 @@ class MPU6000 : public device::SPI { public: MPU6000(int bus, spi_dev_e device); - ~MPU6000(); + virtual ~MPU6000(); virtual int init(); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 44014d969..08420822a 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -104,7 +104,7 @@ class MS5611 : public device::I2C { public: MS5611(int bus); - ~MS5611(); + virtual ~MS5611(); virtual int init(); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index c29fe0ba3..8e13f7c62 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -82,7 +82,7 @@ public: MODE_NONE }; PX4FMU(); - ~PX4FMU(); + virtual ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f9ffa6bcd..90320d381 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -91,7 +91,7 @@ class PX4IO : public device::I2C { public: PX4IO(); - ~PX4IO(); + virtual ~PX4IO(); virtual int init(); diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h index 915ee9259..f983d1981 100644 --- a/apps/drivers/px4io/uploader.h +++ b/apps/drivers/px4io/uploader.h @@ -46,7 +46,7 @@ class PX4IO_Uploader { public: PX4IO_Uploader(); - ~PX4IO_Uploader(); + virtual ~PX4IO_Uploader(); int upload(const char *filenames[]); diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 33ebe8600..5a2685560 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -498,7 +498,7 @@ int mavlink_onboard_main(int argc, char *argv[]) mavlink_task = task_spawn("mavlink_onboard", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 6000 /* XXX probably excessive */, + 2048, mavlink_thread_main, (const char**)argv); exit(0); diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 00ddf1581..71386cba7 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -160,7 +160,7 @@ public: * @param control_cb Callback invoked when reading controls. */ Mixer(ControlCallback control_cb, uintptr_t cb_handle); - ~Mixer() {}; + virtual ~Mixer() {}; /** * Perform the mixing function. -- cgit v1.2.3