diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/airspeed_calibration.cpp | 44 | ||||
-rw-r--r-- | src/modules/mavlink/missionlib.c | 1 | ||||
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 28 | ||||
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_rate_control.c | 52 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 1 | ||||
-rw-r--r-- | src/modules/systemlib/airspeed.c | 2 | ||||
-rw-r--r-- | src/modules/uORB/topics/differential_pressure.h | 4 |
7 files changed, 70 insertions, 62 deletions
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 248eb4a66..1809f9688 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -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; } diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index d37b18a19..fa23f996f 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -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" diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index f6860930c..abc91d34f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -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 */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index adb63186c..86ac0e4ff 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -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++; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372..6d38b98ec 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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 diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 310fbf60f..1a479c205 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -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); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index e4d2c92ce..5d94d4288 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -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 */ |