fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 15:57:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/PX4/Firmware into bottledrop

Browse Source
This commit is contained in:
Juchli D
2013-11-15 10:34:12 +01:00
parent adee479782 2116966b1e
commit 4b8c3c38cd
39 changed files with 307 additions and 336 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+3 -6
View File
@@ -34,7 +34,7 @@
/**
* @file KalmanNav.cpp
*
* kalman filter navigation code
* Kalman filter navigation code
*/
#include <poll.h>
@@ -228,10 +228,7 @@ void KalmanNav::update()
updateSubscriptions();
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate &&
_sensors.accelerometer_counter > 10 &&
_sensors.gyro_counter > 10 &&
_sensors.magnetometer_counter > 10) {
if (!_attitudeInitialized && sensorsUpdate) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
@@ -643,7 +640,7 @@ int KalmanNav::correctAtt()
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:\n"); y.print();
warnx("y:"); y.print();
}
// update quaternions from euler
src/modules/commander/airspeed_calibration.cpp
+38 -6
View File
@@ -37,12 +37,15 @@
*/
#include "airspeed_calibration.h"
#include "calibration_messages.h"
#include "commander_helper.h"
#include <stdio.h>
#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
@@ -55,10 +58,13 @@
#endif
static const int ERROR = -1;
static const char *sensor_name = "dpress";
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
const int calibration_count = 2500;
@@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
int calibration_counter = 0;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
0.0f,
1.0f,
};
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_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);
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
@@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
@@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_info(mavlink_fd, "airspeed calibration done");
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral();
close(diff_pres_sub);
return OK;
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
src/modules/commander/commander.cpp
+7 -10
View File
@@ -116,6 +116,8 @@ extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
#define STICK_ON_OFF_LIMIT 0.75f
#define STICK_THRUST_RANGE 1.0f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
@@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
/* try again later */
usleep(20000);
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
}
}
/* Main state machine */
/* make sure we are in preflight state */
memset(&status, 0, sizeof(status));
@@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
/* try to open the mavlink log device every once in a while */
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* update parameters */
orb_check(param_changed_sub, &updated);
src/modules/fw_att_control/fw_att_control_main.cpp
+35 -32
View File
@@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_i = param_find("FW_P_I");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_P_integrator_max");
_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_R_P");
_parameter_handles.r_d = param_find("FW_R_D");
_parameter_handles.r_i = param_find("FW_R_I");
_parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_Y_P");
_parameter_handles.y_i = param_find("FW_Y_I");
_parameter_handles.y_d = param_find("FW_Y_D");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
_parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main()
}
}
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
_parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
_parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
// _actuators.control[2], _actuators.control[3]);
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
// _actuators.control[2], _actuators.control[3]);
rates_sp.timestamp = hrt_absolute_time();
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
if (_rate_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
rates_sp.timestamp = hrt_absolute_time();
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
if (_rate_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
}
}
} else {
src/modules/mavlink/missionlib.c
+1
View File
@@ -64,6 +64,7 @@
#include <systemlib/systemlib.h>
#include <mavlink/mavlink_log.h>
#include "geo/geo.h"
#include "waypoints.h"
#include "orb_topics.h"
#include "missionlib.h"
src/modules/mavlink/orb_listener.c
+16 -12
View File
@@ -75,6 +75,7 @@ struct actuator_armed_s armed;
struct actuator_controls_effective_s actuators_effective_0;
struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct airspeed_s airspeed;
struct mavlink_subscriptions mavlink_subs;
@@ -92,6 +93,8 @@ static unsigned int gps_counter;
*/
static uint64_t last_sensor_timestamp;
static hrt_abstime last_sent_vfr = 0;
static void *uorb_receive_thread(void *arg);
struct listener {
@@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l)
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
if (gcs_link)
if (gcs_link) {
/* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
@@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l)
att.pitchspeed,
att.yawspeed);
/* limit VFR message rate to 10Hz */
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
}
}
attitude_counter++;
}
@@ -681,17 +695,7 @@ l_home(const struct listener *l)
void
l_airspeed(const struct listener *l)
{
struct airspeed_s airspeed;
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
float alt = global_pos.relative_alt;
float climb = -global_pos.vz;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
}
void
@@ -849,7 +853,7 @@ uorb_receive_start(void)
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
/* --- AIRSPEED / VFR / HUD --- */
/* --- AIRSPEED --- */
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */
src/modules/multirotor_att_control/multirotor_rate_control.c
+11 -41
View File
@@ -59,31 +59,23 @@
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
//float yawrate_awu;
//float yawrate_lim;
float attrate_p;
float attrate_d;
float attrate_i;
//float attrate_awu;
//float attrate_lim;
float rate_lim;
};
@@ -93,14 +85,10 @@ struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
//param_t yawrate_awu;
//param_t yawrate_lim;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
//param_t attrate_awu;
//param_t attrate_lim;
};
/**
@@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
//h->yawrate_awu = param_find("MC_YAWRATE_AWU");
//h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
//h->attrate_awu = param_find("MC_ATTRATE_AWU");
//h->attrate_lim = param_find("MC_ATTRATE_LIM");
return OK;
}
@@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
//param_get(h->yawrate_awu, &(p->yawrate_awu));
//param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
//param_get(h->attrate_awu, &(p->attrate_awu));
//param_get(h->attrate_lim, &(p->attrate_lim));
return OK;
}
@@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static PID_t yaw_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
@@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
@@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
// TODO pid_reset_integral(&yaw_rate_controller);
pid_reset_integral(&yaw_rate_controller);
}
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT);
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
/* increase resilience to faulty control inputs */
if (!isfinite(yaw_rate_control)) {
yaw_rate_control = 0.0f;
warnx("rej. NaN ctrl yaw");
}
/* run pitch, roll and yaw controllers */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[2] = yaw_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
src/modules/multirotor_pos_control/multirotor_pos_control.c
+4 -1
View File
@@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
att_sp.yaw_body = global_pos_sp.yaw;
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
src/modules/sensors/sensors.cpp
+1
View File
@@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced)
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
warn("WARNING: failed to set scale / offsets for airspeed sensor");
close(fd);
}
#if 0
src/modules/systemlib/airspeed.c
+1 -1
View File
@@ -59,7 +59,7 @@
float calc_indicated_airspeed(float differential_pressure)
{
if (differential_pressure > 0) {
if (differential_pressure > 0.0f) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
src/modules/uORB/topics/differential_pressure.h
+2 -2
View File
@@ -54,8 +54,8 @@
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
uint16_t differential_pressure_pa; /**< Differential pressure reading */
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
float differential_pressure_pa; /**< 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 */