forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
Lorenz Meier
commit
eb9930de25
|
|
@ -80,6 +80,7 @@
|
|||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
|
@ -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");
|
||||
|
|
@ -1379,6 +1475,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
struct sensor_combined_s sensors;
|
||||
memset(&sensors, 0, sizeof(sensors));
|
||||
|
||||
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s differential_pressure;
|
||||
memset(&differential_pressure, 0, sizeof(differential_pressure));
|
||||
uint64_t last_differential_pressure_time = 0;
|
||||
|
||||
/* Subscribe to command topic */
|
||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
struct vehicle_command_s cmd;
|
||||
|
|
@ -1432,6 +1533,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
|
||||
}
|
||||
|
||||
orb_check(differential_pressure_sub, &new_data);
|
||||
|
||||
if (new_data) {
|
||||
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
|
||||
last_differential_pressure_time = differential_pressure.timestamp;
|
||||
}
|
||||
|
||||
orb_check(cmd_sub, &new_data);
|
||||
|
||||
if (new_data) {
|
||||
|
|
@ -1618,6 +1726,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
|
||||
bool global_pos_valid = current_status.flag_global_position_valid;
|
||||
bool local_pos_valid = current_status.flag_local_position_valid;
|
||||
bool airspeed_valid = current_status.flag_airspeed_valid;
|
||||
|
||||
/* check for global or local position updates, set a timeout of 2s */
|
||||
if (hrt_absolute_time() - last_global_position_time < 2000000) {
|
||||
|
|
@ -1636,6 +1745,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
current_status.flag_local_position_valid = false;
|
||||
}
|
||||
|
||||
/* Check for valid airspeed/differential pressure measurements */
|
||||
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
|
||||
current_status.flag_airspeed_valid = true;
|
||||
|
||||
} else {
|
||||
current_status.flag_airspeed_valid = false;
|
||||
}
|
||||
|
||||
/*
|
||||
* Consolidate global position and local position valid flags
|
||||
* for vector flight mode.
|
||||
|
|
@ -1651,7 +1768,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* consolidate state change, flag as changed if required */
|
||||
if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
|
||||
global_pos_valid != current_status.flag_global_position_valid ||
|
||||
local_pos_valid != current_status.flag_local_position_valid) {
|
||||
local_pos_valid != current_status.flag_local_position_valid ||
|
||||
airspeed_valid != current_status.flag_airspeed_valid) {
|
||||
state_changed = true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -299,8 +299,12 @@ up_pwm_servo_arm(bool armed)
|
|||
rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
|
||||
|
||||
} else {
|
||||
/* on disarm, just stop auto-reload so we don't generate runts */
|
||||
rCR1(i) &= ~GTIM_CR1_ARPE;
|
||||
// XXX This leads to FMU PWM being still active
|
||||
// but uncontrollable. Just disable the timer
|
||||
// and risk a runt.
|
||||
///* on disarm, just stop auto-reload so we don't generate runts */
|
||||
//rCR1(i) &= ~GTIM_CR1_ARPE;
|
||||
rCR1(i) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
@ -1025,8 +1033,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,24 +1041,24 @@ 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 diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
|
||||
|
||||
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, 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;
|
||||
_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;
|
||||
_differential_pressure.voltage = voltage;
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
if (_airspeed_pub > 0) {
|
||||
|
|
@ -1064,7 +1071,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
}
|
||||
|
||||
_last_adc = hrt_absolute_time();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -126,7 +126,7 @@ static inline int readline_rawgetc(int infd)
|
|||
* error occurs).
|
||||
*/
|
||||
|
||||
do
|
||||
for (;;)
|
||||
{
|
||||
/* Read one character from the incoming stream */
|
||||
|
||||
|
|
@ -154,13 +154,21 @@ static inline int readline_rawgetc(int infd)
|
|||
{
|
||||
return -errcode;
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
else if (buffer == '\0')
|
||||
{
|
||||
/* Ignore NUL characters, since they look like EOF to our caller */
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Success, return the character that was read */
|
||||
|
||||
return (int)buffer;
|
||||
}
|
||||
while (nread < 1);
|
||||
|
||||
/* On success, return the character that was read */
|
||||
|
||||
return (int)buffer;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
|
|
|
|||
|
|
@ -40,14 +40,31 @@
|
|||
*
|
||||
*/
|
||||
|
||||
#include "math.h"
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -55,14 +72,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 +87,25 @@ 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/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
|
||||
*/
|
||||
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 || !isfinite(density)) {
|
||||
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
|
||||
printf("[airspeed] Invalid air density, using density at sea level\n");
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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/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
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -44,10 +44,10 @@
|
|||
#include <float.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
|
||||
|
|
|
|||
|
|
@ -49,15 +49,16 @@
|
|||
*/
|
||||
|
||||
/**
|
||||
* Battery voltages and status
|
||||
* 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) */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -209,6 +210,7 @@ struct vehicle_status_s
|
|||
bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
|
||||
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
|
||||
bool flag_valid_launch_position; /**< indicates a valid launch position */
|
||||
bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue