zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ArduPlane: remove HIL support

c415-sdk
Peter Barker 4 years ago committed by Andrew Tridgell
parent
cdbf6d216e
commit
055e82276e
9 changed files with 4 additions and 205 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      ArduPlane/ArduPlane.cpp
  2. 122
      ArduPlane/GCS_Mavlink.cpp
  3. 2
      ArduPlane/GCS_Mavlink.h
  4. 26
      ArduPlane/Parameters.cpp
  5. 12
      ArduPlane/Parameters.h
  6. 3
      ArduPlane/Plane.h
  7. 4
      ArduPlane/config.h
  8. 13
      ArduPlane/servos.cpp
  9. 20
      ArduPlane/system.cpp

7
ArduPlane/ArduPlane.cpp
Unescape View File

@ -133,13 +133,6 @@ void Plane::ahrs_update() @@ -133,13 +133,6 @@ void Plane::ahrs_update()
{
arming.update_soft_armed();
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// update hil before AHRS update
gcs().update_receive();
}
#endif
ahrs.update();
if (should_log(MASK_LOG_IMU)) {

122
ArduPlane/GCS_Mavlink.cpp
Unescape View File

@ -74,12 +74,6 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const @@ -74,12 +74,6 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
}
}
#if HIL_SUPPORT
if (plane.g.hil_mode == 1) {
_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
#endif
// we are armed if we are not initialising
if (plane.control_mode != &plane.mode_initializing && plane.arming.is_armed()) {
_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
@ -216,26 +210,6 @@ void GCS_MAVLINK_Plane::send_position_target_global_int() @@ -216,26 +210,6 @@ void GCS_MAVLINK_Plane::send_position_target_global_int()
}
void Plane::send_servo_out(mavlink_channel_t chan)
{
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with
// HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
millis(),
0, // port 0
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) / 4500.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f),
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / 4500.0f),
0,
0,
0,
0,
rssi.read_receiver_rssi_uint8());
}
float GCS_MAVLINK_Plane::vfr_hud_airspeed() const
{
// airspeed sensors are best. While the AHRS airspeed_estimate
@ -271,33 +245,11 @@ float GCS_MAVLINK_Plane::vfr_hud_climbrate() const @@ -271,33 +245,11 @@ float GCS_MAVLINK_Plane::vfr_hud_climbrate() const
return AP::baro().get_climb_rate();
}
/*
keep last HIL_STATE message to allow sending SIM_STATE
*/
#if HIL_SUPPORT
static mavlink_hil_state_t last_hil_state;
#endif
// report simulator state
void GCS_MAVLINK_Plane::send_simstate() const
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
GCS_MAVLINK::send_simstate();
#elif HIL_SUPPORT
if (plane.g.hil_mode == 1) {
mavlink_msg_simstate_send(chan,
last_hil_state.roll,
last_hil_state.pitch,
last_hil_state.yaw,
last_hil_state.xacc*0.001f*GRAVITY_MSS,
last_hil_state.yacc*0.001f*GRAVITY_MSS,
last_hil_state.zacc*0.001f*GRAVITY_MSS,
last_hil_state.rollspeed,
last_hil_state.pitchspeed,
last_hil_state.yawspeed,
last_hil_state.lat,
last_hil_state.lon);
}
#endif
}
@ -403,12 +355,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) @@ -403,12 +355,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
switch (id) {
case MSG_SERVO_OUT:
#if HIL_SUPPORT
if (plane.g.hil_mode == 1) {
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
plane.send_servo_out(chan);
}
#endif
// unused
break;
case MSG_TERRAIN:
@ -629,14 +576,6 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -629,14 +576,6 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
MAV_STREAM_TERMINATOR // must have this at end of stream_entries
};
bool GCS_MAVLINK_Plane::in_hil_mode() const
{
#if HIL_SUPPORT
return plane.g.hil_mode == 1;
#endif
return false;
}
/*
handle a request to switch to guided mode. This happens via a
callback from handle_mission_item()
@ -1162,65 +1101,6 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) @@ -1162,65 +1101,6 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg)
break;
}
case MAVLINK_MSG_ID_HIL_STATE:
{
#if HIL_SUPPORT
if (plane.g.hil_mode != 1) {
break;
}
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(&msg, &packet);
// sanity check location
if (!check_latlng(packet.lat, packet.lon)) {
break;
}
last_hil_state = packet;
// set gps hil sensor
const Location loc{packet.lat, packet.lon, packet.alt/10, Location::AltFrame::ABSOLUTE};
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
// setup airspeed pressure based on 3D speed, no wind
plane.airspeed.setHIL(sq(vel.length()) / 2.0f + 2013);
plane.gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000,
loc, vel, 10, 0);
// rad/sec
Vector3f gyros;
gyros.x = packet.rollspeed;
gyros.y = packet.pitchspeed;
gyros.z = packet.yawspeed;
// m/s/s
Vector3f accels;
accels.x = packet.xacc * GRAVITY_MSS*0.001f;
accels.y = packet.yacc * GRAVITY_MSS*0.001f;
accels.z = packet.zacc * GRAVITY_MSS*0.001f;
plane.ins.set_gyro(0, gyros);
plane.ins.set_accel(0, accels);
plane.barometer.setHIL(packet.alt*0.001f);
plane.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
plane.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
// cope with DCM getting badly off due to HIL lag
if (plane.g.hil_err_limit > 0 &&
(fabsf(packet.roll - plane.ahrs.roll) > ToRad(plane.g.hil_err_limit) ||
fabsf(packet.pitch - plane.ahrs.pitch) > ToRad(plane.g.hil_err_limit) ||
wrap_PI(fabsf(packet.yaw - plane.ahrs.yaw)) > ToRad(plane.g.hil_err_limit))) {
plane.ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw);
}
#endif
break;
}
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS:
{

2
ArduPlane/GCS_Mavlink.h
Unescape View File

@ -27,8 +27,6 @@ protected: @@ -27,8 +27,6 @@ protected:
void send_position_target_global_int() override;
virtual bool in_hil_mode() const override;
void send_aoa_ssa();
void send_attitude() const override;
void send_simstate() const override;

26
ArduPlane/Parameters.cpp
Unescape View File

@ -740,32 +740,6 @@ const AP_Param::Info Plane::var_info[] = { @@ -740,32 +740,6 @@ const AP_Param::Info Plane::var_info[] = {
GSCALAR(override_safety, "OVERRIDE_SAFETY", 1),
#endif
#if HIL_SUPPORT
// @Param: HIL_MODE
// @DisplayName: HIL mode enable
// @Description: This enables and disables hardware in the loop mode. If HIL_MODE is 1 then on the next reboot all sensors are replaced with HIL sensors which come from the GCS.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @RebootRequired: True
GSCALAR(hil_mode, "HIL_MODE", 0),
#endif
// @Param: HIL_SERVOS
// @DisplayName: HIL Servos enable
// @Description: This controls whether real servo controls are used in HIL mode. If you enable this then the APM will control the real servos in HIL mode. If disabled it will report servo values, but will not output to the real servos. Be careful that your motor and propeller are not connected if you enable this option.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
GSCALAR(hil_servos, "HIL_SERVOS", 0),
// @Param: HIL_ERR_LIMIT
// @DisplayName: Limit of error in HIL attitude before reset
// @Description: This controls the maximum error in degrees on any axis before HIL will reset the DCM attitude to match the HIL_STATE attitude. This limit will prevent poor timing on HIL from causing a major attitude error. If the value is zero then no limit applies.
// @Units: deg
// @Range: 0 90
// @Increment: 0.1
// @User: Advanced
GSCALAR(hil_err_limit, "HIL_ERR_LIMIT", 5),
// @Param: RTL_AUTOLAND
// @DisplayName: RTL auto land
// @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location.

12
ArduPlane/Parameters.h
Unescape View File

@ -79,7 +79,7 @@ public: @@ -79,7 +79,7 @@ public:
k_param_takeoff_throttle_min_accel,
k_param_takeoff_heading_hold, // unused
k_param_level_roll_limit,
k_param_hil_servos,
k_param_hil_servos_unused, // unused
k_param_vtail_output, // unused
k_param_nav_controller,
k_param_elevon_output, // unused
@ -94,7 +94,7 @@ public: @@ -94,7 +94,7 @@ public:
k_param_ground_steer_alt,
k_param_ground_steer_dps,
k_param_rally_limit_km_old, //unused anymore -- just holding this index
k_param_hil_err_limit,
k_param_hil_err_limit_unused, // unused
k_param_sonar_old, // unused
k_param_log_bitmask,
k_param_BoardConfig,
@ -130,7 +130,7 @@ public: @@ -130,7 +130,7 @@ public:
k_param_optflow,
k_param_cli_enabled_old, // unused - CLI removed
k_param_trim_rc_at_start, // unused
k_param_hil_mode,
k_param_hil_mode_unused, // unused
k_param_land_disarm_delay, // unused - moved to AP_Landing
k_param_glide_slope_threshold,
k_param_rudder_only,
@ -361,8 +361,6 @@ public: @@ -361,8 +361,6 @@ public:
AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay;
AP_Float hil_err_limit;
AP_Int8 rtl_autoland;
AP_Int8 crash_accel_threshold;
@ -449,10 +447,6 @@ public: @@ -449,10 +447,6 @@ public:
AP_Int32 RTL_altitude_cm;
AP_Int16 pitch_trim_cd;
AP_Int16 FBWB_min_altitude_cm;
AP_Int8 hil_servos;
#if HIL_SUPPORT
AP_Int8 hil_mode;
#endif
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;

3
ArduPlane/Plane.h
Unescape View File

@ -854,9 +854,6 @@ private: @@ -854,9 +854,6 @@ private:
void calc_nav_yaw_course(void);
void calc_nav_yaw_ground(void);
// GCS_Mavlink.cpp
void send_servo_out(mavlink_channel_t chan);
// Log.cpp
void Log_Write_Fast(void);
void Log_Write_Attitude(void);

4
ArduPlane/config.h
Unescape View File

@ -257,10 +257,6 @@ @@ -257,10 +257,6 @@
# define FENCE_TRIGGERED_PIN -1
#endif
#ifndef HIL_SUPPORT
# define HIL_SUPPORT !HAL_MINIMIZE_FEATURES
#endif
//////////////////////////////////////////////////////////////////////////////
// Parachute release
#ifndef PARACHUTE

13
ArduPlane/servos.cpp
Unescape View File

@ -869,19 +869,6 @@ void Plane::set_servos(void) @@ -869,19 +869,6 @@ void Plane::set_servos(void)
}
}
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// get the servos to the GCS immediately for HIL
if (HAVE_PAYLOAD_SPACE(MAVLINK_COMM_0, RC_CHANNELS_SCALED)) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
// we don't run the output mixer
return;
}
}
#endif
if (landing.get_then_servos_neutral() > 0 &&
control_mode == &mode_auto &&
landing.get_disarm_delay() > 0 &&

20
ArduPlane/system.cpp
Unescape View File

@ -20,15 +20,6 @@ void Plane::init_ardupilot() @@ -20,15 +20,6 @@ void Plane::init_ardupilot()
g2.stats.init();
#endif
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// set sensors to HIL mode
ins.set_hil_mode();
compass.set_hil_mode();
barometer.set_hil_mode();
}
#endif
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
// setup any board specific drivers
@ -366,17 +357,6 @@ void Plane::check_short_failsafe() @@ -366,17 +357,6 @@ void Plane::check_short_failsafe()
void Plane::startup_INS_ground(void)
{
#if HIL_SUPPORT
if (g.hil_mode == 1) {
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");
hal.scheduler->delay(1000);
}
}
#endif
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
gcs().send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane");
} else {

Write Preview
Loading…
Cancel
Save