aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Remove HIL_MODE stuff

Browse Source
This commit is contained in:
Jacob Walser 2017-04-05 18:14:40 -04:00
parent fc9eb797be
commit ea8dbbf96c
9 changed files with 2 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/APM_Config.h
View File

@ -24,7 +24,6 @@
// other settings
//#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000)
//#define HIL_MODE HIL_MODE_SENSORS // build for hardware-in-the-loop simulation
// User Hooks : For User Developed code that you wish to run
// Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below).

28
ArduSub/APM_Config_mavlink_hil.h
View File

@ -1,28 +0,0 @@
// HIL_MODE SELECTION
//
// Mavlink supports
// 1. HIL_MODE_SENSORS: full sensor simulation
#define HIL_MODE HIL_MODE_SENSORS
// HIL_PORT SELCTION
//
// PORT 1
// If you would like to run telemetry communications for a groundstation
// while you are running hardware in the loop it is necessary to set
// HIL_PORT to 1. This uses the port that would have been used for the gps
// as the hardware in the loop port. You will have to solder
// headers onto the gps port connection on the apm
// and connect via an ftdi cable.
//
// The baud rate is set to 115200 in this mode.
//
// PORT 3
// If you don't require telemetry communication with a gcs while running
// hardware in the loop you may use the telemetry port as the hardware in
// the loop port. Alternatively, use a telemetry/HIL shim like FGShim
// https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear
//
// The buad rate is controlled by SERIAL1_BAUD in this mode.
#define HIL_PORT 3

12
ArduSub/ArduSub.cpp
View File

@ -314,12 +314,6 @@ void Sub::ten_hz_logging_loop()
// should be run at 25hz
void Sub::twentyfive_hz_logging()
{
#if HIL_MODE != HIL_MODE_DISABLED
// HIL needs very fast update of the servo values
gcs_send_message(MSG_RADIO_OUT);
#endif
#if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
@ -335,7 +329,6 @@ void Sub::twentyfive_hz_logging()
if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_IMU(ins);
}
#endif
}
void Sub::dataflash_periodic(void)
@ -443,11 +436,6 @@ void Sub::read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs_check_input();
#endif
// <true> tells AHRS to skip INS update as we have already done it in fast_loop()
ahrs.update(true);
ahrs_view.update(true);

76
ArduSub/GCS_Mavlink.cpp
View File

@ -65,10 +65,6 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
// override if stick mixing is 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
if (motors.armed()) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
@ -304,28 +300,6 @@ void NOINLINE Sub::send_hwstatus(mavlink_channel_t chan)
0);
}
void NOINLINE Sub::send_servo_out(mavlink_channel_t chan)
{
#if HIL_MODE != HIL_MODE_DISABLED
// 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
g.rc_1.get_servo_out(),
g.rc_2.get_servo_out(),
g.rc_3.get_servo_out(),
g.rc_4.get_servo_out(),
10000 * g.rc_1.norm_output(),
10000 * g.rc_2.norm_output(),
10000 * g.rc_3.norm_output(),
10000 * g.rc_4.norm_output(),
0);
#endif // HIL_MODE
}
void NOINLINE Sub::send_radio_out(mavlink_channel_t chan)
{
mavlink_msg_servo_output_raw_send(
@ -477,7 +451,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
return false;
}
#if HIL_MODE != HIL_MODE_SENSORS
// if we don't have at least 250 micros remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
@ -485,7 +458,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
sub.gcs_out_of_time = true;
return false;
}
#endif
switch (id) {
case MSG_HEARTBEAT:
@ -539,8 +511,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
sub.send_servo_out(chan);
break;
case MSG_RADIO_IN:
@ -1722,52 +1692,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE: { // MAV ID: 90
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
// sanity check location
if (!check_latlng(packet.lat, packet.lon)) {
break;
}
// set gps hil sensor
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
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/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros);
ins.set_accel(0, accels);
sub.barometer.setHIL(packet.alt*0.001f);
sub.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
sub.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
break;
}
#endif // HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS: { // MAV ID: 109
handle_radio_status(msg, sub.DataFlash, sub.should_log(MASK_LOG_PM));

1
ArduSub/Sub.h
View File

@ -501,7 +501,6 @@ private:
void send_nav_controller_output(mavlink_channel_t chan);
void send_simstate(mavlink_channel_t chan);
void send_hwstatus(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan);
void send_radio_out(mavlink_channel_t chan);
void send_vfr_hud(mavlink_channel_t chan);
void send_current_waypoint(mavlink_channel_t chan);

7
ArduSub/config.h
View File

@ -38,13 +38,6 @@
#error CONFIG_HAL_BOARD must be defined to build ArduSub
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#define MAGNETOMETER ENABLED
// run at 400Hz on all systems

4
ArduSub/defines.h
View File

@ -29,10 +29,6 @@ enum autopilot_yaw_mode {
AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following
};
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1
// Auto Pilot Modes enumeration
enum control_mode_t {
STABILIZE = 0, // manual angle with manual depth/throttle

4
ArduSub/motors.cpp
View File

@ -37,7 +37,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
update_notify();
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif
@ -92,7 +92,7 @@ void Sub::init_disarm_motors()
return;
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors");
#endif

12
ArduSub/system.cpp
View File

@ -135,18 +135,6 @@ void Sub::init_ardupilot()
USERHOOK_INIT
#endif
#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");
hal.scheduler->delay(1000);
}
// set INS to HIL mode
ins.set_hil_mode();
#endif
// read Baro pressure at ground
//-----------------------------
init_barometer(false);