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

Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Browse Source 
Conflicts:
	src/modules/mavlink/mavlink_main.cpp
	src/modules/mavlink/mavlink_main.h
	src/modules/mavlink/mavlink_receiver.cpp
This commit is contained in:
Thomas Gubler
2014-08-11 10:33:11 +02:00
parent 7bc0e26734 5abdacc907
commit 8bbaacb1e9
73 changed files with 2901 additions and 1900 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+28 -24
View File
@@ -124,7 +124,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
@@ -163,7 +163,8 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static float eph_epv_threshold = 5.0f;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
static struct vehicle_status_s status;
static struct actuator_armed_s armed;
@@ -335,6 +336,7 @@ void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
/* read all relevant states */
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -391,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -758,6 +760,7 @@ int commander_thread_main(int argc, char *argv[])
// CIRCUIT BREAKERS
status.circuit_breaker_engaged_power_check = false;
status.circuit_breaker_engaged_airspd_check = false;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -999,6 +1002,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_component_id, &(status.component_id));
status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status_changed = true;
@@ -1103,7 +1107,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1128,32 +1132,32 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
bool eph_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
if (global_position.eph > eph_threshold * 2.5f) {
eph_good = false;
} else {
eph_epv_good = true;
eph_good = true;
}
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
if (global_position.eph < eph_threshold) {
eph_good = true;
} else {
eph_epv_good = false;
eph_good = false;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@@ -1183,8 +1187,8 @@ int commander_thread_main(int argc, char *argv[])
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_global_position_valid) {
if (local_position.eph > eph_epv_threshold * 2.0f) {
if (status.condition_local_position_valid) {
if (local_position.eph > eph_threshold * 2.5f) {
local_eph_good = false;
} else {
@@ -1192,7 +1196,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
if (local_position.eph < eph_epv_threshold) {
if (local_position.eph < eph_threshold) {
local_eph_good = true;
} else {
@@ -1306,14 +1310,14 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1327,7 +1331,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1386,7 +1390,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1412,7 +1416,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1524,7 +1528,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;
@@ -2184,7 +2188,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -2247,7 +2251,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+5 -5
View File
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",
src/modules/commander/state_machine_helper.cpp
+10 -7
View File
@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
int prearm_ret = OK;
/* only perform the check if we have to */
if (new_arming_state == ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
@@ -669,7 +670,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
goto system_eval;
}
if (!status->is_rotary_wing) {
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = orb_subscribe(ORB_ID(airspeed));
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -57,7 +57,7 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);