fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 01:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta' into beta_uavacan

Browse Source
This commit is contained in:
Pavel Kirienko
2015-08-13 22:06:26 +03:00
parent fc6144c46e 1b8c98386b
commit 05b6a48d1b
132 changed files with 2633 additions and 838 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+1 -1
View File
@@ -205,7 +205,7 @@ int AttitudeEstimatorQ::start() {
_control_task = task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2500,
2000,
(main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
src/modules/commander/accelerometer_calibration.cpp
+10 -2
View File
@@ -559,6 +559,7 @@ int do_level_calibration(int mavlink_fd) {
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
const unsigned settle_time = 30;
bool success = false;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
@@ -597,7 +598,15 @@ int do_level_calibration(int mavlink_fd) {
start = hrt_absolute_time();
// average attitude for 5 seconds
while(hrt_elapsed_time(&start) < cal_time * 1000000) {
poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
int pollret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
if (pollret <= 0) {
// attitude estimator is not running
mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
goto out;
}
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
roll_mean += att.roll;
pitch_mean += att.pitch;
@@ -606,7 +615,6 @@ int do_level_calibration(int mavlink_fd) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
bool success = false;
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;
src/modules/commander/airspeed_calibration.cpp
+6 -6
View File
@@ -72,12 +72,12 @@ int do_airspeed_calibration(int mavlink_fd)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 3000;
const unsigned maxcount = 2400;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
const unsigned calibration_count = (maxcount * 2) / 3;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@@ -201,10 +201,10 @@ int do_airspeed_calibration(int mavlink_fd)
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
src/modules/commander/calibration_routines.cpp
+2 -2
View File
@@ -469,8 +469,8 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
continue;
}
src/modules/commander/commander.cpp
+59 -11
View File
@@ -275,7 +275,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("commander already running");
warnx("already running");
/* this is not an error */
exit(0);
}
@@ -288,11 +288,18 @@ int commander_main(int argc, char *argv[])
commander_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
while (!thread_running) {
usleep(200);
unsigned constexpr max_wait_us = 1000000;
unsigned constexpr max_wait_steps = 2000;
unsigned i;
for (i = 0; i < max_wait_steps; i++) {
usleep(max_wait_us / max_wait_steps);
if (thread_running) {
break;
}
}
exit(0);
exit(!(i < max_wait_steps));
}
if (!strcmp(argv[1], "stop")) {
@@ -870,6 +877,8 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
param_t _param_eph = param_find("COM_HOME_H_T");
param_t _param_epv = param_find("COM_HOME_V_T");
const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
@@ -1293,6 +1302,10 @@ int commander_thread_main(int argc, char *argv[])
/* Parameter autosave setting */
param_get(_param_autosave_params, &autosave_params);
/* EPH / EPV */
param_get(_param_eph, &eph_threshold);
param_get(_param_epv, &epv_threshold);
}
/* Set flag to autosave parameters if necessary */
@@ -1342,10 +1355,15 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
/* perform system checks when new telemetry link connected */
if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 &&
if (
/* we actually have a way to talk to the user */
mavlink_fd &&
/* we first connect a link or re-connect a link after loosing it */
(telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) &&
/* and this link has a communication partner */
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
/* and the system is not already armed (and potentially flying) */
!armed.armed) {
bool chAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
@@ -1817,7 +1835,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",
mavlink_log_info(mavlink_fd, "RC SIGNAL REGAINED after %llums",
(hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
status_changed = true;
}
@@ -1944,7 +1962,7 @@ int commander_thread_main(int argc, char *argv[])
/* only report a regain */
if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
}
telemetry_lost[i] = false;
@@ -1962,7 +1980,7 @@ int commander_thread_main(int argc, char *argv[])
/* only reset the timestamp to a different time on state change */
telemetry_last_dl_loss[i] = hrt_absolute_time();
mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
mavlink_and_console_log_info(mavlink_fd, "data link #%i lost", i);
telemetry_lost[i] = true;
}
}
@@ -1977,7 +1995,9 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.data_link_lost) {
mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
if (armed.armed) {
mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
}
status.data_link_lost = true;
status.data_link_lost_counter++;
status_changed = true;
@@ -2138,6 +2158,19 @@ int commander_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
armed.timestamp = now;
/* set prearmed state if safety is off, or safety is not present and 5 seconds passed */
if (safety.safety_switch_available) {
/* safety is off, go into prearmed */
armed.prearmed = safety.safety_off;
} else {
/* safety is not present, go into prearmed
* (all output drivers should be started / unlocked last in the boot process
* when the rest of the system is fully initialized)
*/
armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);
}
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -2959,6 +2992,21 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
} else if (((int)(cmd.param1)) == 2) {
/* reset parameters and save empty file */
param_reset_all();
int ret = param_save_default();
if (ret == OK) {
/* do not spam MAVLink, but provide the answer / green led mechanism */
mavlink_log_critical(mavlink_fd, "onboard parameters reset");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "param reset error");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
}
src/modules/commander/commander_params.c
+70 -6
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,16 +36,54 @@
*
* Parameters defined by the sensors task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@px4.io>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Roll trim
*
* The trim value is the actuator control value the system needs
* for straight and level flight. It can be calibrated by
* flying manually straight and level using the RC trims and
* copying them using the GCS.
*
* @group Radio Calibration
* @min -0.25
* @max 0.25
*/
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
/**
* Pitch trim
*
* The trim value is the actuator control value the system needs
* for straight and level flight. It can be calibrated by
* flying manually straight and level using the RC trims and
* copying them using the GCS.
*
* @group Radio Calibration
* @min -0.25
* @max 0.25
*/
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
/**
* Yaw trim
*
* The trim value is the actuator control value the system needs
* for straight and level flight. It can be calibrated by
* flying manually straight and level using the RC trims and
* copying them using the GCS.
*
* @group Radio Calibration
* @min -0.25
* @max 0.25
*/
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
/**
@@ -77,6 +114,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
*
* @group Battery Calibration
* @unit V
* @min 0.0f
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
@@ -87,6 +125,8 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
*
* @group Battery Calibration
* @unit S
* @min 1
* @max 10
*/
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
@@ -182,7 +222,31 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
* @min 0
* @max 35
*/
PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
/**
* Home set horizontal threshold
*
* The home position will be set if the estimated positioning accuracy is below the threshold.
*
* @group Commander
* @unit meter
* @min 2
* @max 15
*/
PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
/**
* Home set vertical threshold
*
* The home position will be set if the estimated positioning accuracy is below the threshold.
*
* @group Commander
* @unit meter
* @min 5
* @max 25
*/
PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);
/**
* Autosaving of params
src/modules/commander/mag_calibration.cpp
+22 -8
View File
@@ -64,9 +64,11 @@
static const int ERROR = -1;
static const char *sensor_name = "mag";
static const unsigned max_mags = 3;
static constexpr unsigned max_mags = 3;
static constexpr float mag_sphere_radius = 0.2f;
static const unsigned int calibration_sides = 6;
static constexpr unsigned int calibration_sides = 6; ///< The total number of sides
static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer
static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
@@ -204,6 +206,10 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl
return false;
}
static unsigned progress_percentage(mag_worker_data_t* worker_data) {
return 100 * ((float)worker_data->done_count) / calibration_sides;
}
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
calibrate_return result = calibrate_return_ok;
@@ -223,7 +229,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
* for a good result, so we're not constraining the user more than we have to.
*/
hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5;
hrt_abstime last_gyro = 0;
float gyro_x_integral = 0.0f;
float gyro_y_integral = 0.0f;
@@ -344,8 +350,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
detect_orientation_str(orientation), progress_percentage(worker_data) +
(unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
}
} else {
poll_errcount++;
@@ -362,7 +368,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
}
return result;
@@ -376,8 +382,8 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
worker_data.calibration_points_perside = 40;
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: Right-side up, Left Side, Nose down
@@ -496,6 +502,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
printf("RAW DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (worker_data.calibration_counter_total[cur_mag] == 0) {
continue;
}
printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
@@ -511,6 +521,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
printf("CALIBRATED DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
if (worker_data.calibration_counter_total[cur_mag] == 0) {
continue;
}
printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
src/modules/fw_att_control/fw_att_control_main.cpp
+5 -8
View File
@@ -163,7 +163,6 @@ private:
struct {
float tconst;
float p_p;
float p_d;
float p_i;
float p_ff;
float p_rmax_pos;
@@ -171,7 +170,6 @@ private:
float p_integrator_max;
float p_roll_feedforward;
float r_p;
float r_d;
float r_i;
float r_ff;
float r_integrator_max;
@@ -208,7 +206,6 @@ private:
param_t tconst;
param_t p_p;
param_t p_d;
param_t p_i;
param_t p_ff;
param_t p_rmax_pos;
@@ -216,7 +213,6 @@ private:
param_t p_integrator_max;
param_t p_roll_feedforward;
param_t r_p;
param_t r_d;
param_t r_i;
param_t r_ff;
param_t r_integrator_max;
@@ -699,7 +695,6 @@ FixedwingAttitudeControl::task_main()
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
@@ -803,6 +798,11 @@ FixedwingAttitudeControl::task_main()
//warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
continue;
}
/* default flaps to center */
float flaps_control = 0.0f;
@@ -812,11 +812,8 @@ FixedwingAttitudeControl::task_main()
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
src/modules/fw_att_control/fw_att_control_params.c
+10 -10
View File
@@ -84,11 +84,11 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 50.0
* @min 0.005
* @max 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f);
PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f);
/**
* Maximum positive / up pitch rate.
@@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f);
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
/**
* Maximum negative / down pitch rate.
@@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
/**
* Pitch rate integrator limit
@@ -157,11 +157,11 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 100.0
* @min 0.005
* @max 0.2
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f);
PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f);
/**
* Roll Integrator Anti-Windup
@@ -185,7 +185,7 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
/**
* Yaw rate proportional gain
@@ -258,7 +258,7 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
/**
* Yaw rate feed forward
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+49 -24
View File
@@ -179,6 +179,8 @@ private:
float _hdg_hold_yaw; /**< hold heading for velocity mode */
bool _hdg_hold_enabled; /**< heading hold enabled */
bool _yaw_lock_engaged; /**< yaw is locked for heading hold */
float _althold_epv; /**< the position estimate accuracy when engaging alt hold */
bool _was_in_deadband; /**< wether the last stick input was in althold deadband */
struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */
struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */
hrt_abstime _control_position_last_called; /**<last call of control_position */
@@ -508,6 +510,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_hdg_hold_yaw(0.0f),
_hdg_hold_enabled(false),
_yaw_lock_engaged(false),
_althold_epv(0.0f),
_was_in_deadband(false),
_hdg_hold_prev_wp{},
_hdg_hold_curr_wp{},
_control_position_last_called(0),
@@ -968,12 +972,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
bool FixedwingPositionControl::update_desired_altitude(float dt)
{
const float deadBand = (60.0f/1000.0f);
/*
* The complete range is -1..+1, so this is 6%
* of the up or down range or 3% of the total range.
*/
const float deadBand = 0.06f;
/*
* The correct scaling of the complete range needs
* to account for the missing part of the slope
* due to the deadband
*/
const float factor = 1.0f - deadBand;
// XXX this should go into a manual stick mapper
// class
static float _althold_epv = 0.0f;
static bool was_in_deadband = false;
/* Climbout mode sets maximum throttle and pitch up */
bool climbout_mode = false;
/*
@@ -988,24 +1000,29 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
_althold_epv = _global_pos.epv;
}
// XXX the sign magic in this function needs to be fixed
/*
* Manual control has as convention the rotation around
* an axis. Positive X means to rotate positively around
* the X axis in NED frame, which is pitching down
*/
if (_manual.x > deadBand) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
was_in_deadband = false;
climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH);
/* pitching down */
float pitch = -(_manual.x - deadBand) / factor;
_hold_alt += (_parameters.max_sink_rate * dt) * pitch;
_was_in_deadband = false;
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
was_in_deadband = false;
} else if (!was_in_deadband) {
/* pitching up */
float pitch = -(_manual.x + deadBand) / factor;
_hold_alt += (_parameters.max_climb_rate * dt) * pitch;
_was_in_deadband = false;
climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH);
} else if (!_was_in_deadband) {
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
_althold_epv = _global_pos.epv;
was_in_deadband = true;
_was_in_deadband = true;
}
return climbout_mode;
@@ -1037,7 +1054,7 @@ bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
float dt = FLT_MIN; // Using non zero value to a avoid division by zero
float dt = 0.01; // Using non zero value to a avoid division by zero
if (_control_position_last_called > 0) {
dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
}
@@ -1045,19 +1062,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
bool setpoint = true;
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_vehicle_status.condition_landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));
/* update TECS filters */
if (!_mTecs.getEnabled()) {
_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
}
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
/* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
@@ -1477,7 +1502,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
pitch_limit_min,
((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@@ -1587,7 +1612,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
pitch_limit_min,
((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+2 -2
View File
@@ -327,7 +327,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
/**
* Speed <--> Altitude priority
@@ -378,7 +378,7 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);
/**
* Landing slope angle
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+3 -3
View File
@@ -204,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
mode = tecs_status_s::TECS_MODE_UNDERSPEED;
}
/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
/* Set special output limiters if we are not in TECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
@@ -221,7 +221,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
/* Apply override given by the limitOverride argument (this is used for limits which are not given by
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
* is running) */
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
@@ -253,7 +253,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
(double)accelerationLongitudinalSp, (double)airspeedDerivative);
}
/* publish status messge */
/* publish status message */
_status.update();
/* clean up */
src/modules/land_detector/FixedwingLandDetector.cpp
+14 -1
View File
@@ -49,7 +49,11 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_vehicleLocalPositionSub(-1),
_vehicleLocalPosition({}),
_airspeedSub(-1),
_airspeed({}),
_vehicleStatusSub(-1),
_armingSub(-1),
_airspeed{},
_vehicleStatus{},
_arming{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
@@ -66,6 +70,8 @@ void FixedwingLandDetector::initialize()
// Subscribe to local position and airspeed data
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
updateParameterCache(true);
}
@@ -74,6 +80,8 @@ void FixedwingLandDetector::updateSubscriptions()
{
orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
}
bool FixedwingLandDetector::update()
@@ -81,6 +89,11 @@ bool FixedwingLandDetector::update()
// First poll for new data from our subscriptions
updateSubscriptions();
// only trigger flight conditions if we are armed
if (!_arming.armed) {
return true;
}
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
src/modules/land_detector/FixedwingLandDetector.h
+11 -5
View File
@@ -44,7 +44,9 @@
#include "LandDetector.h"
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <systemlib/param/param.h>
class FixedwingLandDetector : public LandDetector
@@ -90,11 +92,15 @@ private:
} _params;
private:
int _vehicleLocalPositionSub; /**< notification of local position */
struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */
int _airspeedSub;
struct airspeed_s _airspeed;
int _parameterSub;
int _vehicleLocalPositionSub; /**< notification of local position */
struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */
int _airspeedSub;
int _vehicleStatusSub;
int _armingSub;
struct airspeed_s _airspeed;
struct vehicle_status_s _vehicleStatus;
struct actuator_armed_s _arming;
int _parameterSub;
float _velocity_xy_filtered;
float _velocity_z_filtered;
src/modules/land_detector/LandDetector.cpp
+3 -2
View File
@@ -46,8 +46,9 @@
LandDetector::LandDetector() :
_landDetectedPub(-1),
_landDetected({0, false}),
_taskShouldExit(false),
_taskIsRunning(false)
_arming_time(0),
_taskShouldExit(false),
_taskIsRunning(false)
{
// ctor
}
src/modules/land_detector/LandDetector.h
+4 -3
View File
@@ -96,10 +96,11 @@ protected:
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold
before triggering a land */
static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */
protected:
orb_advert_t _landDetectedPub; /**< publisher for position in local frame */
struct vehicle_land_detected_s _landDetected; /**< local vehicle position */
orb_advert_t _landDetectedPub; /**< publisher for position in local frame */
struct vehicle_land_detected_s _landDetected; /**< local vehicle position */
uint64_t _arming_time; /**< timestamp of arming time */
private:
bool _taskShouldExit; /**< true if it is requested that this task should exit */
src/modules/land_detector/MulticopterLandDetector.cpp
+26 -11
View File
@@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_actuatorsSub(-1),
_armingSub(-1),
_parameterSub(-1),
_attitudeSub(-1),
_vehicleGlobalPosition({}),
_vehicleStatus({}),
_actuators({}),
_arming({}),
_vehicleAttitude({}),
_attitudeSub(-1),
_vehicleGlobalPosition{},
_vehicleStatus{},
_actuators{},
_arming{},
_vehicleAttitude{},
_landTimer(0)
{
_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
@@ -97,7 +97,10 @@ bool MulticopterLandDetector::update()
// only trigger flight conditions if we are armed
if (!_arming.armed) {
_arming_time = 0;
return true;
} else if (_arming_time == 0) {
_arming_time = hrt_absolute_time();
}
// return status based on armed state if no position lock is available
@@ -110,8 +113,18 @@ bool MulticopterLandDetector::update()
const uint64_t now = hrt_absolute_time();
// check if we are moving vertically
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
float armThresholdFactor = 1.0f;
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives
if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
armThresholdFactor = 2.5f;
}
// check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor;
// check if we are moving horizontally
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
@@ -119,9 +132,11 @@ bool MulticopterLandDetector::update()
&& _vehicleStatus.condition_global_position_valid;
// next look if all rotation angles are not moving
bool rotating = fabsf(_vehicleAttitude.rollspeed) > _params.maxRotation ||
fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation ||
fabsf(_vehicleAttitude.yawspeed) > _params.maxRotation;
float maxRotationScaled = _params.maxRotation * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
// check if thrust output is minimal (about half of default)
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
src/modules/land_detector/MulticopterLandDetector.h
+7 -7
View File
@@ -97,20 +97,20 @@ private:
} _params;
private:
int _vehicleGlobalPositionSub; /**< notification of global position */
int _vehicleGlobalPositionSub; /**< notification of global position */
int _vehicleStatusSub;
int _actuatorsSub;
int _armingSub;
int _parameterSub;
int _attitudeSub;
struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */
struct vehicle_status_s _vehicleStatus;
struct actuator_controls_s _actuators;
struct actuator_armed_s _arming;
struct vehicle_attitude_s _vehicleAttitude;
struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */
struct vehicle_status_s _vehicleStatus;
struct actuator_controls_s _actuators;
struct actuator_armed_s _arming;
struct vehicle_attitude_s _vehicleAttitude;
uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/
uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/
};
#endif //__MULTICOPTER_LAND_DETECTOR_H__
src/modules/land_detector/land_detector_params.c
+1 -1
View File
@@ -49,7 +49,7 @@
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.60f);
PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.70f);
/**
* Multicopter max horizontal velocity
src/modules/mavlink/mavlink_main.cpp
+68 -6
View File
@@ -115,6 +115,8 @@ extern mavlink_system_t mavlink_system;
static void usage(void);
bool Mavlink::_boot_complete = false;
Mavlink::Mavlink() :
_device_name(DEFAULT_DEVICE_NAME),
_task_should_exit(false),
@@ -150,6 +152,7 @@ Mavlink::Mavlink() :
_datarate(1000),
_datarate_events(500),
_rate_mult(1.0f),
_last_hw_rate_timestamp(0),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
@@ -1230,6 +1233,7 @@ Mavlink::update_rate_mult()
float const_rate = 0.0f;
float rate = 0.0f;
/* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
if (stream->const_rate()) {
@@ -1241,7 +1245,55 @@ Mavlink::update_rate_mult()
}
/* don't scale up rates, only scale down if needed */
_rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
float bandwidth_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
/* check if we have radio feedback */
struct telemetry_status_s &tstatus = get_rx_status();
bool radio_critical = false;
bool radio_found = false;
/* 2nd pass: Now check hardware limits */
if (tstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
radio_found = true;
if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
radio_critical = true;
}
}
float hardware_mult = _rate_mult;
/* scale down if we have a TX err rate suggesting link congestion */
if (_rate_txerr > 0.0f && !radio_critical) {
hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr);
} else if (radio_found && tstatus.timestamp != _last_hw_rate_timestamp) {
if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 20% */
hardware_mult *= 0.80f;
} else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */
hardware_mult *= 0.975f;
} else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
/* this indicates spare bandwidth, increase by 2.5% */
hardware_mult *= 1.025f;
/* limit to a max multiplier of 1 */
hardware_mult = fminf(1.0f, hardware_mult);
}
} else {
/* no limitation, set hardware to 1 */
hardware_mult = 1.0f;
}
_last_hw_rate_timestamp = tstatus.timestamp;
/* pick the minimum from bandwidth mult and hardware mult as limit */
_rate_mult = fminf(bandwidth_mult, hardware_mult);
/* ensure the rate multiplier never drops below 5% so that something is always sent */
_rate_mult = fmaxf(0.05f, _rate_mult);
}
int
@@ -1440,22 +1492,22 @@ Mavlink::task_main(int argc, char *argv[])
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f);
configure_stream("ATTITUDE", 10.0f);
configure_stream("HIGHRES_IMU", 2.0f);
configure_stream("ATTITUDE", 20.0f);
configure_stream("VFR_HUD", 8.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("RC_CHANNELS_RAW", 1.0f);
configure_stream("RC_CHANNELS_RAW", 4.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("ATTITUDE_TARGET", 8.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW_RAD", 5.0f);
break;
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 50.0f);
configure_stream("ATTITUDE", 250.0f);
configure_stream("HIGHRES_IMU", 50.0f);
configure_stream("GPS_RAW_INT", 5.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
@@ -1737,6 +1789,12 @@ Mavlink::start(int argc, char *argv[])
// before returning to the shell
int ic = Mavlink::instance_count();
if (ic == MAVLINK_COMM_NUM_BUFFERS) {
warnx("Maximum MAVLink instance count of %d reached.",
(int)MAVLINK_COMM_NUM_BUFFERS);
return 1;
}
// Instantiate thread
char buf[24];
sprintf(buf, "mavlink_if%d", ic);
@@ -1909,6 +1967,10 @@ int mavlink_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream_command(argc, argv);
} else if (!strcmp(argv[1], "boot_complete")) {
Mavlink::set_boot_complete();
return 0;
} else {
usage();
exit(1);
src/modules/mavlink/mavlink_main.h
+16
View File
@@ -145,6 +145,14 @@ public:
bool get_forwarding_on() { return _forwarding_on; }
/**
* Set the boot complete flag on all instances
*
* Setting the flag unblocks parameter transmissions, which are gated
* beforehand to ensure that the system is fully initialized.
*/
static void set_boot_complete() { _boot_complete = true; }
/**
* Get the free space in the transmit buffer
*
@@ -294,6 +302,8 @@ public:
unsigned get_system_type() { return _system_type; }
static bool boot_complete() { return _boot_complete; }
protected:
Mavlink *next;
@@ -302,6 +312,7 @@ private:
int _mavlink_fd;
bool _task_running;
static bool _boot_complete;
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
@@ -340,6 +351,7 @@ private:
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
int _datarate_events; ///< data rate for params, waypoints, text messages
float _rate_mult;
hrt_abstime _last_hw_rate_timestamp;
/**
* If the queue index is not at 0, the queue sending
@@ -398,6 +410,10 @@ private:
static unsigned int interval_from_rate(float rate);
static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50;
int configure_stream(const char *stream_name, const float rate);
/**
src/modules/mavlink/mavlink_messages.cpp
+2 -2
View File
@@ -537,7 +537,7 @@ protected:
msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
msg.load = status.load * 1000.0f;
msg.voltage_battery = status.battery_voltage * 1000.0f;
msg.current_battery = status.battery_current / 10.0f;
msg.current_battery = status.battery_current * 100.0f;
msg.drop_rate_comm = status.drop_rate_comm;
msg.errors_comm = status.errors_comm;
msg.errors_count1 = status.errors_count1;
@@ -562,7 +562,7 @@ protected:
bat_msg.voltages[i] = 0;
}
}
bat_msg.current_battery = status.battery_current / 10.0f;
bat_msg.current_battery = status.battery_current * 100.0f;
bat_msg.current_consumed = status.battery_discharged_mah;
bat_msg.energy_consumed = -1.0f;
bat_msg.battery_remaining = (status.battery_voltage > 0) ?
src/modules/mavlink/mavlink_parameters.cpp
+5 -1
View File
@@ -193,7 +193,7 @@ void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested, but only after the system has booted */
if (_send_all_index >= 0 && t > 4 * 1000 * 1000) {
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) {
@@ -215,6 +215,10 @@ MavlinkParametersManager::send(const hrt_abstime t)
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
}
} else if (_send_all_index == 0 && hrt_absolute_time() > 20 * 1000 * 1000) {
/* the boot did not seem to ever complete, warn user and set boot complete */
_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
_mavlink->set_boot_complete();
}
}
src/modules/mavlink/mavlink_receiver.cpp
+53 -15
View File
@@ -35,9 +35,9 @@
* @file mavlink_receiver.cpp
* MAVLink protocol message receive and dispatch
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Thomas Gubler <thomas@px4.io>
*/
/* XXX trim includes */
@@ -133,7 +133,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_sp{},
_rates_sp{},
_time_offset_avg_alpha(0.6),
_time_offset(0)
_time_offset(0),
_mom_switch_pos{},
_mom_switch_state(0)
{
}
@@ -904,15 +906,45 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
}
}
static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) {
switch_pos_t
MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
{
// XXX non-standard 3 pos switch decoding
return (buttons >> (sw * 2)) & 3;
}
static int decode_switch_pos_n(uint16_t buttons, int sw) {
if (buttons & (1 << sw)) {
return 1;
int
MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw)
{
bool on = (buttons & (1 << sw));
if (sw < MOM_SWITCH_COUNT) {
bool last_on = (_mom_switch_state & (1 << sw));
/* first switch is 2-pos, rest is 2 pos */
unsigned state_count = (sw == 0) ? 3 : 2;
/* only transition on low state */
if (!on && (on != last_on)) {
_mom_switch_pos[sw]++;
if (_mom_switch_pos[sw] == state_count) {
_mom_switch_pos[sw] = 0;
}
}
/* state_count - 1 is the number of intervals and 1000 is the range,
* with 2 states 0 becomes 0, 1 becomes 1000. With
* 3 states 0 becomes 0, 1 becomes 500, 2 becomes 1000,
* and so on for more states.
*/
return (_mom_switch_pos[sw] * 1000) / (state_count - 1) + 1000;
} else {
return 0;
/* return the current state */
return on * 1000 + 1000;
}
}
@@ -1009,12 +1041,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.values[3] = 1000;
}
rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
rc.values[6] = decode_switch_pos_n(man.buttons, 2) * 1000 + 1000;
rc.values[7] = decode_switch_pos_n(man.buttons, 3) * 1000 + 1000;
rc.values[8] = decode_switch_pos_n(man.buttons, 4) * 1000 + 1000;
rc.values[9] = decode_switch_pos_n(man.buttons, 5) * 1000 + 1000;
/* decode all switches which fit into the channel mask */
unsigned max_switch = (sizeof(man.buttons) * 8);
unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
if (max_switch > (max_channels - 4)) {
max_switch = (max_channels - 4);
}
/* fill all channels */
for (unsigned i = 0; i < max_switch; i++) {
rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
}
_mom_switch_state = man.buttons;
if (_rc_pub <= 0) {
_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
src/modules/mavlink/mavlink_receiver.h
+24 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,8 +35,8 @@
* @file mavlink_orb_listener.h
* MAVLink 1.0 uORB listener definition
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#pragma once
@@ -139,15 +139,26 @@ private:
void *receive_thread(void *arg);
/**
* Convert remote timestamp to local hrt time (usec)
* Use timesync if available, monotonic boot time otherwise
*/
* Convert remote timestamp to local hrt time (usec)
* Use timesync if available, monotonic boot time otherwise
*/
uint64_t sync_stamp(uint64_t usec);
/**
* Exponential moving average filter to smooth time offset
*/
* Exponential moving average filter to smooth time offset
*/
void smooth_time_offset(uint64_t offset_ns);
/**
* Decode a switch position from a bitfield
*/
switch_pos_t decode_switch_pos(uint16_t buttons, unsigned sw);
/**
* Decode a switch position from a bitfield and state
*/
int decode_switch_pos_n(uint16_t buttons, unsigned sw);
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
struct vehicle_land_detected_s hil_land_detector;
@@ -192,6 +203,11 @@ private:
double _time_offset_avg_alpha;
uint64_t _time_offset;
static constexpr unsigned MOM_SWITCH_COUNT = 8;
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT];
uint16_t _mom_switch_state;
/* do not allow copying this class */
MavlinkReceiver(const MavlinkReceiver &);
MavlinkReceiver operator=(const MavlinkReceiver &);
src/modules/mavlink/module.mk
+2 -2
View File
@@ -53,6 +53,6 @@ MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed
EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed -DMAVLINK_COMM_NUM_BUFFERS=3
EXTRACFLAGS = -Wno-packed
EXTRACFLAGS = -Wno-packed -DMAVLINK_COMM_NUM_BUFFERS=3
src/modules/mc_att_control/mc_att_control_params.c
+4 -2
View File
@@ -232,14 +232,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f);
/**
* Max yaw rate
*
* Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
* Limit for yaw rate, has effect for large rotations in autonomous mode,
* to avoid large control output and mixer saturation. A value of significantly
* over 60 degrees per second can already lead to mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
/**
* Max acro roll rate
src/modules/mc_att_control_multiplatform/mc_att_control_params.h
+1 -1
View File
@@ -56,7 +56,7 @@
#define PARAM_MP_YAWRATE_I_DEFAULT 0.0f
#define PARAM_MP_YAWRATE_D_DEFAULT 0.0f
#define PARAM_MP_YAW_FF_DEFAULT 0.5f
#define PARAM_MP_YAWRATE_MAX_DEFAULT 120.0f
#define PARAM_MP_YAWRATE_MAX_DEFAULT 60.0f
#define PARAM_MP_ACRO_R_MAX_DEFAULT 35.0f
#define PARAM_MP_ACRO_P_MAX_DEFAULT 35.0f
#define PARAM_MP_ACRO_Y_MAX_DEFAULT 120.0f
src/modules/mc_pos_control/mc_pos_control_main.cpp
+22 -12
View File
@@ -80,6 +80,9 @@
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
#define MIN_DIST 0.01f
@@ -92,7 +95,7 @@
*/
extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
class MulticopterPositionControl
class MulticopterPositionControl : public control::SuperBlock
{
public:
/**
@@ -119,15 +122,15 @@ private:
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
int _local_pos_sub; /**< vehicle local position */
int _pos_sp_triplet_sub; /**< position setpoint triplet */
int _pos_sp_triplet_sub; /**< position setpoint triplet */
int _local_pos_sp_sub; /**< offboard local position setpoint */
int _global_vel_sp_sub; /**< offboard global velocity setpoint */
@@ -135,17 +138,19 @@ private:
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct actuator_armed_s _arming; /**< actuator arming status */
struct vehicle_local_position_s _local_pos; /**< vehicle local position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
control::BlockParamFloat _manual_thr_min;
control::BlockParamFloat _manual_thr_max;
struct {
param_t thr_min;
@@ -210,7 +215,7 @@ private:
/**
* Update our local parameter cache.
*/
int parameters_update(bool force);
int parameters_update(bool force);
/**
* Update control outputs
@@ -290,7 +295,7 @@ MulticopterPositionControl *g_control;
}
MulticopterPositionControl::MulticopterPositionControl() :
SuperBlock(NULL, "MPC"),
_task_should_exit(false),
_control_task(-1),
_mavlink_fd(-1),
@@ -310,7 +315,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_att_sp_pub(-1),
_local_pos_sp_pub(-1),
_global_vel_sp_pub(-1),
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_ref_alt(0.0f),
_ref_timestamp(0),
@@ -410,6 +416,10 @@ MulticopterPositionControl::parameters_update(bool force)
}
if (updated || force) {
/* update C++ param system */
updateParams();
/* update legacy C interface params */
param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
@@ -1413,11 +1423,11 @@ MulticopterPositionControl::task_main()
/* control throttle directly if no climb rate controller is active */
if (!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, _params.thr_max);
_att_sp.thrust = math::min(_manual.z, _manual_thr_max.get());
/* enforce minimum throttle if not landed */
if (!_vehicle_status.condition_landed) {
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
}
}
src/modules/mc_pos_control/mc_pos_control_params.c
+29 -4
View File
@@ -41,7 +41,32 @@
#include <systemlib/param/param.h>
/**
* Minimum thrust
* Minimum thrust in auto thrust control
*
* It's recommended to set it > 0 to avoid free fall with zero thrust.
*
* @min 0.05
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
/**
* Maximum thrust in auto thrust control
*
* Limit max allowed thrust. Setting a value of one can put
* the system into actuator saturation as no spread between
* the motors is possible any more. A value of 0.8 - 0.9
* is recommended.
*
* @min 0.0
* @max 0.95
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
/**
* Minimum manual thrust
*
* Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
*
@@ -49,10 +74,10 @@
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f);
/**
* Maximum thrust
* Maximum manual thrust
*
* Limit max allowed thrust. Setting a value of one can put
* the system into actuator saturation as no spread between
@@ -63,7 +88,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f);
/**
* Proportional gain for vertical position error
src/modules/navigator/mission.cpp
+44 -19
View File
@@ -70,8 +70,8 @@ Mission::Mission(Navigator *navigator, const char *name) :
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_altmode(this, "MIS_ALTMODE", false),
_param_yawmode(this, "MIS_YAWMODE", false),
_onboard_mission({0}),
_offboard_mission({0}),
_onboard_mission{},
_offboard_mission{},
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_need_takeoff(true),
@@ -110,32 +110,29 @@ Mission::on_inactive()
update_offboard_mission();
}
/* check if the home position became valid in the meantime */
if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
!_home_inited && _navigator->home_position_valid()) {
} else {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
/* load missions from storage */
mission_s mission_state;
_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
dm_lock(DM_KEY_MISSION_STATE);
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
/* read current state */
int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
_home_inited = true;
dm_unlock(DM_KEY_MISSION_STATE);
if (read_res == sizeof(mission_s)) {
_offboard_mission.dataman_id = mission_state.dataman_id;
_offboard_mission.count = mission_state.count;
_current_offboard_mission_index = mission_state.current_seq;
}
} else {
/* read mission topics on initialization */
_inited = true;
update_onboard_mission();
update_offboard_mission();
}
check_mission_valid();
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
@@ -151,6 +148,8 @@ Mission::on_activation()
void
Mission::on_active()
{
check_mission_valid();
/* check if anything has changed */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@@ -272,6 +271,8 @@ Mission::update_offboard_mission()
_offboard_mission.count = 0;
_offboard_mission.current_seq = 0;
_current_offboard_mission_index = 0;
warnx("mission check failed");
}
set_current_offboard_mission_item();
@@ -652,6 +653,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
return false;
}
@@ -795,3 +797,26 @@ Mission::set_mission_finished()
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
}
bool
Mission::check_mission_valid()
{
/* check if the home position became valid in the meantime */
if (!_home_inited && _navigator->home_position_valid()) {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
_home_inited = true;
}
return _navigator->get_mission_result()->valid;
}
src/modules/navigator/mission.h
+7 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -39,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Ban Siesta <bansiesta@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#ifndef NAVIGATOR_MISSION_H
@@ -165,6 +166,11 @@ private:
*/
void set_mission_finished();
/**
* Check wether a mission is ready to go
*/
bool check_mission_valid();
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;
src/modules/sdlog2/sdlog2.c
+18 -4
View File
@@ -156,7 +156,7 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
* @max 1
* @group SD Logging
*/
PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0);
PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1);
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
log_msgs_written++; \
@@ -344,7 +344,20 @@ int sdlog2_main(int argc, char *argv[])
3000,
sdlog2_thread_main,
(char * const *)argv);
exit(0);
/* wait for the task to launch */
unsigned const max_wait_us = 1000000;
unsigned const max_wait_steps = 2000;
unsigned i;
for (i = 0; i < max_wait_steps; i++) {
usleep(max_wait_us / max_wait_steps);
if (thread_running) {
break;
}
}
exit(!(i < max_wait_steps));
}
if (!strcmp(argv[1], "stop")) {
@@ -1228,8 +1241,6 @@ int sdlog2_thread_main(int argc, char *argv[])
/* close stdout */
close(1);
thread_running = true;
/* initialize thread synchronization */
pthread_mutex_init(&logbuffer_mutex, NULL);
pthread_cond_init(&logbuffer_cond, NULL);
@@ -1263,6 +1274,9 @@ int sdlog2_thread_main(int argc, char *argv[])
sdlog2_start_log();
}
/* running, report */
thread_running = true;
while (!main_thread_should_exit) {
usleep(sleep_delay);
src/modules/sensors/sensor_params.c
+5 -5
View File
@@ -680,7 +680,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
* This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @unit radians
* @unit degrees
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
@@ -691,7 +691,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
* This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @unit radians
* @unit degrees
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
@@ -702,7 +702,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
* This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
* to fine tune the board offset in the event of misalignment.
*
* @unit radians
* @unit degrees
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
@@ -2273,7 +2273,7 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0);
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* Set to 1000 for default or 900 to increase servo travel
* Set to 1000 for industry default or 900 to increase servo travel.
*
* @min 800
* @max 1400
@@ -2289,7 +2289,7 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000);
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* Set to 2000 for default or 2100 to increase servo travel
* Set to 2000 for industry default or 2100 to increase servo travel.
*
* @min 1600
* @max 2200
src/modules/systemlib/cpuload.c
-4
View File
@@ -78,8 +78,6 @@ void cpuload_initialize_once()
system_load.tasks[i].valid = false;
}
uint64_t now = hrt_absolute_time();
int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init"
#ifdef CONFIG_PAGING
@@ -94,7 +92,6 @@ void cpuload_initialize_once()
// perform static initialization of "system" threads
for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) {
system_load.tasks[system_load.total_count].start_time = now;
system_load.tasks[system_load.total_count].total_runtime = 0;
system_load.tasks[system_load.total_count].curr_start_time = 0;
system_load.tasks[system_load.total_count].tcb = sched_gettcb(
@@ -111,7 +108,6 @@ void sched_note_start(FAR struct tcb_s *tcb)
for (i = 1; i < CONFIG_MAX_TASKS; i++) {
if (!system_load.tasks[i].valid) {
/* slot is available */
system_load.tasks[i].start_time = hrt_absolute_time();
system_load.tasks[i].total_runtime = 0;
system_load.tasks[i].curr_start_time = 0;
system_load.tasks[i].tcb = tcb;
src/modules/systemlib/cpuload.h
-1
View File
@@ -42,7 +42,6 @@ __BEGIN_DECLS
struct system_load_taskinfo_s {
uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
uint64_t curr_start_time; ///< Start time of the current scheduling slot
uint64_t start_time; ///< FIRST start time of task
FAR struct tcb_s *tcb; ///<
bool valid; ///< Task is currently active / valid
};
src/modules/vtol_att_control/module.mk
+3 -1
View File
@@ -42,6 +42,8 @@ SRCS = vtol_att_control_main.cpp \
tiltrotor_params.c \
tiltrotor.cpp \
vtol_type.cpp \
tailsitter.cpp
tailsitter.cpp \
standard_params.c \
standard.cpp
EXTRACXXFLAGS = -Wno-write-strings
src/modules/vtol_att_control/standard.cpp
+296
View File
@@ -0,0 +1,296 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file standard.cpp
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "standard.h"
#include "vtol_att_control_main.h"
Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc),
_flag_enable_mc_motors(true),
_pusher_throttle(0.0f),
_mc_att_ctl_weight(1.0f),
_airspeed_trans_blend_margin(0.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
}
Standard::~Standard()
{
}
int
Standard::parameters_update()
{
float v;
/* duration of a forwards transition to fw mode */
param_get(_params_handles_standard.front_trans_dur, &v);
_params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* duration of a back transition to mc mode */
param_get(_params_handles_standard.back_trans_dur, &v);
_params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* target throttle value for pusher motor during the transition to fw mode */
param_get(_params_handles_standard.pusher_trans, &v);
_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);
/* airspeed at which it we should switch to fw mode */
param_get(_params_handles_standard.airspeed_trans, &v);
_params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f);
/* airspeed at which we start blending mc/fw controls */
param_get(_params_handles_standard.airspeed_blend, &v);
_params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f);
_airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend;
return OK;
}
void Standard::update_vtol_state()
{
parameters_update();
/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
* forward speed. After the vehicle has picked up enough speed the rotors shutdown.
* For the back transition the pusher motor is immediately stopped and rotors reactivated.
*/
if (_manual_control_sp->aux1 < 0.0f) {
// the transition to fw mode switch is off
if (_vtol_schedule.flight_mode == MC_MODE) {
// in mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_att_ctl_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// transition to mc mode
_vtol_schedule.flight_mode = TRANSITION_TO_MC;
_flag_enable_mc_motors = true;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// failsafe back to mc mode
_vtol_schedule.flight_mode = MC_MODE;
_mc_att_ctl_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// keep transitioning to mc mode
_vtol_schedule.flight_mode = MC_MODE;
}
// the pusher motor should never be powered when in or transitioning to mc mode
_pusher_throttle = 0.0f;
} else {
// the transition to fw mode switch is on
if (_vtol_schedule.flight_mode == MC_MODE) {
// start transition to fw mode
_vtol_schedule.flight_mode = TRANSITION_TO_FW;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == FW_MODE) {
// in fw mode
_vtol_schedule.flight_mode = FW_MODE;
_mc_att_ctl_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) {
_vtol_schedule.flight_mode = FW_MODE;
// we can turn off the multirotor motors now
_flag_enable_mc_motors = false;
// don't set pusher throttle here as it's being ramped up elsewhere
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transitioning to mc mode & transition switch on - failsafe back into fw mode
_vtol_schedule.flight_mode = FW_MODE;
}
}
// map specific control phases to simple control modes
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_TO_FW:
case TRANSITION_TO_MC:
_vtol_mode = TRANSITION;
break;
}
}
void Standard::update_transition_state()
{
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.front_trans_dur <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params_standard.pusher_trans;
} else if (_pusher_throttle <= _params_standard.pusher_trans) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params_standard.pusher_trans *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls if a blending airspeed has been provided
if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
_mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
} else {
// at low speeds give full weight to mc
_mc_att_ctl_weight = 1.0f;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_dur > 0.0f) {
_mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
} else {
_mc_att_ctl_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
}
_mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f);
}
void Standard::update_mc_state()
{
// do nothing
}
void Standard::process_mc_data()
{
fill_att_control_output();
}
void Standard::process_fw_data()
{
fill_att_control_output();
}
void Standard::update_fw_state()
{
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (!_flag_enable_mc_motors) {
set_max_mc(950);
set_idle_fw(); // force them to stop, not just idle
_flag_enable_mc_motors = true;
}
}
void Standard::update_external_state()
{
}
/**
* Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine
* what proportion of control should be applied to each of the control groups (mc and fw).
*/
void Standard::fill_att_control_output()
{
/* multirotor controls */
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw
_actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle
/* fixed wing controls */
const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight;
_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch
_actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw
// set the fixed wing throttle control
if (_vtol_schedule.flight_mode == FW_MODE) {
// take the throttle value commanded by the fw controller
_actuators_out_1->control[3] = _actuators_fw_in->control[3];
} else {
// otherwise we may be ramping up the throttle during the transition to fw mode
_actuators_out_1->control[3] = _pusher_throttle;
}
}
/**
* Disable all multirotor motors when in fw mode.
*/
void
Standard::set_max_mc(unsigned pwm_value)
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
close(fd);
}
src/modules/vtol_att_control/standard.h
+107
View File
@@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file standard.h
* VTOL with fixed multirotor motor configurations (such as quad) and a pusher
* (or puller aka tractor) motor for forward flight.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef STANDARD_H
#define STANDARD_H
#include "vtol_type.h"
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
class Standard : public VtolType
{
public:
Standard(VtolAttitudeControl * _att_controller);
~Standard();
void update_vtol_state();
void update_mc_state();
void process_mc_data();
void update_fw_state();
void process_fw_data();
void update_transition_state();
void update_external_state();
private:
struct {
float front_trans_dur;
float back_trans_dur;
float pusher_trans;
float airspeed_blend;
float airspeed_trans;
} _params_standard;
struct {
param_t front_trans_dur;
param_t back_trans_dur;
param_t pusher_trans;
param_t airspeed_blend;
param_t airspeed_trans;
} _params_handles_standard;
enum vtol_mode {
MC_MODE = 0,
TRANSITION_TO_FW,
TRANSITION_TO_MC,
FW_MODE
};
struct {
vtol_mode flight_mode; // indicates in which mode the vehicle is in
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
}_vtol_schedule;
bool _flag_enable_mc_motors;
float _pusher_throttle;
float _mc_att_ctl_weight; // the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning
float _airspeed_trans_blend_margin;
void fill_att_control_output();
void set_max_mc(unsigned pwm_value);
int parameters_update();
};
#endif
src/modules/vtol_att_control/standard_params.c
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file standard_params.c
* Parameters for the standard VTOL controller.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*/
#include <systemlib/param/param.h>
/**
* Target throttle value for pusher/puller motor during the transition to fw mode
*
* @min 0.0
* @max 1.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f);
src/modules/vtol_att_control/tiltrotor.cpp
+25 -33
View File
@@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state()
void Tiltrotor::process_mc_data()
{
fill_mc_att_control_output();
fill_att_control_output();
}
void Tiltrotor::update_fw_state()
@@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data()
void Tiltrotor::process_fw_data()
{
fill_fw_att_control_output();
fill_att_control_output();
}
void Tiltrotor::update_transition_state()
{
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f);
if (_params_tiltrotor.front_trans_dur <= 0.0f) {
_tilt_control = _params_tiltrotor.tilt_transition;
} else if (_tilt_control <= _params_tiltrotor.tilt_transition) {
_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) *
(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls
if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) {
if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) {
_roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START);
} else {
// at low speeds give full weight to mc
@@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state()
_roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f);
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
_tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f);
_tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) *
(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f);
_roll_weight_mc = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
// tilt rotors forward up to certain angle
float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f);
float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress;
_tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress;
}
_roll_weight_mc = progress;
@@ -263,38 +267,26 @@ void Tiltrotor::update_external_state()
}
/**
* Prepare message to acutators with data from mc attitude controller.
* Prepare message to acutators with data from the attitude controllers.
*/
void Tiltrotor::fill_mc_att_control_output()
void Tiltrotor::fill_att_control_output()
{
_actuators_out_0->control[0] = _actuators_mc_in->control[0];
_actuators_out_0->control[1] = _actuators_mc_in->control[1];
_actuators_out_0->control[2] = _actuators_mc_in->control[2];
_actuators_out_0->control[3] = _actuators_mc_in->control[3];
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_1->control[3] = _actuators_fw_in->control[3]; // fw throttle
} else {
_actuators_out_0->control[3] = _actuators_mc_in->control[3]; // mc throttle
}
_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon
_actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control
}
/**
* Prepare message to acutators with data from fw attitude controller.
*/
void Tiltrotor::fill_fw_att_control_output()
{
/*For the first test in fw mode, only use engines for thrust!!!*/
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;
_actuators_out_0->control[3] = _actuators_fw_in->control[3];
/*controls for the elevons */
_actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon
_actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon
// unused now but still logged
_actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw
_actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle
_actuators_out_1->control[4] = _tilt_control;
_actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw
}
/**
src/modules/vtol_att_control/tiltrotor.h
+1 -2
View File
@@ -99,8 +99,7 @@ private:
float _tilt_control;
float _roll_weight_mc;
void fill_mc_att_control_output();
void fill_fw_att_control_output();
void fill_att_control_output();
void set_max_mc();
void set_max_fw(unsigned pwm_value);
src/modules/vtol_att_control/tiltrotor_params.c
+3 -36
View File
@@ -39,28 +39,6 @@
*/
#include <systemlib/param/param.h>
/**
* Duration of a front transition
*
* Time in seconds used for a transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f);
/**
* Duration of a back transition
*
* Time in seconds used for a back transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
/**
* Position of tilt servo in mc mode
@@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f);
/**
* Position of tilt servo in transition mode
@@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f);
/**
* Position of tilt servo in fw mode
@@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f);
/**
* Transition airspeed
*
* Airspeed at which we can switch to fw mode
*
* @min 0.0
* @max 20
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f);
PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);
src/modules/vtol_att_control/vtol_att_control_main.cpp
+6
View File
@@ -122,6 +122,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
} else if (_params.vtol_type == 1) {
_tiltrotor = new Tiltrotor(this);
_vtol_type = _tiltrotor;
} else if (_params.vtol_type == 2) {
_standard = new Standard(this);
_vtol_type = _standard;
} else {
_task_should_exit = true;
}
@@ -530,6 +533,9 @@ void VtolAttitudeControl::task_main()
// update transition state if got any new data
if (got_new_data) {
_vtol_type->update_transition_state();
_vtol_type->process_mc_data();
fill_mc_att_rates_sp();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
src/modules/vtol_att_control/vtol_att_control_main.h
+2
View File
@@ -84,6 +84,7 @@
#include "tiltrotor.h"
#include "tailsitter.h"
#include "standard.h"
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
@@ -188,6 +189,7 @@ private:
VtolType * _vtol_type; // base class for different vtol types
Tiltrotor * _tiltrotor; // tailsitter vtol type
Tailsitter * _tailsitter; // tiltrotor vtol type
Standard * _standard; // standard vtol type
//*****************Member functions***********************************************************************
src/modules/vtol_att_control/vtol_att_control_params.c
+46 -2
View File
@@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f);
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1)
* VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
*
* @min 0
* @max 1
* @max 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_TYPE, 0);
@@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0);
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0);
/**
* Duration of a front transition
*
* Time in seconds used for a transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f);
/**
* Duration of a back transition
*
* Time in seconds used for a back transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f);
/**
* Transition blending airspeed
*
* Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable.
*
* @min 0.0
* @max 20.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f);
/**
* Transition airspeed
*
* Airspeed at which we can switch to fw mode
*
* @min 1.0
* @max 20
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
src/modules/vtol_att_control/vtol_type.h
+2 -2
View File
@@ -38,8 +38,8 @@
*
*/
#ifndef VTOL_YYPE_H
#define VTOL_YYPE_H
#ifndef VTOL_TYPE_H
#define VTOL_TYPE_H
struct Params {
int idle_pwm_mc; // pwm value for idle in mc mode