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

commander move mission_result to class

Browse Source
This commit is contained in:
Daniel Agar 2017-12-26 16:48:34 -05:00 committed by Lorenz Meier
parent f49c061a3c
commit fe6006e22f
2 changed files with 46 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/commander/Commander.hpp
View File

@ -38,22 +38,23 @@
#include <px4_module.h>
// publications
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/Publication.hpp>
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/Subscription.hpp>
using control::BlockParamFloat;
using control::BlockParamInt;
@ -64,7 +65,8 @@ class Commander : public control::SuperBlock, public ModuleBase<Commander>
{
public:
Commander() :
SuperBlock(nullptr, "COM")
SuperBlock(nullptr, "COM"),
_mission_result_sub(ORB_ID(mission_result), 0, 0, &getSubscriptions())
{
updateParams();
}
@ -88,6 +90,9 @@ public:
private:
// Subscriptions
Subscription<mission_result_s> _mission_result_sub;
bool handle_command(vehicle_status_s *status, const safety_s *safety, vehicle_command_s *cmd,
actuator_armed_s *armed, home_position_s *home, vehicle_global_position_s *global_pos,
vehicle_local_position_s *local_pos, vehicle_attitude_s *attitude, orb_advert_t *home_pub,

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

@ -229,8 +229,6 @@ static struct home_position_s _home = {};
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];
static struct commander_state_s internal_state = {};
static struct mission_result_s _mission_result = {};
static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
static manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint
@ -1119,7 +1117,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
if (status_flags.condition_auto_mission_available) {
// requested first mission item valid
if (PX4_ISFINITE(cmd->param1) && (cmd->param1 >= -1) && (cmd->param1 < _mission_result.seq_total)) {
if (PX4_ISFINITE(cmd->param1) && (cmd->param1 >= -1) && (cmd->param1 < _mission_result_sub.get().seq_total)) {
// switch to AUTO_MISSION and ARM
if ((TRANSITION_DENIED != main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state))
@ -1459,9 +1457,6 @@ Commander::run()
safety.safety_switch_available = false;
safety.safety_off = false;
/* Subscribe to mission result topic */
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
/* Subscribe to geofence result topic */
int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
struct geofence_result_s geofence_result;
@ -1909,8 +1904,9 @@ Commander::run()
}
// Provide feedback on mission state
if ((_mission_result.timestamp > commander_boot_timestamp) && hotplug_timeout &&
(_mission_result.instance_count > 0) && !_mission_result.valid) {
const mission_result_s& mission_result = _mission_result_sub.get();
if ((mission_result.timestamp > commander_boot_timestamp) && hotplug_timeout &&
(mission_result.instance_count > 0) && !mission_result.valid) {
mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
}
@ -2440,47 +2436,41 @@ Commander::run()
}
/* start mission result check */
orb_check(mission_result_sub, &updated);
const auto prev_mission_instance_count = _mission_result_sub.get().instance_count;
if (_mission_result_sub.update()) {
const mission_result_s& mission_result = _mission_result_sub.get();
if (updated) {
mission_result_s new_mission_result = {};
// if mission_result is valid for the current mission
const bool mission_result_ok = (mission_result.timestamp > commander_boot_timestamp) && (mission_result.instance_count > 0);
if (orb_copy(ORB_ID(mission_result), mission_result_sub, &new_mission_result) == PX4_OK) {
status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid;
// if mission_result is valid for the current mission
const bool mission_result_ok = (new_mission_result.timestamp > commander_boot_timestamp) && (new_mission_result.instance_count > 0);
if (mission_result_ok) {
status_flags.condition_auto_mission_available = mission_result_ok && new_mission_result.valid;
if (status.mission_failure != mission_result.failure) {
status.mission_failure = mission_result.failure;
status_changed = true;
if (mission_result_ok) {
if (status.mission_failure != new_mission_result.failure) {
status.mission_failure = new_mission_result.failure;
status_changed = true;
if (status.mission_failure) {
mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed");
}
}
/* Only evaluate mission state if home is set */
if (status_flags.condition_home_position_valid &&
(_mission_result.instance_count != new_mission_result.instance_count)) {
if (!status_flags.condition_auto_mission_available) {
/* the mission is invalid */
tune_mission_fail(true);
} else if (new_mission_result.warning) {
/* the mission has a warning */
tune_mission_fail(true);
} else {
/* the mission is valid */
tune_mission_ok(true);
}
if (status.mission_failure) {
mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed");
}
}
_mission_result = new_mission_result;
/* Only evaluate mission state if home is set */
if (status_flags.condition_home_position_valid &&
(prev_mission_instance_count != mission_result.instance_count)) {
if (!status_flags.condition_auto_mission_available) {
/* the mission is invalid */
tune_mission_fail(true);
} else if (mission_result.warning) {
/* the mission has a warning */
tune_mission_fail(true);
} else {
/* the mission is valid */
tune_mission_ok(true);
}
}
}
}
@ -2597,7 +2587,7 @@ Commander::run()
/* Check for mission flight termination */
if (armed.armed && _mission_result.flight_termination &&
if (armed.armed && _mission_result_sub.get().flight_termination &&
!status_flags.circuit_breaker_flight_termination_disabled) {
armed.force_failsafe = true;
@ -2891,11 +2881,11 @@ Commander::run()
* as finished even though we only just started with the takeoff. Therefore, we also
* check the timestamp of the mission_result topic. */
if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
&& (_mission_result.timestamp > internal_state.timestamp)
&& _mission_result.finished) {
&& (_mission_result_sub.get().timestamp > internal_state.timestamp)
&& _mission_result_sub.get().finished) {
const bool mission_available = (_mission_result.timestamp > commander_boot_timestamp)
&& (_mission_result.instance_count > 0) && _mission_result.valid;
const bool mission_available = (_mission_result_sub.get().timestamp > commander_boot_timestamp)
&& (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid;
if ((takeoff_complete_act == 1) && mission_available) {
main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);
@ -3049,8 +3039,8 @@ Commander::run()
&internal_state,
&mavlink_log_pub,
(link_loss_actions_t)datalink_loss_act,
_mission_result.finished,
_mission_result.stay_in_failsafe,
_mission_result_sub.get().finished,
_mission_result_sub.get().stay_in_failsafe,
&status_flags,
land_detector.landed,
(link_loss_actions_t)rc_loss_act,