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

Commander: Fix reposition handling, run faster to allow catching of consecutive commands

Browse Source
This commit is contained in:
Lorenz Meier 2016-05-09 23:01:06 +02:00
parent b9333d95f4
commit ffb0d37c8a
1 changed files with 43 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -138,7 +138,7 @@ 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_INTERVAL 10000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
#define MAVLINK_OPEN_INTERVAL 50000
@ -210,7 +210,8 @@ static struct commander_state_s internal_state = {};
static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
static unsigned _last_mission_instance = 0;
static manual_control_setpoint_s _last_sp_man = {};
struct manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint
static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch
static struct vtol_vehicle_status_s vtol_status = {};
@ -262,7 +263,7 @@ void get_circuit_breaker_params();
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
void set_control_mode();
@ -677,7 +678,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// the data at the exact same time.
// Check if a mode switch had been requested
if ((((uint8_t)cmd->param1) & 1) > 0) {
if ((((uint32_t)cmd->param2) & 1) > 0) {
transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
if ((main_ret != TRANSITION_DENIED)) {
@ -837,12 +838,20 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
else {
// Refuse to arm if preflight checks have failed
if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) {
if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) {
mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed.");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
// Refuse to arm if in manual with non-zero throttle
if ((status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO) && sp_man.z > 0.1f) {
mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Manual throttle non-zero.");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
}
transition_result_t arming_res = arm_disarm(cmd_arms,&mavlink_log_pub, "arm/disarm component command");
@ -1384,7 +1393,6 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
memset(&sp_man, 0, sizeof(sp_man));
/* Subscribe to offboard control data */
@ -2446,7 +2454,7 @@ int commander_thread_main(int argc, char *argv[])
/* evaluate the main state machine according to mode switches */
bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0);
transition_result_t main_res = set_main_state_rc(&status, &sp_man);
transition_result_t main_res = set_main_state_rc(&status);
/* play tune on mode change only if armed, blink LED always */
if (main_res == TRANSITION_CHANGED || first_rc_eval) {
@ -2958,7 +2966,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
}
transition_result_t
set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man)
set_main_state_rc(struct vehicle_status_s *status_local)
{
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
@ -2971,34 +2979,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
/* manual setpoint has not updated, do not re-evaluate it */
if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man->timestamp)) ||
((_last_sp_man.offboard_switch == sp_man->offboard_switch) &&
(_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) &&
(_last_sp_man.mode_slot == sp_man->mode_slot))) {
if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) ||
((_last_sp_man.offboard_switch == sp_man.offboard_switch) &&
(_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) &&
(_last_sp_man.mode_slot == sp_man.mode_slot))) {
// update these fields for the geofence system
if (!rtl_on) {
_last_sp_man.timestamp = sp_man->timestamp;
_last_sp_man.x = sp_man->x;
_last_sp_man.y = sp_man->y;
_last_sp_man.z = sp_man->z;
_last_sp_man.r = sp_man->r;
_last_sp_man.timestamp = sp_man.timestamp;
_last_sp_man.x = sp_man.x;
_last_sp_man.y = sp_man.y;
_last_sp_man.z = sp_man.z;
_last_sp_man.r = sp_man.r;
}
/* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED;
}
_last_sp_man = *sp_man;
_last_sp_man = sp_man;
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
if (res == TRANSITION_DENIED) {
@ -3012,7 +3020,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
/* RTL switch overrides main switch */
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
warnx("RTL switch changed and ON!");
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);
@ -3032,14 +3040,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
/* we know something has changed - check if we are in mode slot operation */
if (sp_man->mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
if (sp_man->mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {
warnx("overflow");
if (sp_man.mode_slot >= sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0])) {
warnx("m slot overflow");
return TRANSITION_DENIED;
}
int new_mode = _flight_mode_slots[sp_man->mode_slot];
int new_mode = _flight_mode_slots[sp_man.mode_slot];
if (new_mode < 0) {
/* slot is unused */
@ -3171,13 +3179,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
/* offboard and RTL switches off or denied, check main mode switch */
switch (sp_man->mode_switch) {
switch (sp_man.mode_switch) {
case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
@ -3192,7 +3200,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
}
else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
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) {
@ -3208,7 +3216,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break;
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {
@ -3225,7 +3233,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break; // changed successfully or already in this mode
}
if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
print_reject_mode(status_local, "ALTITUDE CONTROL");
}
@ -3235,7 +3243,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->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, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
if (res != TRANSITION_DENIED) {