Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

commander initial class structure

Browse Source
This commit is contained in:
Daniel Agar
2017-12-20 15:39:20 -05:00
committed by Lorenz Meier
parent 55be098e3b
commit 294fbc46a9
5 changed files with 187 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.hpp
+102
View File
@@ -0,0 +1,102 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef COMMANDER_HPP_
#define COMMANDER_HPP_
#include <controllib/blocks.hpp>
#include <px4_module.h>
// publications
#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/topics/geofence_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;
using uORB::Publication;
using uORB::Subscription;
class Commander : public control::SuperBlock, public ModuleBase<Commander>
{
public:
Commander() :
SuperBlock(nullptr, "COM")
{
updateParams();
}
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static Commander *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
void enable_hil();
private:
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,
orb_advert_t *command_ack_pub, bool *changed);
bool set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude, bool set_alt_only_to_lpos_ref);
};
#endif /* COMMANDER_HPP_ */
src/modules/commander/PreflightCheck.cpp
+1 -1
View File
@@ -75,7 +75,7 @@
using namespace DriverFramework; using namespace DriverFramework;
namespace Commander namespace Preflight
{ {
static int check_calibration(DevHandle &h, const char *param_template, int &devid) static int check_calibration(DevHandle &h, const char *param_template, int &devid)
src/modules/commander/PreflightCheck.h
+1 -1
View File
@@ -43,7 +43,7 @@
#pragma once #pragma once
namespace Commander namespace Preflight
{ {
/** /**
* Runs a preflight check on all sensors to see if they are properly calibrated and healthy * Runs a preflight check on all sensors to see if they are properly calibrated and healthy
src/modules/commander/commander.cpp
+82 -135
View File
@@ -36,14 +36,10 @@
* *
* Main state machine / business logic * Main state machine / business logic
* *
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton@px4.io>
* @author Sander Smeets <sander@droneslab.com>
*/ */
#include "Commander.hpp"
#include <cmath> // NAN #include <cmath> // NAN
/* commander module headers */ /* commander module headers */
@@ -104,7 +100,6 @@
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h> #include <uORB/topics/mission_result.h>
#include <uORB/topics/offboard_control_mode.h> #include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/power_button_state.h> #include <uORB/topics/power_button_state.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h> #include <uORB/topics/safety.h>
@@ -184,10 +179,9 @@ static orb_advert_t mavlink_log_pub = nullptr;
static orb_advert_t power_button_state_pub = nullptr; static orb_advert_t power_button_state_pub = nullptr;
/* flags */ /* flags */
static bool commander_initialized = false;
static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_should_exit = false; /**< daemon exit flag */
static volatile bool thread_running = false; /**< daemon status flag */ static volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
static bool _usb_telemetry_active = false; static bool _usb_telemetry_active = false;
static hrt_abstime commander_boot_timestamp = 0; static hrt_abstime commander_boot_timestamp = 0;
@@ -276,19 +270,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]);
*/ */
void usage(const char *reason); void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, bool *changed);
/**
* Mainloop of commander.
*/
int commander_thread_main(int argc, char *argv[]);
void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
battery_status_s *battery_local, const cpuload_s *cpuload_local); battery_status_s *battery_local, const cpuload_s *cpuload_local);
@@ -327,15 +308,6 @@ void print_status();
transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy); transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy);
/**
* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
* time the vehicle is armed with a good GPS fix.
**/
static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude,
bool set_alt_only_to_lpos_ref);
/** /**
* Loop that runs at a lower rate and priority for calibration and parameter tasks. * Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/ */
@@ -399,12 +371,8 @@ int commander_main(int argc, char *argv[])
} }
thread_should_exit = false; thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT, Commander::main(argc, argv);
SCHED_PRIORITY_DEFAULT + 40,
3700,
commander_thread_main,
(char * const *)&argv[0]);
unsigned constexpr max_wait_us = 1000000; unsigned constexpr max_wait_us = 1000000;
unsigned constexpr max_wait_steps = 2000; unsigned constexpr max_wait_steps = 2000;
@@ -429,10 +397,7 @@ int commander_main(int argc, char *argv[])
thread_should_exit = true; thread_should_exit = true;
while (thread_running) { Commander::main(argc, argv);
usleep(200000);
warnx(".");
}
warnx("terminated."); warnx("terminated.");
@@ -761,10 +726,11 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
return arming_res; return arming_res;
} }
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, bool
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, vehicle_command_s *cmd, actuator_armed_s *armed_local,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, 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,
orb_advert_t *command_ack_pub, bool *changed) orb_advert_t *command_ack_pub, bool *changed)
{ {
/* only handle commands that are meant to be handled by this system and component */ /* only handle commands that are meant to be handled by this system and component */
@@ -974,7 +940,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) &&
!home->manual_home) { !home->manual_home) {
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false); set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false);
} }
} }
} }
@@ -1043,7 +1009,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (use_current) { if (use_current) {
/* use current position */ /* use current position */
if (commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false)) { if (set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -1250,10 +1216,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
* @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each * @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each
* time the vehicle is armed with a good GPS fix. * time the vehicle is armed with a good GPS fix.
**/ **/
static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &home, bool
Commander::set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude, const vehicle_attitude_s &attitude, bool set_alt_only_to_lpos_ref)
bool set_alt_only_to_lpos_ref)
{ {
if (!set_alt_only_to_lpos_ref) { if (!set_alt_only_to_lpos_ref) {
//Need global and local position fix to be able to set home //Need global and local position fix to be able to set home
@@ -1312,11 +1278,9 @@ static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &
return true; return true;
} }
int commander_thread_main(int argc, char *argv[]) void
Commander::run()
{ {
/* not yet initialized */
commander_initialized = false;
bool sensor_fail_tune_played = false; bool sensor_fail_tune_played = false;
bool arm_tune_played = false; bool arm_tune_played = false;
bool was_landed = true; bool was_landed = true;
@@ -1326,26 +1290,11 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized // XXX for now just set sensors as initialized
status_flags.condition_system_sensors_initialized = true; status_flags.condition_system_sensors_initialized = true;
#ifdef __PX4_NUTTX
/* NuttX indicates 3 arguments when only 2 are present */
argc -= 1;
argv += 1;
#endif
/* vehicle status topic */ /* vehicle status topic */
status = {}; status = {};
status.hil_state = vehicle_status_s::HIL_STATE_OFF; status.hil_state = vehicle_status_s::HIL_STATE_OFF;
if (argc > 2) {
if (!strcmp(argv[2],"--hil")) {
status.hil_state = vehicle_status_s::HIL_STATE_ON;
} else {
PX4_ERR("Argument %s not supported, abort.", argv[2]);
thread_should_exit = true;
}
}
/* set parameters */ /* set parameters */
param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_sys_type = param_find("MAV_TYPE");
param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_system_id = param_find("MAV_SYS_ID");
@@ -1397,40 +1346,6 @@ int commander_thread_main(int argc, char *argv[])
/* failsafe response to loss of navigation accuracy */ /* failsafe response to loss of navigation accuracy */
param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL"); param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL");
// These are too verbose, but we will retain them a little longer
// until we are sure we really don't need them.
// const char *main_states_str[commander_state_s::MAIN_STATE_MAX];
// main_states_str[commander_state_s::MAIN_STATE_MANUAL] = "MANUAL";
// main_states_str[commander_state_s::MAIN_STATE_ALTCTL] = "ALTCTL";
// main_states_str[commander_state_s::MAIN_STATE_POSCTL] = "POSCTL";
// main_states_str[commander_state_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
// main_states_str[commander_state_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
// main_states_str[commander_state_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
// main_states_str[commander_state_s::MAIN_STATE_ACRO] = "ACRO";
// main_states_str[commander_state_s::MAIN_STATE_STAB] = "STAB";
// main_states_str[commander_state_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
// const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF] = "AUTO_TAKEOFF";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LAND] = "LAND";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION";
// nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
/* pthread for slow low prio thread */ /* pthread for slow low prio thread */
pthread_t commander_low_prio_thread; pthread_t commander_low_prio_thread;
@@ -1567,8 +1482,6 @@ int commander_thread_main(int argc, char *argv[])
orb_unadvertise(mission_pub); orb_unadvertise(mission_pub);
} }
int ret;
/* Start monitoring loop */ /* Start monitoring loop */
unsigned counter = 0; unsigned counter = 0;
unsigned stick_off_counter = 0; unsigned stick_off_counter = 0;
@@ -1678,11 +1591,6 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s info; struct subsystem_info_s info;
memset(&info, 0, sizeof(info)); memset(&info, 0, sizeof(info));
/* Subscribe to position setpoint triplet */
int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
struct position_setpoint_triplet_s pos_sp_triplet;
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
/* Subscribe to system power */ /* Subscribe to system power */
int system_power_sub = orb_subscribe(ORB_ID(system_power)); int system_power_sub = orb_subscribe(ORB_ID(system_power));
struct system_power_s system_power; struct system_power_s system_power;
@@ -1724,8 +1632,6 @@ int commander_thread_main(int argc, char *argv[])
posctl_nav_loss_prob = init_param_val * sec_to_usec; // convert to uSec posctl_nav_loss_prob = init_param_val * sec_to_usec; // convert to uSec
param_get(param_find("COM_POS_FS_GAIN"), &posctl_nav_loss_gain); param_get(param_find("COM_POS_FS_GAIN"), &posctl_nav_loss_gain);
/* now initialized */
commander_initialized = true;
thread_running = true; thread_running = true;
/* update vehicle status to find out vehicle type (required for preflight checks) */ /* update vehicle status to find out vehicle type (required for preflight checks) */
@@ -1769,7 +1675,7 @@ int commander_thread_main(int argc, char *argv[])
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else { } else {
// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet // sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, status_flags.condition_system_sensors_initialized = Preflight::preflightCheck(&mavlink_log_pub, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check,
false, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp)); false, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
@@ -1835,7 +1741,7 @@ int commander_thread_main(int argc, char *argv[])
arm_auth_init(&mavlink_log_pub, &status.system_id); arm_auth_init(&mavlink_log_pub, &status.system_id);
while (!thread_should_exit) { while (!should_exit()) {
arming_ret = TRANSITION_NOT_CHANGED; arming_ret = TRANSITION_NOT_CHANGED;
@@ -1907,8 +1813,7 @@ int commander_thread_main(int argc, char *argv[])
// After that it will be set in the main state // After that it will be set in the main state
// machine based on the arming state. // machine based on the arming state.
if (param_init_forced) { if (param_init_forced) {
auto_disarm_hysteresis.set_hysteresis_time_from(false, auto_disarm_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)disarm_when_landed * 1000000);
(hrt_abstime)disarm_when_landed * 1000000);
} }
param_get(_param_low_bat_act, &low_bat_action); param_get(_param_low_bat_act, &low_bat_action);
@@ -2037,12 +1942,12 @@ int commander_thread_main(int argc, char *argv[])
/* provide RC and sensor status feedback to the user */ /* provide RC and sensor status feedback to the user */
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
/* HITL configuration: check only RC input */ /* HITL configuration: check only RC input */
(void)Commander::preflightCheck(&mavlink_log_pub, false, false, Preflight::preflightCheck(&mavlink_log_pub, false, false,
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false,
true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp)); true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
} else { } else {
/* check sensors also */ /* check sensors also */
(void)Commander::preflightCheck(&mavlink_log_pub, true, checkAirspeed, Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed,
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT,
true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
} }
@@ -2512,13 +2417,6 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true; status_changed = true;
} }
/* update position setpoint triplet */
orb_check(pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
}
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, arming_ret = arming_state_transition(&status,
@@ -3145,7 +3043,7 @@ int commander_thread_main(int argc, char *argv[])
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); set_home_position(home_pub, _home, local_position, global_position, attitude, false);
} }
} else { } else {
if (status_flags.condition_home_position_valid) { if (status_flags.condition_home_position_valid) {
@@ -3159,12 +3057,12 @@ int commander_thread_main(int argc, char *argv[])
if (home_dist_xy > local_position.epv * 2 || home_dist_z > local_position.eph * 2) { if (home_dist_xy > local_position.epv * 2 || home_dist_z > local_position.eph * 2) {
/* update when disarmed, landed and moved away from current home position */ /* update when disarmed, landed and moved away from current home position */
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); set_home_position(home_pub, _home, local_position, global_position, attitude, false);
} }
} }
} else { } else {
/* First time home position update - but only if disarmed */ /* First time home position update - but only if disarmed */
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); set_home_position(home_pub, _home, local_position, global_position, attitude, false);
} }
} }
@@ -3172,7 +3070,7 @@ int commander_thread_main(int argc, char *argv[])
* This allows home atitude to be used in the calculation of height above takeoff location when GPS * This allows home atitude to be used in the calculation of height above takeoff location when GPS
* use has commenced after takeoff. */ * use has commenced after takeoff. */
if (!_home.valid_alt && local_position.z_global) { if (!_home.valid_alt && local_position.z_global) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, true); set_home_position(home_pub, _home, local_position, global_position, attitude, true);
} }
} }
@@ -3307,7 +3205,7 @@ int commander_thread_main(int argc, char *argv[])
} }
/* update timeout flag */ /* update timeout flag */
if(!(hotplug_timeout == status_flags.condition_system_hotplug_timeout)) { if (!(hotplug_timeout == status_flags.condition_system_hotplug_timeout)) {
status_flags.condition_system_hotplug_timeout = hotplug_timeout; status_flags.condition_system_hotplug_timeout = hotplug_timeout;
status_changed = true; status_changed = true;
} }
@@ -3340,8 +3238,10 @@ int commander_thread_main(int argc, char *argv[])
usleep(COMMANDER_MONITORING_INTERVAL); usleep(COMMANDER_MONITORING_INTERVAL);
} }
thread_should_exit = true;
/* wait for threads to complete */ /* wait for threads to complete */
ret = pthread_join(commander_low_prio_thread, nullptr); int ret = pthread_join(commander_low_prio_thread, nullptr);
if (ret) { if (ret) {
warn("join failed: %d", ret); warn("join failed: %d", ret);
@@ -3368,8 +3268,6 @@ int commander_thread_main(int argc, char *argv[])
px4_close(estimator_status_sub); px4_close(estimator_status_sub);
thread_running = false; thread_running = false;
return 0;
} }
void void
@@ -3983,7 +3881,7 @@ check_posvel_validity(bool data_valid, float data_accuracy, float required_accur
} else { } else {
if (*probation_time_us < POSVEL_PROBATION_MIN) { if (*probation_time_us < POSVEL_PROBATION_MIN) {
*probation_time_us = POSVEL_PROBATION_MIN; *probation_time_us = POSVEL_PROBATION_MIN;
} else if (*probation_time_us > POSVEL_PROBATION_MAX) { } else if (*probation_time_us > POSVEL_PROBATION_MAX) {
*probation_time_us = POSVEL_PROBATION_MAX; *probation_time_us = POSVEL_PROBATION_MAX;
} }
} }
@@ -4467,7 +4365,7 @@ void *commander_low_prio_loop(void *arg)
checkAirspeed = true; checkAirspeed = true;
} }
status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, checkAirspeed, status_flags.condition_system_sensors_initialized = Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed,
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT,
true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
@@ -4699,3 +4597,52 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status
} }
} }
} }
int Commander::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int Commander::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
return 0;
}
int Commander::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
3000,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return 0;
}
Commander *Commander::instantiate(int argc, char *argv[])
{
Commander *instance = new Commander();
if (instance) {
if (argc >= 2 && !strcmp(argv[1], "--hil")) {
instance->enable_hil();
}
}
return instance;
}
void Commander::enable_hil()
{
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
};
src/modules/commander/state_machine_helper.cpp
+1 -1
View File
@@ -1111,7 +1111,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF); bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF);
bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, sensor_checks, bool preflight_ok = Preflight::preflightCheck(mavlink_log_pub, sensor_checks,
checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot); arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot);