Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

HIL: first step in simplifying the HIL/GCS code

Browse Source 
this gets rid of the messy #ifdefs around HIL_PORT, and removes
non-MAVLink GCS and HIL support
This commit is contained in:
Andrew Tridgell 2011-09-18 12:15:23 +10:00
parent 6dd2cc058a
commit 9baab490f1
9 changed files with 19 additions and 441 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/APM_Config.h
View File

@ -16,9 +16,5 @@
// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable:
/*
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PORT 3
*/

30
ArduPlane/APM_Config_mavlink_hil.h
View File

@ -23,41 +23,11 @@
// 2. HIL_MODE_SENSORS: full sensor simulation
#define HIL_MODE HIL_MODE_ATTITUDE
// HIL_PORT SELCTION
//
// PORT 0
// Port 0 is the port usually used for HIL. You attatch your HIL computer
// to APM using a USB cable. You can run telemetry on Port 3
//
// PORT 1
// Available for special situations. 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 buad rate is controlled by SERIAL1_BAUD 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 SERIAL3_BAUD in this mode.
#define HIL_PORT 0
// You can set your gps protocol here for your actual
// hardware and leave it without affecting the hardware
// in the loop simulation
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
// Ground control station comms
#if HIL_PORT != 3
# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
# define GCS_PORT 3
#endif
// Sensors
// All sensors are supported in all modes.
// The magnetometer is not used in

22
ArduPlane/APM_Config_xplane.h
View File

@ -1,22 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE ORIGINAL X-PLANE INTERFACE
// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE!
#define FLIGHT_MODE_CHANNEL 8
#define FLIGHT_MODE_1 AUTO
#define FLIGHT_MODE_2 RTL
#define FLIGHT_MODE_3 FLY_BY_WIRE_A
#define FLIGHT_MODE_4 FLY_BY_WIRE_A
#define FLIGHT_MODE_5 MANUAL
#define FLIGHT_MODE_6 MANUAL
#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED
#define AIRSPEED_SENSOR ENABLED

95
ArduPlane/ArduPlane.pde
View File

@ -51,7 +51,6 @@ version 2.1 of the License, or (at your option) any later version.
#include "defines.h"
#include "Parameters.h"
#include "GCS.h"
#include "HIL.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
@ -149,19 +148,6 @@ AP_IMU_Shim imu; // never used
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
GCS_MAVLINK hil(Parameters::k_param_streamrates_port0);
#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
HIL_XPLANE hil;
#endif // HIL PROTOCOL
#endif // HIL_MODE
// We may have a hil object instantiated just for mission planning
#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
GCS_MAVLINK hil(Parameters::k_param_streamrates_port0);
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_SENSORS
// Normal
@ -178,12 +164,8 @@ AP_IMU_Shim imu; // never used
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3);
#else
// If we are not using a GCS, we need a stub that does nothing.
GCS_Class gcs;
#endif
GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3);
GCS_MAVLINK hil(Parameters::k_param_streamrates_port0);
////////////////////////////////////////////////////////////////////////////////
// SONAR selection
@ -481,9 +463,7 @@ static void fast_loop()
// try to send any deferred messages if the serial port now has
// some space available
gcs.send_message(MSG_RETRY_DEFERRED);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_RETRY_DEFERRED);
#endif
// check for loss of control signal failsafe condition
// ------------------------------------
@ -538,28 +518,10 @@ static void fast_loop()
// XXX is it appropriate to be doing the comms below on the fast loop?
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT
// kick the HIL to process incoming sensor packets
hil.update();
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
hil.data_stream_send(45,1000);
#else
hil.send_message(MSG_SERVO_OUT);
#endif
#elif HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE == HIL_MODE_DISABLED && HIL_PORT == 0
// Case for hil object on port 0 just for mission planning
hil.update();
hil.data_stream_send(45,1000);
#endif
// kick the GCS to process uplink data
gcs.update();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(45,1000);
#endif
// XXX this should be absorbed into the above,
// or be a "GCS fast loop" interface
hil.update();
gcs.data_stream_send(45,1000);
hil.data_stream_send(45,1000);
}
static void medium_loop()
@ -650,18 +612,10 @@ Serial.println(tempaccel.z, DEC);
if (g.log_bitmask & MASK_LOG_GPS)
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
// XXX this should be a "GCS medium loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(5,45);
// send all requested output streams with rates requested
// between 5 and 45 Hz
#else
gcs.send_message(MSG_ATTITUDE); // Sends attitude data
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(5,45);
#endif
// send all requested output streams with rates requested
// between 5 and 45 Hz
gcs.data_stream_send(5,45);
hil.data_stream_send(5,45);
break;
// This case controls the slow loop
@ -719,21 +673,9 @@ static void slow_loop()
slow_loopCounter = 0;
update_events();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
gcs.data_stream_send(3,5);
// send all requested output streams with rates requested
// between 3 and 5 Hz
#else
gcs.send_message(MSG_LOCATION);
gcs.send_message(MSG_CPU_LOAD, load*100);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(3,5);
#endif
mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
gcs.data_stream_send(3,5);
hil.data_stream_send(3,5);
break;
}
}
@ -745,16 +687,9 @@ static void one_second_loop()
// send a heartbeat
gcs.send_message(MSG_HEARTBEAT);
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,3);
// send all requested output streams with rates requested
// between 1 and 3 Hz
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
hil.data_stream_send(1,3);
#endif
hil.send_message(MSG_HEARTBEAT);
gcs.data_stream_send(1,3);
hil.data_stream_send(1,3);
}
static void update_GPS(void)

6
ArduPlane/GCS_Mavlink.pde
View File

@ -1111,18 +1111,14 @@ static void mavlink_delay(unsigned long t)
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs.send_message(MSG_HEARTBEAT);
gcs.send_message(MSG_EXTENDED_STATUS1);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
gcs.send_message(MSG_EXTENDED_STATUS1);
hil.send_message(MSG_EXTENDED_STATUS1);
#endif
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs.update();
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.update();
#endif
}
delay(1);
} while (millis() - tstart < t);

135
ArduPlane/HIL.h
View File

@ -1,135 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file HIL.h
/// @brief Interface definition for the various Ground Control System protocols.
#ifndef __HIL_H
#define __HIL_H
# if HIL_MODE != HIL_MODE_DISABLED
#include <FastSerial.h>
#include <AP_Common.h>
#include <GPS.h>
#include <Stream.h>
#include <stdint.h>
///
/// @class HIL
/// @brief Class describing the interface between the APM code
/// proper and the HIL implementation.
///
/// HIL' are currently implemented inside the sketch and as such have
/// access to all global state. The sketch should not, however, call HIL
/// internal functions - all calls to the HIL should be routed through
/// this interface (or functions explicitly exposed by a subclass).
///
class HIL_Class
{
public:
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required before
/// HIL messages are exchanged.
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @note The stream is currently BetterStream so that we can use the _P
/// methods; this may change if Arduino adds them to Print.
///
/// @param port The stream over which messages are exchanged.
///
void init(FastSerial *port) { _port = port; }
/// Update HIL state.
///
/// This may involve checking for received bytes on the stream,
/// or sending additional periodic messages.
void update(void) {}
/// Send a message with a single numeric parameter.
///
/// This may be a standalone message, or the HIL driver may
/// have its own way of locating additional parameters to send.
///
/// @param id ID of the message to send.
/// @param param Explicit message parameter.
///
void send_message(uint8_t id, int32_t param = 0) {}
/// Send a text message.
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const char *str) {}
/// Send a text message with a PSTR()
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const prog_char_t *str) {}
/// Send acknowledgement for a message.
///
/// @param id The message ID being acknowledged.
/// @param sum1 Checksum byte 1 from the message being acked.
/// @param sum2 Checksum byte 2 from the message being acked.
///
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {}
protected:
/// The stream we are communicating over
FastSerial *_port;
};
//
// HIL class definitions.
//
// These are here so that we can declare the HIL object early in the sketch
// and then reference it statically rather than via a pointer.
//
///
/// @class HIL_MAVLINK
/// @brief The mavlink protocol for hil
///
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
// uses gcs
#endif // HIL_PROTOCOL_MAVLINK
///
/// @class HIL_XPLANE
/// @brief The xplane protocol for hil
///
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
class HIL_XPLANE : public HIL_Class
{
public:
HIL_XPLANE();
void update(void);
void init(FastSerial *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
private:
void output_HIL();
void output_HIL_();
void output_int(int value);
void output_byte(byte value);
void print_buffer();
AP_GPS_IMU * hilPacketDecoder;
byte buf_len;
byte out_buffer[32];
};
#endif // HIL_PROTOCOL_XPLANE
#endif // HIL not disabled
#endif // __HIL_H

129
ArduPlane/HIL_Xplane.pde
View File

@ -1,129 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
void HIL_XPLANE::output_HIL(void)
{
// output real-time sensor data
Serial.printf_P(PSTR("AAA")); // Message preamble
output_int((int)(g.channel_roll.servo_out)); // 0 bytes 0, 1
output_int((int)(g.channel_pitch.servo_out)); // 1 bytes 2, 3
output_int((int)(g.channel_throttle.servo_out)); // 2 bytes 4, 5
output_int((int)(g.channel_rudder.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)altitude_error); // 6 bytes 12, 13
output_int((int)energy_error); // 7 bytes 14,15
output_byte((int)g.waypoint_index); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17
// print out the buffer and checksum
// ---------------------------------
print_buffer();
}
void HIL_XPLANE::output_int(int value)
{
//buf_len += 2;
out_buffer[buf_len++] = value & 0xff;
out_buffer[buf_len++] = (value >> 8) & 0xff;
}
void HIL_XPLANE::output_byte(byte value)
{
out_buffer[buf_len++] = value;
}
void HIL_XPLANE::print_buffer()
{
byte ck_a = 0;
byte ck_b = 0;
for (byte i = 0;i < buf_len; i++){
Serial.print (out_buffer[i]);
}
Serial.print('\r');
Serial.print('\n');
buf_len = 0;
}
HIL_XPLANE::HIL_XPLANE() :
buf_len(0)
{
}
void
HIL_XPLANE::init(FastSerial * port)
{
HIL_Class::init(port);
hilPacketDecoder = new AP_GPS_IMU(port);
hilPacketDecoder->init();
}
void
HIL_XPLANE::update(void)
{
hilPacketDecoder->update();
airspeed = (float)hilPacketDecoder->airspeed; //airspeed is * 100 coming in from Xplane for accuracy
calc_airspeed_errors();
dcm.setHil(hilPacketDecoder->roll_sensor*M_PI/18000,
hilPacketDecoder->pitch_sensor*M_PI/18000,
hilPacketDecoder->ground_course*M_PI/18000,
0,0,0);
// set gps hil sensor
g_gps->setHIL(hilPacketDecoder->time/1000.0,(float)hilPacketDecoder->latitude/1.0e7,(float)hilPacketDecoder->longitude/1.0e7,(float)hilPacketDecoder->altitude/1.0e2,
(float)hilPacketDecoder->speed_3d/1.0e2,(float)hilPacketDecoder->ground_course/1.0e2,0,0);
}
void
HIL_XPLANE::send_message(uint8_t id, uint32_t param)
{
// TODO: split output by actual request
uint64_t timeStamp = micros();
switch(id) {
case MSG_HEARTBEAT:
break;
case MSG_EXTENDED_STATUS:
break;
case MSG_ATTITUDE:
break;
case MSG_LOCATION:
break;
case MSG_GPS_RAW:
break;
case MSG_SERVO_OUT:
output_HIL();
break;
case MSG_RADIO_OUT:
break;
case MSG_RAW_IMU:
break;
case MSG_GPS_STATUS:
break;
case MSG_CURRENT_WAYPOINT:
break;
defualt:
break;
}
}
void
HIL_XPLANE::send_text(uint8_t severity, const char *str)
{
}
void
HIL_XPLANE::send_text(uint8_t severity, const prog_char_t *str)
{
}
void
HIL_XPLANE::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
}
#endif

12
ArduPlane/config.h
View File

@ -72,27 +72,15 @@
//////////////////////////////////////////////////////////////////////////////
// HIL_PROTOCOL OPTIONAL
// HIL_MODE OPTIONAL
// HIL_PORT OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#ifndef HIL_PROTOCOL
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
#endif
#ifndef HIL_PORT
#define HIL_PORT 0
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#endif

27
ArduPlane/system.pde
View File

@ -60,8 +60,6 @@ static void init_ardupilot()
// GPS serial port.
//
// Not used if the IMU/X-Plane GPS is in use.
//
// XXX currently the EM406 (SiRF receiver) is nominally configured
// at 57600, however it's not been supported to date. We should
// probably standardise on 38400.
@ -69,13 +67,8 @@ static void init_ardupilot()
// XXX the 128 byte receive buffer may be too small for NMEA, depending
// on the message set configured.
//
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT == 1 // TODO: figure out a better way to do this
// Steal gps port for hardware in the loop
Serial1.begin(115200, 128, 128);
#else
// standard gps running
Serial1.begin(38400, 128, 16);
#endif
// standard gps running
Serial1.begin(38400, 128, 16);
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %lu\n"),
@ -176,22 +169,8 @@ static void init_ardupilot()
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
mavlink_system.type = MAV_FIXED_WING;
// init the HIL
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PORT == 3
hil.init(&Serial3);
#elif HIL_PORT == 1
hil.init(&Serial1);
#else
// init the HIL
hil.init(&Serial);
#endif
#endif
// We may have a hil object instantiated just for mission planning
#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
hil.init(&Serial);
#endif
rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override
init_rc_in(); // sets up rc channels from radio