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

Merged master into linux

Browse Source
This commit is contained in:
Lorenz Meier 2015-05-19 21:00:02 +02:00
commit d906fb8f31
16 changed files with 218 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ROMFS/px4fmu_common/init.d/rcS
View File

@ -302,6 +302,18 @@ then
# Sensors System (start before Commander so Preflight checks are properly run)
#
sh /etc/init.d/rc.sensors
if [ $GPS == yes ]
then
if [ $GPS_FAKE == yes ]
then
echo "[i] Faking GPS"
gps start -f
else
gps start
fi
fi
unset GPS_FAKE
# Needs to be this early for in-air-restarts
commander start
@ -479,22 +491,10 @@ then
sh /etc/init.d/rc.uavcan
#
# Logging, GPS
# Logging
#
sh /etc/init.d/rc.logging
if [ $GPS == yes ]
then
if [ $GPS_FAKE == yes ]
then
echo "[i] Faking GPS"
gps start -f
else
gps start
fi
fi
unset GPS_FAKE
#
# Start up ARDrone Motor interface
#

22
src/drivers/gps/gps.cpp
View File

@ -354,6 +354,20 @@ GPS::task_main()
if (_Helper->configure(_baudrate) == 0) {
unlock();
//Publish initial report that we have access to a GPS
//Make sure to clear any stale data in case driver is reset
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.timestamp_time = hrt_absolute_time();
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
@ -364,13 +378,7 @@ GPS::task_main()
if (!(_pub_blocked)) {
if (helper_ret & 1) {
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
}
if (_p_report_sat_info && (helper_ret & 2)) {
if (_report_sat_info_pub > 0) {

2
src/drivers/l3gd20/l3gd20.cpp
View File

@ -195,7 +195,7 @@ static const int ERROR = -1;
This time reduction is enough to cope with worst case timing jitter
due to other timers
*/
#define L3GD20_TIMER_REDUCTION 200
#define L3GD20_TIMER_REDUCTION 600
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }

41
src/modules/commander/PreflightCheck.cpp
View File

@ -49,6 +49,7 @@
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
@ -62,6 +63,7 @@
#include <drivers/drv_airspeed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
@ -270,8 +272,38 @@ out:
return success;
}
static bool gnssCheck(int mavlink_fd)
{
bool success = true;
int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
//Wait up to 2000ms to allow the driver to detect a GNSS receiver module
struct pollfd fds[1];
fds[0].fd = gpsSub;
fds[0].events = POLLIN;
if(poll(fds, 1, 2000) <= 0) {
success = false;
}
else {
struct vehicle_gps_position_s gps;
if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
(hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {
success = false;
}
}
//Report failure to detect module
if(!success) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}
close(gpsSub);
return success;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
{
bool failed = false;
@ -337,6 +369,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if(checkGNSS) {
if(!gnssCheck(mavlink_fd)) {
failed = true;
}
}
/* Report status */
return !failed;
}

6
src/modules/commander/PreflightCheck.h
View File

@ -59,11 +59,15 @@ namespace Commander
* true if the gyroscopes should be checked
* @param checkBaro
* true if the barometer should be checked
* @param checkAirspeed
* true if the airspeed sensor should be checked
* @param checkRC
* true if the Remote Controller should be checked
* @param checkGNSS
* true if the GNSS receiver should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false);
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
const unsigned max_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3;

73
src/modules/commander/accelerometer_calibration.cpp
View File

@ -135,6 +135,7 @@
#include <fcntl.h>
//#include <sys/prctl.h>
#include <math.h>
#include <poll.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <string.h>
@ -145,6 +146,7 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@ -554,3 +556,74 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
return calibrate_return_ok;
}
int do_level_calibration(int mavlink_fd) {
const unsigned cal_time = 5;
const unsigned cal_hz = 250;
const unsigned settle_time = 30;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level");
param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF");
float zero = 0.0f;
param_set(roll_offset_handler, &zero);
param_set(pitch_offset_handler, &zero);
struct pollfd fds[1];
fds[0].fd = att_sub;
fds[0].events = POLLIN;
float roll_mean = 0.0f;
float pitch_mean = 0.0f;
unsigned counter = 0;
// sleep for some time
hrt_abstime start = hrt_absolute_time();
while(hrt_elapsed_time(&start) < settle_time * 1000000) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
sleep(settle_time / 10);
}
start = hrt_absolute_time();
// average attitude for 5 seconds
while(hrt_elapsed_time(&start) < cal_time * 1000000) {
poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
roll_mean += att.roll;
pitch_mean += att.pitch;
counter++;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;
} else {
mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
return 1;
}
if (fabsf(roll_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess roll angle");
return 1;
} else if (fabsf(pitch_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle");
return 1;
}
else {
roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG;
param_set(roll_offset_handler, &roll_mean);
param_set(pitch_offset_handler, &pitch_mean);
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
return 0;
}

1
src/modules/commander/accelerometer_calibration.h
View File

@ -45,5 +45,6 @@
#include <stdint.h>
int do_accel_calibration(int mavlink_fd);
int do_level_calibration(int mavlink_fd);
#endif /* ACCELEROMETER_CALIBRATION_H_ */

52
src/modules/commander/commander.cpp
View File

@ -130,6 +130,8 @@ static const int ERROR = -1;
extern struct system_load_s system_load;
static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */
/* Decouple update interval and hysteris counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 50000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
@ -322,6 +324,8 @@ int commander_main(int argc, char *argv[])
calib_ret = do_accel_calibration(mavlink_fd);
} else if (!strcmp(argv[2], "gyro")) {
calib_ret = do_gyro_calibration(mavlink_fd);
} else if (!strcmp(argv[2], "level")) {
calib_ret = do_level_calibration(mavlink_fd);
} else {
warnx("argument %s unsupported.", argv[2]);
}
@ -923,6 +927,7 @@ int commander_thread_main(int argc, char *argv[])
status.circuit_breaker_engaged_airspd_check = false;
status.circuit_breaker_engaged_enginefailure_check = false;
status.circuit_breaker_engaged_gpsfailure_check = false;
get_circuit_breaker_params();
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@ -1124,8 +1129,6 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_sys_type, &(status.system_type)); // get system type
status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
get_circuit_breaker_params();
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
@ -1134,7 +1137,7 @@ int commander_thread_main(int argc, char *argv[])
}
// Run preflight check
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
@ -1307,7 +1310,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* provide RC and sensor status feedback to the user */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true);
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@ -1644,19 +1647,31 @@ int commander_thread_main(int argc, char *argv[])
(float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
}
/* check if GPS fix is ok */
if (status.circuit_breaker_engaged_gpsfailure_check ||
(gps_position.fix_type >= 3 &&
hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
/* handle the case where gps was regained */
if (status.gps_failure) {
status.gps_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps regained");
/* check if GPS is ok */
if (!status.circuit_breaker_engaged_gpsfailure_check) {
bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE;
//Check if GPS receiver is too noisy while we are disarmed
if (!armed.armed && gpsIsNoisy) {
if (!status.gps_failure) {
mavlink_log_critical(mavlink_fd, "GPS signal noisy");
set_tune_override(TONE_GPS_WARNING_TUNE);
//GPS suffers from signal jamming or excessive noise, disable GPS-aided flight
status.gps_failure = true;
status_changed = true;
}
}
} else {
if (!status.gps_failure) {
if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
/* handle the case where gps was regained */
if (status.gps_failure && !gpsIsNoisy) {
status.gps_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps regained");
}
} else if (!status.gps_failure) {
status.gps_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps fix lost");
@ -2716,7 +2731,10 @@ void *commander_low_prio_loop(void *arg)
/* accelerometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_accel_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_level_calibration(mavlink_fd);
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@ -2760,7 +2778,7 @@ void *commander_low_prio_loop(void *arg)
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);

2
src/modules/commander/state_machine_helper.cpp
View File

@ -739,5 +739,5 @@ 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, true, true);
return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status->circuit_breaker_engaged_gpsfailure_check, true);
}

17
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -37,8 +36,8 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Parameters defined by the fixed-wing attitude control task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomas@px4.io>
*/
#include <px4_config.h>
@ -73,6 +72,8 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
* This defines how much the elevator input will be commanded depending on the
* current body angular rate error.
*
* @min 0.005
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
@ -144,6 +145,8 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f);
* This defines how much the aileron input will be commanded depending on the
* current body angular rate error.
*
* @min 0.005
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
@ -190,6 +193,8 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
* This defines how much the rudder input will be commanded depending on the
* current body angular rate error.
*
* @min 0.005
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
@ -234,7 +239,9 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
/**
* Roll rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
* Direct feed forward from rate setpoint to control surface output. Use this
* to obtain a tigher response of the controller without introducing
* noise amplification.
*
* @min 0.0
* @max 10.0

3
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -389,8 +389,7 @@ MavlinkFTP::_workList(PayloadHeader* payload)
#else
case DT_DIR:
#endif
// XXX @DonLakeFlyer: Remove the first condition for the test setup
if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
// Don't bother sending these back
direntType = kDirentSkip;
} else {

4
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -40,7 +40,7 @@
* Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.

2
src/modules/mc_att_control/mc_att_control_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

2
src/modules/navigator/mission_params.c
View File

@ -110,4 +110,4 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
* @max 3
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_YAWMODE, 0);
PARAM_DEFINE_INT32(MIS_YAWMODE, 1);

2
src/modules/sensors/sensors.cpp
View File

@ -860,7 +860,7 @@ Sensors::parameters_update()
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = _board_rotation * board_rotation_offset;
_board_rotation = board_rotation_offset * _board_rotation;
/* update barometer qnh setting */
param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));

16
src/modules/systemlib/circuit_breaker_params.c
View File

@ -120,3 +120,19 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
/**
* Circuit breaker for GPS failure detection
*
* Setting this parameter to 240024 will disable the GPS failure detection.
* If this check is enabled, then the sensor check will fail if the GPS module
* is missing. It will also check for excessive signal noise on the GPS receiver
* and warn the user if detected.
*
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 240024
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);