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

Merge branch 'master' into linux

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	makefiles/firmware.mk
This commit is contained in:
Mark Charlebois
2015-04-22 10:38:46 -07:00
parent 8e500f543e 67956341e6
commit f63ab3d5e3
22 changed files with 229 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+35 -31
View File
@@ -1127,6 +1127,10 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = true;
thread_running = true;
/* update vehicle status to find out vehicle type (required for preflight checks) */
param_get(_param_sys_type, &(status.system_type)); // get system type
status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
@@ -1206,15 +1210,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* disable manual override for all systems that rely on electronic stabilization */
if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) {
status.is_rotary_wing = true;
} else {
@@ -1222,8 +1218,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* set vehicle_status.is_vtol flag */
status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR);
status.is_vtol = is_vtol(&status);
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
@@ -1424,8 +1419,8 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) {
/* Make sure that this is only adjusted if vehicle really is of type vtol*/
if (is_vtol(&status)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
}
}
@@ -1604,7 +1599,11 @@ int commander_thread_main(int argc, char *argv[])
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
mavlink_and_console_log_critical(mavlink_fd, "LOW BATTERY, LOCKING ARMING DOWN");
}
} else {
mavlink_and_console_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
}
status_changed = true;
@@ -2222,13 +2221,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
/* mode rejected, continue to evaluate the main system mode */
} else {
/* changed successfully or already in this state */
return res;
}
}
/* offboard switched off or denied, check main mode switch */
/* RTL switch overrides main switch */
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "AUTO_RTL");
/* fallback to LOITER if home position not set */
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
}
if (res != TRANSITION_DENIED) {
/* changed successfully or already in this state */
return res;
}
/* if we get here mode was rejected, continue to evaluate the main system mode */
}
/* offboard and RTL switches off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
@@ -2273,23 +2293,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break;
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode(status_local, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
src/modules/commander/commander_helper.cpp
+5
View File
@@ -86,6 +86,11 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
|| (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
}
bool is_vtol(const struct vehicle_status_s * current_status) {
return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR;
}
static int buzzer = -1;
static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message
static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence
src/modules/commander/commander_helper.h
+1
View File
@@ -51,6 +51,7 @@
bool is_multirotor(const struct vehicle_status_s *current_status);
bool is_rotary_wing(const struct vehicle_status_s *current_status);
bool is_vtol(const struct vehicle_status_s *current_status);
int buzzer_init(void);
void buzzer_deinit(void);
src/modules/mavlink/mavlink_messages.cpp
+2 -71
View File
@@ -1867,29 +1867,8 @@ protected:
struct rc_input_values rc;
if (_rc_sub->update(&_rc_time, &rc)) {
const unsigned port_width = 8;
/* deprecated message (but still needed for compatibility!) */
for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
/* channels are sent in MAVLink main loop at a fixed interval */
mavlink_rc_channels_raw_t msg;
msg.time_boot_ms = rc.timestamp_publication / 1000;
msg.port = i;
msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX;
msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
msg.rssi = rc.rssi;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
}
/* new message */
/* send RC channel data and RSSI */
mavlink_rc_channels_t msg;
msg.time_boot_ms = rc.timestamp_publication / 1000;
@@ -1913,55 +1892,7 @@ protected:
msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
/* RSSI has a max value of 100, and when Spektrum or S.BUS are
* available, the RSSI field is invalid, as they do not provide
* an RSSI measurement. Use an out of band magic value to signal
* these digital ports. XXX revise MAVLink spec to address this.
* One option would be to use the top bit to toggle between RSSI
* and input source mode.
*
* Full RSSI field: 0b 1 111 1111
*
* ^ If bit is set, RSSI encodes type + RSSI
*
* ^ These three bits encode a total of 8
* digital RC input types.
* 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
* ^ These four bits encode a total of
* 16 RSSI levels. 15 = full, 0 = no signal
*
*/
/* Initialize RSSI with the special mode level flag */
msg.rssi = (1 << 7);
/* Set RSSI */
msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
switch (rc.input_source) {
case RC_INPUT_SOURCE_PX4FMU_PPM:
/* fallthrough */
case RC_INPUT_SOURCE_PX4IO_PPM:
msg.rssi |= (0 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
msg.rssi |= (1 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_SBUS:
msg.rssi |= (2 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_ST24:
msg.rssi |= (3 << 4);
break;
case RC_INPUT_SOURCE_UNKNOWN:
// do nothing
break;
}
if (rc.rc_lost) {
/* RSSI is by definition zero */
msg.rssi = 0;
}
msg.rssi = rc.rssi;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
}
src/modules/px4iofirmware/controls.c
+22 -7
View File
@@ -99,6 +99,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
if (*st24_updated) {
/* ensure ADC RSSI is disabled */
r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);
*rssi = st24_rssi;
r_raw_rc_count = st24_channel_count;
@@ -116,14 +119,14 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
sumd_rssi = RC_INPUT_RSSI_MAX;
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
if (*sumd_updated) {
*rssi = sumd_rssi;
/* not setting RSSI since SUMD does not provide one */
r_raw_rc_count = sumd_channel_count;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
@@ -187,12 +190,20 @@ controls_tick() {
/* use 1:1 scaling on 3.3V ADC input */
unsigned mV = counts * 3300 / 4096;
/* scale to 0..253 */
rssi = mV / 13;
/* scale to 0..253 and lowpass */
rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
if (rssi > RC_INPUT_RSSI_MAX) {
rssi = RC_INPUT_RSSI_MAX;
}
}
}
#endif
/* zero RSSI if signal is lost */
if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) {
rssi = 0;
}
perf_begin(c_gather_dsm);
bool dsm_updated, st24_updated, sumd_updated;
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
@@ -215,22 +226,26 @@ controls_tick() {
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
rssi = 255;
unsigned sbus_rssi = RC_INPUT_RSSI_MAX;
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
rssi = 100;
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
rssi = 0;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
/* set RSSI to an emulated value if ADC RSSI is off */
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
rssi = sbus_rssi;
}
}
perf_end(c_gather_sbus);
src/modules/sensors/sensor_params.c
+41
View File
@@ -960,6 +960,7 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
/**
* Scaling factor for battery voltage sensor on FMU v2.
*
* @board CONFIG_ARCH_BOARD_PX4FMU_V2
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
@@ -969,6 +970,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
*
* For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063
*
* @board CONFIG_ARCH_BOARD_AEROCORE
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f);
@@ -1360,3 +1362,42 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
*
*/
PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
/**
* PWM input channel that provides RSSI.
*
* 0: do not read RSSI from input channel
* 1-18: read RSSI from specified input channel
*
* Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters.
*
* @min 0
* @max 18
* @group Radio Calibration
*
*/
PARAM_DEFINE_INT32(RC_RSSI_PWM_CHAN, 0);
/**
* Max input value for RSSI reading.
*
* Only used if RC_RSSI_PWM_CHAN > 0
*
* @min 0
* @max 2000
* @group Radio Calibration
*
*/
PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000);
/**
* Min input value for RSSI reading.
*
* Only used if RC_RSSI_PWM_CHAN > 0
*
* @min 0
* @max 2000
* @group Radio Calibration
*
*/
PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000);
src/modules/systemlib/circuit_breaker_params.c
+6 -7
View File
@@ -43,7 +43,6 @@
*/
#include <px4.h>
#include <systemlib/circuit_breaker_params.h>
/**
* Circuit breaker for power supply check
@@ -56,7 +55,7 @@
* @max 894281
* @group Circuit Breaker
*/
PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
/**
* Circuit breaker for rate controller output
@@ -69,7 +68,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
* @max 140253
* @group Circuit Breaker
*/
PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
/**
* Circuit breaker for IO safety
@@ -81,7 +80,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
* @max 22027
* @group Circuit Breaker
*/
PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
/**
* Circuit breaker for airspeed sensor
@@ -93,7 +92,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
* @max 162128
* @group Circuit Breaker
*/
PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
/**
* Circuit breaker for flight termination
@@ -106,7 +105,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
* @max 121212
* @group Circuit Breaker
*/
PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
/**
* Circuit breaker for engine failure detection
@@ -120,4 +119,4 @@ PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
* @max 284953
* @group Circuit Breaker
*/
PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
src/modules/systemlib/circuit_breaker_params.h
-7
View File
@@ -1,7 +0,0 @@
#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
#define PARAM_CBRK_GPSFAIL_DEFAULT 240024
src/modules/vtol_att_control/vtol_att_control_main.cpp
+4 -2
View File
@@ -176,7 +176,7 @@ private:
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
unsigned _motor_count; // number of motors
float _airspeed_tot;
float _tilt_control;
//*****************Member functions***********************************************************************
void task_main(); //main task
@@ -241,6 +241,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
flag_idle_mc = true;
_airspeed_tot = 0.0f;
_tilt_control = 0.0f;
memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
@@ -521,6 +522,7 @@ void VtolAttitudeControl::fill_mc_att_control_output()
//set neutral position for elevons
_actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon
_actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon
_actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control
}
/**
@@ -763,7 +765,7 @@ void VtolAttitudeControl::task_main()
vehicle_battery_poll();
if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */
if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */
_vtol_vehicle_status.vtol_in_rw_mode = true;
if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */