mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-23 05:26:38 +08:00
Merge branch 'release_v1.0.0' into beta
This commit is contained in:
@@ -67,6 +67,21 @@
|
||||
*/
|
||||
#define RC_INPUT_RSSI_MAX 100
|
||||
|
||||
/**
|
||||
* Minimum value
|
||||
*/
|
||||
#define RC_INPUT_LOWEST_MIN_US 500
|
||||
|
||||
/**
|
||||
* Maximum value
|
||||
*/
|
||||
#define RC_INPUT_HIGHEST_MAX_US 2500
|
||||
|
||||
/**
|
||||
* Maximum deadzone value
|
||||
*/
|
||||
#define RC_INPUT_MAX_DEADZONE_US 500
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
|
||||
@@ -1478,20 +1478,20 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
/* Update land detector */
|
||||
orb_check(land_detector_sub, &updated);
|
||||
if(updated) {
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
|
||||
}
|
||||
|
||||
if (status.condition_local_altitude_valid) {
|
||||
if (updated && status.condition_local_altitude_valid) {
|
||||
if (status.condition_landed != land_detector.landed) {
|
||||
status.condition_landed = land_detector.landed;
|
||||
status_changed = true;
|
||||
|
||||
if (status.condition_landed) {
|
||||
mavlink_log_critical(mavlink_fd, "LANDED MODE");
|
||||
mavlink_log_critical(mavlink_fd, "LANDING DETECTED");
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "IN AIR MODE");
|
||||
mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -52,19 +52,24 @@ public:
|
||||
virtual ~LandDetector();
|
||||
|
||||
/**
|
||||
* @return true if this task is currently running
|
||||
**/
|
||||
* @return true if this task is currently running
|
||||
**/
|
||||
inline bool isRunning() const {return _taskIsRunning;}
|
||||
|
||||
/**
|
||||
* @brief Tells the Land Detector task that it should exit
|
||||
**/
|
||||
* @return the current landed state
|
||||
*/
|
||||
bool isLanded() { return _landDetected.landed; }
|
||||
|
||||
/**
|
||||
* @brief Tells the Land Detector task that it should exit
|
||||
**/
|
||||
void shutdown();
|
||||
|
||||
/**
|
||||
* @brief Blocking function that should be called from it's own task thread. This method will
|
||||
* run the underlying algorithm at the desired update rate and publish if the landing state changes.
|
||||
**/
|
||||
* @brief Blocking function that should be called from it's own task thread. This method will
|
||||
* run the underlying algorithm at the desired update rate and publish if the landing state changes.
|
||||
**/
|
||||
void start();
|
||||
|
||||
protected:
|
||||
|
||||
@@ -195,7 +195,7 @@ int land_detector_main(int argc, char *argv[])
|
||||
if (land_detector_task) {
|
||||
|
||||
if (land_detector_task->isRunning()) {
|
||||
warnx("running (%s)", _currentMode);
|
||||
warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR");
|
||||
|
||||
} else {
|
||||
errx(1, "exists, but not running (%s)", _currentMode);
|
||||
|
||||
@@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f);
|
||||
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f);
|
||||
|
||||
/**
|
||||
* Fixedwing max climb rate
|
||||
|
||||
@@ -316,14 +316,8 @@ MavlinkFTP::_workList(PayloadHeader* payload)
|
||||
_mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)");
|
||||
_mavlink->send_statustext_critical(dirPath);
|
||||
#endif
|
||||
// this is not an FTP error, abort directory read and continue
|
||||
|
||||
payload->data[offset++] = kDirentSkip;
|
||||
*((char *)&payload->data[offset]) = '\0';
|
||||
offset++;
|
||||
payload->size = offset;
|
||||
|
||||
return errorCode;
|
||||
// this is not an FTP error, abort directory by simulating eof
|
||||
return kErrEOF;
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
|
||||
@@ -158,6 +158,7 @@ private:
|
||||
perf_counter_t _controller_latency_perf;
|
||||
|
||||
math::Vector<3> _rates_prev; /**< angular rates on previous step */
|
||||
math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */
|
||||
math::Vector<3> _rates_sp; /**< angular rates setpoint */
|
||||
math::Vector<3> _rates_int; /**< angular rates integral error */
|
||||
float _thrust_sp; /**< thrust setpoint */
|
||||
@@ -353,6 +354,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
||||
|
||||
_rates_prev.zero();
|
||||
_rates_sp.zero();
|
||||
_rates_sp_prev.zero();
|
||||
_rates_int.zero();
|
||||
_thrust_sp = 0.0f;
|
||||
_att_control.zero();
|
||||
@@ -712,7 +714,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
|
||||
|
||||
/* angular rates error */
|
||||
math::Vector<3> rates_err = _rates_sp - rates;
|
||||
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp);
|
||||
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
|
||||
_rates_sp_prev = _rates_sp;
|
||||
_rates_prev = rates;
|
||||
|
||||
/* update integral only if not saturated on low limit and if motor commands are not saturated */
|
||||
|
||||
@@ -48,29 +48,72 @@
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
int rc_calibration_check(int mavlink_fd) {
|
||||
#define RC_INPUT_MAP_UNMAPPED 0
|
||||
|
||||
int rc_calibration_check(int mavlink_fd)
|
||||
{
|
||||
|
||||
char nbuf[20];
|
||||
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
|
||||
_parameter_handles_rev, _parameter_handles_dz;
|
||||
_parameter_handles_rev, _parameter_handles_dz;
|
||||
|
||||
float param_min, param_max, param_trim, param_rev, param_dz;
|
||||
unsigned map_fail_count = 0;
|
||||
|
||||
const char *rc_map_mandatory[] = { "RC_MAP_MODE_SW",
|
||||
/* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/
|
||||
0 /* end marker */
|
||||
};
|
||||
|
||||
unsigned j = 0;
|
||||
|
||||
/* first check channel mappings */
|
||||
/* check which map param applies */
|
||||
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
|
||||
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
|
||||
// /* give system time to flush error message in case there are more */
|
||||
// usleep(100000);
|
||||
// count++;
|
||||
// }
|
||||
while (rc_map_mandatory[j] != 0) {
|
||||
|
||||
int channel_fail_count = 0;
|
||||
param_t map_parm = param_find(rc_map_mandatory[j]);
|
||||
|
||||
for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
|
||||
if (map_parm == PARAM_INVALID) {
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
map_fail_count++;
|
||||
j++;
|
||||
continue;
|
||||
}
|
||||
|
||||
int32_t mapping;
|
||||
param_get(map_parm, &mapping);
|
||||
|
||||
if (mapping > RC_INPUT_MAX_CHANNELS) {
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
map_fail_count++;
|
||||
}
|
||||
|
||||
if (mapping == 0) {
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
map_fail_count++;
|
||||
}
|
||||
|
||||
j++;
|
||||
}
|
||||
|
||||
unsigned total_fail_count = 0;
|
||||
unsigned channels_failed = 0;
|
||||
|
||||
for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
|
||||
/* should the channel be enabled? */
|
||||
uint8_t count = 0;
|
||||
|
||||
/* initialize values to values failing the check */
|
||||
float param_min = 0.0f;
|
||||
float param_max = 0.0f;
|
||||
float param_trim = 0.0f;
|
||||
float param_rev = 0.0f;
|
||||
float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;
|
||||
|
||||
/* min values */
|
||||
sprintf(nbuf, "RC%d_MIN", i + 1);
|
||||
_parameter_handles_min = param_find(nbuf);
|
||||
@@ -97,56 +140,55 @@ int rc_calibration_check(int mavlink_fd) {
|
||||
param_get(_parameter_handles_dz, ¶m_dz);
|
||||
|
||||
/* assert min..center..max ordering */
|
||||
if (param_min < 500) {
|
||||
if (param_min < RC_INPUT_LOWEST_MIN_US) {
|
||||
count++;
|
||||
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MIN < 500", i+1);
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
}
|
||||
if (param_max > 2500) {
|
||||
|
||||
if (param_max > RC_INPUT_HIGHEST_MAX_US) {
|
||||
count++;
|
||||
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MAX > 2500", i+1);
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
if (param_trim < param_min) {
|
||||
count++;
|
||||
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
if (param_trim > param_max) {
|
||||
count++;
|
||||
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
/* assert deadzone is sane */
|
||||
if (param_dz > 500) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_DZ > 500", i+1);
|
||||
if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
|
||||
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US);
|
||||
/* give system time to flush error message in case there are more */
|
||||
usleep(100000);
|
||||
count++;
|
||||
}
|
||||
|
||||
/* check which map param applies */
|
||||
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
|
||||
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
|
||||
// /* give system time to flush error message in case there are more */
|
||||
// usleep(100000);
|
||||
// count++;
|
||||
// }
|
||||
total_fail_count += count;
|
||||
|
||||
/* sanity checks pass, enable channel */
|
||||
if (count) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
|
||||
warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
|
||||
usleep(100000);
|
||||
channels_failed++;
|
||||
}
|
||||
|
||||
channel_fail_count += count;
|
||||
}
|
||||
|
||||
return channel_fail_count;
|
||||
if (channels_failed) {
|
||||
sleep(2);
|
||||
mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count,
|
||||
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
return total_fail_count + map_fail_count;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user