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

AC2 - Mavlink on 2 ports

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2233 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
mich146@hotmail.com 2011-05-09 12:20:22 +00:00
parent 888bd62be1
commit 884bb4f103
7 changed files with 77 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopterMega/ArduCopterMega.pde
View File

@ -95,7 +95,7 @@ void update_events(void);
// All GPS access should be through this pointer. // All GPS access should be through this pointer.
GPS *g_gps; GPS *g_gps;
#if HIL_MODE == HIL_MODE_NONE #if HIL_MODE == HIL_MODE_DISABLED
// real sensors // real sensors
AP_ADC_ADS7844 adc; AP_ADC_ADS7844 adc;
@ -145,15 +145,19 @@ GPS *g_gps;
#error Unrecognised HIL_MODE setting. #error Unrecognised HIL_MODE setting.
#endif // HIL MODE #endif // HIL MODE
// HIL
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
GCS_MAVLINK hil; GCS_MAVLINK hil(Parameters::k_param_streamrates_port0);
#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE #elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
HIL_XPLANE hil; HIL_XPLANE hil;
#endif // HIL PROTOCOL #endif // HIL PROTOCOL
#endif // HIL_MODE #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_ATTITUDE
#if HIL_MODE != HIL_MODE_SENSORS #if HIL_MODE != HIL_MODE_SENSORS
// Normal // Normal
@ -171,7 +175,7 @@ GPS *g_gps;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// //
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
GCS_MAVLINK gcs; GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3);
#else #else
// If we are not using a GCS, we need a stub that does nothing. // If we are not using a GCS, we need a stub that does nothing.
GCS_Class gcs; GCS_Class gcs;
@ -1280,4 +1284,4 @@ void tuning(){
g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000 g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000
#endif #endif
} }

25
ArduCopterMega/GCS.h
View File

@ -136,7 +136,7 @@ protected:
class GCS_MAVLINK : public GCS_Class class GCS_MAVLINK : public GCS_Class
{ {
public: public:
GCS_MAVLINK(); GCS_MAVLINK(AP_Var::Key key);
void update(void); void update(void);
void init(BetterStream *port); void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0); void send_message(uint8_t id, uint32_t param = 0);
@ -171,12 +171,6 @@ private:
mavlink_channel_t chan; mavlink_channel_t chan;
uint16_t packet_drops; uint16_t packet_drops;
uint16_t rawSensorStreamRate;
uint16_t attitudeStreamRate;
uint16_t positionStreamRate;
uint16_t rawControllerStreamRate;
uint16_t rcStreamRate;
uint16_t extraStreamRate[3];
// waypoints // waypoints
uint16_t requested_interface; // request port to use uint16_t requested_interface; // request port to use
@ -193,14 +187,15 @@ private:
float junk; //used to return a junk value for interface float junk; //used to return a junk value for interface
// data stream rates // data stream rates
uint16_t streamRateRawSensors; AP_Var_group _group;
uint16_t streamRateExtendedStatus; AP_Int16 streamRateRawSensors;
uint16_t streamRateRCChannels; AP_Int16 streamRateExtendedStatus;
uint16_t streamRateRawController; AP_Int16 streamRateRCChannels;
uint16_t streamRatePosition; AP_Int16 streamRateRawController;
uint16_t streamRateExtra1; AP_Int16 streamRatePosition;
uint16_t streamRateExtra2; AP_Int16 streamRateExtra1;
uint16_t streamRateExtra3; AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
}; };
#endif // GCS_PROTOCOL_MAVLINK #endif // GCS_PROTOCOL_MAVLINK

41
ArduCopterMega/GCS_Mavlink.pde
View File

@ -4,22 +4,22 @@
#include "Mavlink_Common.h" #include "Mavlink_Common.h"
GCS_MAVLINK::GCS_MAVLINK() : GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0), packet_drops(0),
// parameters // parameters
// note, all values not explicitly initialised here are zeroed // note, all values not explicitly initialised here are zeroed
waypoint_send_timeout(1000), // 1 second waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second waypoint_receive_timeout(1000), // 1 second
// stream rates // stream rates
streamRateRawSensors(0), _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
streamRateExtendedStatus(0), streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateRCChannels(0), streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRawController(0), streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
//streamRateRawSensorFusion(0), streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRatePosition(0), streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra1(0), streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra2(0), streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3(0) streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
{ {
} }
@ -173,34 +173,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
streamRatePosition = freq; streamRatePosition = freq;
streamRateExtra1 = freq; streamRateExtra1 = freq;
streamRateExtra2 = freq; streamRateExtra2 = freq;
streamRateExtra3 = freq; streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group.
break; break;
case MAV_DATA_STREAM_RAW_SENSORS: case MAV_DATA_STREAM_RAW_SENSORS:
streamRateRawSensors = freq; streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly
// we will not continue to broadcast raw sensor data at 50Hz.
break; break;
case MAV_DATA_STREAM_EXTENDED_STATUS: case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRateExtendedStatus = freq; streamRateExtendedStatus.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_RC_CHANNELS: case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels = freq; streamRateRCChannels.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_RAW_CONTROLLER: case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController = freq; streamRateRawController.set_and_save(freq);
break; break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION: //case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion = freq; // streamRateRawSensorFusion.set_and_save(freq);
// break; // break;
case MAV_DATA_STREAM_POSITION: case MAV_DATA_STREAM_POSITION:
streamRatePosition = freq; streamRatePosition.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA1: case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1 = freq; streamRateExtra1.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA2: case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2 = freq; streamRateExtra2.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA3: case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3 = freq; streamRateExtra3.set_and_save(freq);
break; break;
default: default:
break; break;

4
ArduCopterMega/HIL.h
View File

@ -71,7 +71,7 @@ public:
/// @param severity A value describing the importance of the message. /// @param severity A value describing the importance of the message.
/// @param str The text to be sent. /// @param str The text to be sent.
/// ///
void send_text_P(uint8_t severity, const prog_char_t *str) {} void send_text(uint8_t severity, const prog_char_t *str) {}
/// Send acknowledgement for a message. /// Send acknowledgement for a message.
/// ///
@ -114,7 +114,7 @@ public:
void init(BetterStream *port); void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0); 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 char *str);
void send_text_P(uint8_t severity, const prog_char_t *str); void send_text(uint8_t severity, const prog_char_t *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
private: private:
void output_HIL(); void output_HIL();

7
ArduCopterMega/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly // The increment will prevent old parameters from being used incorrectly
// by newer code. // by newer code.
// //
static const uint16_t k_format_version = 5; static const uint16_t k_format_version = 6;
// //
// Parameter identities. // Parameter identities.
@ -49,6 +49,11 @@ public:
// //
k_param_log_bitmask, k_param_log_bitmask,
k_param_frame_type, k_param_frame_type,
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
// //
// 140: Sensor parameters // 140: Sensor parameters

20
ArduCopterMega/config.h
View File

@ -73,18 +73,20 @@
// HIL_MODE OPTIONAL // HIL_MODE OPTIONAL
// HIL_PORT OPTIONAL // HIL_PORT OPTIONAL
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode #ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
# undef GPS_PROTOCOL #endif
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#ifndef HIL_PROTOCOL #ifndef HIL_PROTOCOL
# error Must define HIL_PROTOCOL if HIL_MODE is not disabled #define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
#endif #endif
#ifndef HIL_PORT #ifndef HIL_PORT
# error Must define HIL_PORT if HIL_PROTOCOL is set #define HIL_PORT 0
#endif #endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#endif #endif
@ -125,9 +127,9 @@
#endif #endif
//Chris: Commenting out assignment of GCS to port 3 because it kills MAVLink on Port 0 //Chris: Commenting out assignment of GCS to port 3 because it kills MAVLink on Port 0
// #ifndef GCS_PORT #ifndef GCS_PORT
// # define GCS_PORT 3 # define GCS_PORT 3
// #endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Serial port speeds. // Serial port speeds.

19
ArduCopterMega/system.pde
View File

@ -155,6 +155,23 @@ void init_ardupilot()
#else #else
gcs.init(&Serial); gcs.init(&Serial);
#endif #endif
// init the HIL
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PORT == 3
hil.init(&Serial3);
#elif HIL_PORT == 1
hil.init(&Serial1);
#else
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
if(g.compass_enabled) if(g.compass_enabled)
init_compass(); init_compass();
@ -508,4 +525,4 @@ init_throttle_cruise()
//Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get()); //Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get());
} }
} }
} }