Browse Source
this gets rid of the messy #ifdefs around HIL_PORT, and removes non-MAVLink GCS and HIL supportmission-4.1.18
Andrew Tridgell
14 years ago
9 changed files with 19 additions and 441 deletions
@ -1,22 +0,0 @@
@@ -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 |
||||
@ -1,135 +0,0 @@
@@ -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
|
||||
|
||||
@ -1,129 +0,0 @@
@@ -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 |
||||
Loading…
Reference in new issue