Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -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.
GPS *g_gps;
#if HIL_MODE == HIL_MODE_NONE
#if HIL_MODE == HIL_MODE_DISABLED
// real sensors
AP_ADC_ADS7844 adc;
@ -145,15 +145,19 @@ GPS *g_gps;
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
// HIL
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
GCS_MAVLINK hil;
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
@ -171,7 +175,7 @@ GPS *g_gps;
////////////////////////////////////////////////////////////////////////////////
//
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
GCS_MAVLINK gcs;
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;
@ -1280,4 +1284,4 @@ void tuning(){
g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000
#endif
}
}

25
ArduCopterMega/GCS.h
View File

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

41
ArduCopterMega/GCS_Mavlink.pde
View File

@ -4,22 +4,22 @@
#include "Mavlink_Common.h"
GCS_MAVLINK::GCS_MAVLINK() :
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0),
// parameters
// note, all values not explicitly initialised here are zeroed
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second
// stream rates
streamRateRawSensors(0),
streamRateExtendedStatus(0),
streamRateRCChannels(0),
streamRateRawController(0),
//streamRateRawSensorFusion(0),
streamRatePosition(0),
streamRateExtra1(0),
streamRateExtra2(0),
streamRateExtra3(0)
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
{
}
@ -173,34 +173,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
streamRatePosition = freq;
streamRateExtra1 = 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;
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;
case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRateExtendedStatus = freq;
streamRateExtendedStatus.set_and_save(freq);
break;
case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels = freq;
streamRateRCChannels.set_and_save(freq);
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController = freq;
streamRateRawController.set_and_save(freq);
break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion = freq;
// streamRateRawSensorFusion.set_and_save(freq);
// break;
case MAV_DATA_STREAM_POSITION:
streamRatePosition = freq;
streamRatePosition.set_and_save(freq);
break;
case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1 = freq;
streamRateExtra1.set_and_save(freq);
break;
case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2 = freq;
streamRateExtra2.set_and_save(freq);
break;
case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3 = freq;
streamRateExtra3.set_and_save(freq);
break;
default:
break;

4
ArduCopterMega/HIL.h
View File

@ -71,7 +71,7 @@ public:
/// @param severity A value describing the importance of the message.
/// @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.
///
@ -114,7 +114,7 @@ public:
void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0);
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);
private:
void output_HIL();

7
ArduCopterMega/Parameters.h
View File

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

20
ArduCopterMega/config.h
View File

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

19
ArduCopterMega/system.pde
View File

@ -155,6 +155,23 @@ void init_ardupilot()
#else
gcs.init(&Serial);
#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)
init_compass();
@ -508,4 +525,4 @@ init_throttle_cruise()
//Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get());
}
}
}
}