dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander initial class structure

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
src/modules/commander/Commander.hpp Normal file
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_ */

2
src/modules/commander/PreflightCheck.cpp
View File

@ -75,7 +75,7 @@
using namespace DriverFramework;
namespace Commander
namespace Preflight
{
static int check_calibration(DevHandle &h, const char *param_template, int &devid)

2
src/modules/commander/PreflightCheck.h
View File

@ -43,7 +43,7 @@
#pragma once
namespace Commander
namespace Preflight
{
/**
* Runs a preflight check on all sensors to see if they are properly calibrated and healthy

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

@ -36,14 +36,10 @@
*
* 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
/* commander module headers */
@ -104,7 +100,6 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.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/parameter_update.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;
/* flags */
static bool commander_initialized = false;
static volatile bool thread_should_exit = false; /**< daemon exit 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 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);
/**
* 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,
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);
/**
* @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.
*/
@ -399,12 +371,8 @@ int commander_main(int argc, char *argv[])
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
3700,
commander_thread_main,
(char * const *)&argv[0]);
Commander::main(argc, argv);
unsigned constexpr max_wait_us = 1000000;
unsigned constexpr max_wait_steps = 2000;
@ -429,10 +397,7 @@ int commander_main(int argc, char *argv[])
thread_should_exit = true;
while (thread_running) {
usleep(200000);
warnx(".");
}
Commander::main(argc, argv);
warnx("terminated.");
@ -761,10 +726,11 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
return arming_res;
}
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
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,
bool
Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety_local,
vehicle_command_s *cmd, actuator_armed_s *armed_local,
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)
{
/* 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)) &&
!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) {
/* 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;
} 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
* 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_attitude_s &attitude,
bool set_alt_only_to_lpos_ref)
const vehicle_attitude_s &attitude, bool 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
@ -1312,11 +1278,9 @@ static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &
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 arm_tune_played = false;
bool was_landed = true;
@ -1326,26 +1290,11 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
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 */
status = {};
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 */
param_t _param_sys_type = param_find("MAV_TYPE");
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 */
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_t commander_low_prio_thread;
@ -1567,8 +1482,6 @@ int commander_thread_main(int argc, char *argv[])
orb_unadvertise(mission_pub);
}
int ret;
/* Start monitoring loop */
unsigned counter = 0;
unsigned stick_off_counter = 0;
@ -1678,11 +1591,6 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s 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 */
int system_power_sub = orb_subscribe(ORB_ID(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
param_get(param_find("COM_POS_FS_GAIN"), &posctl_nav_loss_gain);
/* now initialized */
commander_initialized = true;
thread_running = true;
/* 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
} else {
// 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,
false, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
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);
while (!thread_should_exit) {
while (!should_exit()) {
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
// machine based on the arming state.
if (param_init_forced) {
auto_disarm_hysteresis.set_hysteresis_time_from(false,
(hrt_abstime)disarm_when_landed * 1000000);
auto_disarm_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)disarm_when_landed * 1000000);
}
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 */
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
/* 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,
true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
} else {
/* 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,
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;
}
/* 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 (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
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)) {
/* 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 {
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) {
/* 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 {
/* 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
* use has commenced after takeoff. */
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 */
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_changed = true;
}
@ -3340,8 +3238,10 @@ int commander_thread_main(int argc, char *argv[])
usleep(COMMANDER_MONITORING_INTERVAL);
}
thread_should_exit = true;
/* wait for threads to complete */
ret = pthread_join(commander_low_prio_thread, nullptr);
int ret = pthread_join(commander_low_prio_thread, nullptr);
if (ret) {
warn("join failed: %d", ret);
@ -3368,8 +3268,6 @@ int commander_thread_main(int argc, char *argv[])
px4_close(estimator_status_sub);
thread_running = false;
return 0;
}
void
@ -3983,7 +3881,7 @@ check_posvel_validity(bool data_valid, float data_accuracy, float required_accur
} else {
if (*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;
}
}
@ -4467,7 +4365,7 @@ void *commander_low_prio_loop(void *arg)
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,
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;
};

2
src/modules/commander/state_machine_helper.cpp
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 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),
arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot);