forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/navigator_new' into fw_autoland_att_tecs_navigator_termination_controlgroups
This commit is contained in:
Thomas Gubler
commit
7e860d8e74
|
|
@ -69,7 +69,6 @@
|
|||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
|
|
@ -194,7 +193,7 @@ void usage(const char *reason);
|
|||
/**
|
||||
* React to commands that are sent e.g. from the mavlink module.
|
||||
*/
|
||||
void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
|
||||
void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
|
||||
|
||||
/**
|
||||
* Mainloop of commander.
|
||||
|
|
@ -215,8 +214,6 @@ void print_reject_arm(const char *msg);
|
|||
|
||||
void print_status();
|
||||
|
||||
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
|
||||
|
||||
/**
|
||||
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
|
||||
*/
|
||||
|
|
@ -346,13 +343,12 @@ void print_status()
|
|||
warnx("arming: %s", armed_str);
|
||||
}
|
||||
|
||||
static orb_advert_t control_mode_pub;
|
||||
static orb_advert_t status_pub;
|
||||
|
||||
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
|
||||
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
|
||||
{
|
||||
/* result of the command */
|
||||
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
|
||||
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
|
||||
|
||||
/* only handle high-priority commands here */
|
||||
|
||||
|
|
@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
|
||||
/* set HIL state */
|
||||
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
|
||||
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
|
||||
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
|
||||
|
||||
/* if HIL got enabled, reset battery status state */
|
||||
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
|
||||
if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
|
||||
/* reset the arming mode to disarmed */
|
||||
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
|
||||
arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
|
||||
|
||||
if (arming_res != TRANSITION_DENIED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
|
||||
|
|
@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
|
||||
if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
|
||||
if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
arming_res = TRANSITION_DENIED;
|
||||
|
||||
} else {
|
||||
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
|
||||
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
|
||||
}
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
|
|
@ -401,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
} else {
|
||||
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||
arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
||||
arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
|
||||
arming_res = arming_state_transition(status, safety, new_arming_state, armed);
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
|
||||
|
|
@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
break;
|
||||
}
|
||||
|
||||
case VEHICLE_CMD_NAV_TAKEOFF: {
|
||||
if (armed->armed) {
|
||||
transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
|
||||
|
||||
if (nav_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
|
||||
}
|
||||
|
||||
if (nav_res != TRANSITION_DENIED) {
|
||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* reject TAKEOFF not armed */
|
||||
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
||||
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
|
|
@ -494,7 +467,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
arming_res = TRANSITION_DENIED;
|
||||
|
||||
} else {
|
||||
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
|
||||
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
|
||||
}
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
|
|
@ -513,7 +486,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
|
||||
|
||||
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
|
||||
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
|
||||
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
|
||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
|
|
@ -524,29 +497,22 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
}
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
||||
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
|
||||
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
|
||||
case VEHICLE_CMD_PREFLIGHT_STORAGE:
|
||||
/* ignore commands that handled in low prio loop */
|
||||
break;
|
||||
|
||||
default:
|
||||
/* warn about unsupported commands */
|
||||
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
|
||||
break;
|
||||
}
|
||||
|
||||
/* supported command handling stop */
|
||||
if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
|
||||
tune_positive();
|
||||
|
||||
} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
|
||||
/* we do not care in the high prio loop about commands we don't know */
|
||||
} else {
|
||||
tune_negative();
|
||||
|
||||
if (result == VEHICLE_CMD_RESULT_DENIED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
|
||||
|
||||
} else if (result == VEHICLE_CMD_RESULT_FAILED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
|
||||
|
||||
} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
|
||||
|
||||
}
|
||||
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
|
||||
/* already warned about unsupported commands in "default" case */
|
||||
answer_command(*cmd, result);
|
||||
}
|
||||
|
||||
/* send any requested ACKs */
|
||||
|
|
@ -564,9 +530,6 @@ static struct actuator_armed_s armed;
|
|||
|
||||
static struct safety_s safety;
|
||||
|
||||
/* flags for control apps */
|
||||
struct vehicle_control_mode_s control_mode;
|
||||
|
||||
int commander_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* not yet initialized */
|
||||
|
|
@ -609,11 +572,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* Initialize armed with all false */
|
||||
memset(&armed, 0, sizeof(armed));
|
||||
|
||||
/* Initialize all flags to false */
|
||||
memset(&control_mode, 0, sizeof(control_mode));
|
||||
|
||||
status.main_state = MAIN_STATE_MANUAL;
|
||||
status.navigation_state = NAVIGATION_STATE_DIRECT;
|
||||
status.set_nav_state = NAV_STATE_INIT;
|
||||
status.set_nav_state_timestamp = 0;
|
||||
status.arming_state = ARMING_STATE_INIT;
|
||||
status.hil_state = HIL_STATE_OFF;
|
||||
|
||||
|
|
@ -625,9 +586,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status.rc_signal_lost = true;
|
||||
status.offboard_control_signal_lost = true;
|
||||
|
||||
/* allow manual override initially */
|
||||
control_mode.flag_external_manual_override_ok = true;
|
||||
|
||||
/* set battery warning flag */
|
||||
status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
|
||||
status.condition_battery_voltage_valid = false;
|
||||
|
|
@ -635,9 +593,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
// XXX for now just set sensors as initialized
|
||||
status.condition_system_sensors_initialized = true;
|
||||
|
||||
// XXX just disable offboard control for now
|
||||
control_mode.flag_control_offboard_enabled = false;
|
||||
|
||||
/* advertise to ORB */
|
||||
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
|
||||
/* publish current state machine */
|
||||
|
|
@ -649,8 +604,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||
|
||||
control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
|
||||
|
||||
/* home position */
|
||||
orb_advert_t home_pub = -1;
|
||||
struct home_position_s home;
|
||||
|
|
@ -803,11 +756,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status.system_type == VEHICLE_TYPE_QUADROTOR ||
|
||||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
|
||||
status.system_type == VEHICLE_TYPE_OCTOROTOR) {
|
||||
control_mode.flag_external_manual_override_ok = false;
|
||||
status.is_rotary_wing = true;
|
||||
|
||||
} else {
|
||||
control_mode.flag_external_manual_override_ok = true;
|
||||
status.is_rotary_wing = false;
|
||||
}
|
||||
|
||||
|
|
@ -859,7 +810,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
// XXX this would be the right approach to do it, but do we *WANT* this?
|
||||
// /* disarm if safety is now on and still armed */
|
||||
// if (safety.safety_switch_available && !safety.safety_off) {
|
||||
// (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
|
||||
// (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
|
||||
// }
|
||||
}
|
||||
|
||||
|
|
@ -990,10 +941,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
battery_tune_played = false;
|
||||
|
||||
if (armed.armed) {
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
|
||||
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
|
||||
|
||||
} else {
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
|
||||
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
|
|
@ -1012,7 +963,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) {
|
||||
// XXX check for sensors
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
|
||||
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
|
||||
|
||||
} else {
|
||||
// XXX: Add emergency stuff if sensors are lost
|
||||
|
|
@ -1055,7 +1006,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
home.lat = (double)global_position.lat / 1e7d;
|
||||
home.lon = (double)global_position.lon / 1e7d;
|
||||
home.altitude = (float)global_position.alt / 1e3f;
|
||||
home.altitude = (float)global_position.alt;
|
||||
|
||||
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
|
||||
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
|
||||
|
|
@ -1102,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||
* do it only for rotary wings */
|
||||
if (status.is_rotary_wing &&
|
||||
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
|
||||
(status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
|
||||
(status.condition_landed && (
|
||||
status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
|
||||
status.navigation_state == NAVIGATION_STATE_VECTOR
|
||||
))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
|
||||
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
|
||||
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);
|
||||
res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
|
||||
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
|
||||
stick_off_counter = 0;
|
||||
|
||||
} else {
|
||||
|
|
@ -1132,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
|
||||
|
||||
} else {
|
||||
res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
|
||||
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
|
||||
}
|
||||
|
||||
stick_on_counter = 0;
|
||||
|
|
@ -1185,12 +1134,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
|
||||
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode);
|
||||
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
|
||||
if (flighttermination_res == TRANSITION_CHANGED) {
|
||||
tune_positive();
|
||||
}
|
||||
} else {
|
||||
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
|
||||
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -1202,32 +1151,18 @@ int commander_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||
|
||||
/* handle it */
|
||||
handle_command(&status, &safety, &control_mode, &cmd, &armed);
|
||||
}
|
||||
|
||||
/* evaluate the navigation state machine */
|
||||
transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* DENIED here indicates bug in the commander */
|
||||
warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
|
||||
mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
|
||||
handle_command(&status, &safety, &cmd, &armed);
|
||||
}
|
||||
|
||||
/* check which state machines for changes, clear "changed" flag */
|
||||
bool arming_state_changed = check_arming_state_changed();
|
||||
bool main_state_changed = check_main_state_changed();
|
||||
bool navigation_state_changed = check_navigation_state_changed();
|
||||
bool flighttermination_state_changed = check_flighttermination_state_changed();
|
||||
|
||||
hrt_abstime t1 = hrt_absolute_time();
|
||||
|
||||
if (navigation_state_changed || arming_state_changed) {
|
||||
control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
|
||||
}
|
||||
|
||||
if (arming_state_changed || main_state_changed || navigation_state_changed) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
|
||||
if (arming_state_changed || main_state_changed) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
|
|
@ -1235,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
|
||||
status.timestamp = t1;
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
|
||||
control_mode.timestamp = t1;
|
||||
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
|
||||
armed.timestamp = t1;
|
||||
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
|
||||
}
|
||||
|
|
@ -1549,133 +1482,6 @@ print_reject_arm(const char *msg)
|
|||
}
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
|
||||
{
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
|
||||
if (status->main_state == MAIN_STATE_AUTO) {
|
||||
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||
// TODO AUTO_LAND handling
|
||||
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
|
||||
/* don't switch to other states until takeoff not completed */
|
||||
// XXX: only respect the condition_landed when the local position is actually valid
|
||||
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
|
||||
return TRANSITION_NOT_CHANGED;
|
||||
}
|
||||
}
|
||||
|
||||
if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
|
||||
status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
|
||||
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
|
||||
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
|
||||
/* possibly on ground, switch to TAKEOFF if needed */
|
||||
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
|
||||
return res;
|
||||
}
|
||||
}
|
||||
|
||||
/* switch to AUTO mode */
|
||||
if (status->rc_signal_found_once && !status->rc_signal_lost) {
|
||||
/* act depending on switches when manual control enabled */
|
||||
if (status->return_switch == RETURN_SWITCH_RETURN) {
|
||||
/* RTL */
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
|
||||
|
||||
} else {
|
||||
if (status->mission_switch == MISSION_SWITCH_MISSION) {
|
||||
/* MISSION */
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
|
||||
|
||||
} else {
|
||||
/* LOITER */
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* switch to MISSION when no RC control and first time in some AUTO mode */
|
||||
if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
|
||||
status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
|
||||
status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
|
||||
status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
} else {
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* disarmed, always switch to AUTO_READY */
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* manual control modes */
|
||||
if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
|
||||
/* switch to failsafe mode */
|
||||
bool manual_control_old = control_mode->flag_control_manual_enabled;
|
||||
|
||||
if (!status->condition_landed && status->condition_local_position_valid) {
|
||||
/* in air: try to hold position if possible */
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
|
||||
|
||||
} else {
|
||||
/* landed: don't try to hold position but land (if taking off) */
|
||||
res = TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
|
||||
}
|
||||
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
|
||||
if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
|
||||
/* mark navigation state as changed to force immediate flag publishing */
|
||||
set_navigation_state_changed();
|
||||
res = TRANSITION_CHANGED;
|
||||
}
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
if (control_mode->flag_control_position_enabled) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
|
||||
|
||||
} else {
|
||||
if (status->condition_landed) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
switch (status->main_state) {
|
||||
case MAIN_STATE_MANUAL:
|
||||
res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
|
||||
break;
|
||||
|
||||
case MAIN_STATE_SEATBELT:
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
|
||||
break;
|
||||
|
||||
case MAIN_STATE_EASY:
|
||||
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
|
||||
{
|
||||
switch (result) {
|
||||
|
|
@ -1784,7 +1590,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
/* try to go to INIT/PREFLIGHT arming state */
|
||||
|
||||
// XXX disable interrupts in arming_state_transition
|
||||
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
|
||||
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||
break;
|
||||
}
|
||||
|
|
@ -1824,7 +1630,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
else
|
||||
tune_negative();
|
||||
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
|
||||
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
|
||||
|
||||
break;
|
||||
}
|
||||
|
|
@ -1876,7 +1682,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
}
|
||||
|
||||
default:
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
|
||||
/* don't answer on unsupported commands, it will be done in main loop */
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
|
|
@ -64,12 +63,10 @@ static const int ERROR = -1;
|
|||
|
||||
static bool arming_state_changed = true;
|
||||
static bool main_state_changed = true;
|
||||
static bool navigation_state_changed = true;
|
||||
static bool flighttermination_state_changed = true;
|
||||
|
||||
transition_result_t
|
||||
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
|
||||
const struct vehicle_control_mode_s *control_mode,
|
||||
arming_state_t new_arming_state, struct actuator_armed_s *armed)
|
||||
{
|
||||
/*
|
||||
|
|
@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
|
|||
} else {
|
||||
|
||||
/* enforce lockdown in HIL */
|
||||
if (control_mode->flag_system_hil_enabled) {
|
||||
if (status->hil_state == HIL_STATE_ON) {
|
||||
armed->lockdown = true;
|
||||
} else {
|
||||
armed->lockdown = false;
|
||||
|
|
@ -109,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
|
|||
/* allow coming from INIT and disarming from ARMED */
|
||||
if (status->arming_state == ARMING_STATE_INIT
|
||||
|| status->arming_state == ARMING_STATE_ARMED
|
||||
|| control_mode->flag_system_hil_enabled) {
|
||||
|| status->hil_state == HIL_STATE_ON) {
|
||||
|
||||
/* sensors need to be initialized for STANDBY state */
|
||||
if (status->condition_system_sensors_initialized) {
|
||||
|
|
@ -126,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
|
|||
/* allow arming from STANDBY and IN-AIR-RESTORE */
|
||||
if ((status->arming_state == ARMING_STATE_STANDBY
|
||||
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
|
||||
&& (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
|
||||
&& (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
|
||||
ret = TRANSITION_CHANGED;
|
||||
armed->armed = true;
|
||||
armed->ready_to_arm = true;
|
||||
|
|
@ -289,169 +286,6 @@ check_main_state_changed()
|
|||
}
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
|
||||
{
|
||||
transition_result_t ret = TRANSITION_DENIED;
|
||||
|
||||
/* only check transition if the new state is actually different from the current one */
|
||||
if (new_navigation_state == status->navigation_state) {
|
||||
ret = TRANSITION_NOT_CHANGED;
|
||||
|
||||
} else {
|
||||
|
||||
switch (new_navigation_state) {
|
||||
case NAVIGATION_STATE_DIRECT:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = false;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_altitude_enabled = false;
|
||||
control_mode->flag_control_climb_rate_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = true;
|
||||
control_mode->flag_control_auto_enabled = false;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_STABILIZE:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_altitude_enabled = false;
|
||||
control_mode->flag_control_climb_rate_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = true;
|
||||
control_mode->flag_control_auto_enabled = false;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_ALTHOLD:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_altitude_enabled = true;
|
||||
control_mode->flag_control_climb_rate_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = true;
|
||||
control_mode->flag_control_auto_enabled = false;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_VECTOR:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_altitude_enabled = true;
|
||||
control_mode->flag_control_climb_rate_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = true;
|
||||
control_mode->flag_control_auto_enabled = false;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_READY:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = false;
|
||||
control_mode->flag_control_attitude_enabled = false;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_altitude_enabled = false;
|
||||
control_mode->flag_control_climb_rate_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_auto_enabled = true;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_TAKEOFF:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_altitude_enabled = true;
|
||||
control_mode->flag_control_climb_rate_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_auto_enabled = true;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_LOITER:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_altitude_enabled = true;
|
||||
control_mode->flag_control_climb_rate_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_auto_enabled = false;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_MISSION:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_altitude_enabled = true;
|
||||
control_mode->flag_control_climb_rate_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_auto_enabled = true;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_RTL:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_altitude_enabled = true;
|
||||
control_mode->flag_control_climb_rate_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_auto_enabled = true;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_LAND:
|
||||
|
||||
/* deny transitions from landed state */
|
||||
if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
control_mode->flag_control_velocity_enabled = true;
|
||||
control_mode->flag_control_position_enabled = true;
|
||||
control_mode->flag_control_altitude_enabled = true;
|
||||
control_mode->flag_control_climb_rate_enabled = true;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
control_mode->flag_control_auto_enabled = true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (ret == TRANSITION_CHANGED) {
|
||||
status->navigation_state = new_navigation_state;
|
||||
control_mode->auto_state = status->navigation_state;
|
||||
navigation_state_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool
|
||||
check_navigation_state_changed()
|
||||
{
|
||||
if (navigation_state_changed) {
|
||||
navigation_state_changed = false;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
check_flighttermination_state_changed()
|
||||
{
|
||||
|
|
@ -464,16 +298,10 @@ check_flighttermination_state_changed()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
set_navigation_state_changed()
|
||||
{
|
||||
navigation_state_changed = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Transition from one hil state to another
|
||||
*/
|
||||
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
|
||||
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
bool valid_transition = false;
|
||||
int ret = ERROR;
|
||||
|
|
@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|
|||
|| current_status->arming_state == ARMING_STATE_STANDBY
|
||||
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
|
||||
|
||||
current_control_mode->flag_system_hil_enabled = true;
|
||||
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
|
||||
valid_transition = true;
|
||||
}
|
||||
|
|
@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|
|||
current_status->timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
||||
|
||||
current_control_mode->timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
|
||||
|
||||
// XXX also set lockdown here
|
||||
|
||||
ret = OK;
|
||||
|
|
@ -539,7 +363,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|
|||
/**
|
||||
* Transition from one flightermination state to another
|
||||
*/
|
||||
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
|
||||
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
|
||||
{
|
||||
transition_result_t ret = TRANSITION_DENIED;
|
||||
|
||||
|
|
@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
|
|||
|
||||
if (ret == TRANSITION_CHANGED) {
|
||||
flighttermination_state_changed = true;
|
||||
control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
|
||||
// TODO
|
||||
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@
|
|||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/safety.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
||||
typedef enum {
|
||||
TRANSITION_DENIED = -1,
|
||||
|
|
@ -58,7 +57,7 @@ typedef enum {
|
|||
} transition_result_t;
|
||||
|
||||
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
|
||||
const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
|
||||
arming_state_t new_arming_state, struct actuator_armed_s *armed);
|
||||
|
||||
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
|
||||
|
||||
|
|
@ -68,9 +67,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
|
|||
|
||||
bool check_main_state_changed();
|
||||
|
||||
transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
|
||||
|
||||
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode);
|
||||
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state);
|
||||
|
||||
bool check_navigation_state_changed();
|
||||
|
||||
|
|
@ -78,6 +75,6 @@ bool check_flighttermination_state_changed();
|
|||
|
||||
void set_navigation_state_changed();
|
||||
|
||||
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
|
||||
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
|
||||
|
||||
#endif /* STATE_MACHINE_HELPER_H_ */
|
||||
|
|
|
|||
|
|
@ -221,6 +221,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
|
|||
} else if (v_status.main_state == MAIN_STATE_AUTO) {
|
||||
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||
// TODO use control_mode topic
|
||||
/*
|
||||
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
|
||||
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
|
||||
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
|
||||
|
|
@ -234,6 +236,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
|
|||
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
|
||||
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
|
||||
}
|
||||
*/
|
||||
}
|
||||
*mavlink_custom_mode = custom_mode.data;
|
||||
|
||||
|
|
|
|||
|
|
@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
|
||||
|
||||
/* send heartbeat */
|
||||
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
|
||||
// TODO fix navigation state, use control_mode topic
|
||||
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state);
|
||||
|
||||
/* send status (values already copied in the section above) */
|
||||
mavlink_msg_sys_status_send(chan,
|
||||
|
|
|
|||
|
|
@ -316,7 +316,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
} else {
|
||||
if (!control_mode.flag_control_auto_enabled) {
|
||||
if (!control_mode.flag_control_attitude_enabled) {
|
||||
/* no control, try to stay on place */
|
||||
if (!control_mode.flag_control_velocity_enabled) {
|
||||
/* no velocity control, reset attitude setpoint */
|
||||
|
|
|
|||
|
|
@ -415,140 +415,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
|
|||
|
||||
/* force reprojection of global setpoint after manual mode */
|
||||
reset_mission_sp = true;
|
||||
|
||||
} else if (control_mode.flag_control_auto_enabled) {
|
||||
/* AUTO mode, use global setpoint */
|
||||
if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
|
||||
reset_auto_sp_xy = true;
|
||||
reset_auto_sp_z = true;
|
||||
|
||||
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
|
||||
if (reset_takeoff_sp) {
|
||||
reset_takeoff_sp = false;
|
||||
local_pos_sp.x = local_pos.x;
|
||||
local_pos_sp.y = local_pos.y;
|
||||
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
|
||||
att_sp.yaw_body = att.yaw;
|
||||
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
|
||||
}
|
||||
|
||||
reset_auto_sp_xy = false;
|
||||
reset_auto_sp_z = true;
|
||||
|
||||
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
|
||||
// TODO
|
||||
reset_auto_sp_xy = true;
|
||||
reset_auto_sp_z = true;
|
||||
|
||||
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
|
||||
/* init local projection using local position ref */
|
||||
if (local_pos.ref_timestamp != local_ref_timestamp) {
|
||||
reset_mission_sp = true;
|
||||
local_ref_timestamp = local_pos.ref_timestamp;
|
||||
double lat_home = local_pos.ref_lat * 1e-7;
|
||||
double lon_home = local_pos.ref_lon * 1e-7;
|
||||
map_projection_init(lat_home, lon_home);
|
||||
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
|
||||
}
|
||||
|
||||
if (reset_mission_sp) {
|
||||
reset_mission_sp = false;
|
||||
/* update global setpoint projection */
|
||||
|
||||
if (global_pos_sp_valid) {
|
||||
|
||||
/* project global setpoint to local setpoint */
|
||||
map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y));
|
||||
|
||||
if (triplet.current.altitude_is_relative) {
|
||||
local_pos_sp.z = -triplet.current.altitude;
|
||||
|
||||
} else {
|
||||
local_pos_sp.z = local_pos.ref_alt - triplet.current.lat;
|
||||
}
|
||||
/* update yaw setpoint only if value is valid */
|
||||
if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) {
|
||||
att_sp.yaw_body = triplet.current.yaw;
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
|
||||
|
||||
} else {
|
||||
if (reset_auto_sp_xy) {
|
||||
reset_auto_sp_xy = false;
|
||||
/* local position setpoint is invalid,
|
||||
* use current position as setpoint for loiter */
|
||||
local_pos_sp.x = local_pos.x;
|
||||
local_pos_sp.y = local_pos.y;
|
||||
local_pos_sp.yaw = att.yaw;
|
||||
}
|
||||
|
||||
if (reset_auto_sp_z) {
|
||||
reset_auto_sp_z = false;
|
||||
local_pos_sp.z = local_pos.z;
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
|
||||
}
|
||||
}
|
||||
|
||||
reset_auto_sp_xy = true;
|
||||
reset_auto_sp_z = true;
|
||||
}
|
||||
|
||||
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
|
||||
reset_takeoff_sp = true;
|
||||
}
|
||||
|
||||
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
|
||||
reset_mission_sp = true;
|
||||
}
|
||||
|
||||
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
|
||||
local_pos_sp.yaw = att_sp.yaw_body;
|
||||
|
||||
/* reset setpoints after AUTO mode */
|
||||
reset_man_sp_xy = true;
|
||||
reset_man_sp_z = true;
|
||||
|
||||
} else {
|
||||
/* no control (failsafe), loiter or stay on ground */
|
||||
if (local_pos.landed) {
|
||||
/* landed: move setpoint down */
|
||||
/* in air: hold altitude */
|
||||
if (local_pos_sp.z < 5.0f) {
|
||||
/* set altitude setpoint to 5m under ground,
|
||||
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
|
||||
local_pos_sp.z = 5.0f;
|
||||
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
|
||||
}
|
||||
|
||||
reset_man_sp_z = true;
|
||||
|
||||
} else {
|
||||
/* in air: hold altitude */
|
||||
if (reset_man_sp_z) {
|
||||
reset_man_sp_z = false;
|
||||
local_pos_sp.z = local_pos.z;
|
||||
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
|
||||
}
|
||||
|
||||
reset_auto_sp_z = false;
|
||||
}
|
||||
|
||||
if (control_mode.flag_control_position_enabled) {
|
||||
if (reset_man_sp_xy) {
|
||||
reset_man_sp_xy = false;
|
||||
local_pos_sp.x = local_pos.x;
|
||||
local_pos_sp.y = local_pos.y;
|
||||
local_pos_sp.yaw = att.yaw;
|
||||
att_sp.yaw_body = att.yaw;
|
||||
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
|
||||
}
|
||||
|
||||
reset_auto_sp_xy = false;
|
||||
}
|
||||
}
|
||||
/* AUTO not implemented */
|
||||
|
||||
/* publish local position setpoint */
|
||||
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@
|
|||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
|
|
@ -145,8 +146,10 @@ private:
|
|||
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
|
||||
orb_advert_t _fence_pub; /**< publish fence topic */
|
||||
orb_advert_t _mission_result_pub; /**< publish mission result topic */
|
||||
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
|
||||
|
||||
struct vehicle_status_s _vstatus; /**< vehicle status */
|
||||
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
|
||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||
struct home_position_s _home_pos; /**< home position for RTL */
|
||||
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
|
||||
|
|
@ -168,6 +171,8 @@ private:
|
|||
|
||||
MissionFeasibilityChecker missionFeasiblityChecker;
|
||||
|
||||
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
|
||||
|
||||
struct {
|
||||
float min_altitude;
|
||||
float loiter_radius;
|
||||
|
|
@ -192,21 +197,10 @@ private:
|
|||
MAX_EVENT
|
||||
};
|
||||
|
||||
enum State {
|
||||
STATE_INIT,
|
||||
STATE_NONE,
|
||||
STATE_LOITER,
|
||||
STATE_MISSION,
|
||||
STATE_MISSION_LOITER,
|
||||
STATE_RTL,
|
||||
STATE_RTL_LOITER,
|
||||
MAX_STATE
|
||||
};
|
||||
|
||||
/**
|
||||
* State machine transition table
|
||||
*/
|
||||
static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT];
|
||||
static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
|
|
@ -300,6 +294,10 @@ private:
|
|||
*/
|
||||
void publish_mission_item_triplet();
|
||||
|
||||
/**
|
||||
* Publish vehicle_control_mode topic for controllers
|
||||
*/
|
||||
void publish_control_mode();
|
||||
|
||||
|
||||
/**
|
||||
|
|
@ -328,7 +326,7 @@ Navigator *g_navigator;
|
|||
Navigator::Navigator() :
|
||||
|
||||
/* state machine transition table */
|
||||
StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT),
|
||||
StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
|
||||
|
||||
_task_should_exit(false),
|
||||
_navigator_task(-1),
|
||||
|
|
@ -347,6 +345,7 @@ Navigator::Navigator() :
|
|||
_triplet_pub(-1),
|
||||
_fence_pub(-1),
|
||||
_mission_result_pub(-1),
|
||||
_control_mode_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
|
||||
|
|
@ -357,7 +356,8 @@ Navigator::Navigator() :
|
|||
_mission(),
|
||||
_waypoint_position_reached(false),
|
||||
_waypoint_yaw_reached(false),
|
||||
_time_first_inside_orbit(0)
|
||||
_time_first_inside_orbit(0),
|
||||
_set_nav_state_timestamp(0)
|
||||
{
|
||||
memset(&_fence, 0, sizeof(_fence));
|
||||
|
||||
|
|
@ -375,7 +375,7 @@ Navigator::Navigator() :
|
|||
memset(&_mission_result, 0, sizeof(struct mission_result_s));
|
||||
|
||||
/* Initialize state machine */
|
||||
myState = STATE_INIT;
|
||||
myState = NAV_STATE_INIT;
|
||||
}
|
||||
|
||||
Navigator::~Navigator()
|
||||
|
|
@ -513,7 +513,6 @@ Navigator::task_main()
|
|||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
|
||||
|
||||
|
||||
/* copy all topics first time */
|
||||
vehicle_status_update();
|
||||
|
|
@ -524,9 +523,6 @@ Navigator::task_main()
|
|||
offboard_mission_update(_vstatus.is_rotary_wing);
|
||||
onboard_mission_update();
|
||||
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vstatus_sub, 200);
|
||||
/* rate limit position updates to 50 Hz */
|
||||
orb_set_interval(_global_pos_sub, 20);
|
||||
|
||||
|
|
@ -575,49 +571,41 @@ Navigator::task_main()
|
|||
|
||||
/* Evaluate state machine from commander and set the navigator mode accordingly */
|
||||
if (_vstatus.main_state == MAIN_STATE_AUTO) {
|
||||
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
|
||||
/* commander requested new navigation mode, try to set it */
|
||||
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
|
||||
|
||||
switch (_vstatus.navigation_state) {
|
||||
case NAVIGATION_STATE_DIRECT:
|
||||
case NAVIGATION_STATE_STABILIZE:
|
||||
case NAVIGATION_STATE_ALTHOLD:
|
||||
case NAVIGATION_STATE_VECTOR:
|
||||
|
||||
/* don't publish a mission item triplet */
|
||||
dispatch(EVENT_NONE_REQUESTED);
|
||||
switch (_vstatus.set_nav_state) {
|
||||
case NAV_STATE_INIT:
|
||||
case NAV_STATE_NONE:
|
||||
/* nothing to do */
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_READY:
|
||||
case NAVIGATION_STATE_AUTO_TAKEOFF:
|
||||
case NAVIGATION_STATE_AUTO_MISSION:
|
||||
|
||||
/* try mission if none is available, fallback to loiter instead */
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_LOITER:
|
||||
|
||||
case NAV_STATE_LOITER:
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
break;
|
||||
|
||||
case NAV_STATE_MISSION:
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_RTL:
|
||||
|
||||
case NAV_STATE_RTL:
|
||||
dispatch(EVENT_RTL_REQUESTED);
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_AUTO_LAND:
|
||||
|
||||
/* TODO add this */
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
warnx("ERROR: Navigation state not supported");
|
||||
warnx("ERROR: Requested navigation state not supported");
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* try mission, if none is available fallback to loiter instead */
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -632,10 +620,8 @@ Navigator::task_main()
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
/* only update parameters if it changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
||||
parameters_update();
|
||||
/* note that these new parameters won't be in effect until a mission triplet is published again */
|
||||
}
|
||||
|
|
@ -670,9 +656,7 @@ Navigator::task_main()
|
|||
if (mission_item_reached()) {
|
||||
|
||||
if (_vstatus.main_state == MAIN_STATE_AUTO &&
|
||||
(_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY ||
|
||||
_vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF ||
|
||||
_vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) {
|
||||
(myState == NAV_STATE_MISSION)) {
|
||||
|
||||
/* advance by one mission item */
|
||||
_mission.move_to_next();
|
||||
|
|
@ -688,6 +672,9 @@ Navigator::task_main()
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
publish_control_mode();
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
|
|
@ -740,25 +727,25 @@ Navigator::status()
|
|||
}
|
||||
|
||||
switch (myState) {
|
||||
case STATE_INIT:
|
||||
case NAV_STATE_INIT:
|
||||
warnx("State: Init");
|
||||
break;
|
||||
case STATE_NONE:
|
||||
case NAV_STATE_NONE:
|
||||
warnx("State: None");
|
||||
break;
|
||||
case STATE_LOITER:
|
||||
case NAV_STATE_LOITER:
|
||||
warnx("State: Loiter");
|
||||
break;
|
||||
case STATE_MISSION:
|
||||
case NAV_STATE_MISSION:
|
||||
warnx("State: Mission");
|
||||
break;
|
||||
case STATE_MISSION_LOITER:
|
||||
case NAV_STATE_MISSION_LOITER:
|
||||
warnx("State: Loiter after Mission");
|
||||
break;
|
||||
case STATE_RTL:
|
||||
case NAV_STATE_RTL:
|
||||
warnx("State: RTL");
|
||||
break;
|
||||
case STATE_RTL_LOITER:
|
||||
case NAV_STATE_RTL_LOITER:
|
||||
warnx("State: Loiter after RTL");
|
||||
break;
|
||||
default:
|
||||
|
|
@ -857,76 +844,76 @@ Navigator::fence_point(int argc, char *argv[])
|
|||
|
||||
|
||||
|
||||
StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = {
|
||||
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
|
||||
{
|
||||
/* STATE_INIT */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT},
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
|
||||
},
|
||||
{
|
||||
/* STATE_NONE */
|
||||
/* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE},
|
||||
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
|
||||
},
|
||||
{
|
||||
/* STATE_LOITER */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER},
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
|
||||
},
|
||||
{
|
||||
/* STATE_MISSION */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION},
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
|
||||
},
|
||||
{
|
||||
/* STATE_MISSION_LOITER */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER},
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
|
||||
},
|
||||
{
|
||||
/* STATE_RTL */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL},
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL},
|
||||
},
|
||||
{
|
||||
/* STATE_RTL_LOITER */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
},
|
||||
};
|
||||
|
||||
|
|
@ -949,7 +936,7 @@ Navigator::start_loiter()
|
|||
|
||||
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
|
||||
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
|
||||
_mission_item_triplet.current.yaw = 0.0f;
|
||||
_mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined?
|
||||
|
||||
get_loiter_item(&_mission_item_triplet.current);
|
||||
|
||||
|
|
@ -1239,6 +1226,76 @@ Navigator::publish_mission_item_triplet()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::publish_control_mode()
|
||||
{
|
||||
/* update vehicle_control_mode topic*/
|
||||
_control_mode.main_state = _vstatus.main_state;
|
||||
_control_mode.nav_state = static_cast<nav_state_t>(myState);
|
||||
_control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR;
|
||||
_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
|
||||
_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
|
||||
|
||||
_control_mode.flag_control_offboard_enabled = false;
|
||||
_control_mode.flag_control_flighttermination_enabled = false;
|
||||
|
||||
switch (_vstatus.main_state) {
|
||||
case MAIN_STATE_MANUAL:
|
||||
_control_mode.flag_control_manual_enabled = true;
|
||||
_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
|
||||
_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
|
||||
_control_mode.flag_control_altitude_enabled = false;
|
||||
_control_mode.flag_control_climb_rate_enabled = false;
|
||||
_control_mode.flag_control_position_enabled = false;
|
||||
_control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
|
||||
case MAIN_STATE_SEATBELT:
|
||||
_control_mode.flag_control_manual_enabled = true;
|
||||
_control_mode.flag_control_rates_enabled = true;
|
||||
_control_mode.flag_control_attitude_enabled = true;
|
||||
_control_mode.flag_control_altitude_enabled = true;
|
||||
_control_mode.flag_control_climb_rate_enabled = true;
|
||||
_control_mode.flag_control_position_enabled = false;
|
||||
_control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
|
||||
case MAIN_STATE_EASY:
|
||||
_control_mode.flag_control_manual_enabled = true;
|
||||
_control_mode.flag_control_rates_enabled = true;
|
||||
_control_mode.flag_control_attitude_enabled = true;
|
||||
_control_mode.flag_control_altitude_enabled = true;
|
||||
_control_mode.flag_control_climb_rate_enabled = true;
|
||||
_control_mode.flag_control_position_enabled = true;
|
||||
_control_mode.flag_control_velocity_enabled = true;
|
||||
break;
|
||||
|
||||
case MAIN_STATE_AUTO:
|
||||
_control_mode.flag_control_manual_enabled = false;
|
||||
_control_mode.flag_control_rates_enabled = true;
|
||||
_control_mode.flag_control_attitude_enabled = true;
|
||||
_control_mode.flag_control_position_enabled = true;
|
||||
_control_mode.flag_control_velocity_enabled = true;
|
||||
_control_mode.flag_control_altitude_enabled = true;
|
||||
_control_mode.flag_control_climb_rate_enabled = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
_control_mode.timestamp = hrt_absolute_time();
|
||||
|
||||
/* lazily publish the mission triplet only once available */
|
||||
if (_control_mode_pub > 0) {
|
||||
/* publish the mission triplet */
|
||||
orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
|
||||
if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON &&
|
||||
|
|
|
|||
|
|
@ -969,7 +969,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
// Don't orb_copy, it's already done few lines above
|
||||
log_msg.msg_type = LOG_STAT_MSG;
|
||||
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
|
||||
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
|
||||
// TODO use control_mode topic
|
||||
//log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
|
||||
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
|
||||
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
|
||||
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@
|
|||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
#include "vehicle_status.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics @{
|
||||
|
|
@ -59,10 +60,25 @@
|
|||
*
|
||||
* Encodes the complete system state and is set by the commander app.
|
||||
*/
|
||||
|
||||
typedef enum {
|
||||
NAV_STATE_INIT = 0,
|
||||
NAV_STATE_NONE,
|
||||
NAV_STATE_LOITER,
|
||||
NAV_STATE_MISSION,
|
||||
NAV_STATE_MISSION_LOITER,
|
||||
NAV_STATE_RTL,
|
||||
NAV_STATE_RTL_LOITER,
|
||||
NAV_STATE_MAX
|
||||
} nav_state_t;
|
||||
|
||||
struct vehicle_control_mode_s
|
||||
{
|
||||
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
|
||||
|
||||
main_state_t main_state;
|
||||
nav_state_t nav_state;
|
||||
|
||||
bool flag_armed;
|
||||
|
||||
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
|
||||
|
|
@ -79,9 +95,6 @@ struct vehicle_control_mode_s
|
|||
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
|
||||
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
|
||||
bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */
|
||||
|
||||
bool flag_control_auto_enabled; // TEMP
|
||||
uint8_t auto_state; // TEMP navigation state for AUTO modes
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -66,20 +66,6 @@ typedef enum {
|
|||
MAIN_STATE_AUTO,
|
||||
} main_state_t;
|
||||
|
||||
/* navigation state machine */
|
||||
typedef enum {
|
||||
NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization
|
||||
NAVIGATION_STATE_STABILIZE, // attitude stabilization
|
||||
NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
|
||||
NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
|
||||
NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff
|
||||
NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode
|
||||
NAVIGATION_STATE_AUTO_LOITER, // pause mission
|
||||
NAVIGATION_STATE_AUTO_MISSION, // fly mission
|
||||
NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND
|
||||
NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector)
|
||||
} navigation_state_t;
|
||||
|
||||
typedef enum {
|
||||
ARMING_STATE_INIT = 0,
|
||||
ARMING_STATE_STANDBY,
|
||||
|
|
@ -95,7 +81,6 @@ typedef enum {
|
|||
HIL_STATE_ON
|
||||
} hil_state_t;
|
||||
|
||||
|
||||
typedef enum {
|
||||
FLIGHTTERMINATION_STATE_OFF = 0,
|
||||
FLIGHTTERMINATION_STATE_ON
|
||||
|
|
@ -182,7 +167,8 @@ struct vehicle_status_s
|
|||
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
|
||||
|
||||
main_state_t main_state; /**< main state machine */
|
||||
navigation_state_t navigation_state; /**< navigation state machine */
|
||||
unsigned int set_nav_state; /**< set navigation state machine to specified value */
|
||||
uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */
|
||||
arming_state_t arming_state; /**< current arming state */
|
||||
hil_state_t hil_state; /**< current hil state */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue