diff options
-rw-r--r-- | apps/drivers/bma180/bma180.cpp | 15 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 101 | ||||
-rw-r--r-- | apps/systemcmds/param/param.c | 78 | ||||
-rw-r--r-- | apps/systemlib/Makefile | 3 | ||||
-rw-r--r-- | apps/systemlib/airspeed.c | 8 | ||||
-rw-r--r-- | apps/systemlib/airspeed.h | 11 | ||||
-rw-r--r-- | apps/systemlib/conversions.c | 6 | ||||
-rw-r--r-- | apps/systemlib/conversions.h | 5 |
8 files changed, 178 insertions, 49 deletions
diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index bc4d4b3bf..32eb5333e 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -714,14 +714,17 @@ BMA180::measure() * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 1)) << 8) | (read_reg(ADDR_ACC_X_LSB)); // XXX PX4DEV raw_report.x; - report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 3)) << 8) | (read_reg(ADDR_ACC_X_LSB + 2)); // XXX PX4DEV raw_report.y; - report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 5)) << 8) | (read_reg(ADDR_ACC_X_LSB + 4)); // XXX PX4DEV raw_report.z; + report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report->x_raw = (report->x_raw >> 2); - report->y_raw = (report->y_raw >> 2); - report->z_raw = (report->z_raw >> 2); + report->x_raw = (report->x_raw / 4); + report->y_raw = (report->y_raw / 4); + report->z_raw = (report->z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ report->y_raw = -report->y_raw; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 2697cf3d9..d8d200ea9 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -67,6 +67,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> +#include <systemlib/airspeed.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -75,6 +76,7 @@ #include <uORB/topics/vehicle_status.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/differential_pressure.h> #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ @@ -88,9 +90,10 @@ #define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 -#define BAT_VOL_INITIAL 12.f +#define BAT_VOL_INITIAL 0.f #define BAT_VOL_LOWPASS_1 0.99f #define BAT_VOL_LOWPASS_2 0.01f #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f @@ -161,11 +164,14 @@ private: orb_advert_t _manual_control_pub; /**< manual control signal topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ + orb_advert_t _airspeed_pub; /**< airspeed */ perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ struct battery_status_s _battery_status; /**< battery status */ + struct baro_report _barometer; /**< barometer data */ + struct differential_pressure_s _differential_pressure; struct { float min[_rc_max_chan_count]; @@ -389,6 +395,7 @@ Sensors::Sensors() : _manual_control_pub(-1), _rc_pub(-1), _battery_pub(-1), + _airspeed_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) @@ -861,13 +868,12 @@ Sensors::baro_poll(struct sensor_combined_s &raw) orb_check(_baro_sub, &baro_updated); if (baro_updated) { - struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); - raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius raw.baro_counter++; } @@ -988,29 +994,72 @@ Sensors::adc_poll(struct sensor_combined_s &raw) for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - if (ret >= sizeof(buf_adc[0]) && ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { - /* Voltage in volts */ - float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); + if (ret >= (int)sizeof(buf_adc[0])) { - if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { + if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + /* Voltage in volts */ + float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); - /* one-time initialization of low-pass value to avoid long init delays */ - if (_battery_status.voltage_v < 3.0f) { - _battery_status.voltage_v = voltage; - } + if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { + + /* one-time initialization of low-pass value to avoid long init delays */ + if (_battery_status.voltage_v < 3.0f) { + _battery_status.voltage_v = voltage; + } + + _battery_status.timestamp = hrt_absolute_time(); + _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; + /* current and discharge are unknown */ + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + + /* announce the battery voltage if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } + + } 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; + + /** + * The voltage divider pulls the signal down, only act on + * a valid voltage from a connected sensor + */ + 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 airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure, + _barometer.pressure, _barometer.temperature - 5.0f); + // 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 - _battery_status.timestamp = hrt_absolute_time(); - _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; + _differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure.static_pressure_mbar = _barometer.pressure; + _differential_pressure.differential_pressure_mbar = pres_mbar; + _differential_pressure.temperature_celcius = _barometer.temperature; + _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; + _differential_pressure.true_airspeed_m_s = airspeed_true; - /* announce the battery voltage if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + /* announce the airspeed if needed, just publish else */ + if (_airspeed_pub > 0) { + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure); - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } else { + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure); + } } } @@ -1114,7 +1163,7 @@ Sensors::ppm_poll() } /* reverse channel if required */ - if (i == _rc.function[THROTTLE]) { + if (i == (int)_rc.function[THROTTLE]) { if ((int)_parameters.rev[i] == -1) { _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; } diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index 210841604..56f5317e3 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -61,6 +61,7 @@ static void do_load(const char* param_file_name); static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); +static void do_set(const char* name, const char* val); int param_main(int argc, char *argv[]) @@ -100,15 +101,24 @@ param_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "show")) + if (!strcmp(argv[1], "show")) { if (argc >= 3) { do_show(argv[2]); } else { do_show(NULL); } + } + + if (!strcmp(argv[1], "set")) { + if (argc >= 4) { + do_set(argv[2], argv[3]); + } else { + errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); + } + } } - errx(1, "expected a command, try 'load', 'import', 'show', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); } static void @@ -185,9 +195,11 @@ do_show_print(void *arg, param_t param) float f; const char *search_string = (const char*)arg; - /* print nothing if search string valid and not matching */ - if (arg != NULL && (strcmp(search_string, param_name(param) != 0))) + /* print nothing if search string is invalid and not matching */ + if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { + /* param not found */ return; + } printf("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), @@ -225,3 +237,61 @@ do_show_print(void *arg, param_t param) printf("<error fetching parameter %d>\n", param); } + +static void +do_set(const char* name, const char* val) +{ + int32_t i; + float f; + param_t param = param_find(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found */ + errx(1, "Error: Parameter %s not found.", name); + } + + printf("%c %s: ", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + + /* + * Set parameter if type is known and conversion from string to value turns out fine + */ + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + printf("old: %d", i); + + /* convert string */ + char* end; + i = strtol(val,&end,10); + param_set(param, &i); + printf(" -> new: %d\n", i); + + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + printf("float values are not yet supported."); + // printf("old: %4.4f", (double)f); + + // /* convert string */ + // char* end; + // f = strtof(val,&end); + // param_set(param, &f); + // printf(" -> new: %4.4f\n", f); + + } + + break; + + default: + errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param)); + } + + exit(0); +} diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index 942116faa..8240dbe43 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -43,7 +43,8 @@ CSRCS = err.c \ conversions.c \ cpuload.c \ getopt_long.c \ - up_cxxinitialize.c + up_cxxinitialize.c \ + airspeed.c # ppm_decode.c \ diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index e213b66c2..5c68f8ea5 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -41,11 +41,13 @@ */ #include "math.h" +#include "conversions.h" +#include "airspeed.h" float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / air_density_sea_level); + return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } /** @@ -60,7 +62,7 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa */ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature) { - return speed * sqrtf(air_density_sea_level / get_air_density(pressure_ambient, temperature)); + return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature)); } /** @@ -76,4 +78,4 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature) { return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature)); -}
\ No newline at end of file +} diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index 62acfe2b0..b1beb79ae 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -33,13 +33,16 @@ ****************************************************************************/ /** - * @file airspeed.c - * Airspeed estimation + * @file airspeed.h + * Airspeed estimation declarations * * @author Lorenz Meier <lm@inf.ethz.ch> * */ +#ifndef AIRSPEED_H_ +#define AIRSPEED_H_ + #include "math.h" #include "conversions.h" @@ -83,4 +86,6 @@ __EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_amb */ __EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature); -__END_DECLS
\ No newline at end of file +__END_DECLS + +#endif diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index fed97f101..2b8003e45 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -42,10 +42,6 @@ #include "conversions.h" -#define air_gas_constant 8.31432f -#define air_density_sea_level 1.225f -#define absolute_null_kelvin 273.15f - int16_t int16_t_from_bytes(uint8_t bytes[]) { @@ -154,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 / (air_gas_constant * (temperature_celsius + absolute_null_kelvin)); + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN)); } diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index 4db6de772..c2987709b 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,7 +44,10 @@ #include <float.h> #include <stdint.h> -#define CONSTANTS_ONE_G 9.80665f +#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 __BEGIN_DECLS |