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

updated to master (solve merge conflicts)

Browse Source
This commit is contained in:
Youssef Demitri
2015-11-06 12:18:31 +01:00
parent 50c6d720f0 9b45daa8f7
commit 170f0032fd
72 changed files with 1418 additions and 830 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+92 -6
View File
@@ -57,6 +57,8 @@
#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>
@@ -118,6 +120,8 @@ 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;
@@ -125,16 +129,19 @@ private:
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;
@@ -142,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;
@@ -170,6 +184,7 @@ private:
bool _data_good = false;
bool _failsafe = false;
bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1;
@@ -198,12 +213,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_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");
}
/**
@@ -271,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));
@@ -376,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);
@@ -503,6 +565,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);
@@ -515,6 +578,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);
}
}
@@ -570,12 +634,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
+136 -35
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;
@@ -197,7 +201,7 @@ static struct home_position_s _home;
static unsigned _last_mission_instance = 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
@@ -221,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.
@@ -246,6 +250,9 @@ void print_reject_arm(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
/**
@@ -253,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.
@@ -291,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]);
@@ -396,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()
@@ -490,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)
@@ -524,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) {
@@ -833,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) {
@@ -855,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);
@@ -881,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;
@@ -917,6 +929,8 @@ int commander_thread_main(int argc, char *argv[])
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";
@@ -941,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";
@@ -1029,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));
@@ -1138,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
@@ -1255,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
float rc_loss_timeout = 0.5;
int32_t datalink_regain_timeout = 0;
uint8_t geofence_action = 0;
int32_t geofence_action = 0;
/* Thresholds for engine failure detection */
int32_t ef_throttle_thres = 1.0f;
@@ -1265,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;
@@ -1329,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);
}
@@ -1344,6 +1372,8 @@ int commander_thread_main(int argc, char *argv[])
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);
@@ -1434,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;
}
}
@@ -1489,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;
}
}
@@ -1574,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) {
@@ -1618,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;
@@ -1635,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 */
@@ -1978,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) {
@@ -2184,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;
}
}
@@ -2196,6 +2272,7 @@ int commander_thread_main(int argc, char *argv[])
* 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 &&
@@ -2220,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 ||
@@ -2246,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;
@@ -2261,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,
@@ -2518,6 +2598,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
(_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))) {
@@ -2585,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);
}
@@ -2708,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;
@@ -2956,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/ekf_att_pos_estimator_main.cpp
+37 -16
View File
@@ -590,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);
@@ -1295,6 +1300,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++) {
@@ -1317,6 +1339,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;
@@ -1389,20 +1416,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
@@ -1414,7 +1429,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);
@@ -1433,7 +1448,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();
}
@@ -1721,8 +1744,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
+1 -1
View File
@@ -2827,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_pos_control_l1/fw_pos_control_l1_main.cpp
+6
View File
@@ -1102,6 +1102,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -1461,6 +1463,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1572,6 +1576,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
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_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
+32 -36
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>
*
@@ -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;
/**
@@ -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;
@@ -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) {
@@ -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_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
+40 -16
View File
@@ -716,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);
@@ -859,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
@@ -880,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) {
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/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
+1 -1
View File
@@ -1916,7 +1916,7 @@ void sdlog2_status()
}
/**
* @return 0 if file exists
* @return true if file exists
*/
bool file_exist(const char *filename)
{
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
+18
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;
@@ -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,
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/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);