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

Plane: added HIL_MODE parameter 

this allows for HIL with any firmware
master
Andrew Tridgell 10 years ago
parent
3c9af9e474
commit
b0cdf8952b
11 changed files with 91 additions and 126 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. 12
      ArduPlane/APM_Config.h
  2. 8
      ArduPlane/ArduPlane.pde
  3. 18
      ArduPlane/Attitude.pde
  4. 81
      ArduPlane/GCS_Mavlink.pde
  5. 20
      ArduPlane/Log.pde
  6. 6
      ArduPlane/Parameters.h
  7. 9
      ArduPlane/Parameters.pde
  8. 14
      ArduPlane/config.h
  9. 6
      ArduPlane/defines.h
  10. 29
      ArduPlane/system.pde
  11. 14
      ArduPlane/test.pde

12
ArduPlane/APM_Config.h
Unescape View File

@ -8,15 +8,3 @@ @@ -8,15 +8,3 @@
// longer valid! You should switch to using CONFIG_HAL_BOARD via the HAL_BOARD
// flag in your local config.mk instead.
// The following are the recommended settings for Xplane
// simulation. Remove the leading "/* and trailing "*/" to enable:
//#define HIL_MODE HIL_MODE_DISABLED
/*
* // HIL_MODE SELECTION
* //
* // Mavlink supports
* // 2. HIL_MODE_SENSORS: full sensor simulation
*
*/

8
ArduPlane/ArduPlane.pde
Unescape View File

@ -858,10 +858,10 @@ static void ahrs_update() @@ -858,10 +858,10 @@ static void ahrs_update()
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before AHRS update
gcs_update();
#endif
if (g.hil_mode == 1) {
// update hil before AHRS update
gcs_update();
}
ahrs.update();

18
ArduPlane/Attitude.pde
Unescape View File

@ -951,16 +951,16 @@ static void set_servos(void) @@ -951,16 +951,16 @@ static void set_servos(void)
obc.check_crash_plane();
#endif
#if HIL_MODE != HIL_MODE_DISABLED
// get the servos to the GCS immediately for HIL
if (comm_get_txspace(MAVLINK_COMM_0) >=
MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
return;
if (g.hil_mode == 1) {
// get the servos to the GCS immediately for HIL
if (comm_get_txspace(MAVLINK_COMM_0) >=
MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
return;
}
}
#endif
// send values to the PWM timers for output
// ----------------------------------------

81
ArduPlane/GCS_Mavlink.pde
Unescape View File

@ -81,9 +81,9 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) @@ -81,9 +81,9 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
#if HIL_MODE != HIL_MODE_DISABLED
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
#endif
if (g.hil_mode == 1) {
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
// we are armed if we are not initialising
if (control_mode != INITIALISING && hal.util->get_soft_armed()) {
@ -352,7 +352,6 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) @@ -352,7 +352,6 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
}
#if HIL_MODE != HIL_MODE_DISABLED
void NOINLINE send_servo_out(mavlink_channel_t chan)
{
// normalized values scaled to -10000 to 10000
@ -372,12 +371,10 @@ void NOINLINE send_servo_out(mavlink_channel_t chan) @@ -372,12 +371,10 @@ void NOINLINE send_servo_out(mavlink_channel_t chan)
0,
receiver_rssi);
}
#endif
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
#if HIL_MODE != HIL_MODE_DISABLED
if (!g.hil_servos) {
if (g.hil_mode==1 && !g.hil_servos) {
mavlink_msg_servo_output_raw_send(
chan,
micros(),
@ -392,7 +389,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan) @@ -392,7 +389,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan)
RC_Channel::rc_channel(7)->radio_out);
return;
}
#endif
mavlink_msg_servo_output_raw_send(
chan,
micros(),
@ -425,31 +421,31 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) @@ -425,31 +421,31 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
barometer.get_climb_rate());
}
#if HIL_MODE != HIL_MODE_DISABLED
/*
keep last HIL_STATE message to allow sending SIM_STATE
*/
static mavlink_hil_state_t last_hil_state;
#endif
// report simulator state
static void NOINLINE send_simstate(mavlink_channel_t chan)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
sitl.simstate_send(chan);
#elif HIL_MODE != HIL_MODE_DISABLED
mavlink_msg_simstate_send(chan,
last_hil_state.roll,
last_hil_state.pitch,
last_hil_state.yaw,
last_hil_state.xacc*0.001*GRAVITY_MSS,
last_hil_state.yacc*0.001*GRAVITY_MSS,
last_hil_state.zacc*0.001*GRAVITY_MSS,
last_hil_state.rollspeed,
last_hil_state.pitchspeed,
last_hil_state.yawspeed,
last_hil_state.lat,
last_hil_state.lon);
#else
if (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
}
@ -582,10 +578,10 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) @@ -582,10 +578,10 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
break;
case MSG_SERVO_OUT:
#if HIL_MODE != HIL_MODE_DISABLED
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
#endif
if (g.hil_mode == 1) {
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
}
break;
case MSG_RADIO_IN:
@ -866,17 +862,17 @@ GCS_MAVLINK::data_stream_send(void) @@ -866,17 +862,17 @@ GCS_MAVLINK::data_stream_send(void)
if (gcs_out_of_time) return;
if (in_mavlink_delay) {
#if HIL_MODE != HIL_MODE_DISABLED
// in HIL we need to keep sending servo values to ensure
// the simulator doesn't pause, otherwise our sensor
// calibration could stall
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
if (g.hil_mode == 1) {
// in HIL we need to keep sending servo values to ensure
// the simulator doesn't pause, otherwise our sensor
// calibration could stall
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
}
}
#endif
// don't send any other stream types while in the delay callback
return;
}
@ -1496,9 +1492,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1496,9 +1492,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
if (g.hil_mode != 1) {
break;
}
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
@ -1528,9 +1526,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1528,9 +1526,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// m/s/s
Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0);
accels.x = packet.xacc * GRAVITY_MSS*0.001f;
accels.y = packet.yacc * GRAVITY_MSS*0.001f;
accels.z = packet.zacc * GRAVITY_MSS*0.001f;
ins.set_gyro(0, gyros);
ins.set_accel(0, accels);
@ -1547,7 +1545,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1547,7 +1545,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
break;
}
#endif // HIL_MODE
#if CAMERA == ENABLED
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:

20
ArduPlane/Log.pde
Unescape View File

@ -2,6 +2,7 @@ @@ -2,6 +2,7 @@
#if LOGGING_ENABLED == ENABLED
#if CLI_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash.log memory
// Code to interact with the user to dump or erase logs
@ -93,13 +94,6 @@ dump_log(uint8_t argc, const Menu::arg *argv) @@ -93,13 +94,6 @@ dump_log(uint8_t argc, const Menu::arg *argv)
return 0;
}
static void do_erase_logs(void)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs"));
DataFlash.EraseAll();
gcs_send_text_P(SEVERITY_LOW, PSTR("Log erase complete"));
}
static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv)
{
@ -164,6 +158,16 @@ process_logs(uint8_t argc, const Menu::arg *argv) @@ -164,6 +158,16 @@ process_logs(uint8_t argc, const Menu::arg *argv)
return 0;
}
#endif // CLI_ENABLED == ENABLED
static void do_erase_logs(void)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs"));
DataFlash.EraseAll();
gcs_send_text_P(SEVERITY_LOW, PSTR("Log erase complete"));
}
// Write an attitude packet
static void Log_Write_Attitude(void)
{
@ -453,6 +457,7 @@ static const struct LogStructure log_structure[] PROGMEM = { @@ -453,6 +457,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
TECS_LOG_FORMAT(LOG_TECS_MSG)
};
#if CLI_ENABLED == ENABLED
// Read the DataFlash.log memory : Packet Parser
static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
{
@ -466,6 +471,7 @@ static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page) @@ -466,6 +471,7 @@ static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
print_flight_mode,
cliSerial);
}
#endif // CLI_ENABLED
// start a new log
static void start_logging()

6
ArduPlane/Parameters.h
Unescape View File

@ -133,6 +133,7 @@ public: @@ -133,6 +133,7 @@ public:
k_param_optflow,
k_param_cli_enabled,
k_param_trim_rc_at_start,
k_param_hil_mode,
// 100: Arming parameters
k_param_arming = 100,
@ -327,9 +328,7 @@ public: @@ -327,9 +328,7 @@ public:
AP_Int8 cli_enabled;
#endif
#if HIL_MODE != HIL_MODE_DISABLED
AP_Float hil_err_limit;
#endif
AP_Int8 rtl_autoland;
@ -442,9 +441,8 @@ public: @@ -442,9 +441,8 @@ public:
AP_Int32 min_gndspeed_cm;
AP_Int16 pitch_trim_cd;
AP_Int16 FBWB_min_altitude_cm;
#if HIL_MODE != HIL_MODE_DISABLED
AP_Int8 hil_servos;
#endif
AP_Int8 hil_mode;
AP_Int8 compass_enabled;
AP_Int8 flap_1_percent;

9
ArduPlane/Parameters.pde
Unescape View File

@ -898,7 +898,13 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -898,7 +898,13 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH", 0),
#if HIL_MODE != HIL_MODE_DISABLED
// @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
GSCALAR(hil_mode, "HIL_MODE", 0),
// @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.
@ -914,7 +920,6 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -914,7 +920,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Increment: 0.1
// @User: Advanced
GSCALAR(hil_err_limit, "HIL_ERR_LIMIT", 5),
#endif
// @Param: RTL_AUTOLAND
// @DisplayName: RTL auto land

14
ArduPlane/config.h
Unescape View File

@ -54,20 +54,6 @@ @@ -54,20 +54,6 @@
#error CONFIG_APM_HARDWARE option is depreated! use CONFIG_HAL_BOARD instead.
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef CONFIG_BARO
#define CONFIG_BARO HAL_BARO_HIL
#undef CONFIG_COMPASS
#define CONFIG_COMPASS HAL_COMPASS_HIL
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif

6
ArduPlane/defines.h
Unescape View File

@ -47,12 +47,6 @@ enum gcs_failsafe { @@ -47,12 +47,6 @@ enum gcs_failsafe {
#define T6 1000000
#define T7 10000000
// HIL enumerations. Note that HIL_MODE_ATTITUDE and HIL_MODE_SENSORS
// are now the same thing, and are sensors based. The old define is
// kept to allow old APM_Config.h headers to keep working
#define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1
enum FlightMode {
MANUAL = 0,
CIRCLE = 1,

29
ArduPlane/system.pde
Unescape View File

@ -83,6 +83,13 @@ static void init_ardupilot() @@ -83,6 +83,13 @@ static void init_ardupilot()
//
load_parameters();
if (g.hil_mode == 1) {
// set sensors to HIL mode
ins.set_hil_mode();
compass.set_hil_mode();
barometer.set_hil_mode();
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// this must be before BoardConfig.init() so if
// BRD_SAFETYENABLE==0 then we don't have safety off yet
@ -502,18 +509,14 @@ static void check_short_failsafe() @@ -502,18 +509,14 @@ static void check_short_failsafe()
static void startup_INS_ground(void)
{
#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_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message"));
hal.scheduler->delay(1000);
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_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message"));
hal.scheduler->delay(1000);
}
}
// set INS to HIL mode
ins.set_hil_mode();
barometer.set_hil_mode();
#endif
AP_InertialSensor::Start_style style;
if (g.skip_gyro_cal) {
@ -649,14 +652,12 @@ static void print_comma(void) @@ -649,14 +652,12 @@ static void print_comma(void)
*/
static void servo_write(uint8_t ch, uint16_t pwm)
{
#if HIL_MODE != HIL_MODE_DISABLED
if (!g.hil_servos) {
if (g.hil_mode==1 && !g.hil_servos) {
if (ch < 8) {
RC_Channel::rc_channel(ch)->radio_out = pwm;
}
return;
}
#endif
hal.rcout->enable_ch(ch);
hal.rcout->write(ch, pwm);
}

14
ArduPlane/test.pde
Unescape View File

@ -44,20 +44,14 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { @@ -44,20 +44,14 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
#if HIL_MODE == HIL_MODE_DISABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
{"adc", test_adc},
#endif
#endif
{"gps", test_gps},
{"ins", test_ins},
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
{"compass", test_mag},
#else
{"gps", test_gps},
{"ins", test_ins},
{"compass", test_mag},
#endif
{"logging", test_logging},
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
{"shell", test_shell},
@ -539,8 +533,6 @@ test_mag(uint8_t argc, const Menu::arg *argv) @@ -539,8 +533,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here
#if HIL_MODE == HIL_MODE_DISABLED
static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
@ -594,6 +586,4 @@ test_pressure(uint8_t argc, const Menu::arg *argv) @@ -594,6 +586,4 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED

Write Preview
Loading…
Cancel
Save