mirror of https://github.com/ArduPilot/ardupilot
Blimp: remove HIL support
This commit is contained in:
parent
055e82276e
commit
d66220e3fc
|
@ -199,12 +199,6 @@ void Blimp::ten_hz_logging_loop()
|
||||||
// twentyfive_hz_logging - should be run at 25hz
|
// twentyfive_hz_logging - should be run at 25hz
|
||||||
void Blimp::twentyfive_hz_logging()
|
void Blimp::twentyfive_hz_logging()
|
||||||
{
|
{
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED
|
|
||||||
// HIL for a blimp needs very fast update of the servo values
|
|
||||||
gcs().send_message(MSG_SERVO_OUTPUT_RAW);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if HIL_MODE == HIL_MODE_DISABLED
|
|
||||||
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||||
Log_Write_EKF_POS();
|
Log_Write_EKF_POS();
|
||||||
}
|
}
|
||||||
|
@ -212,8 +206,6 @@ void Blimp::twentyfive_hz_logging()
|
||||||
if (should_log(MASK_LOG_IMU)) {
|
if (should_log(MASK_LOG_IMU)) {
|
||||||
AP::ins().Write_IMU();
|
AP::ins().Write_IMU();
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// three_hz_loop - 3.3hz loop
|
// three_hz_loop - 3.3hz loop
|
||||||
|
@ -247,13 +239,6 @@ void Blimp::one_hz_loop()
|
||||||
|
|
||||||
void Blimp::read_AHRS(void)
|
void Blimp::read_AHRS(void)
|
||||||
{
|
{
|
||||||
// Perform IMU calculations and get attitude info
|
|
||||||
//-----------------------------------------------
|
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED
|
|
||||||
// update hil before ahrs update
|
|
||||||
gcs().update();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// we tell AHRS to skip INS update as we have already done it in fast_loop()
|
// we tell AHRS to skip INS update as we have already done it in fast_loop()
|
||||||
ahrs.update(true);
|
ahrs.update(true);
|
||||||
}
|
}
|
||||||
|
|
|
@ -53,10 +53,6 @@ MAV_MODE GCS_MAVLINK_Blimp::base_mode() const
|
||||||
// override if stick mixing is enabled
|
// override if stick mixing is enabled
|
||||||
_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED
|
|
||||||
_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// we are armed if we are not initialising
|
// we are armed if we are not initialising
|
||||||
if (blimp.motors != nullptr && blimp.motors->armed()) {
|
if (blimp.motors != nullptr && blimp.motors->armed()) {
|
||||||
_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||||
|
@ -304,8 +300,8 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
|
||||||
AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 0),
|
AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 0),
|
||||||
|
|
||||||
// @Param: RAW_CTRL
|
// @Param: RAW_CTRL
|
||||||
// @DisplayName: Raw Control stream rate to ground station
|
// @DisplayName: Unused
|
||||||
// @Description: Stream rate of RC_CHANNELS_SCALED (HIL only) to ground station
|
// @Description: Unused
|
||||||
// @Units: Hz
|
// @Units: Hz
|
||||||
// @Range: 0 10
|
// @Range: 0 10
|
||||||
// @Increment: 1
|
// @Increment: 1
|
||||||
|
|
|
@ -22,13 +22,6 @@
|
||||||
# define ADVANCED_FAILSAFE DISABLED
|
# define ADVANCED_FAILSAFE DISABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
|
||||||
// HIL_MODE OPTIONAL
|
|
||||||
|
|
||||||
#ifndef HIL_MODE
|
|
||||||
#define HIL_MODE HIL_MODE_DISABLED
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef ARMING_DELAY_SEC
|
#ifndef ARMING_DELAY_SEC
|
||||||
# define ARMING_DELAY_SEC 2.0f
|
# define ARMING_DELAY_SEC 2.0f
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -27,10 +27,6 @@ enum autopilot_yaw_mode {
|
||||||
#define MULTICOPTER_FRAME 1
|
#define MULTICOPTER_FRAME 1
|
||||||
#define HELI_FRAME 2
|
#define HELI_FRAME 2
|
||||||
|
|
||||||
// HIL enumerations
|
|
||||||
#define HIL_MODE_DISABLED 0
|
|
||||||
#define HIL_MODE_SENSORS 1
|
|
||||||
|
|
||||||
// Tuning enumeration
|
// Tuning enumeration
|
||||||
enum tuning_func {
|
enum tuning_func {
|
||||||
TUNING_NONE = 0, //
|
TUNING_NONE = 0, //
|
||||||
|
|
|
@ -72,18 +72,6 @@ void Blimp::init_ardupilot()
|
||||||
AP::compass().set_log_bit(MASK_LOG_COMPASS);
|
AP::compass().set_log_bit(MASK_LOG_COMPASS);
|
||||||
AP::compass().init();
|
AP::compass().init();
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED
|
|
||||||
while (barometer.get_last_update() == 0) {
|
|
||||||
// the barometer begins updating when we get the first
|
|
||||||
// HIL_STATE message
|
|
||||||
gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
|
|
||||||
delay(1000);
|
|
||||||
}
|
|
||||||
|
|
||||||
// set INS to HIL mode
|
|
||||||
ins.set_hil_mode();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// read Baro pressure at ground
|
// read Baro pressure at ground
|
||||||
//-----------------------------
|
//-----------------------------
|
||||||
barometer.set_log_baro_bit(MASK_LOG_IMU);
|
barometer.set_log_baro_bit(MASK_LOG_IMU);
|
||||||
|
|
Loading…
Reference in New Issue