fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 06:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into uavcan_footprint_reduction

Browse Source
This commit is contained in:
Pavel Kirienko
2015-11-12 14:49:02 +03:00
parent 96a12a6027 de39e7de62
commit b95609834c
141 changed files with 2592 additions and 2178 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+126 -13
View File
@@ -54,8 +54,11 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -117,22 +120,28 @@ private:
int _sensors_sub = -1;
int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
struct {
param_t w_acc;
param_t w_mag;
param_t w_ext_hdg;
param_t w_gyro_bias;
param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
param_t vibe_thresh;
param_t ext_hdg_mode;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
float _w_mag = 0.0f;
float _w_ext_hdg = 0.0f;
float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f;
bool _mag_decl_auto = false;
@@ -140,11 +149,18 @@ private:
float _bias_max = 0.0f;
float _vibration_warning_threshold = 1.0f;
hrt_abstime _vibration_warning_timestamp = 0;
int _ext_hdg_mode = 0;
Vector<3> _gyro;
Vector<3> _accel;
Vector<3> _mag;
vision_position_estimate_s _vision = {};
Vector<3> _vision_hdg;
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
@@ -168,6 +184,7 @@ private:
bool _data_good = false;
bool _failsafe = false;
bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1;
@@ -190,18 +207,20 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_voter_mag(3),
_lp_roll_rate(250.0f, 18.0f),
_lp_pitch_rate(250.0f, 18.0f),
_lp_yaw_rate(250, 10.0f)
_lp_yaw_rate(250.0f, 10.0f)
{
_voter_mag.set_timeout(200000);
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
}
/**
@@ -269,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
void AttitudeEstimatorQ::task_main()
{
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -374,6 +397,47 @@ void AttitudeEstimatorQ::task_main()
}
}
// Update vision and motion capture heading
bool vision_updated = false;
orb_check(_vision_sub, &vision_updated);
bool mocap_updated = false;
orb_check(_mocap_sub, &mocap_updated);
if (vision_updated) {
orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
math::Quaternion q(_vision.q);
math::Matrix<3, 3> Rvis = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transposed() * v;
}
if (mocap_updated) {
orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
math::Quaternion q(_mocap.q);
math::Matrix<3, 3> Rmoc = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transposed() * v;
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
} else if (_ext_hdg_mode == 2) {
_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
}
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
@@ -431,12 +495,9 @@ void AttitudeEstimatorQ::task_main()
att.pitch = euler(1);
att.yaw = euler(2);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _lp_yaw_rate.apply(_rates(2));
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -461,6 +522,34 @@ void AttitudeEstimatorQ::task_main()
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
struct control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* Attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
/* Attitude rates for control state */
ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
}
}
}
@@ -478,6 +567,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
param_get(_params_handles.w_acc, &_w_accel);
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -490,6 +580,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
}
}
@@ -545,12 +636,34 @@ bool AttitudeEstimatorQ::update(float dt)
// Angular rate of correction
Vector<3> corr;
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
if (_ext_hdg_mode > 0 && _ext_hdg_good) {
if (_ext_hdg_mode == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
}
if (_ext_hdg_mode == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
}
}
if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+20
View File
@@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
*/
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
/**
* Complimentary filter external heading weight
*
* @group Attitude Q estimator
* @min 0
* @max 1
*/
PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
/**
* Complimentary filter gyroscope bias weight
*
@@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
*/
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
/**
* External heading usage mode (from Motion capture/Vision)
* Set to 1 to use heading estimate from vision.
* Set to 2 to use heading from motion capture.
*
* @group Attitude Q estimator
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
/**
* Enable acceleration compensation based on GPS
* velocity.
src/modules/commander/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
set(MODULE_CFLAGS -Os)
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450)
endif()
px4_add_module(
MODULE modules__commander
src/modules/commander/commander.cpp
+249 -65
View File
@@ -72,6 +72,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
@@ -92,6 +93,7 @@
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/input_rc.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -149,7 +151,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 10000000
#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000
#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000
#define HIL_ID_MIN 1000
#define HIL_ID_MAX 1999
@@ -178,11 +180,13 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */
static volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
static bool _usb_telemetry_active = false;
static hrt_abstime commander_boot_timestamp = 0;
static unsigned int leds_counter;
/* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0;
static uint64_t _inair_last_time = 0;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
@@ -195,15 +199,9 @@ static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home;
static unsigned _last_mission_instance = 0;
static uint64_t last_manual_input = 0;
static switch_pos_t last_offboard_switch = 0;
static switch_pos_t last_return_switch = 0;
static switch_pos_t last_mode_switch = 0;
static switch_pos_t last_acro_switch = 0;
static switch_pos_t last_posctl_switch = 0;
static switch_pos_t last_loiter_switch = 0;
static manual_control_setpoint_s _last_sp_man;
struct vtol_vehicle_status_s vtol_status;
static struct vtol_vehicle_status_s vtol_status;
/**
* The daemon app only briefly exists to start
@@ -227,7 +225,7 @@ void usage(const char *reason);
*/
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub);
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub);
/**
* Mainloop of commander.
@@ -262,7 +260,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -300,7 +299,7 @@ int commander_main(int argc, char *argv[])
daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
3400,
3500,
commander_thread_main,
(char * const *)&argv[0]);
@@ -405,11 +404,11 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason)
{
if (reason) {
PX4_INFO("%s\n", reason);
if (reason && *reason > 0) {
PX4_INFO("%s", reason);
}
PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n");
PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n");
}
void print_status()
@@ -499,7 +498,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub)
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -533,7 +532,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude);
}
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
@@ -842,7 +841,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude)
{
//Need global position fix to be able to set home
if (!status.condition_global_position_valid) {
@@ -864,6 +864,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
home.y = localPosition.y;
home.z = localPosition.z;
home.yaw = attitude.yaw;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
@@ -890,6 +892,7 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = false;
bool arm_tune_played = false;
bool was_landed = true;
bool was_armed = false;
bool startup_in_hil = false;
@@ -925,6 +928,9 @@ int commander_thread_main(int argc, char *argv[])
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");
param_t _param_geofence_action = param_find("GF_ACTION");
param_t _param_disarm_land = param_find("COM_DISARM_LAND");
param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW");
// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
@@ -949,6 +955,7 @@ int commander_thread_main(int argc, char *argv[])
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
@@ -1037,17 +1044,15 @@ int commander_thread_main(int argc, char *argv[])
px4_task_exit(ERROR);
}
/* armed topic */
orb_advert_t armed_pub;
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* armed topic */
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
/* vehicle control mode topic */
memset(&control_mode, 0, sizeof(control_mode));
orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
/* home position */
orb_advert_t home_pub = nullptr;
memset(&_home, 0, sizeof(_home));
@@ -1146,13 +1151,15 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
struct vehicle_local_position_s local_position;
memset(&local_position, 0, sizeof(local_position));
struct vehicle_local_position_s local_position = {};
/* Subscribe to attitude data */
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s attitude = {};
/* Subscribe to land detector */
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
struct vehicle_land_detected_s land_detector;
memset(&land_detector, 0, sizeof(land_detector));
struct vehicle_land_detected_s land_detector = {};
/*
* The home position is set based on GPS only, to prevent a dependency between
@@ -1263,6 +1270,8 @@ int commander_thread_main(int argc, char *argv[])
float rc_loss_timeout = 0.5;
int32_t datalink_regain_timeout = 0;
int32_t geofence_action = 0;
/* Thresholds for engine failure detection */
int32_t ef_throttle_thres = 1.0f;
int32_t ef_current2throttle_thres = 0.0f;
@@ -1271,6 +1280,9 @@ int commander_thread_main(int argc, char *argv[])
int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */
int32_t disarm_when_landed = 0;
int32_t map_mode_sw = 0;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
bool main_state_changed = false;
@@ -1335,6 +1347,16 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
// check if main mode switch has been assigned and if so run preflight checks in order
// to update status.condition_sensors_initialised
int32_t map_mode_sw_new;
param_get(_param_map_mode_sw, &map_mode_sw_new);
if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
}
/* re-check RC calibration */
rc_calibration_check(mavlink_fd);
}
@@ -1349,6 +1371,9 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
param_get(_param_geofence_action, &geofence_action);
param_get(_param_disarm_land, &disarm_when_landed);
param_get(_param_map_mode_sw, &map_mode_sw);
/* Autostart id */
param_get(_param_autostart_id, &autostart_id);
@@ -1439,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
_usb_telemetry_active = true;
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
}
@@ -1494,7 +1524,20 @@ int commander_thread_main(int argc, char *argv[])
/* copy avionics voltage */
status.avionics_power_rail_voltage = system_power.voltage5V_v;
status.usb_connected = system_power.usb_connected;
/* if the USB hardware connection went away, reboot */
if (status.usb_connected && !system_power.usb_connected) {
/*
* apparently the USB cable went away but we are still powered,
* so lets reset to a classic non-usb state.
*/
mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.")
usleep(400000);
px4_systemreset(false);
}
/* finally judge the USB connected state based on software detection */
status.usb_connected = _usb_telemetry_active;
}
}
@@ -1579,6 +1622,14 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* update attitude estimate */
orb_check(attitude_sub, &updated);
if (updated) {
/* position changed */
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
}
//update condition_global_position_valid
//Global positions are only published by the estimators if they are valid
if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
@@ -1623,12 +1674,13 @@ int commander_thread_main(int argc, char *argv[])
&(status.condition_local_altitude_valid), &status_changed);
/* Update land detector */
static bool check_for_disarming = false;
orb_check(land_detector_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
}
if (updated && status.condition_local_altitude_valid) {
if ((updated && status.condition_local_altitude_valid) || check_for_disarming) {
if (status.condition_landed != land_detector.landed) {
status.condition_landed = land_detector.landed;
status_changed = true;
@@ -1640,6 +1692,24 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
}
}
if (disarm_when_landed > 0) {
if (land_detector.landed) {
if (!check_for_disarming && _inair_last_time > 0) {
_inair_last_time = land_detector.timestamp;
check_for_disarming = true;
}
if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING");
arm_disarm(false, mavlink_fd, "auto disarm on land");
_inair_last_time = 0;
check_for_disarming = false;
}
} else {
_inair_last_time = land_detector.timestamp;
}
}
}
/* update battery status */
@@ -1838,24 +1908,106 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
}
/* Check for geofence violation */
if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
//XXX: make this configurable to select different actions (e.g. navigation modes)
/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
// Geofence actions
if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) {
static bool geofence_loiter_on = false;
static bool geofence_rtl_on = false;
static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX;
// check for geofence violation
if (geofence_result.geofence_violated) {
static hrt_abstime last_geofence_violation = 0;
const hrt_abstime geofence_violation_action_interval = 10000000; // 10 seconds
if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) {
last_geofence_violation = hrt_absolute_time();
switch (geofence_result.geofence_action) {
case (geofence_result_s::GF_ACTION_NONE) : {
// do nothing
break;
}
case (geofence_result_s::GF_ACTION_WARN) : {
// do nothing, mavlink critical messages are sent by navigator
break;
}
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) {
geofence_loiter_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) {
geofence_rtl_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_TERMINATE) : {
warnx("Flight termination because of geofence");
mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
armed.force_failsafe = true;
status_changed = true;
break;
}
}
}
}
// reset if no longer in LOITER or if manually switched to LOITER
geofence_loiter_on = geofence_loiter_on
&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER)
&& (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
// reset if no longer in RTL or if manually switched to RTL
geofence_rtl_on = geofence_rtl_on
&& (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL)
&& (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
if (!geofence_loiter_on && !geofence_rtl_on) {
// store the last good main_state when not in a geofence triggered action (LOITER or RTL)
geofence_main_state_before_violation = status.main_state;
}
// revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST
if ((geofence_loiter_on || geofence_rtl_on) &&
(geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO ||
geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) {
// transition to previous state if sticks are increased
const float min_stick_change = 0.2;
if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
main_state_transition(&status, geofence_main_state_before_violation);
}
}
}
/* Check for mission flight termination */
if (armed.armed && mission_result.flight_termination) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of navigator request or geofence");
mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
warnx("Flight termination because of navigator request");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "Flight termination active");
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
}
/* Only evaluate mission state if home is set,
* this prevents false positives for the mission
@@ -1901,13 +2053,14 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
/* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
@@ -1940,10 +2093,15 @@ int commander_thread_main(int argc, char *argv[])
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else if (!status.condition_home_position_valid &&
geofence_action == geofence_result_s::GF_ACTION_RTL) {
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@@ -2102,7 +2260,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) {
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) {
status_changed = true;
}
}
@@ -2110,10 +2268,11 @@ int commander_thread_main(int argc, char *argv[])
/* Check for failure combinations which lead to flight termination */
if (armed.armed) {
/* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode)
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE &&
status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
@@ -2138,6 +2297,7 @@ int commander_thread_main(int argc, char *argv[])
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
@@ -2164,14 +2324,18 @@ int commander_thread_main(int argc, char *argv[])
/* First time home position update - but only if disarmed */
if (!status.condition_home_position_valid && !armed.armed) {
commander_set_home_position(home_pub, _home, local_position, global_position);
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
commander_set_home_position(home_pub, _home, local_position, global_position);
/* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */
else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) &&
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
was_landed = status.condition_landed;
was_armed = armed.armed;
/* print new state */
if (arming_state_changed) {
status_changed = true;
@@ -2179,8 +2343,6 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = false;
}
was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.finished,
@@ -2374,7 +2536,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
rgbled_set_color(RGBLED_COLOR_RED);
} else {
if (status_local->condition_home_position_valid) {
if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN);
} else {
@@ -2425,31 +2587,33 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
/* if offboard is set allready by a mavlink command, abort */
/* if offboard is set already by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
}
/* manual setpoint has not updated, do not re-evaluate it */
if ((last_manual_input == sp_man->timestamp) ||
((last_offboard_switch == sp_man->offboard_switch) &&
(last_return_switch == sp_man->return_switch) &&
(last_mode_switch == sp_man->mode_switch) &&
(last_acro_switch == sp_man->acro_switch) &&
(last_posctl_switch == sp_man->posctl_switch) &&
(last_loiter_switch == sp_man->loiter_switch))) {
if ((_last_sp_man.timestamp == sp_man->timestamp) ||
((_last_sp_man.offboard_switch == sp_man->offboard_switch) &&
(_last_sp_man.return_switch == sp_man->return_switch) &&
(_last_sp_man.mode_switch == sp_man->mode_switch) &&
(_last_sp_man.acro_switch == sp_man->acro_switch) &&
(_last_sp_man.rattitude_switch == sp_man->rattitude_switch) &&
(_last_sp_man.posctl_switch == sp_man->posctl_switch) &&
(_last_sp_man.loiter_switch == sp_man->loiter_switch))) {
// update these fields for the geofence system
_last_sp_man.timestamp = sp_man->timestamp;
_last_sp_man.x = sp_man->x;
_last_sp_man.y = sp_man->y;
_last_sp_man.z = sp_man->z;
_last_sp_man.r = sp_man->r;
/* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED;
}
last_manual_input = sp_man->timestamp;
last_offboard_switch = sp_man->offboard_switch;
last_return_switch = sp_man->return_switch;
last_mode_switch = sp_man->mode_switch;
last_acro_switch = sp_man->acro_switch;
last_posctl_switch = sp_man->posctl_switch;
last_loiter_switch = sp_man->loiter_switch;
_last_sp_man = *sp_man;
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
@@ -2502,7 +2666,16 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
} else {
}
else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
}else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
}
@@ -2625,6 +2798,18 @@ set_control_mode()
control_mode.flag_external_manual_override_ok = false;
break;
case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
@@ -2873,11 +3058,10 @@ void *commander_low_prio_loop(void *arg)
if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) {
int ret = param_save_default();
if (ret == OK) {
mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
} else {
mavlink_and_console_log_critical(mavlink_fd, "settings save error");
warnx("settings saved.");
}
need_param_autosave = false;
src/modules/commander/commander_params.c
+13
View File
@@ -274,3 +274,16 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
* @max 2
*/
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
/**
* Time-out for auto disarm after landing
*
* A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be
* automatically disarmed in case a landing situation has been detected during this period.
* A value of zero means that automatic disarming is disabled.
*
* @group Commander
* @min 0
*/
PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
src/modules/commander/state_machine_helper.cpp
+14 -1
View File
@@ -304,6 +304,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB:
ret = TRANSITION_CHANGED;
break;
@@ -528,6 +529,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_RATTITUDE:
case vehicle_status_s::MAIN_STATE_STAB:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
@@ -555,6 +557,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case vehicle_status_s::MAIN_STATE_RATTITUDE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
break;
case vehicle_status_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
@@ -737,5 +743,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
checkAirspeed = true;
}
return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
if (status->usb_connected) {
prearm_ok = false;
mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
}
return !prearm_ok;
}
src/modules/controllib/CMakeLists.txt
+2
View File
@@ -32,9 +32,11 @@
############################################################################
px4_add_module(
MODULE modules__controllib
MAIN controllib_test
COMPILE_FLAGS
-Os
SRCS
controllib_test_main.cpp
test_params.c
block/Block.cpp
block/BlockParam.cpp
src/modules/controllib/blocks.cpp
+124 -112
View File
@@ -43,6 +43,8 @@
#include "blocks.hpp"
#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
namespace control
{
@@ -62,7 +64,8 @@ int basicBlocksTest()
blockPIDTest();
blockOutputTest();
blockRandUniformTest();
blockRandGaussTest();
// known failures
// blockRandGaussTest();
return 0;
}
@@ -83,13 +86,13 @@ int blockLimitTest()
printf("Test BlockLimit\t\t\t: ");
BlockLimit limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(-1.0f, limit.getMin()));
ASSERT(equal(0.0f, limit.getDt()));
ASSERT_CL(equal(1.0f, limit.getMax()));
ASSERT_CL(equal(-1.0f, limit.getMin()));
ASSERT_CL(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
ASSERT_CL(equal(1.0f, limit.update(2.0f)));
ASSERT_CL(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
@@ -111,12 +114,12 @@ int blockLimitSymTest()
printf("Test BlockLimitSym\t\t: ");
BlockLimitSym limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(0.0f, limit.getDt()));
ASSERT_CL(equal(1.0f, limit.getMax()));
ASSERT_CL(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
ASSERT_CL(equal(1.0f, limit.update(2.0f)));
ASSERT_CL(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
@@ -138,25 +141,25 @@ int blockLowPassTest()
printf("Test BlockLowPass\t\t: ");
BlockLowPass lowPass(NULL, "TEST_LP");
// test initial state
ASSERT(equal(10.0f, lowPass.getFCut()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
ASSERT_CL(equal(10.0f, lowPass.getFCut()));
ASSERT_CL(equal(0.0f, lowPass.getState()));
ASSERT_CL(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
ASSERT_CL(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
ASSERT_CL(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
ASSERT_CL(equal(2.0f, lowPass.getState()));
ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
@@ -175,28 +178,28 @@ int blockHighPassTest()
printf("Test BlockHighPass\t\t: ");
BlockHighPass highPass(NULL, "TEST_HP");
// test initial state
ASSERT(equal(10.0f, highPass.getFCut()));
ASSERT(equal(0.0f, highPass.getU()));
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.getDt()));
ASSERT_CL(equal(10.0f, highPass.getFCut()));
ASSERT_CL(equal(0.0f, highPass.getU()));
ASSERT_CL(equal(0.0f, highPass.getY()));
ASSERT_CL(equal(0.0f, highPass.getDt()));
// set dt
highPass.setDt(0.1f);
ASSERT(equal(0.1f, highPass.getDt()));
ASSERT_CL(equal(0.1f, highPass.getDt()));
// set state
highPass.setU(1.0f);
ASSERT(equal(1.0f, highPass.getU()));
ASSERT_CL(equal(1.0f, highPass.getU()));
highPass.setY(1.0f);
ASSERT(equal(1.0f, highPass.getY()));
ASSERT_CL(equal(1.0f, highPass.getY()));
// test update
ASSERT(equal(0.2746051f, highPass.update(2.0f)));
ASSERT_CL(equal(0.2746051f, highPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
highPass.update(2.0f);
}
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.update(2.0f)));
ASSERT_CL(equal(0.0f, highPass.getY()));
ASSERT_CL(equal(0.0f, highPass.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -220,25 +223,25 @@ int blockLowPass2Test()
printf("Test BlockLowPass2\t\t: ");
BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
// test initial state
ASSERT(equal(10.0f, lowPass.getFCutParam()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
ASSERT_CL(equal(10.0f, lowPass.getFCutParam()));
ASSERT_CL(equal(0.0f, lowPass.getState()));
ASSERT_CL(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
ASSERT_CL(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
ASSERT_CL(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
ASSERT_CL(equal(2.0f, lowPass.getState()));
ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
@@ -255,34 +258,34 @@ int blockIntegralTest()
printf("Test BlockIntegral\t\t: ");
BlockIntegral integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
ASSERT_CL(equal(1.0f, integral.getMax()));
ASSERT_CL(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
ASSERT_CL(equal(0.1f, integral.getDt()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
ASSERT_CL(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
ASSERT_CL(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
ASSERT_CL(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setY(0.1f);
ASSERT(equal(0.2f, integral.update(1.0)));
ASSERT(equal(0.2f, integral.getY()));
ASSERT_CL(equal(0.2f, integral.update(1.0)));
ASSERT_CL(equal(0.2f, integral.getY()));
printf("PASS\n");
return 0;
}
@@ -301,40 +304,40 @@ int blockIntegralTrapTest()
printf("Test BlockIntegralTrap\t\t: ");
BlockIntegralTrap integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
ASSERT_CL(equal(1.0f, integral.getMax()));
ASSERT_CL(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
ASSERT_CL(equal(0.1f, integral.getDt()));
// set U
integral.setU(1.0f);
ASSERT(equal(1.0f, integral.getU()));
ASSERT_CL(equal(1.0f, integral.getU()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
ASSERT_CL(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
ASSERT_CL(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setU(-1.0f);
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
ASSERT_CL(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setU(2.0f);
integral.setY(0.1f);
ASSERT(equal(0.25f, integral.update(1.0)));
ASSERT(equal(0.25f, integral.getY()));
ASSERT(equal(1.0f, integral.getU()));
ASSERT_CL(equal(0.25f, integral.update(1.0)));
ASSERT_CL(equal(0.25f, integral.getY()));
ASSERT_CL(equal(1.0f, integral.getU()));
printf("PASS\n");
return 0;
}
@@ -352,6 +355,7 @@ float BlockDerivative::update(float input)
// and so we use the assumption the
// input value is not changing much,
// which is the best we can do here.
_lowPass.update(0.0f);
output = 0.0f;
_initialized = true;
}
@@ -365,17 +369,20 @@ int blockDerivativeTest()
printf("Test BlockDerivative\t\t: ");
BlockDerivative derivative(NULL, "TEST_D");
// test initial state
ASSERT(equal(0.0f, derivative.getU()));
ASSERT(equal(10.0f, derivative.getLP()));
ASSERT_CL(equal(0.0f, derivative.getU()));
ASSERT_CL(equal(10.0f, derivative.getLP()));
// set dt
derivative.setDt(0.1f);
ASSERT(equal(0.1f, derivative.getDt()));
ASSERT_CL(equal(0.1f, derivative.getDt()));
// set U
derivative.setU(1.0f);
ASSERT(equal(1.0f, derivative.getU()));
ASSERT_CL(equal(1.0f, derivative.getU()));
// perform one update so initialized is set
derivative.update(1.0);
ASSERT_CL(equal(1.0f, derivative.getU()));
// test update
ASSERT(equal(8.6269744f, derivative.update(2.0f)));
ASSERT(equal(2.0f, derivative.getU()));
ASSERT_CL(equal(8.6269744f, derivative.update(2.0f)));
ASSERT_CL(equal(2.0f, derivative.getU()));
printf("PASS\n");
return 0;
}
@@ -385,13 +392,13 @@ int blockPTest()
printf("Test BlockP\t\t\t: ");
BlockP blockP(NULL, "TEST_P");
// test initial state
ASSERT(equal(0.2f, blockP.getKP()));
ASSERT(equal(0.0f, blockP.getDt()));
ASSERT_CL(equal(0.2f, blockP.getKP()));
ASSERT_CL(equal(0.0f, blockP.getDt()));
// set dt
blockP.setDt(0.1f);
ASSERT(equal(0.1f, blockP.getDt()));
ASSERT_CL(equal(0.1f, blockP.getDt()));
// test update
ASSERT(equal(0.4f, blockP.update(2.0f)));
ASSERT_CL(equal(0.4f, blockP.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -401,19 +408,19 @@ int blockPITest()
printf("Test BlockPI\t\t\t: ");
BlockPI blockPI(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPI.getKP()));
ASSERT(equal(0.1f, blockPI.getKI()));
ASSERT(equal(0.0f, blockPI.getDt()));
ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
ASSERT_CL(equal(0.2f, blockPI.getKP()));
ASSERT_CL(equal(0.1f, blockPI.getKI()));
ASSERT_CL(equal(0.0f, blockPI.getDt()));
ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax()));
// set dt
blockPI.setDt(0.1f);
ASSERT(equal(0.1f, blockPI.getDt()));
ASSERT_CL(equal(0.1f, blockPI.getDt()));
// set integral state
blockPI.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
ASSERT(equal(0.43f, blockPI.update(2.0f)));
ASSERT_CL(equal(0.43f, blockPI.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -423,19 +430,22 @@ int blockPDTest()
printf("Test BlockPD\t\t\t: ");
BlockPD blockPD(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPD.getKP()));
ASSERT(equal(0.01f, blockPD.getKD()));
ASSERT(equal(0.0f, blockPD.getDt()));
ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
ASSERT_CL(equal(0.2f, blockPD.getKP()));
ASSERT_CL(equal(0.01f, blockPD.getKD()));
ASSERT_CL(equal(0.0f, blockPD.getDt()));
ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP()));
// set dt
blockPD.setDt(0.1f);
ASSERT(equal(0.1f, blockPD.getDt()));
ASSERT_CL(equal(0.1f, blockPD.getDt()));
// set derivative state
blockPD.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
// perform one update so initialized is set
blockPD.getDerivative().update(1.0);
ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
// test update
// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -445,24 +455,27 @@ int blockPIDTest()
printf("Test BlockPID\t\t\t: ");
BlockPID blockPID(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPID.getKP()));
ASSERT(equal(0.1f, blockPID.getKI()));
ASSERT(equal(0.01f, blockPID.getKD()));
ASSERT(equal(0.0f, blockPID.getDt()));
ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
ASSERT_CL(equal(0.2f, blockPID.getKP()));
ASSERT_CL(equal(0.1f, blockPID.getKI()));
ASSERT_CL(equal(0.01f, blockPID.getKD()));
ASSERT_CL(equal(0.0f, blockPID.getDt()));
ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP()));
ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax()));
// set dt
blockPID.setDt(0.1f);
ASSERT(equal(0.1f, blockPID.getDt()));
ASSERT_CL(equal(0.1f, blockPID.getDt()));
// set derivative state
blockPID.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
// perform one update so initialized is set
blockPID.getDerivative().update(1.0);
ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
// set integral state
blockPID.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f)));
printf("PASS\n");
return 0;
}
@@ -472,19 +485,19 @@ int blockOutputTest()
printf("Test BlockOutput\t\t: ");
BlockOutput blockOutput(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockOutput.getDt()));
ASSERT(equal(0.5f, blockOutput.get()));
ASSERT(equal(-1.0f, blockOutput.getMin()));
ASSERT(equal(1.0f, blockOutput.getMax()));
ASSERT_CL(equal(0.0f, blockOutput.getDt()));
ASSERT_CL(equal(0.5f, blockOutput.get()));
ASSERT_CL(equal(-1.0f, blockOutput.getMin()));
ASSERT_CL(equal(1.0f, blockOutput.getMax()));
// test update below min
blockOutput.update(-2.0f);
ASSERT(equal(-1.0f, blockOutput.get()));
ASSERT_CL(equal(-1.0f, blockOutput.get()));
// test update above max
blockOutput.update(2.0f);
ASSERT(equal(1.0f, blockOutput.get()));
ASSERT_CL(equal(1.0f, blockOutput.get()));
// test trim
blockOutput.update(0.0f);
ASSERT(equal(0.5f, blockOutput.get()));
ASSERT_CL(equal(0.5f, blockOutput.get()));
printf("PASS\n");
return 0;
}
@@ -495,23 +508,22 @@ int blockRandUniformTest()
printf("Test BlockRandUniform\t\t: ");
BlockRandUniform blockRandUniform(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandUniform.getDt()));
ASSERT(equal(-1.0f, blockRandUniform.getMin()));
ASSERT(equal(1.0f, blockRandUniform.getMax()));
ASSERT_CL(equal(0.0f, blockRandUniform.getDt()));
ASSERT_CL(equal(-1.0f, blockRandUniform.getMin()));
ASSERT_CL(equal(1.0f, blockRandUniform.getMax()));
// test update
int n = 10000;
float mean = blockRandUniform.update();
// recursive mean algorithm from Knuth
for (int i = 2; i < n + 1; i++) {
float val = blockRandUniform.update();
mean += (val - mean) / i;
ASSERT(val <= blockRandUniform.getMax());
ASSERT(val >= blockRandUniform.getMin());
ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax()));
ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin()));
}
ASSERT(equal(mean, (blockRandUniform.getMin() +
blockRandUniform.getMax()) / 2, 1e-1));
ASSERT_CL(equal(mean, (blockRandUniform.getMin() +
blockRandUniform.getMax()) / 2, 1e-1));
printf("PASS\n");
return 0;
}
@@ -522,9 +534,9 @@ int blockRandGaussTest()
printf("Test BlockRandGauss\t\t: ");
BlockRandGauss blockRandGauss(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandGauss.getDt()));
ASSERT(equal(1.0f, blockRandGauss.getMean()));
ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
ASSERT_CL(equal(0.0f, blockRandGauss.getDt()));
ASSERT_CL(equal(1.0f, blockRandGauss.getMean()));
ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev()));
// test update
int n = 10000;
float mean = blockRandGauss.update();
@@ -540,8 +552,8 @@ int blockRandGaussTest()
float stdDev = sqrt(sum / (n - 1));
(void)(stdDev);
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
printf("PASS\n");
return 0;
}
src/modules/controllib/controllib_test_main.cpp
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* 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
* 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 controllib.cpp
* Unit testing for controllib.
*
* @author James Goppert <james.goppert@gmail.com>
*/
#include "blocks.hpp"
extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]);
int controllib_test_main(int argc, char *argv[])
{
(void)argc;
(void)argv;
control::basicBlocksTest();
return 0;
}
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+9
View File
@@ -49,6 +49,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
@@ -154,12 +155,14 @@ private:
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
@@ -320,6 +323,12 @@ private:
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
src/modules/ekf_att_pos_estimator/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
set(MODULE_CFLAGS )
if(NOT ${OS} STREQUAL "qurt")
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400)
endif()
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+116 -20
View File
@@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
/* publications */
_att_pub(nullptr),
_ctrl_state_pub(nullptr),
_global_pos_pub(nullptr),
_local_pos_pub(nullptr),
_estimator_status_pub(nullptr),
_wind_pub(nullptr),
_att{},
_ctrl_state{},
_gyro{},
_accel{},
_mag{},
@@ -588,6 +590,11 @@ void AttitudePositionEstimatorEKF::task_main()
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
/* HIL is slow, set permissive timeouts */
_voter_gyro.set_timeout(500000);
_voter_accel.set_timeout(500000);
_voter_mag.set_timeout(500000);
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
@@ -693,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main()
// Publish attitude estimations
publishAttitude();
// publish control state
publishControlState();
// Publish Local Position estimations
publishLocalPosition();
@@ -817,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -828,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude()
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
/* publish the attitude setpoint */
/* publish the attitude */
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
} else {
@@ -837,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude()
}
}
void AttitudePositionEstimatorEKF::publishControlState()
{
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z;
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
Vector3f v_b = _ekf->Tnb * v_n;
Vector3f v_b_var = _ekf->Tnb * v_n_var;
_ctrl_state.x_vel = v_b.x;
_ctrl_state.y_vel = v_b.y;
_ctrl_state.z_vel = v_b.z;
_ctrl_state.vel_variance[0] = v_b_var.x;
_ctrl_state.vel_variance[1] = v_b_var.y;
_ctrl_state.vel_variance[2] = v_b_var.z;
/* Local Position */
_ctrl_state.x_pos = _ekf->states[7];
_ctrl_state.y_pos = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
_ctrl_state.pos_variance[0] = _ekf->P[7][7];
_ctrl_state.pos_variance[1] = _ekf->P[8][8];
_ctrl_state.pos_variance[2] = _ekf->P[9][9];
/* Attitude */
_ctrl_state.timestamp = _last_sensor_timestamp;
_ctrl_state.q[0] = _ekf->states[0];
_ctrl_state.q[1] = _ekf->states[1];
_ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
/* Guard from bad data */
if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos))
{
// bad data, abort publication
return;
}
/* lazily publish the control state only once available */
if (_ctrl_state_pub != nullptr) {
/* publish the control state */
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
} else {
/* advertise and publish */
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
}
}
void AttitudePositionEstimatorEKF::publishLocalPosition()
{
_local_pos.timestamp = _last_sensor_timestamp;
@@ -1223,6 +1303,23 @@ void AttitudePositionEstimatorEKF::pollData()
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
/* set time fields */
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.0001f) {
if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) {
deltaT = _ekf->dtIMUfilt;
} else {
deltaT = 0.01f;
}
}
/* fill in last data set */
_ekf->dtIMU = deltaT;
// Feed validator with recent sensor data
for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
@@ -1245,6 +1342,11 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
} else {
float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f;
if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) {
deltaT = dt_gyro;
_ekf->dtIMU = deltaT;
}
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
@@ -1317,20 +1419,8 @@ void AttitudePositionEstimatorEKF::pollData()
_vibration_warning_timestamp = 0;
}
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
_last_sensor_timestamp = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
// XXX this is for assessing the filter performance
// leave this in as long as larger improvements are still being made.
#if 0
@@ -1342,7 +1432,7 @@ void AttitudePositionEstimatorEKF::pollData()
static unsigned dtoverflow10 = 0;
static hrt_abstime lastprint = 0;
if (hrt_elapsed_time(&lastprint) > 1000000) {
if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) {
PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
@@ -1361,7 +1451,15 @@ void AttitudePositionEstimatorEKF::pollData()
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
(double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
(double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f",
(double)_sensor_combined.gyro_rad_s[0],
(double)_sensor_combined.gyro_rad_s[1],
(double)_sensor_combined.gyro_rad_s[2]);
lastprint = hrt_absolute_time();
}
@@ -1649,8 +1747,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
PX4_INFO(".");
}
PX4_INFO(" ");
return 0;
}
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+5 -3
View File
@@ -48,6 +48,8 @@
#define M_PI_F static_cast<float>(M_PI)
#endif
#define MIN_AIRSPEED_MEAS 5.0f
constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
AttPosEKF::AttPosEKF() :
@@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed()
// Calculate the predicted airspeed
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit()
P[13][13] = sq(0.2f*dtIMU);
//Wind velocities
P[14][14] = 0.0f;
P[14][14] = 0.01f;
P[15][15] = P[14][14];
//Earth magnetic field
@@ -2825,7 +2827,7 @@ bool AttPosEKF::VelNEDDiverged()
Vector3f delta = current_vel - gps_vel;
float delta_len = delta.length();
bool excessive = (delta_len > 20.0f);
bool excessive = (delta_len > 30.0f);
current_ekf_state.error |= excessive;
current_ekf_state.velOffsetExcessive = excessive;
src/modules/fw_att_control/fw_att_control_main.cpp
+52 -86
View File
@@ -58,7 +58,6 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@@ -67,7 +66,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -125,11 +124,10 @@ private:
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
@@ -144,12 +142,11 @@ private:
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
@@ -242,6 +239,11 @@ private:
} _parameter_handles; /**< handles for interesting parameters */
// Rotation matrix and euler angles to extract from control state
math::Matrix<3, 3> _R;
float _roll;
float _pitch;
float _yaw;
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
@@ -269,12 +271,6 @@ private:
*/
void vehicle_manual_poll();
/**
* Check for airspeed updates.
*/
void vehicle_airspeed_poll();
/**
* Check for accel updates.
*/
@@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_accel_sub(-1),
_airspeed_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
@@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_debug(false)
{
/* safely initialize structs */
_att = {};
_ctrl_state = {};
_accel = {};
_att_sp = {};
_rates_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
_actuators = {};
_actuators_airframe = {};
@@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
}
}
void
FixedwingAttitudeControl::vehicle_airspeed_poll()
{
/* check if there is a new position */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
}
void
FixedwingAttitudeControl::vehicle_accel_poll()
{
@@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main()
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main()
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
@@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main()
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].fd = _ctrl_state_sub;
fds[1].events = POLLIN;
_task_running = true;
@@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main()
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
@@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main()
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
R_adapted(0, 0) = _R(0, 2);
R_adapted(1, 0) = _R(1, 2);
R_adapted(2, 0) = _R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
R_adapted(0, 2) = _R(0, 0);
R_adapted(1, 2) = _R(1, 0);
R_adapted(2, 2) = _R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* fill in new attitude data */
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
_R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
float helper = _ctrl_state.roll_rate;
_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
_ctrl_state.yaw_rate = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
@@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
airspeed = math::max(0.5f, _ctrl_state.airspeed);
}
/*
@@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main()
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
@@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main()
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.roll = _roll;
control_input.pitch = _pitch;
control_input.yaw = _yaw;
control_input.roll_rate = _ctrl_state.roll_rate;
control_input.pitch_rate = _ctrl_state.pitch_rate;
control_input.yaw_rate = _ctrl_state.yaw_rate;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
@@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+59 -71
View File
@@ -68,14 +68,13 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
@@ -153,8 +152,7 @@ private:
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
int _ctrl_state_sub; /**< control state subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@@ -165,11 +163,10 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
@@ -222,6 +219,9 @@ private:
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Matrix<3, 3> _R_nb; ///< current attitude
float _roll;
float _pitch;
float _yaw;
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
@@ -356,14 +356,9 @@ private:
bool vehicle_manual_control_setpoint_poll();
/**
* Check for airspeed updates.
* Check for changes in control state.
*/
bool vehicle_airspeed_poll();
/**
* Check for position updates.
*/
void vehicle_attitude_poll();
void control_state_poll();
/**
* Check for accel updates.
@@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */
_global_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_ctrl_state_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_params_sub(-1),
@@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_nav_capabilities_pub(nullptr),
/* states */
_att(),
_ctrl_state(),
_att_sp(),
_nav_capabilities(),
_manual(),
_airspeed(),
_control_mode(),
_vehicle_status(),
_global_pos(),
@@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll()
}
}
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
/* check if there is an airspeed update or if it timed out */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
bool
FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
{
@@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
return manual_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
FixedwingPositionControl::control_state_poll()
{
/* check if there is a new position */
bool att_updated;
orb_check(_att_sub, &att_updated);
bool ctrl_state_updated;
orb_check(_ctrl_state_sub, &ctrl_state_updated);
if (att_updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_R_nb(i, j) = PX4_R(_att.R, i, j);
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* set rotation matrix and euler angles */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R_nb = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R_nb.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
}
void
@@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
float airspeed;
if (_airspeed_valid) {
airspeed = _airspeed.true_airspeed_m_s;
airspeed = _ctrl_state.airspeed;
} else {
airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
@@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update TECS filters */
if (!_mTecs.getEnabled()) {
_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
}
@@ -1117,7 +1101,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -1125,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* reset hold yaw */
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -1219,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (pos_sp_triplet.previous.valid) {
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);
land_noreturn_horizontal = true;
@@ -1467,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
}
@@ -1476,7 +1462,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1520,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
@@ -1539,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
@@ -1587,7 +1575,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
@@ -1698,11 +1688,10 @@ FixedwingPositionControl::task_main()
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1779,10 +1768,9 @@ FixedwingPositionControl::task_main()
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
control_state_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
vehicle_manual_control_setpoint_poll();
// vehicle_baro_poll();
@@ -1888,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
/* use pitch max set by MT param */
limitOverride.disablePitchMaxOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
limitOverride);
} else {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
@@ -1901,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
_tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp,
_ctrl_state.airspeed, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+25 -26
View File
@@ -749,8 +749,7 @@ void BlockLocalPositionEstimator::predict()
if (_integrate.get() && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
Vector3f b(_x(X_bx), _x(X_by), _x(X_bz));
_u = R_att * (a - b);
_u = R_att * a;
_u(U_az) += 9.81f; // add g
} else {
@@ -812,7 +811,7 @@ void BlockLocalPositionEstimator::predict()
Q(X_bz, X_bz) = _pn_b_noise_power.get();
// continuous time kalman filter prediction
Matrix<float, n_x, 1> dx = (A * _x + B * _u) * getDt();
Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
// only predict for components we have
// valid measurements for
@@ -905,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow()
_flowY += global_speed[1] * dt;
// measurement
Vector2f y;
Vector<float, 2> y;
y(0) = _flowX;
y(1) = _flowY;
// residual
Vector2f r = y - C * _x;
Vector<float, 2> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_flow>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -982,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar()
}
// measurement
Matrix<float, n_y_sonar, 1> y;
Vector<float, n_y_sonar> y;
y(0) = (d - _sonarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_sonar, 1> r = y - C * _x;
Vector<float, n_y_sonar> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_sonar, n_y_sonar> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_sonar>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1033,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar()
void BlockLocalPositionEstimator::correctBaro()
{
Matrix<float, n_y_baro, 1> y;
Vector<float, n_y_baro> y;
y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome;
// baro measurement matrix
@@ -1047,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro()
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_baro, 1> r = y - (C * _x);
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1061,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro()
}
// lower baro trust
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);
} else if (_baroFault) {
_baroFault = FAULT_NONE;
@@ -1105,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar()
R(0, 0) = cov;
}
Matrix<float, n_y_lidar, 1> y;
Vector<float, n_y_lidar> y;
y.setZero();
y(0) = (d - _lidarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_lidar, n_y_lidar> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_lidar, 1> r = y - C * _x;
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
Vector<float, n_y_lidar> r = y - C * _x;
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1167,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));
Matrix<float, 6, 1> y;
Vector<float, 6> y;
y.setZero();
y(0) = px;
y(1) = py;
@@ -1214,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
R(5, 5) = var_vz;
// residual
Matrix<float, 6, 1> r = y - C * _x;
Matrix<float, 6, 6> S_I = (C * _P * C.transpose() + R).inverse();
Vector<float, 6> r = y - C * _x;
Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1246,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
}
// trust GPS less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
@@ -1267,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
void BlockLocalPositionEstimator::correctVision()
{
Matrix<float, 3, 1> y;
Vector<float, 3> y;
y.setZero();
y(0) = _sub_vision_pos.get().x - _visionHome(0);
y(1) = _sub_vision_pos.get().y - _visionHome(1);
@@ -1288,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision()
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
// residual
Matrix<float, n_y_vision, n_y_vision> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
Matrix<float, n_y_vision, 1> r = y - C * _x;
// fault detection
@@ -1302,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);
} else if (_visionFault) {
_visionFault = FAULT_NONE;
@@ -1323,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision()
void BlockLocalPositionEstimator::correctmocap()
{
Matrix<float, n_y_mocap, 1> y;
Vector<float, n_y_mocap> y;
y.setZero();
y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0);
y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1);
@@ -1346,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap()
R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
// residual
Matrix<float, n_y_mocap, n_y_mocap> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
Matrix<float, n_y_mocap, 1> r = y - C * _x;
// fault detection
@@ -1360,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+3 -3
View File
@@ -6,7 +6,7 @@
#include <lib/geo/geo.h>
#ifdef USE_MATRIX_LIB
#include "matrix/src/Matrix.hpp"
#include "matrix/Matrix.hpp"
using namespace matrix;
#else
#include <Eigen/Eigen>
@@ -304,7 +304,7 @@ private:
perf_counter_t _err_perf;
// state space
Matrix<float, n_x, 1> _x; // state vector
Matrix<float, n_u, 1> _u; // input vector
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
};
src/modules/local_position_estimator/CMakeLists.txt
-2
View File
@@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
elseif(${OS} STREQUAL "posix")
list(APPEND MODULE_CFLAGS -Wno-error)
# add matrix tests
add_subdirectory(matrix)
endif()
# use custom matrix lib instead of Eigen
src/modules/local_position_estimator/matrix/.gitignore
-1
View File
@@ -1 +0,0 @@
build*/
src/modules/local_position_estimator/matrix/CMakeLists.txt
-13
View File
@@ -1,13 +0,0 @@
cmake_minimum_required(VERSION 2.8)
project(matrix CXX)
set(CMAKE_BUILD_TYPE "RelWithDebInfo")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
enable_testing()
include_directories(src)
add_subdirectory(test)
src/modules/local_position_estimator/matrix/src/Matrix.hpp
-464
View File
@@ -1,464 +0,0 @@
#pragma once
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
namespace matrix
{
template<typename T, size_t M, size_t N>
class Matrix
{
private:
T _data[M][N];
size_t _rows;
size_t _cols;
public:
Matrix() :
_data(),
_rows(M),
_cols(N)
{
}
Matrix(const T *data) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, data, sizeof(_data));
}
Matrix(const Matrix &other) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, other._data, sizeof(_data));
}
Matrix(T x, T y, T z) :
_data(),
_rows(M),
_cols(N)
{
// TODO, limit to only 3x1 matrices
_data[0][0] = x;
_data[1][0] = y;
_data[2][0] = z;
}
/**
* Accessors/ Assignment etc.
*/
T *data()
{
return _data[0];
}
inline size_t rows() const
{
return _rows;
}
inline size_t cols() const
{
return _rows;
}
inline T operator()(size_t i) const
{
return _data[i][0];
}
inline T operator()(size_t i, size_t j) const
{
return _data[i][j];
}
inline T &operator()(size_t i)
{
return _data[i][0];
}
inline T &operator()(size_t i, size_t j)
{
return _data[i][j];
}
/**
* Matrix Operations
*/
// this might use a lot of programming memory
// since it instantiates a class for every
// required mult pair, but it provides
// compile time size_t checking
template<size_t P>
Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
{
const Matrix<T, M, N> &self = *this;
Matrix<T, M, P> res;
res.setZero();
for (size_t i = 0; i < M; i++) {
for (size_t k = 0; k < P; k++) {
for (size_t j = 0; j < N; j++) {
res(i, k) += self(i, j) * other(j, k);
}
}
}
return res;
}
Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + other(i, j);
}
}
return res;
}
bool operator==(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (self(i , j) != other(i, j)) {
return false;
}
}
}
return true;
}
Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) - other(i, j);
}
}
return res;
}
void operator+=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self + other;
}
void operator-=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self - other;
}
void operator*=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self * other;
}
/**
* Scalar Operations
*/
Matrix<T, M, N> operator*(T scalar) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) * scalar;
}
}
return res;
}
Matrix<T, M, N> operator+(T scalar) const
{
Matrix<T, M, N> res;
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + scalar;
}
}
return res;
}
void operator*=(T scalar)
{
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = self(i, j) * scalar;
}
}
}
void operator/=(T scalar)
{
Matrix<T, M, N> &self = *this;
self = self * (1.0f / scalar);
}
/**
* Misc. Functions
*/
void print()
{
Matrix<T, M, N> &self = *this;
printf("\n");
for (size_t i = 0; i < M; i++) {
printf("[");
for (size_t j = 0; j < N; j++) {
printf("%10g\t", double(self(i, j)));
}
printf("]\n");
}
}
Matrix<T, N, M> transpose() const
{
Matrix<T, N, M> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(j, i) = self(i, j);
}
}
return res;
}
Matrix<T, M, M> expm(float dt, size_t n) const
{
Matrix<float, M, M> res;
res.setIdentity();
Matrix<float, M, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) { break; }
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
Matrix<T, M, 1> diagonal() const
{
Matrix<T, M, 1> res;
// force square for now
const Matrix<T, M, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
void setZero()
{
memset(_data, 0, sizeof(_data));
}
void setIdentity()
{
setZero();
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M and i < N; i++) {
self(i, i) = 1;
}
}
inline void swapRows(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t j = 0; j < cols(); j++) {
T tmp = self(a, j);
self(a, j) = self(b, j);
self(b, j) = tmp;
}
}
inline void swapCols(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < rows(); i++) {
T tmp = self(i, a);
self(i, a) = self(i, b);
self(i, b) = tmp;
}
}
/**
* inverse based on LU factorization with partial pivotting
*/
Matrix <T, M, M> inverse() const
{
Matrix<T, M, M> L;
L.setIdentity();
const Matrix<T, M, M> &A = (*this);
Matrix<T, M, M> U = A;
Matrix<T, M, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < N; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) {
if (i == n) { continue; }
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
Matrix<T, M, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
Matrix<T, M, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < N; c++) {
// for all rows of L
for (size_t i = 0; i < N; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
Matrix<T, M, M> X = Y;
// for all columns of X
for (size_t c = 0; c < N; c++) {
// for all rows of U
for (size_t k = 0; k < N; k++) {
// have to go in reverse order
size_t i = N - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < N; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
};
typedef Matrix<float, 2, 1> Vector2f;
typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<float, 3, 3> Matrix3f;
}; // namespace matrix
src/modules/local_position_estimator/matrix/test/CMakeLists.txt
-15
View File
@@ -1,15 +0,0 @@
set(tests
setIdentity
inverse
matrixMult
vectorAssignment
matrixAssignment
matrixScalarMult
transpose
)
foreach(test ${tests})
add_executable(${test}
${test}.cpp)
add_test(${test} ${test})
endforeach()
src/modules/local_position_estimator/matrix/test/inverse.cpp
-30
View File
@@ -1,30 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
Matrix3f A_I = A.inverse();
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I_check(data_check);
(void)A_I;
assert(A_I == A_I_check);
// stess test
static const size_t n = 100;
Matrix<float, n, n> A_large;
A_large.setIdentity();
Matrix<float, n, n> A_large_I;
A_large_I.setZero();
for (size_t i = 0; i < 100; i++) {
A_large_I = A_large.inverse();
assert(A_large == A_large_I);
}
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp
-29
View File
@@ -1,29 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f m;
m.setZero();
m(0, 0) = 1;
m(0, 1) = 2;
m(0, 2) = 3;
m(1, 0) = 4;
m(1, 1) = 5;
m(1, 2) = 6;
m(2, 0) = 7;
m(2, 1) = 8;
m(2, 2) = 9;
m.print();
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data);
m2.print();
assert(m == m2);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixMult.cpp
-26
View File
@@ -1,26 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I(data_check);
Matrix3f I;
I.setIdentity();
A.print();
A_I.print();
Matrix3f R = A * A_I;
R.print();
assert(R == I);
Matrix3f R2 = A;
R2 *= A_I;
R2.print();
assert(R2 == I);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp
-18
View File
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f A(data);
A = A * 2;
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f A_check(data_check);
A.print();
A_check.print();
assert(A == A_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/setIdentity.cpp
-25
View File
@@ -1,25 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f A;
A.setIdentity();
assert(A.rows() == 3);
assert(A.cols() == 3);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
assert(A(i, j) == 1);
} else {
assert(A(i, j) == 0);
}
}
}
return 0;
}
src/modules/local_position_estimator/matrix/test/transpose.cpp
-18
View File
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[9] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
A_T.print();
A_T_check.print();
assert(A_T == A_T_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp
-28
View File
@@ -1,28 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Vector3f v;
v(0) = 1;
v(1) = 2;
v(2) = 3;
v.print();
assert(v(0) == 1);
assert(v(1) == 2);
assert(v(2) == 3);
Vector3f v2(4, 5, 6);
v2.print();
assert(v2(0) == 4);
assert(v2(1) == 5);
assert(v2(2) == 6);
return 0;
}
src/modules/mavlink/CMakeLists.txt
-1
View File
@@ -56,7 +56,6 @@ px4_add_module(
mavlink_rate_limiter.cpp
mavlink_receiver.cpp
mavlink_ftp.cpp
mavlink_params.c
DEPENDS
platforms__common
)
src/modules/mavlink/mavlink_main.cpp
+8 -1
View File
@@ -698,6 +698,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
} else {
_is_usb_uart = true;
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
}
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
@@ -2094,8 +2097,12 @@ Mavlink::display_status()
printf("3DR RADIO\n");
break;
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
printf("USB CDC\n");
break;
default:
printf("UNKNOWN RADIO\n");
printf("GENERIC LINK OR RADIO\n");
break;
}
src/modules/mavlink/mavlink_main.h
+2
View File
@@ -288,6 +288,8 @@ public:
float get_rate_mult();
float get_baudrate() { return _baudrate; }
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
bool get_has_received_messages() { return _received_messages; }
src/modules/mavlink/mavlink_mission.cpp
+10 -11
View File
@@ -422,7 +422,9 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
} else {
_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
if (_verbose) {
warnx("WPM: MISSION_ACK ERR: ID mismatch");
}
}
}
}
@@ -473,13 +475,13 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
if (_count > 0) {
_state = MAVLINK_WPM_STATE_SENDLIST;
_transfer_seq = 0;
_transfer_count = _count;
_transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid;
_state = MAVLINK_WPM_STATE_SENDLIST;
_transfer_seq = 0;
_transfer_count = _count;
_transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid;
if (_count > 0) {
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
} else {
@@ -598,9 +600,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
/* alternate dataman ID anyway to let navigator know about changes */
update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
// TODO send ACK?
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
_transfer_in_progress = false;
return;
}
@@ -712,8 +713,6 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
/* got all new mission items successfully */
if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
_mavlink->send_statustext_info("WPM: Transfer complete.");
_state = MAVLINK_WPM_STATE_IDLE;
if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
src/modules/mavlink/mavlink_params.c
-2
View File
@@ -31,8 +31,6 @@
*
****************************************************************************/
#include <systemlib/param/param.h>
/**
* MAVLink system ID
* @group MAVLink
src/modules/mavlink/mavlink_receiver.cpp
+39 -34
View File
@@ -1116,7 +1116,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
/* yaw */
rc.values[2] = man.r / 2 + 1500;
/* throttle */
rc.values[3] = man.z / 1 + 1000;
rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f);
/* decode all switches which fit into the channel mask */
unsigned max_switch = (sizeof(man.buttons) * 8);
@@ -1307,18 +1307,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
float dt;
if (_hil_last_frame == 0 ||
(imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) ||
(_hil_last_frame >= imu.time_usec)) {
(timestamp <= _hil_last_frame) ||
(timestamp - _hil_last_frame) > (0.1f * 1e6f) ||
(_hil_last_frame >= timestamp)) {
dt = 0.01f; /* default to 100 Hz */
memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro));
_hil_prev_accel[0] = 0.0f;
_hil_prev_accel[1] = 0.0f;
_hil_prev_accel[2] = 9.866f;
} else {
dt = (imu.time_usec - _hil_last_frame) / 1e6f;
dt = (timestamp - _hil_last_frame) / 1e6f;
}
_hil_last_frame = imu.time_usec;
_hil_last_frame = timestamp;
/* airspeed */
{
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
struct airspeed_s airspeed = {};
float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
// XXX need to fix this
@@ -1338,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* gyro */
{
struct gyro_report gyro;
memset(&gyro, 0, sizeof(gyro));
struct gyro_report gyro = {};
gyro.timestamp = timestamp;
gyro.x_raw = imu.xgyro * 1000.0f;
@@ -1360,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* accelerometer */
{
struct accel_report accel;
memset(&accel, 0, sizeof(accel));
struct accel_report accel = {};
accel.timestamp = timestamp;
accel.x_raw = imu.xacc / mg2ms2;
@@ -1382,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* magnetometer */
{
struct mag_report mag;
memset(&mag, 0, sizeof(mag));
struct mag_report mag = {};
mag.timestamp = timestamp;
mag.x_raw = imu.xmag * 1000.0f;
@@ -1404,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* baro */
{
struct baro_report baro;
memset(&baro, 0, sizeof(baro));
struct baro_report baro = {};
baro.timestamp = timestamp;
baro.pressure = imu.abs_pressure;
@@ -1422,21 +1422,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* sensor combined */
{
struct sensor_combined_s hil_sensors;
memset(&hil_sensors, 0, sizeof(hil_sensors));
struct sensor_combined_s hil_sensors = {};
hil_sensors.timestamp = timestamp;
hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
hil_sensors.gyro_integral_rad[0] = (hil_sensors.gyro_rad_s[0] * dt + _hil_prev_gyro[0]) / 2.0f;
hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f;
hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f;
memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro));
hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro));
hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
hil_sensors.gyro_timestamp[0] = timestamp;
hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH;
@@ -1447,10 +1446,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f;
memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel));
hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
@@ -1493,12 +1492,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* battery status */
{
struct battery_status_s hil_battery_status;
memset(&hil_battery_status, 0, sizeof(hil_battery_status));
struct battery_status_s hil_battery_status = {};
hil_battery_status.timestamp = timestamp;
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.voltage_filtered_v = 11.1f;
hil_battery_status.voltage_v = 11.5f;
hil_battery_status.voltage_filtered_v = 11.5f;
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
@@ -1759,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg)
uint8_t buf[1600];
#else
/* the serial port buffers internally as well, we just need to fit a small chunk */
uint8_t buf[32];
uint8_t buf[64];
#endif
mavlink_message_t msg;
@@ -1799,10 +1797,17 @@ MavlinkReceiver::receive_thread(void *arg)
while (!_mavlink->_task_should_exit) {
if (poll(&fds[0], 1, timeout) > 0) {
if (_mavlink->get_protocol() == SERIAL) {
/*
* to avoid reading very small chunks wait for data before reading
* this is designed to target one message, so >20 bytes at a time
*/
const unsigned character_count = 20;
/* non-blocking read. read may return negative values */
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
/* to avoid reading very small chunks wait for data before reading */
usleep(1000);
if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) {
unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000;
usleep(sleeptime);
}
}
#ifdef __PX4_POSIX
src/modules/mc_att_control/CMakeLists.txt
-1
View File
@@ -40,7 +40,6 @@ px4_add_module(
COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS
mc_att_control_main.cpp
mc_att_control_params.c
DEPENDS
platforms__common
)
src/modules/mc_att_control/mc_att_control_main.cpp
+48 -52
View File
@@ -39,7 +39,6 @@
* Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
* Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*
@@ -75,7 +74,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
@@ -127,7 +126,7 @@ private:
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
@@ -146,7 +145,7 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@@ -190,12 +189,10 @@ private:
param_t pitch_rate_max;
param_t yaw_rate_max;
param_t man_roll_max;
param_t man_pitch_max;
param_t man_yaw_max;
param_t acro_roll_max;
param_t acro_pitch_max;
param_t acro_yaw_max;
param_t rattitude_thres;
} _params_handles; /**< handles for interesting parameters */
@@ -212,11 +209,8 @@ private:
float yaw_rate_max;
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
float rattitude_thres;
} _params;
/**
@@ -303,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_ctrl_state_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
@@ -325,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
@@ -346,11 +340,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.man_roll_max = 0.0f;
_params.man_pitch_max = 0.0f;
_params.man_yaw_max = 0.0f;
_params.mc_rate_max.zero();
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_rates_prev.zero();
_rates_sp.zero();
@@ -380,12 +372,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.rattitude_thres = param_find("MC_RATT_TH");
/* fetch initial parameter values */
parameters_update();
@@ -466,14 +456,6 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* manual attitude control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
@@ -482,6 +464,9 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
@@ -614,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att.R);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
@@ -708,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
@@ -748,7 +733,7 @@ MulticopterAttitudeControl::task_main()
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -762,7 +747,7 @@ MulticopterAttitudeControl::task_main()
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _v_att_sub;
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
@@ -798,8 +783,8 @@ MulticopterAttitudeControl::task_main()
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* copy attitude and control state topics */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* check for updates in other topics */
parameter_update_poll();
@@ -809,23 +794,34 @@ MulticopterAttitudeControl::task_main()
vehicle_status_poll();
vehicle_motor_limits_poll();
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){
if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
fabsf(_manual_control_sp.x) > _params.rattitude_thres){
_v_control_mode.flag_control_attitude_enabled = false;
}
}
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
//}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
@@ -866,7 +862,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
@@ -976,4 +972,4 @@ int mc_att_control_main(int argc, char *argv[])
warnx("unrecognized command");
return 1;
}
}
src/modules/mc_att_control/mc_att_control_params.c
+16 -6
View File
@@ -35,13 +35,10 @@
* @file mc_att_control_params.c
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Roll P gain
*
@@ -251,7 +248,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f);
/**
* Max acro pitch rate
@@ -261,7 +258,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f);
/**
* Max acro yaw rate
@@ -270,4 +267,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
/**
* Threshold for Rattitude mode
*
* Manual input needed in order to override attitude control rate setpoints
* and instead pass manual stick inputs as rate setpoints
*
* @unit
* @min 0.0
* @max 1.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);
src/modules/mc_att_control_multiplatform/CMakeLists.txt
-1
View File
@@ -38,7 +38,6 @@ px4_add_module(
mc_att_control_start_nuttx.cpp
mc_att_control.cpp
mc_att_control_base.cpp
mc_att_control_params.c
DEPENDS
platforms__common
)
src/modules/mc_att_control_multiplatform/mc_att_control_params.c
+1 -5
View File
@@ -36,14 +36,10 @@
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_att_control_params.h"
#include <systemlib/param/param.h>
/**
* Roll P gain
*
src/modules/mc_pos_control/CMakeLists.txt
-1
View File
@@ -36,7 +36,6 @@ px4_add_module(
STACK 1200
SRCS
mc_pos_control_main.cpp
mc_pos_control_params.c
DEPENDS
platforms__common
)
src/modules/mc_pos_control/mc_pos_control_main.cpp
+71 -33
View File
@@ -68,7 +68,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@@ -119,14 +119,14 @@ public:
int start();
private:
const float alt_ctl_dz = 0.2f;
const float alt_ctl_dz = 0.1f;
bool _task_should_exit; /**< if true, task should exit */
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 _ctrl_state_sub; /**< control state subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
@@ -142,7 +142,7 @@ private:
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< 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 */
@@ -230,6 +230,9 @@ private:
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
/**
* Update our local parameter cache.
*/
@@ -319,7 +322,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_mavlink_fd(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_att_sp_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
@@ -347,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_hold_engaged(false),
_alt_hold_engaged(false),
_run_pos_control(true),
_run_alt_control(true)
_run_alt_control(true),
_yaw(0.0f)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
@@ -377,6 +381,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_prev.zero();
_vel_ff.zero();
_R.identity();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.z_p = param_find("MPC_Z_P");
@@ -529,10 +535,10 @@ MulticopterPositionControl::poll_subscriptions()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
orb_check(_att_sub, &updated);
orb_check(_ctrl_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
}
orb_check(_att_sp_sub, &updated);
@@ -710,20 +716,22 @@ MulticopterPositionControl::control_manual(float dt)
/* check for pos. hold */
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
{
if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON ||
(fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
{
_pos_hold_engaged = true;
if (!_pos_hold_engaged) {
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) {
_pos_hold_engaged = true;
} else {
_pos_hold_engaged = false;
}
}
}
else {
_pos_hold_engaged = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
/* set requested velocity setpoint */
if (!_pos_hold_engaged) {
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
@@ -853,18 +861,45 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
bool current_setpoint_valid = false;
bool previous_setpoint_valid = false;
math::Vector<3> prev_sp;
math::Vector<3> curr_sp;
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
math::Vector<3> curr_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
if (PX4_ISFINITE(curr_sp(0)) &&
PX4_ISFINITE(curr_sp(1)) &&
PX4_ISFINITE(curr_sp(2))) {
current_setpoint_valid = true;
}
}
if (_pos_sp_triplet.previous.valid) {
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if (PX4_ISFINITE(prev_sp(0)) &&
PX4_ISFINITE(prev_sp(1)) &&
PX4_ISFINITE(prev_sp(2))) {
previous_setpoint_valid = true;
}
}
if (current_setpoint_valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
@@ -874,13 +909,8 @@ void MulticopterPositionControl::control_auto(float dt)
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if ((curr_sp - prev_sp).length() > MIN_DIST) {
@@ -985,7 +1015,7 @@ MulticopterPositionControl::task_main()
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -1040,6 +1070,14 @@ MulticopterPositionControl::task_main()
}
poll_subscriptions();
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
@@ -1115,7 +1153,7 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
@@ -1300,11 +1338,11 @@ MulticopterPositionControl::task_main()
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;
} else {
@@ -1491,7 +1529,7 @@ MulticopterPositionControl::task_main()
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
}
/* do not move yaw while arming */
@@ -1501,7 +1539,7 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it
src/modules/mc_pos_control/mc_pos_control_params.c
-2
View File
@@ -38,8 +38,6 @@
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Minimum thrust in auto thrust control
*
src/modules/navigator/geofence.cpp
+7 -21
View File
@@ -53,14 +53,8 @@
#include <geo/geo.h>
#include <drivers/drv_hrt.h>
#define GEOFENCE_OFF 0
#define GEOFENCE_FILE_ONLY 1
#define GEOFENCE_MAX_DISTANCES_ONLY 2
#define GEOFENCE_FILE_AND_MAX_DISTANCES 3
#define GEOFENCE_RANGE_WARNING_LIMIT 3000000
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
@@ -76,8 +70,8 @@ Geofence::Geofence() :
_last_vertical_range_warning(0),
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
_param_geofence_mode(this, "MODE"),
_vertices_count(0),
_param_action(this, "ACTION"),
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE"),
_param_counter_threshold(this, "COUNT"),
@@ -138,7 +132,6 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
bool Geofence::inside(double lat, double lon, float altitude)
{
if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) {
int32_t max_horizontal_distance = _param_max_hor_distance.get();
int32_t max_vertical_distance = _param_max_ver_distance.get();
@@ -152,7 +145,7 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}
@@ -162,7 +155,7 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}
@@ -171,7 +164,6 @@ bool Geofence::inside(double lat, double lon, float altitude)
}
}
}
}
bool inside_fence = inside_polygon(lat, lon, altitude);
@@ -194,12 +186,6 @@ bool Geofence::inside(double lat, double lon, float altitude)
bool Geofence::inside_polygon(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled or only checking max distances */
if ((_param_geofence_mode.get() == GEOFENCE_OFF)
|| (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) {
return true;
}
if (valid()) {
if (!isEmpty()) {
@@ -219,7 +205,7 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude)
struct fence_vertex_s temp_vertex_j;
/* Red until fence is finished */
for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
@@ -259,7 +245,7 @@ Geofence::valid()
}
// Otherwise
if ((_verticesCount < 4) || (_verticesCount > fence_s::GEOFENCE_MAX_VERTICES)) {
if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) {
warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1);
return false;
}
@@ -415,7 +401,7 @@ Geofence::loadFromFile(const char *filename)
/* Check if import was successful */
if (gotVertical && pointCounter > 0) {
_verticesCount = pointCounter;
_vertices_count = pointCounter;
warnx("Geofence: imported successfully");
mavlink_log_info(_mavlinkFd, "Geofence imported");
rc = OK;
src/modules/navigator/geofence.h
+8 -6
View File
@@ -97,12 +97,14 @@ public:
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
bool isEmpty() {return _vertices_count == 0;}
int getAltitudeMode() { return _param_altitude_mode.get(); }
int getSource() { return _param_source.get(); }
int getGeofenceAction() { return _param_action.get(); }
void setMavlinkFd(int value) { _mavlinkFd = value; }
private:
@@ -114,20 +116,20 @@ private:
hrt_abstime _last_horizontal_range_warning;
hrt_abstime _last_vertical_range_warning;
float _altitude_min;
float _altitude_max;
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
uint8_t _vertices_count;
/* Params */
control::BlockParamInt _param_geofence_mode;
control::BlockParamInt _param_action;
control::BlockParamInt _param_altitude_mode;
control::BlockParamInt _param_source;
control::BlockParamInt _param_counter_threshold;
control::BlockParamInt _param_max_hor_distance;
control::BlockParamInt _param_max_ver_distance;
uint8_t _outside_counter;
uint8_t _outside_counter;
int _mavlinkFd;
src/modules/navigator/geofence_params.c
+6 -6
View File
@@ -44,15 +44,15 @@
*/
/**
* Geofence mode.
* Geofence violation action.
*
* 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both
* 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination
*
* @min 0
* @max 3
* @max 4
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_MODE, 0);
PARAM_DEFINE_INT32(GF_ACTION, 1);
/**
* Geofence altitude mode
@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(GF_COUNT, -1);
/**
* Max horizontal distance in meters.
*
* Set to > 0 to activate RTL if horizontal distance to home exceeds this value.
* Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value.
*
* @group Geofence
*/
@@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1);
/**
* Max vertical distance in meters.
*
* Set to > 0 to activate RTL if vertical distance to home exceeds this value.
* Set to > 0 to activate a geofence action if vertical distance to home exceeds this value.
*
* @group Geofence
*/
src/modules/navigator/navigator_main.cpp
+5 -1
View File
@@ -405,10 +405,14 @@ Navigator::task_main()
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.geofence_action = _geofence.getGeofenceAction();
if (!inside) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
src/modules/navigator/rtl.cpp
+13 -4
View File
@@ -58,6 +58,7 @@
RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_rtl_start_lock(false),
_param_return_alt(this, "RTL_RETURN_ALT", false),
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
_param_land_delay(this, "RTL_LAND_DELAY", false)
@@ -95,6 +96,7 @@ RTL::on_activation()
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
_rtl_start_lock = false;
/* otherwise go straight to return */
} else {
@@ -102,6 +104,7 @@ RTL::on_activation()
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
_rtl_start_lock = false;
}
}
@@ -126,7 +129,10 @@ RTL::set_rtl_item()
/* make sure we have the latest params */
updateParams();
set_previous_pos_setpoint();
if (!_rtl_start_lock) {
set_previous_pos_setpoint();
}
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
@@ -182,6 +188,8 @@ RTL::set_rtl_item()
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
_rtl_start_lock = true;
break;
}
@@ -190,7 +198,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
@@ -213,7 +221,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
@@ -239,7 +247,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
@@ -258,6 +266,7 @@ RTL::set_rtl_item()
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
// Do not change / control yaw in landed
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
src/modules/navigator/rtl.h
+2
View File
@@ -87,6 +87,8 @@ private:
RTL_STATE_LANDED,
} _rtl_state;
bool _rtl_start_lock;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
src/modules/position_estimator_inav/CMakeLists.txt
+1 -1
View File
@@ -31,7 +31,7 @@
#
############################################################################
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3800)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
endif()
px4_add_module(
MODULE modules__position_estimator_inav
src/modules/position_estimator_inav/position_estimator_inav_main.c
+102 -49
View File
@@ -38,7 +38,6 @@
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Nuno Marques <n.marques21@hotmail.com>
*/
#include <px4_posix.h>
#include <unistd.h>
#include <stdlib.h>
@@ -66,6 +65,7 @@
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
#include <systemlib/err.h>
@@ -90,8 +90,8 @@ static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout
static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s
static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
static const hrt_abstime lidar_timeout = 150000; // lidar timeout = 150ms
static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distance during this time after lidar loss
static const unsigned updates_counter_len = 1000000;
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
@@ -116,11 +116,11 @@ static inline int max(int val1, int val2)
*/
static void usage(const char *reason)
{
if (reason) {
PX4_INFO("%s\n", reason);
if (reason && *reason) {
PX4_INFO("%s", reason);
}
PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n");
return;
}
@@ -311,19 +311,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
{ 0.0f }, // D (pos)
};
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
float corr_lidar = 0.0f;
float corr_lidar_filtered = 0.0f;
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
float sonar_prev = 0.0f;
float lidar_prev = 0.0f;
//hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered)
hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered)
int n_flow = 0;
float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };
float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };
float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
float yaw_comp[] = { 0.0f, 0.0f };
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
bool lidar_valid = false; // lidar is valid
bool flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
bool vision_valid = false; // vision is valid
@@ -352,6 +359,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(&mocap, 0, sizeof(mocap));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct distance_sensor_s lidar;
memset(&lidar, 0, sizeof(lidar));
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -364,6 +373,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
int home_position_sub = orb_subscribe(ORB_ID(home_position));
int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
@@ -445,6 +455,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
attitude_updates++;
bool updated;
bool updated2;
/* parameter update */
orb_check(parameter_update_sub, &updated);
@@ -510,56 +521,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* optical flow */
orb_check(optical_flow_sub, &updated);
orb_check(distance_sensor_sub, &updated2);
if (updated) {
if (updated && updated2) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
/* calculate time from previous update */
// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
// flow_prev = flow.flow_timestamp;
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
(PX4_R(att.R, 2, 2) > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
if ((lidar.current_distance > 0.21f) &&
(lidar.current_distance < 4.0f) &&
/*(PX4_R(att.R, 2, 2) > 0.7f) &&*/
(fabsf(lidar.current_distance - lidar_prev) > FLT_EPSILON)) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
lidar_time = t;
lidar_prev = lidar.current_distance;
corr_lidar = lidar.current_distance + surface_offset + z_est[0];
corr_lidar_filtered += (corr_lidar - corr_lidar_filtered) * params.lidar_filt;
if (fabsf(corr_sonar) > params.sonar_err) {
if (fabsf(corr_lidar) > params.lidar_err) {
/* correction is too large: spike or new ground level? */
if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
if (fabsf(corr_lidar - corr_lidar_filtered) > params.lidar_err) {
/* spike detected, ignore */
corr_sonar = 0.0f;
sonar_valid = false;
corr_lidar = 0.0f;
lidar_valid = false;
} else {
/* new ground level */
surface_offset -= corr_sonar;
surface_offset -= corr_lidar;
surface_offset_rate = 0.0f;
corr_sonar = 0.0f;
corr_sonar_filtered = 0.0f;
sonar_valid_time = t;
sonar_valid = true;
corr_lidar = 0.0f;
corr_lidar_filtered = 0.0f;
lidar_valid_time = t;
lidar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
}
} else {
/* correction is ok, use it */
sonar_valid_time = t;
sonar_valid = true;
lidar_valid_time = t;
lidar_valid = true;
}
}
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
if (dist_bottom > 0.21f && flow_q > params.flow_q_min) {
/* distance to surface */
float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
float flow_dist = dist_bottom; //use this if using lidar
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
@@ -571,12 +586,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
//this flag is not working -->
flow_accurate = true; //already checked if flow_q > 0.3
/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f;
flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f;
flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f;
//moving average
if (n_flow >= 100) {
gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
n_flow = 0;
flow_gyrospeed_filtered[0] = 0.0f;
flow_gyrospeed_filtered[1] = 0.0f;
flow_gyrospeed_filtered[2] = 0.0f;
att_gyrospeed_filtered[0] = 0.0f;
att_gyrospeed_filtered[1] = 0.0f;
att_gyrospeed_filtered[2] = 0.0f;
} else {
flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
n_flow++;
}
/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
yaw_comp[1] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
//todo check direction of x und y axis
flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0] - yaw_comp[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1] - yaw_comp[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
@@ -599,6 +650,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate) {
@@ -862,9 +914,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* check for timeout on FLOW topic */
if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
if ((flow_valid || lidar_valid) && t > flow.timestamp + flow_topic_timeout) {
flow_valid = false;
sonar_valid = false;
lidar_valid = false;
warnx("FLOW timeout");
mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
}
@@ -890,10 +942,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
}
/* check for sonar measurement timeout */
if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
corr_sonar = 0.0f;
sonar_valid = false;
/* check for lidar measurement timeout */
if (lidar_valid && (t > (lidar_time + lidar_timeout))) {
corr_lidar = 0.0f;
lidar_valid = false;
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
@@ -921,16 +973,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
if (dist_bottom_valid) {
/* surface distance prediction */
surface_offset += surface_offset_rate * dt;
/* surface distance correction */
if (sonar_valid) {
surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
surface_offset -= corr_sonar * params.w_z_sonar * dt;
if (lidar_valid) {
surface_offset_rate -= corr_lidar * 0.5f * params.w_z_lidar * params.w_z_lidar * dt;
surface_offset -= corr_lidar * params.w_z_lidar * dt;
}
}
@@ -1114,6 +1166,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
//mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow);
}
if (use_gps_xy) {
@@ -1229,7 +1282,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = -z_est[0] - surface_offset;
local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
local_pos.dist_bottom_rate = - z_est[1] - surface_offset_rate;
}
local_pos.timestamp = t;
src/modules/position_estimator_inav/position_estimator_inav_params.c
+44 -16
View File
@@ -86,15 +86,15 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f);
/**
* Z axis weight for sonar
* Z axis weight for lidar
*
* Weight (cutoff frequency) for sonar measurements.
* Weight (cutoff frequency) for lidar measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f);
/**
* XY axis weight for GPS position
@@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
* Weight (cutoff frequency) for optical flow (velocity) measurements.
*
* @min 0.0
* @max 10.0
* @max 30.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f);
/**
* XY axis weight for resetting velocity
@@ -217,18 +217,18 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f);
/**
* Weight for sonar filter
* Weight for lidar filter
*
* Sonar filter detects spikes on sonar measurements and used to detect new surface level.
* Lidar filter detects spikes on lidar measurements and used to detect new surface level.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
PARAM_DEFINE_FLOAT(INAV_LIDAR_FILT, 0.05f);
/**
* Sonar maximal error for new surface
@@ -240,7 +240,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
* @unit m
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.5f);
/**
* Land detector time
@@ -289,6 +289,30 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
*/
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
/**
* Flow module offset (center of rotation) in X direction
*
* Yaw X flow compensation
*
* @min -1.0
* @max 1.0
* @unit m
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
/**
* Flow module offset (center of rotation) in Y direction
*
* Yaw Y flow compensation
*
* @min -1.0
* @max 1.0
* @unit m
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
/**
* Disable vision input
*
@@ -319,7 +343,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_z_lidar = param_find("INAV_W_Z_LIDAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
@@ -331,13 +355,15 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
h->sonar_filt = param_find("INAV_SONAR_FILT");
h->sonar_err = param_find("INAV_SONAR_ERR");
h->lidar_filt = param_find("INAV_LIDAR_FILT");
h->lidar_err = param_find("INAV_LIDAR_ERR");
h->land_t = param_find("INAV_LAND_T");
h->land_disp = param_find("INAV_LAND_DISP");
h->land_thr = param_find("INAV_LAND_THR");
h->no_vision = param_find("CBRK_NO_VISION");
h->delay_gps = param_find("INAV_DELAY_GPS");
h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
return 0;
}
@@ -347,7 +373,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_vision_p, &(p->w_z_vision_p));
param_get(h->w_z_sonar, &(p->w_z_sonar));
param_get(h->w_z_lidar, &(p->w_z_lidar));
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
@@ -359,13 +385,15 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
param_get(h->flow_q_min, &(p->flow_q_min));
param_get(h->sonar_filt, &(p->sonar_filt));
param_get(h->sonar_err, &(p->sonar_err));
param_get(h->lidar_filt, &(p->lidar_filt));
param_get(h->lidar_err, &(p->lidar_err));
param_get(h->land_t, &(p->land_t));
param_get(h->land_disp, &(p->land_disp));
param_get(h->land_thr, &(p->land_thr));
param_get(h->no_vision, &(p->no_vision));
param_get(h->delay_gps, &(p->delay_gps));
param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
return 0;
}
src/modules/position_estimator_inav/position_estimator_inav_params.h
+10 -6
View File
@@ -46,7 +46,7 @@ struct position_estimator_inav_params {
float w_z_gps_p;
float w_z_gps_v;
float w_z_vision_p;
float w_z_sonar;
float w_z_lidar;
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_vision_p;
@@ -58,13 +58,15 @@ struct position_estimator_inav_params {
float w_acc_bias;
float flow_k;
float flow_q_min;
float sonar_filt;
float sonar_err;
float lidar_filt;
float lidar_err;
float land_t;
float land_disp;
float land_thr;
int32_t no_vision;
float delay_gps;
float flow_module_offset_x;
float flow_module_offset_y;
};
struct position_estimator_inav_param_handles {
@@ -72,7 +74,7 @@ struct position_estimator_inav_param_handles {
param_t w_z_gps_p;
param_t w_z_gps_v;
param_t w_z_vision_p;
param_t w_z_sonar;
param_t w_z_lidar;
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_vision_p;
@@ -84,13 +86,15 @@ struct position_estimator_inav_param_handles {
param_t w_acc_bias;
param_t flow_k;
param_t flow_q_min;
param_t sonar_filt;
param_t sonar_err;
param_t lidar_filt;
param_t lidar_err;
param_t land_t;
param_t land_disp;
param_t land_thr;
param_t no_vision;
param_t delay_gps;
param_t flow_module_offset_x;
param_t flow_module_offset_y;
};
#define CBRK_NO_VISION_KEY 328754
src/modules/px4iofirmware/mixer.cpp
+1 -16
View File
@@ -88,9 +88,6 @@ static int mixer_callback(uintptr_t handle,
static MixerGroup mixer_group(mixer_callback, 0);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
static void mixer_set_failsafe();
void
mixer_tick(void)
{
@@ -479,15 +476,6 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */
if (resid != mixer_text_length) {
/* only set mixer ok if no residual is left over */
if (resid == 0) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else {
/* not yet reached the end of the mixer, set as not ok */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
}
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
@@ -496,9 +484,6 @@ mixer_handle_text(const void *buffer, size_t length)
}
mixer_text_length = resid;
/* update failsafe values */
mixer_set_failsafe();
}
break;
@@ -507,7 +492,7 @@ mixer_handle_text(const void *buffer, size_t length)
return 0;
}
static void
void
mixer_set_failsafe()
{
/*
src/modules/px4iofirmware/px4io.h
+2
View File
@@ -190,6 +190,8 @@ extern pwm_limit_t pwm_limit;
*/
extern void mixer_tick(void);
extern int mixer_handle_text(const void *buffer, size_t length);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
extern void mixer_set_failsafe(void);
/**
* Safety switch/LED.
src/modules/px4iofirmware/registers.c
+12 -1
View File
@@ -469,8 +469,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* Allow FMU override of arming state (to allow in-air restores),
* but only if the arming state is not in sync on the IO side.
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
r_status_flags = value;
}
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
/* update failsafe values, now that the mixer is set to ok */
mixer_set_failsafe();
}
break;
src/modules/sdlog2/CMakeLists.txt
-1
View File
@@ -44,7 +44,6 @@ px4_add_module(
SRCS
sdlog2.c
logbuffer.c
params.c
DEPENDS
platforms__common
)
src/modules/sdlog2/params.c
+1 -3
View File
@@ -31,8 +31,6 @@
*
****************************************************************************/
#include <systemlib/param/param.h>
/**
* Logging rate.
*
@@ -43,7 +41,7 @@
* commonly is before arming).
*
* @min -1
* @max 1
* @max 100
* @group SD Logging
*/
PARAM_DEFINE_INT32(SDLOG_RATE, -1);
src/modules/sdlog2/sdlog2.c
+19 -1
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
@@ -1089,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vtol_vehicle_status_s vtol_status;
struct time_offset_s time_offset;
struct mc_att_ctrl_status_s mc_att_ctrl_status;
struct control_state_s ctrl_state;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -1137,6 +1139,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ENCD_s log_ENCD;
struct log_TSYN_s log_TSYN;
struct log_MACS_s log_MACS;
struct log_CTS_s log_CTS;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1179,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int encoders_sub;
int tsync_sub;
int mc_att_ctrl_status_sub;
int ctrl_state_sub;
} subs;
subs.cmd_sub = -1;
@@ -1212,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.wind_sub = -1;
subs.tsync_sub = -1;
subs.mc_att_ctrl_status_sub = -1;
subs.ctrl_state_sub = -1;
subs.encoders_sub = -1;
/* add new topics HERE */
@@ -1856,6 +1861,19 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(MACS);
}
/* --- CONTROL STATE --- */
if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) {
log_msg.msg_type = LOG_CTS_MSG;
log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel;
log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel;
log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel;
log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed;
log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate;
log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate;
log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate;
LOGBUFFER_WRITE_AND_COUNT(CTS);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
@@ -1898,7 +1916,7 @@ void sdlog2_status()
}
/**
* @return 0 if file exists
* @return true if file exists
*/
bool file_exist(const char *filename)
{
src/modules/sdlog2/sdlog2_messages.h
+13
View File
@@ -477,6 +477,18 @@ struct log_MACS_s {
float yaw_rate_integ;
};
/* --- CONTROL STATE --- */
#define LOG_CTS_MSG 47
struct log_CTS_s {
float vx_body;
float vy_body;
float vz_body;
float airspeed;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* WARNING: ID 46 is already in use for ATTC1 */
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
LOG_FORMAT(VTOL, "f", "Arsp"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
src/modules/sensors/sensor_params.c
+26
View File
@@ -1969,6 +1969,15 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
/**
* Rattitude switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
*/
PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0);
/**
* Posctl switch channel mapping.
*
@@ -2132,6 +2141,23 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
*/
PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
/**
* Threshold for selecting rattitude mode
*
* 0-1 indicate where in the full channel range the threshold sits
* 0 : min
* 1 : max
* sign indicates polarity of comparison
* positive : true when channel>th
* negative : true when channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*
*/
PARAM_DEFINE_FLOAT(RC_RATT_TH, 0.5f);
/**
* Threshold for selecting posctl mode
*
src/modules/sensors/sensors.cpp
+21 -3
View File
@@ -269,6 +269,7 @@ private:
int rc_map_mode_sw;
int rc_map_return_sw;
int rc_map_rattitude_sw;
int rc_map_posctl_sw;
int rc_map_loiter_sw;
int rc_map_acro_sw;
@@ -287,6 +288,7 @@ private:
int32_t rc_fails_thr;
float rc_assist_th;
float rc_auto_th;
float rc_rattitude_th;
float rc_posctl_th;
float rc_return_th;
float rc_loiter_th;
@@ -294,6 +296,7 @@ private:
float rc_offboard_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_rattitude_inv;
bool rc_posctl_inv;
bool rc_return_inv;
bool rc_loiter_inv;
@@ -325,6 +328,7 @@ private:
param_t rc_map_mode_sw;
param_t rc_map_return_sw;
param_t rc_map_rattitude_sw;
param_t rc_map_posctl_sw;
param_t rc_map_loiter_sw;
param_t rc_map_acro_sw;
@@ -347,6 +351,7 @@ private:
param_t rc_fails_thr;
param_t rc_assist_th;
param_t rc_auto_th;
param_t rc_rattitude_th;
param_t rc_posctl_th;
param_t rc_return_th;
param_t rc_loiter_th;
@@ -575,6 +580,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW");
_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
@@ -598,6 +604,7 @@ Sensors::Sensors() :
_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
_parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH");
_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
@@ -742,6 +749,10 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_rattitude_sw, &(_parameters.rc_map_rattitude_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
warnx("%s", paramerr);
}
@@ -779,6 +790,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
param_get(_parameter_handles.rc_rattitude_th, &(_parameters.rc_rattitude_th));
_parameters.rc_rattitude_inv = (_parameters.rc_rattitude_th < 0);
_parameters.rc_rattitude_th = fabs(_parameters.rc_rattitude_th);
param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
@@ -803,6 +817,7 @@ Sensors::parameters_update()
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
@@ -835,7 +850,7 @@ Sensors::parameters_update()
_parameters.battery_voltage_scaling = 0.0082f;
#elif CONFIG_ARCH_BOARD_AEROCORE
_parameters.battery_voltage_scaling = 0.0063f;
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
#elif CONFIG_ARCH_BOARD_PX4FMU_V1
_parameters.battery_voltage_scaling = 0.00459340659f;
#else
/* ensure a missing default trips a low voltage lockdown */
@@ -1235,7 +1250,7 @@ Sensors::vehicle_control_mode_poll()
void
Sensors::parameter_update_poll(bool forced)
{
bool param_updated;
bool param_updated = false;
/* Check if any parameter has changed */
orb_check(_params_sub, &param_updated);
@@ -1928,6 +1943,9 @@ Sensors::rc_poll()
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
_parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
_parameters.rc_rattitude_th,
_parameters.rc_rattitude_inv);
manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
_parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,
@@ -2128,7 +2146,7 @@ Sensors::task_main()
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* wakeup source(s) */
px4_pollfd_struct_t fds[1];
px4_pollfd_struct_t fds[1] = {};
/* use the gyro to pace output */
fds[0].fd = _gyro_sub[0];
src/modules/simulator/simulator_mavlink.cpp
+2
View File
@@ -463,10 +463,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
char serial_buf[1024];
struct pollfd fds[2];
memset(fds, 0, sizeof(fds));
unsigned fd_count = 1;
fds[0].fd = _fd;
fds[0].events = POLLIN;
if (serial_fd >= 0) {
fds[1].fd = serial_fd;
fds[1].events = POLLIN;
src/modules/systemlib/mixer/mixer.h
+2 -2
View File
@@ -481,7 +481,7 @@ public:
* compared to thrust.
* @param yaw_wcale Scaling factor applied to yaw inputs compared
* to thrust.
* @param deadband Minumum rotor control output value; usually
* @param idle_speed Minumum rotor control output value; usually
* tuned to ensure that rotors never stall at the
* low end of their control range.
*/
@@ -491,7 +491,7 @@ public:
float roll_scale,
float pitch_scale,
float yaw_scale,
float deadband);
float idle_speed);
~MultirotorMixer();
/**
src/modules/systemlib/mixer/multi_tables.mk
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2014 Anton Matosov <anton.matosov@gmail.com>. 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.
#
############################################################################
SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
MULTI_TABLES := $(SELF_DIR)multi_tables.py
# Add explicit dependency, as implicit one doesn't work often.
$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h
$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES)
$(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h
src/modules/systemlib/system_params.c
+2 -2
View File
@@ -40,7 +40,7 @@
/**
* Auto-start script index.
*
* Defines the auto-start script used to bootstrap the system.
* CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system.
*
* @group System
*/
@@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
/**
* Companion computer interface
*
* Configures the baud rate of the companion computer interface.
* CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface.
* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!)
* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1.
* 157600: enables OSD mode at 57600 baud, 8N1.
src/modules/uORB/objects_common.cpp
+3
View File
@@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s);
#include "topics/vehicle_attitude.h"
ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
#include "topics/control_state.h"
ORB_DEFINE(control_state, struct control_state_s);
#include "topics/sensor_combined.h"
ORB_DEFINE(sensor_combined, struct sensor_combined_s);
src/modules/uavcan/uavcan_params.c
+2 -1
View File
@@ -45,9 +45,10 @@
* 0 - UAVCAN disabled.
* 1 - Enabled support for UAVCAN actuators and sensors.
* 2 - Enabled support for dynamic node ID allocation and firmware update.
* 3 - Sets the motor control outputs to UAVCAN and enables support for dynamic node ID allocation and firmware update.
*
* @min 0
* @max 2
* @max 3
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
src/modules/vtol_att_control/CMakeLists.txt
-3
View File
@@ -38,12 +38,9 @@ px4_add_module(
SRCS
vtol_att_control_main.cpp
vtol_att_control_params.c
tiltrotor_params.c
tiltrotor.cpp
vtol_type.cpp
tailsitter.cpp
standard_params.c
standard.cpp
DEPENDS
platforms__common
src/modules/vtol_att_control/standard.cpp
+55 -39
View File
@@ -60,7 +60,7 @@ Standard::Standard(VtolAttitudeControl *attc) :
_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()
{
@@ -98,12 +98,12 @@ Standard::parameters_update()
void Standard::update_vtol_state()
{
parameters_update();
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.
* 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 (!_attc->is_fixed_wing_requested()) {
// the transition to fw mode switch is off
@@ -130,7 +130,7 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
(_params_standard.back_trans_dur * 1000000.0f)) {
(_params_standard.back_trans_dur * 1000000.0f)) {
_vtol_schedule.flight_mode = MC_MODE;
}
}
@@ -168,17 +168,19 @@ void Standard::update_vtol_state()
}
// 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;
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;
}
}
@@ -188,18 +190,21 @@ void Standard::update_transition_state()
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);
_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) {
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
@@ -210,17 +215,19 @@ void Standard::update_transition_state()
} 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) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *
1000000.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
// 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();
@@ -238,15 +245,15 @@ void Standard::update_mc_state()
// do nothing
}
void Standard::update_fw_state()
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
// 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()
{
@@ -259,22 +266,31 @@ void Standard::update_external_state()
void Standard::fill_actuator_outputs()
{
/* multirotor controls */
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight; // roll
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight; // yaw
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
/* fixed wing controls */
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight); //roll
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_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[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
// otherwise we may be ramping up the throttle during the transition to fw mode
// otherwise we may be ramping up the throttle during the transition to fw mode
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
}
}
@@ -288,11 +304,11 @@ Standard::set_max_mc(unsigned pwm_value)
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
@@ -301,9 +317,9 @@ Standard::set_max_mc(unsigned pwm_value)
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
if (ret != OK) {PX4_WARN("failed setting max values");}
close(fd);
px4_close(fd);
}
src/modules/vtol_att_control/standard.h
+11 -11
View File
@@ -31,15 +31,15 @@
*
****************************************************************************/
/**
* @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>
*
*/
/**
* @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
@@ -52,7 +52,7 @@ class Standard : public VtolType
public:
Standard(VtolAttitudeControl * _att_controller);
Standard(VtolAttitudeControl *_att_controller);
~Standard();
void update_vtol_state();
@@ -89,7 +89,7 @@ private:
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;
} _vtol_schedule;
bool _flag_enable_mc_motors;
float _pusher_throttle;
src/modules/vtol_att_control/standard_params.c
+1 -3
View File
@@ -36,11 +36,9 @@
* Parameters for the standard VTOL controller.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Target throttle value for pusher/puller motor during the transition to fw mode
*
src/modules/vtol_att_control/tailsitter.cpp
+75 -56
View File
@@ -31,21 +31,21 @@
*
****************************************************************************/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
#include "tailsitter.h"
#include "vtol_att_control_main.h"
Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
}
@@ -60,6 +60,7 @@ void Tailsitter::update_vtol_state()
// simply switch between the two modes
if (!_attc->is_fixed_wing_requested()) {
_vtol_mode = ROTARY_WING;
} else {
_vtol_mode = FIXED_WING;
}
@@ -67,7 +68,7 @@ void Tailsitter::update_vtol_state()
void Tailsitter::update_mc_state()
{
if (!flag_idle_mc) {
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
@@ -91,18 +92,18 @@ void Tailsitter::update_external_state()
}
void Tailsitter::calc_tot_airspeed()
{
void Tailsitter::calc_tot_airspeed()
{
float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
P = math::constrain(P,1.0f,_params->power_max);
P = math::constrain(P, 1.0f, _params->power_max);
// calculate prop efficiency
float power_factor = 1.0f - P*_params->prop_eff/_params->power_max;
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
float power_factor = 1.0f - P * _params->prop_eff / _params->power_max;
float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f;
eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed/eta - airspeed)*2.0f;
float v_ind = (airspeed / eta - airspeed) * 2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
@@ -115,16 +116,19 @@ Tailsitter::scale_mc_output()
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
airspeed = _params->mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max);
airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
}
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
@@ -135,8 +139,10 @@ Tailsitter::scale_mc_output()
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min :
airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling,
-1.0f, 1.0f);
}
/**
@@ -144,37 +150,50 @@ Tailsitter::scale_mc_output()
*/
void Tailsitter::fill_actuator_outputs()
{
switch(_vtol_mode) {
case ROTARY_WING:
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
switch (_vtol_mode) {
case ROTARY_WING:
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_params->elevons_mc_lock == 1) {
_actuators_out_1->control[0] = 0;
_actuators_out_1->control[1] = 0;
} else {
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
case FIXED_WING:
// in fixed wing mode we use engines only for providing thrust, no moments are generated
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_params->elevons_mc_lock == 1) {
_actuators_out_1->control[0] = 0;
_actuators_out_1->control[1] = 0;
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
break;
case TRANSITION:
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
} else {
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
_actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
case FIXED_WING:
// in fixed wing mode we use engines only for providing thrust, no moments are generated
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
_actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
break;
case TRANSITION:
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
}
}
src/modules/vtol_att_control/tailsitter.h
+7 -7
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TAILSITTER_H
#define TAILSITTER_H
@@ -48,7 +48,7 @@ class Tailsitter : public VtolType
{
public:
Tailsitter(VtolAttitudeControl * _att_controller);
Tailsitter(VtolAttitudeControl *_att_controller);
~Tailsitter();
void update_vtol_state();
src/modules/vtol_att_control/tiltrotor.cpp
+143 -102
View File
@@ -44,12 +44,10 @@
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
VtolType(attc),
_rear_motors(ENABLED),
_tilt_control(0.0f),
_roll_weight_mc(1.0f),
_yaw_weight_mc(1.0f),
_min_front_trans_dur(0.5f)
VtolType(attc),
_rear_motors(ENABLED),
_tilt_control(0.0f),
_min_front_trans_dur(0.5f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@@ -88,11 +86,11 @@ Tiltrotor::parameters_update()
/* vtol duration of a front transition */
param_get(_params_handles_tiltrotor.front_trans_dur, &v);
_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
_params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
/* vtol duration of a back transition */
param_get(_params_handles_tiltrotor.back_trans_dur, &v);
_params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f);
_params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* vtol tilt mechanism position in mc mode */
param_get(_params_handles_tiltrotor.tilt_mc, &v);
@@ -125,22 +123,26 @@ Tiltrotor::parameters_update()
/* avoid parameters which will lead to zero division in the transition code */
_params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur);
if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) {
if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) {
_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
}
return OK;
}
int Tiltrotor::get_motor_off_channels(int channels) {
int Tiltrotor::get_motor_off_channels(int channels)
{
int channel_bitmap = 0;
int channel;
for (int i = 0; i < _params->vtol_motor_count; ++i) {
channel = channels % 10;
if (channel == 0) {
break;
}
channel_bitmap |= 1 << channel;
channels = channels / 10;
}
@@ -150,82 +152,98 @@ int Tiltrotor::get_motor_off_channels(int channels) {
void Tiltrotor::update_vtol_state()
{
parameters_update();
parameters_update();
/* simple logic using a two way switch to perform transitions.
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
* forward completely. For the backtransition the motors simply rotate back.
*/
*/
if (!_attc->is_fixed_wing_requested()) {
// plane is in multicopter mode
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
break;
}
} else {
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed to switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed to switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
}
break;
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
break;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
break;
}
}
// map tiltrotor 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_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
}
}
@@ -250,7 +268,7 @@ void Tiltrotor::update_mc_state()
_mc_yaw_weight = 1.0f;
}
void Tiltrotor::update_fw_state()
void Tiltrotor::update_fw_state()
{
// make sure motors are tilted forward
_tilt_control = _params_tiltrotor.tilt_fw;
@@ -278,15 +296,18 @@ void Tiltrotor::update_transition_state()
if (_rear_motors != ENABLED) {
set_rear_motor_state(ENABLED);
}
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition ) {
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);
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 >= _params_tiltrotor.airspeed_blend_start) {
_mc_roll_weight = 0.0f;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
@@ -294,6 +315,7 @@ void Tiltrotor::update_transition_state()
// disable mc yaw control once the plane has picked up speed
_mc_yaw_weight = 1.0f;
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}
@@ -301,8 +323,10 @@ void Tiltrotor::update_transition_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = _params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
_mc_roll_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (_rear_motors != IDLE) {
set_rear_motor_state(IDLE);
@@ -312,10 +336,12 @@ void Tiltrotor::update_transition_state()
set_idle_mc();
flag_idle_mc = true;
}
// tilt rotors back
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw -
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
}
// set zero throttle for backtransition otherwise unwanted moments will be created
@@ -339,19 +365,28 @@ void Tiltrotor::update_external_state()
*/
void Tiltrotor::fill_actuator_outputs()
{
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight;
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
}
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[4] = _tilt_control;
}
@@ -360,52 +395,58 @@ void Tiltrotor::fill_actuator_outputs()
* Set state of rear motors.
*/
void Tiltrotor::set_rear_motor_state(rear_motor_state state) {
void Tiltrotor::set_rear_motor_state(rear_motor_state state)
{
int pwm_value = PWM_DEFAULT_MAX;
// map desired rear rotor state to max allowed pwm signal
switch (state) {
case ENABLED:
pwm_value = PWM_DEFAULT_MAX;
_rear_motors = ENABLED;
break;
case DISABLED:
pwm_value = PWM_LOWEST_MAX;
_rear_motors = DISABLED;
break;
case IDLE:
pwm_value = _params->idle_pwm_mc;
_rear_motors = IDLE;
break;
case ENABLED:
pwm_value = PWM_DEFAULT_MAX;
_rear_motors = ENABLED;
break;
case DISABLED:
pwm_value = PWM_LOWEST_MAX;
_rear_motors = DISABLED;
break;
case IDLE:
pwm_value = _params->idle_pwm_mc;
_rear_motors = IDLE;
break;
}
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_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++) {
if (is_motor_off_channel(i)) {
pwm_values.values[i] = pwm_value;
} else {
pwm_values.values[i] = PWM_DEFAULT_MAX;
}
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
if (ret != OK) {PX4_WARN("failed setting max values");}
close(fd);
px4_close(fd);
}
bool Tiltrotor::is_motor_off_channel(const int channel) {
bool Tiltrotor::is_motor_off_channel(const int channel)
{
return (_params_tiltrotor.fw_motors_off >> channel) & 1;
}
src/modules/vtol_att_control/tiltrotor.h
+8 -10
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TILTROTOR_H
#define TILTROTOR_H
@@ -49,7 +49,7 @@ class Tiltrotor : public VtolType
public:
Tiltrotor(VtolAttitudeControl * _att_controller);
Tiltrotor(VtolAttitudeControl *_att_controller);
~Tiltrotor();
/**
@@ -127,11 +127,9 @@ private:
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
}_vtol_schedule;
} _vtol_schedule;
float _tilt_control; /**< actuator value for the tilt servo */
float _roll_weight_mc; /**< multicopter desired roll moment weight */
float _yaw_weight_mc; /**< multicopter desired yaw moment weight */
const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */
src/modules/vtol_att_control/tiltrotor_params.c
+1 -1
View File
@@ -35,7 +35,7 @@
* @file tiltrotor_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
src/modules/vtol_att_control/vtol_att_control_main.cpp
+37 -27
View File
@@ -93,10 +93,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
memset(&_armed, 0, sizeof(_armed));
memset(&_local_pos,0,sizeof(_local_pos));
memset(&_airspeed,0,sizeof(_airspeed));
memset(&_batt_status,0,sizeof(_batt_status));
memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd));
memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_airspeed, 0, sizeof(_airspeed));
memset(&_batt_status, 0, sizeof(_batt_status));
memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
_params.idle_pwm_mc = PWM_LOWEST_MIN;
_params.vtol_motor_count = 0;
@@ -121,12 +121,15 @@ VtolAttitudeControl::VtolAttitudeControl() :
if (_params.vtol_type == 0) {
_tailsitter = new Tailsitter(this);
_vtol_type = _tailsitter;
} 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;
}
@@ -150,7 +153,7 @@ VtolAttitudeControl::~VtolAttitudeControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@@ -258,7 +261,8 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
* Check for airspeed updates.
*/
void
VtolAttitudeControl::vehicle_airspeed_poll() {
VtolAttitudeControl::vehicle_airspeed_poll()
{
bool updated;
orb_check(_airspeed_sub, &updated);
@@ -271,7 +275,8 @@ VtolAttitudeControl::vehicle_airspeed_poll() {
* Check for battery updates.
*/
void
VtolAttitudeControl::vehicle_battery_poll() {
VtolAttitudeControl::vehicle_battery_poll()
{
bool updated;
orb_check(_battery_status_sub, &updated);
@@ -442,7 +447,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
void VtolAttitudeControl::task_main()
{
warnx("started");
PX4_WARN("started");
fflush(stdout);
/* do subscriptions */
@@ -471,7 +476,7 @@ void VtolAttitudeControl::task_main()
_vtol_type->set_idle_mc();
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
@@ -491,7 +496,7 @@ void VtolAttitudeControl::task_main()
}
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
@@ -633,7 +638,7 @@ void VtolAttitudeControl::task_main()
warnx("exit");
_control_task = -1;
_exit(0);
return;
}
int
@@ -643,14 +648,14 @@ VtolAttitudeControl::start()
/* start the task */
_control_task = px4_task_spawn_cmd("vtol_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
(main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
(px4_main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
PX4_WARN("task start failed");
return -errno;
}
@@ -661,49 +666,54 @@ VtolAttitudeControl::start()
int vtol_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: vtol_att_control {start|stop|status}");
PX4_WARN("usage: vtol_att_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (VTOL_att_control::g_control != nullptr) {
errx(1, "already running");
PX4_WARN("already running");
return 0;
}
VTOL_att_control::g_control = new VtolAttitudeControl;
if (VTOL_att_control::g_control == nullptr) {
errx(1, "alloc failed");
PX4_WARN("alloc failed");
return 1;
}
if (OK != VTOL_att_control::g_control->start()) {
delete VTOL_att_control::g_control;
VTOL_att_control::g_control = nullptr;
err(1, "start failed");
PX4_WARN("start failed");
return 1;
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (VTOL_att_control::g_control == nullptr) {
errx(1, "not running");
PX4_WARN("not running");
return 0;
}
delete VTOL_att_control::g_control;
VTOL_att_control::g_control = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (VTOL_att_control::g_control) {
errx(0, "running");
PX4_WARN("running");
} else {
errx(1, "not running");
PX4_WARN("not running");
}
return 0;
}
warnx("unrecognized command");
PX4_WARN("unrecognized command");
return 1;
}
src/modules/vtol_att_control/vtol_att_control_main.h
+25 -27
View File
@@ -46,6 +46,10 @@
#ifndef VTOL_ATT_CONTROL_MAIN_H
#define VTOL_ATT_CONTROL_MAIN_H
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -80,7 +84,6 @@
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <nuttx/fs/ioctl.h>
#include <fcntl.h>
#include "tiltrotor.h"
@@ -101,24 +104,24 @@ public:
int start(); /* start the task and return OK on success */
bool is_fixed_wing_requested();
struct vehicle_attitude_s* get_att () {return &_v_att;}
struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;}
struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;}
struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;}
struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;}
struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;}
struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;}
struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;}
struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;}
struct actuator_armed_s* get_armed () {return &_armed;}
struct vehicle_local_position_s* get_local_pos () {return &_local_pos;}
struct airspeed_s* get_airspeed () {return &_airspeed;}
struct battery_status_s* get_batt_status () {return &_batt_status;}
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;}
struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;}
struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct actuator_armed_s *get_armed() {return &_armed;}
struct vehicle_local_position_s *get_local_pos() {return &_local_pos;}
struct airspeed_s *get_airspeed() {return &_airspeed;}
struct battery_status_s *get_batt_status() {return &_batt_status;}
struct Params* get_params () {return &_params;}
struct Params *get_params() {return &_params;}
private:
@@ -184,17 +187,12 @@ private:
param_t elevons_mc_lock;
} _params_handles;
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
unsigned _motor_count; // number of motors
float _airspeed_tot;
int _transition_command;
VtolType * _vtol_type; // base class for different vtol types
Tiltrotor * _tiltrotor; // tailsitter vtol type
Tailsitter * _tailsitter; // tiltrotor vtol type
Standard * _standard; // standard vtol type
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
+1 -3
View File
@@ -35,11 +35,9 @@
* @file vtol_att_control_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstr@ethz.ch>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
/**
* VTOL number of engines
*
src/modules/vtol_att_control/vtol_type.cpp
+21 -21
View File
@@ -31,21 +31,21 @@
*
****************************************************************************/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "vtol_type.h"
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include <px4_defines.h>
#include "vtol_att_control_main.h"
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
_vtol_mode(ROTARY_WING)
_attc(att_controller),
_vtol_mode(ROTARY_WING)
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
@@ -70,7 +70,7 @@ _vtol_mode(ROTARY_WING)
VtolType::~VtolType()
{
}
/**
@@ -81,11 +81,11 @@ void VtolType::set_idle_mc()
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
@@ -95,11 +95,11 @@ void VtolType::set_idle_mc()
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
if (ret != OK) {PX4_WARN("failed setting min values");}
close(fd);
px4_close(fd);
flag_idle_mc = true;
}
@@ -111,9 +111,9 @@ void VtolType::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
@@ -125,9 +125,9 @@ void VtolType::set_idle_fw()
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
if (ret != OK) {PX4_WARN("failed setting min values");}
close(fd);
px4_close(fd);
}
src/modules/vtol_att_control/vtol_type.h
+7 -7
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef VTOL_TYPE_H
#define VTOL_TYPE_H
@@ -85,7 +85,7 @@ public:
void set_idle_mc();
void set_idle_fw();
mode get_mode () {return _vtol_mode;};
mode get_mode() {return _vtol_mode;};
protected:
VtolAttitudeControl *_attc;