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

merged the GCS updates from ArduPlane to ArduCopter

Browse Source 
this removes all the non-MAVLink GCS options, and simplifies the HIL
and GCS code a lot. It also adds async sending of low priority GCS
text messages.
This commit is contained in:
Andrew Tridgell 2011-10-11 20:12:37 +11:00
parent a10d10f0f3
commit 661b6c4508
26 changed files with 733 additions and 2021 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/APM_Config.h
View File

@ -5,7 +5,6 @@
// GPS is auto-selected
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define HIL_MODE HIL_MODE_ATTITUDE
//#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode)

9
ArduCopter/APM_Config_mavlink_hil.h
View File

@ -1,8 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Hardware in the loop protocol
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
// HIL_MODE SELECTION
//
// Mavlink supports
@ -37,12 +34,6 @@
// 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

14
ArduCopter/APM_Config_xplane.h
View File

@ -1,14 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PORT 3
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED
#define AIRSPEED_SENSOR ENABLED

98
ArduCopter/ArduCopter.pde
View File

@ -76,7 +76,6 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
// Local modules
#include "Parameters.h"
#include "GCS.h"
#include "HIL.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
@ -181,19 +180,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
#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
@ -209,16 +195,8 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
////////////////////////////////////////////////////////////////////////////////
// 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
//#include <GCS_SIMPLE.h>
//GCS_SIMPLE gcs_simple(&Serial);
GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0);
GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
////////////////////////////////////////////////////////////////////////////////
// SONAR selection
@ -552,8 +530,6 @@ void loop()
}
if (millis() - perf_mon_timer > 20000) {
gcs.send_message(MSG_PERF_REPORT);
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
@ -571,10 +547,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
gcs_send_message(MSG_RETRY_DEFERRED);
// Read radio
// ----------
@ -711,17 +684,9 @@ static void medium_loop()
}
#endif
#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
gcs_data_stream_send(5,45);
if (g.log_bitmask & MASK_LOG_MOTORS)
Log_Write_Motors();
@ -773,9 +738,9 @@ static void fifty_hz_loop()
sonar_alt = sonar.read();
}
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
#if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
// HIL for a copter needs very fast update of the servo values
hil.send_message(MSG_RADIO_OUT);
gcs_send_message(MSG_RADIO_OUT);
#endif
camera_stabilization();
@ -788,27 +753,9 @@ static void fifty_hz_loop()
Log_Write_Raw();
#endif
#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
gcs_update();
gcs_data_stream_send(45,1000);
#if FRAME_CONFIG == TRI_FRAME
// servo Yaw
@ -868,18 +815,9 @@ static void slow_loop()
// blink if we are armed
update_lights();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,5);
// send all requested output streams with rates requested
// between 1 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(1,5);
#endif
// send all requested output streams with rates requested
// between 1 and 5 Hz
gcs_data_stream_send(1,5);
if(g.radio_tuning > 0)
tuning();
@ -903,11 +841,7 @@ static void super_slow_loop()
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
gcs.send_message(MSG_HEARTBEAT);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
#endif
gcs_send_message(MSG_HEARTBEAT);
}
static void update_GPS(void)
@ -931,12 +865,6 @@ static void update_GPS(void)
if (g_gps->new_data && g_gps->fix) {
gps_watchdog = 0;
// XXX We should be sending GPS data off one of the regular loops so that we send
// no-GPS-fix data too
#if GCS_PROTOCOL != GCS_PROTOCOL_MAVLINK
gcs.send_message(MSG_LOCATION);
#endif
// for performance
// ---------------
gps_fix_count++;
@ -1177,7 +1105,7 @@ static void read_AHRS(void)
//-----------------------------------------------
#if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update
hil.update();
gcs_update();
#endif
dcm.update_DCM_fast();

71
ArduCopter/GCS.h
View File

@ -6,7 +6,7 @@
#ifndef __GCS_H
#define __GCS_H
#include <BetterStream.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <GCS_MAVLink.h>
#include <GPS.h>
@ -40,7 +40,7 @@ public:
///
/// @param port The stream over which messages are exchanged.
///
void init(BetterStream *port) { _port = port; }
void init(FastSerial *port) { _port = port; }
/// Update GCS state.
///
@ -56,64 +56,28 @@ public:
/// @param id ID of the message to send.
/// @param param Explicit message parameter.
///
void send_message(uint8_t id, int32_t param = 0) {}
void send_message(enum ap_message id) {}
/// 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) {}
#define send_text_P(severity, msg) send_text(severity, msg)
void send_text(gcs_severity 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) {}
/// Emit an update of the "current" waypoints, often previous, current and
/// next.
///
void print_current_waypoints() {}
//
// The following interfaces are not currently implemented as their counterparts
// are not called in the mainline code. XXX ripe for re-specification.
//
/// Send a text message with printf-style formatting.
///
/// @param severity A value describing the importance of the message.
/// @param fmt The format string to send.
/// @param ... Additional arguments to the format string.
///
// void send_message(uint8_t severity, const char *fmt, ...) {}
/// Log a waypoint
///
/// @param wp The waypoint to log.
/// @param index The index of the waypoint.
// void print_waypoint(struct Location *wp, uint8_t index) {}
void send_text(gcs_severity severity, const prog_char_t *str) {}
// test if frequency within range requested for loop
// used by data_stream_send
static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
{
if (freq != 0 && freq >= freqMin && freq < freqMax)
return true;
else
return false;
if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
else return false;
}
// send streams which match frequency range
@ -121,7 +85,7 @@ public:
protected:
/// The stream we are communicating over
BetterStream *_port;
FastSerial *_port;
};
//
@ -135,24 +99,24 @@ protected:
/// @class GCS_MAVLINK
/// @brief The mavlink protocol for qgroundcontrol
///
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK(AP_Var::Key key);
void update(void);
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(uint8_t severity, const prog_char_t *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
void init(FastSerial *port);
void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str);
void send_text(gcs_severity severity, const prog_char_t *str);
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
void queued_param_send();
void queued_waypoint_send();
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
void _queued_send();
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
uint16_t _queued_parameter_index; ///< next queued parameter's index
@ -176,7 +140,6 @@ private:
uint16_t packet_drops;
// waypoints
uint16_t requested_interface; // request port to use
uint16_t waypoint_request_i; // request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "
@ -199,10 +162,6 @@ private:
AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
};
#endif // GCS_PROTOCOL_MAVLINK
#endif // __GCS_H

147
ArduCopter/GCS_Ardupilot.pde
View File

@ -1,147 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY
#if GCS_PORT == 3
# define SendSer Serial3.print
# define SendSerln Serial3.println
#else
# define SendSer Serial.print
# define SendSerln Serial.println
#endif
// This is the standard GCS output file for ArduPilot
/*
Message Prefixes
!!! Position Low rate telemetry
+++ Attitude High rate telemetry
### Mode Change in control mode
%%% Waypoint Current and previous waypoints
XXX Alert Text alert - NOTE: Alert message generation is not localized to a function
PPP IMU Performance Sent every 20 seconds for performance monitoring
Message Suffix
*** All messages use this suffix
*/
/*
void static acknowledge(byte id, byte check1, byte check2) {}
void static send_message(byte id) {}
void static send_message(byte id, long param) {}
void static send_message(byte severity, const char *str) {}
*/
static void acknowledge(byte id, byte check1, byte check2)
{
}
static void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
{
if(severity >= DEBUG_LEVEL){
SendSer("MSG: ");
SendSerln(str);
}
}
static void send_message(byte id)
{
send_message(id,0l);
}
static void send_message(byte id, long param)
{
switch(id) {
case MSG_ATTITUDE: // ** Attitude message
print_attitude();
break;
case MSG_LOCATION: // ** Location/GPS message
print_position();
break;
case MSG_HEARTBEAT: // ** Location/GPS message
print_control_mode();
break;
//case MSG_PERF_REPORT:
// printPerfData();
}
}
static void print_current_waypoints()
{
}
static void print_attitude(void)
{
SendSer("+++");
SendSer("ASP:");
SendSer((int)airspeed / 100, DEC);
SendSer(",THH:");
SendSer(g.rc_3.servo_out, DEC);
SendSer (",RLL:");
SendSer(dcm.roll_sensor / 100, DEC);
SendSer (",PCH:");
SendSer(dcm.pitch_sensor / 100, DEC);
SendSerln(",***");
}
static void print_control_mode(void)
{
SendSer("###");
SendSer(flight_mode_strings[control_mode]);
SendSerln("***");
}
static void print_position(void)
{
SendSer("!!");
SendSer("!");
SendSer("LAT:");
SendSer(current_loc.lat/10,DEC);
SendSer(",LON:");
SendSer(current_loc.lng/10,DEC); //wp_current_lat
SendSer(",SPD:");
SendSer(g_gps->ground_speed/100,DEC);
SendSer(",CRT:");
SendSer(climb_rate,DEC);
SendSer(",ALT:");
SendSer(current_loc.alt/100,DEC);
SendSer(",ALH:");
SendSer(next_WP.alt/100,DEC);
SendSer(",CRS:");
SendSer(dcm.yaw_sensor/100,DEC);
SendSer(",BER:");
SendSer(target_bearing/100,DEC);
SendSer(",WPN:");
SendSer(g.waypoint_index,DEC);//Actually is the waypoint.
SendSer(",DST:");
SendSer(wp_distance,DEC);
SendSer(",BTV:");
SendSer(battery_voltage,DEC);
SendSer(",RSP:");
SendSer(g.rc_1.servo_out/100,DEC);
SendSer(",TOW:");
SendSer(g_gps->time);
SendSerln(",***");
}
static void print_waypoint(struct Location *cmd,byte index)
{
SendSer("command #: ");
SendSer(index, DEC);
SendSer(" id: ");
SendSer(cmd->id,DEC);
SendSer(" p1: ");
SendSer(cmd->p1,DEC);
SendSer(" p2: ");
SendSer(cmd->alt,DEC);
SendSer(" p3: ");
SendSer(cmd->lat,DEC);
SendSer(" p4: ");
SendSerln(cmd->lng,DEC);
}
static void print_waypoints()
{
}
#endif

192
ArduCopter/GCS_IMU_ouput.pde
View File

@ -1,192 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_IMU
// This is the standard GCS output file for ArduPilot
/*
Message Prefixes
!!! Position Low rate telemetry
+++ Attitude High rate telemetry
### Mode Change in control mode
%%% Waypoint Current and previous waypoints
XXX Alert Text alert - NOTE: Alert message generation is not localized to a function
PPP IMU Performance Sent every 20 seconds for performance monitoring
Message Suffix
*** All messages use this suffix
*/
/*
void acknowledge(byte id, byte check1, byte check2) {}
void send_message(byte id) {}
void send_message(byte id, long param) {}
void send_message(byte severity, const char *str) {}
*/
static void acknowledge(byte id, byte check1, byte check2)
{
}
static void send_message(byte id)
{
send_message(id,0l);
}
static void send_message(byte id, long param)
{
switch(id) {
case MSG_ATTITUDE:
print_attitude();
break;
case MSG_LOCATION: // ** Location/GPS message
print_location();
break;
}
}
static void send_message(byte severity, const char *str)
{
if(severity >= DEBUG_LEVEL){
Serial.print("MSG: ");
Serial.println(str);
}
}
static void print_current_waypoints()
{
}
static void print_control_mode(void)
{
}
static void print_attitude(void)
{
//Serial.print("!!VER:");
//Serial.print(SOFTWARE_VER); //output the software version
//Serial.print(",");
// Analogs
Serial.print("AN0:");
Serial.print(read_adc(0)); //Reversing the sign.
Serial.print(",AN1:");
Serial.print(read_adc(1));
Serial.print(",AN2:");
Serial.print(read_adc(2));
Serial.print(",AN3:");
Serial.print(read_adc(3));
Serial.print (",AN4:");
Serial.print(read_adc(4));
Serial.print (",AN5:");
Serial.print(read_adc(5));
Serial.print (",");
// DCM
Serial.print ("EX0:");
Serial.print(convert_to_dec(DCM_Matrix[0][0]));
Serial.print (",EX1:");
Serial.print(convert_to_dec(DCM_Matrix[0][1]));
Serial.print (",EX2:");
Serial.print(convert_to_dec(DCM_Matrix[0][2]));
Serial.print (",EX3:");
Serial.print(convert_to_dec(DCM_Matrix[1][0]));
Serial.print (",EX4:");
Serial.print(convert_to_dec(DCM_Matrix[1][1]));
Serial.print (",EX5:");
Serial.print(convert_to_dec(DCM_Matrix[1][2]));
Serial.print (",EX6:");
Serial.print(convert_to_dec(DCM_Matrix[2][0]));
Serial.print (",EX7:");
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
Serial.print (",EX8:");
Serial.print(convert_to_dec(DCM_Matrix[2][2]));
Serial.print (",");
// Euler
Serial.print("RLL:");
Serial.print(ToDeg(roll));
Serial.print(",PCH:");
Serial.print(ToDeg(pitch));
Serial.print(",YAW:");
Serial.print(ToDeg(yaw));
Serial.print(",IMUH:");
Serial.print(((int)imu_health>>8)&0xff);
Serial.print (",");
/*
#if USE_MAGNETOMETER == 1
Serial.print("MGX:");
Serial.print(magnetom_x);
Serial.print (",MGY:");
Serial.print(magnetom_y);
Serial.print (",MGZ:");
Serial.print(magnetom_z);
Serial.print (",MGH:");
Serial.print(MAG_Heading);
Serial.print (",");
#endif
*/
//Serial.print("Temp:");
//Serial.print(temp_unfilt/20.0); // Convert into degrees C
//alti();
//Serial.print(",Pressure: ");
//Serial.print(press);
//Serial.print(",Alt: ");
//Serial.print(pressure_altitude/1000); // Original floating point full solution in meters
//Serial.print (",");
Serial.println("***");
}
void print_location(void)
{
Serial.print("LAT:");
Serial.print(current_loc.lat);
Serial.print(",LON:");
Serial.print(current_loc.lng);
Serial.print(",ALT:");
Serial.print(current_loc.alt/100); // meters
Serial.print(",COG:");
Serial.print(g_gps->ground_course);
Serial.print(",SOG:");
Serial.print(g_gps->ground_speed);
Serial.print(",FIX:");
Serial.print((int)g_gps->fix);
Serial.print(",SAT:");
Serial.print((int)g_gps->num_sats);
Serial.print (",");
Serial.print("TOW:");
Serial.print(g_gps->time);
Serial.println("***");
}
static void print_waypoints()
{
}
static void print_waypoint(struct Location *cmd,byte index)
{
Serial.print("command #: ");
Serial.print(index, DEC);
Serial.print(" id: ");
Serial.print(cmd->id,DEC);
Serial.print(" p1: ");
Serial.print(cmd->p1,DEC);
Serial.print(" p2: ");
Serial.print(cmd->alt,DEC);
Serial.print(" p3: ");
Serial.print(cmd->lat,DEC);
Serial.print(" p4: ");
Serial.println(cmd->lng,DEC);
}
static long convert_to_dec(float x)
{
return x*10000000;
}
#endif

0
ArduCopter/GCS_Jason_text.pde
View File

756
ArduCopter/GCS_Mavlink.pde
View File

@ -1,12 +1,493 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
#include "Mavlink_Common.h"
// use this to prevent recursion during sensor init
static bool in_mavlink_delay;
// this costs us 51 bytes, but means that low priority
// messages don't block the CPU
static mavlink_statustext_t pending_status;
// check if a message will fit in the payload space available
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
/*
!!NOTE!!
the use of NOINLINE separate functions for each message type avoids
a compiler bug in gcc that would cause it to use far more stack
space than is needed. Without the NOINLINE we use the sum of the
stack needed for each message type. Please be careful to follow the
pattern below when adding any new messages
*/
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
default:
mode = control_mode + 100;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
0,
battery_voltage * 1000,
battery_remaining,
packet_drops);
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
{
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
}
static void NOINLINE send_location(mavlink_channel_t chan)
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
target_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
0,
0);
}
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
mavlink_msg_gps_raw_send(
chan,
micros(),
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
}
static void NOINLINE send_servo_out(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
#if FRAME_CONFIG == HELI_FRAME
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out,
0,
0,
0,
0,
rssi);
#else
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out,
10000 * g.rc_1.norm_output(),
10000 * g.rc_2.norm_output(),
10000 * g.rc_3.norm_output(),
10000 * g.rc_4.norm_output(),
rssi);
#endif
}
static void NOINLINE send_radio_in(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
g.rc_4.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
}
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
mavlink_msg_servo_output_raw_send(
chan,
motor_out[0],
motor_out[1],
motor_out[2],
motor_out[3],
motor_out[4],
motor_out[5],
motor_out[6],
motor_out[7]);
}
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10,
current_loc.alt / 100.0,
climb_rate);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
mavlink_msg_raw_imu_send(
chan,
micros(),
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
mavlink_msg_scaled_pressure_send(
chan,
micros(),
(float)barometer.Press/100.0,
(float)(barometer.Press-ground_pressure)/100.0,
(int)(barometer.Temp*10));
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_gps_status(mavlink_channel_t chan)
{
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
}
static void NOINLINE send_statustext(mavlink_channel_t chan)
{
mavlink_msg_statustext_send(
chan,
pending_status.severity,
pending_status.text);
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
return false;
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
return true;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
break;
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW);
send_gps_raw(chan);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(chan);
break;
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
break;
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
break;
#endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_GPS_STATUS:
CHECK_PAYLOAD_SIZE(GPS_STATUS);
send_gps_status(chan);
break;
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
send_current_waypoint(chan);
break;
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_param_send();
} else {
gcs3.queued_param_send();
}
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_waypoint_send();
} else {
gcs3.queued_waypoint_send();
}
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
break;
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
}
return true;
}
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
static struct mavlink_queue {
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
{
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// don't send status MAVLink messages for 2 seconds after
// bootup, to try to prevent Xbee bricking
return;
}
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
pending_status.severity = (uint8_t)severity;
strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
} else {
// send immediately
mavlink_msg_statustext_send(
chan,
severity,
(const int8_t*) str);
}
}
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0),
@ -32,10 +513,10 @@ _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("
}
void
GCS_MAVLINK::init(BetterStream * port)
GCS_MAVLINK::init(FastSerial * port)
{
GCS_Class::init(port);
if (port == &Serial) { // to split hil vs gcs
if (port == &Serial) {
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
}else{
@ -58,8 +539,6 @@ GCS_MAVLINK::update(void)
{
uint8_t c = comm_receive_ch(chan);
// Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
}
@ -67,20 +546,25 @@ GCS_MAVLINK::update(void)
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
_queued_send();
// send out queued params/ waypoints
if (NULL != _queued_parameter) {
send_message(MSG_NEXT_PARAM);
}
// stop waypoint sending if timeout
if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout"));
waypoint_sending = false;
}
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total) {
send_message(MSG_NEXT_WAYPOINT);
}
// stop waypoint receiving if timeout
if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout"));
waypoint_receiving = false;
}
// stop waypoint sending if timeout
if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
waypoint_sending = false;
}
// stop waypoint receiving if timeout
if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
waypoint_receiving = false;
}
}
void
@ -143,33 +627,27 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
}
void
GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
GCS_MAVLINK::send_message(enum ap_message id)
{
mavlink_send_message(chan,id,packet_drops);
mavlink_send_message(chan,id, packet_drops);
}
void
GCS_MAVLINK::send_text(uint8_t severity, const char *str)
GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
{
mavlink_send_text(chan,severity,str);
mavlink_send_text(chan,severity,str);
}
void
GCS_MAVLINK::send_text(uint8_t severity, const prog_char_t *str)
GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
{
mavlink_statustext_t m;
uint8_t i;
for (i=0; i<sizeof(m.text); i++) {
m.text[i] = pgm_read_byte((const prog_char *)(str++));
}
if (i < sizeof(m.text)) m.text[i] = 0;
mavlink_send_text(chan, severity, (const char *)m.text);
}
void
GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
mavlink_acknowledge(chan,id,sum1,sum2);
mavlink_statustext_t m;
uint8_t i;
for (i=0; i<sizeof(m.text); i++) {
m.text[i] = pgm_read_byte((const prog_char *)(str++));
}
if (i < sizeof(m.text)) m.text[i] = 0;
mavlink_send_text(chan, severity, (const char *)m.text);
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@ -270,7 +748,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
uint8_t result = 0;
// do action
send_text_P(SEVERITY_LOW,PSTR("action received: "));
send_text(SEVERITY_LOW,PSTR("action received: "));
//Serial.println(packet.action);
switch(packet.action){
@ -447,7 +925,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_receiving = false;
waypoint_dest_sysid = msg->sysid;
waypoint_dest_compid = msg->compid;
requested_interface = chan;
break;
}
@ -595,7 +1072,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
_queued_parameter = AP_Var::first();
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
requested_interface = chan;
break;
}
@ -657,6 +1133,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS
case MAVLINK_MSG_ID_SET_MAG_OFFSETS:
{
mavlink_set_mag_offsets_t packet;
mavlink_msg_set_mag_offsets_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z));
break;
}
#endif
// XXX receive a WP from GCS and store in EEPROM
case MAVLINK_MSG_ID_WAYPOINT:
{
@ -677,7 +1164,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
tell_command.options = 0;
tell_command.options = 0; // absolute altitude
break;
}
@ -687,7 +1174,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2;
tell_command.options = 1;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
//case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
@ -696,7 +1183,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2;
tell_command.options = 1; // store altitude relative!! Always!!
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break;
}
}
@ -799,7 +1286,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
msg->compid,
type);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
send_text(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
@ -1084,72 +1571,54 @@ GCS_MAVLINK::_find_parameter(uint16_t index)
}
/**
* @brief Send low-priority messages at a maximum rate of xx Hertz
*
* This function sends messages at a lower rate to not exceed the wireless
* bandwidth. It sends one message each time it is called until the buffer is empty.
* Call this function with xx Hertz to increase/decrease the bandwidth.
* @brief Send the next pending parameter, called from deferred message
* handling code
*/
void
GCS_MAVLINK::_queued_send()
GCS_MAVLINK::queued_param_send()
{
// Check to see if we are sending parameters
if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) {
AP_Var *vp;
float value;
// Check to see if we are sending parameters
if (NULL == _queued_parameter) return;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
_queued_parameter = _queued_parameter->next();
AP_Var *vp;
float value;
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float();
if (!isnan(value)) {
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
_queued_parameter = _queued_parameter->next();
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send(
chan,
(int8_t*)param_name,
value,
_queued_parameter_count,
_queued_parameter_index);
mavlink_msg_param_value_send(
chan,
(int8_t*)param_name,
value,
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter_index++;
}
mavdelay = 0;
}
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
mavdelay++;
// request waypoints one by one
// XXX note that this is pan-interface
if (waypoint_receiving &&
(requested_interface == (unsigned)chan) &&
waypoint_request_i <= (unsigned)g.waypoint_total &&
mavdelay > 15) { // limits to 3.33 hz
mavlink_msg_waypoint_request_send(
chan,
waypoint_dest_sysid,
waypoint_dest_compid,
waypoint_request_i);
mavdelay = 0;
}
_queued_parameter_index++;
}
}
#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
static void send_rate(uint16_t low, uint16_t high) {
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(low, high);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(low,high);
#endif
/**
* @brief Send the next pending waypoint, called from deferred message
* handling code
*/
void
GCS_MAVLINK::queued_waypoint_send()
{
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total) {
mavlink_msg_waypoint_request_send(
chan,
waypoint_dest_sysid,
waypoint_dest_compid,
waypoint_request_i);
}
}
/*
@ -1161,7 +1630,7 @@ static void send_rate(uint16_t low, uint16_t high) {
static void mavlink_delay(unsigned long t)
{
unsigned long tstart;
static unsigned long last_1hz, last_3hz, last_10hz, last_50hz;
static unsigned long last_1hz, last_50hz;
if (in_mavlink_delay) {
// this should never happen, but let's not tempt fate by
@ -1177,30 +1646,77 @@ static void mavlink_delay(unsigned long t)
unsigned long tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs.send_message(MSG_HEARTBEAT);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
#endif
send_rate(1, 3);
}
if (tnow - last_3hz > 333) {
last_3hz = tnow;
send_rate(3, 5);
}
if (tnow - last_10hz > 100) {
last_10hz = tnow;
send_rate(5, 45);
gcs_send_message(MSG_HEARTBEAT);
gcs_send_message(MSG_EXTENDED_STATUS1);
}
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
send_rate(45, 1000);
gcs_update();
}
delay(1);
} while (millis() - tstart < t);
in_mavlink_delay = false;
}
/*
send a message on both GCS links
*/
static void gcs_send_message(enum ap_message id)
{
gcs0.send_message(id);
gcs3.send_message(id);
}
/*
send data streams in the given rate range on both links
*/
static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
{
gcs0.data_stream_send(freqMin, freqMax);
gcs3.data_stream_send(freqMin, freqMax);
}
/*
look for incoming commands on the GCS links
*/
static void gcs_update(void)
{
gcs0.update();
gcs3.update();
}
static void gcs_send_text(gcs_severity severity, const char *str)
{
gcs0.send_text(severity, str);
gcs3.send_text(severity, str);
}
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
{
gcs0.send_text(severity, str);
gcs3.send_text(severity, str);
}
/*
send a low priority formatted message to the GCS
only one fits in the queue, so if you send more than one before the
last one gets into the serial buffer then the old one will be lost
*/
static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
{
char fmtstr[40];
va_list ap;
uint8_t i;
for (i=0; i<sizeof(fmtstr)-1; i++) {
fmtstr[i] = pgm_read_byte((const prog_char *)(fmt++));
if (fmtstr[i] == 0) break;
}
fmtstr[i] = 0;
pending_status.severity = (uint8_t)SEVERITY_LOW;
va_start(ap, fmt);
vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
va_end(ap);
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
}

371
ArduCopter/GCS_Standard.pde
View File

@ -1,371 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Doug _ command index is a byte and not an int
#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD
static byte gcs_messages_sent;
#if GCS_PORT == 3
# define SendSer Serial3.print
#else
# define SendSer Serial.print
#endif
// The functions included in this file are for use with the standard binary communication protocol and standard Ground Control Station
static void acknowledge(byte id, byte check1, byte check2) {
byte mess_buffer[6];
byte mess_ck_a = 0;
byte mess_ck_b = 0;
int ck;
SendSer("4D"); // This is the message preamble
mess_buffer[0] = 0x03;
ck = 3;
mess_buffer[1] = 0x00; // Message ID
mess_buffer[2] = 0x01; // Message version
mess_buffer[3] = id;
mess_buffer[4] = check1;
mess_buffer[5] = check2;
for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]);
for (int i = 0; i < ck + 3; i++) {
mess_ck_a += mess_buffer[i]; // Calculates checksums
mess_ck_b += mess_ck_a;
}
SendSer(mess_ck_a);
SendSer(mess_ck_b);
}
static void send_message(byte id) {
send_message(id, 0l);
}
static void send_message(byte id, long param) {
byte mess_buffer[54];
byte mess_ck_a = 0;
byte mess_ck_b = 0;
int tempint;
int ck;
long templong;
SendSer("4D"); // This is the message preamble
switch(id) {
case MSG_HEARTBEAT: // ** System Status message
mess_buffer[0] = 0x07;
ck = 7;
mess_buffer[3] = control_mode; // Mode
templong = millis() / 1000; // Timestamp - Seconds since power - up
mess_buffer[4] = templong & 0xff;
mess_buffer[5] = (templong >> 8) & 0xff;
tempint = battery_voltage1 * 100; // Battery voltage ( * 100)
mess_buffer[6] = tempint & 0xff;
mess_buffer[7] = (tempint >> 8) & 0xff;
tempint = command_must_index; // Command Index (waypoint level)
mess_buffer[8] = tempint & 0xff;
mess_buffer[9] = (tempint >> 8) & 0xff;
break;
case MSG_ATTITUDE: // ** Attitude message
mess_buffer[0] = 0x06;
ck = 6;
tempint = dcm.roll_sensor; // Roll (degrees * 100)
mess_buffer[3] = tempint & 0xff;
mess_buffer[4] = (tempint >> 8) & 0xff;
tempint = dcm.pitch_sensor; // Pitch (degrees * 100)
mess_buffer[5] = tempint & 0xff;
mess_buffer[6] = (tempint >> 8) & 0xff;
tempint = dcm.yaw_sensor; // Yaw (degrees * 100)
mess_buffer[7] = tempint & 0xff;
mess_buffer[8] = (tempint >> 8) & 0xff;
break;
case MSG_LOCATION: // ** Location / GPS message
mess_buffer[0] = 0x12;
ck = 18;
templong = current_loc.lat; // Latitude *10 * *7 in 4 bytes
mess_buffer[3] = templong & 0xff;
mess_buffer[4] = (templong >> 8) & 0xff;
mess_buffer[5] = (templong >> 16) & 0xff;
mess_buffer[6] = (templong >> 24) & 0xff;
templong = current_loc.lng; // Longitude *10 * *7 in 4 bytes
mess_buffer[7] = templong & 0xff;
mess_buffer[8] = (templong >> 8) & 0xff;
mess_buffer[9] = (templong >> 16) & 0xff;
mess_buffer[10] = (templong >> 24) & 0xff;
tempint = g_gps->altitude / 100; // Altitude MSL in meters * 10 in 2 bytes
mess_buffer[11] = tempint & 0xff;
mess_buffer[12] = (tempint >> 8) & 0xff;
tempint = g_gps->ground_speed; // Speed in M / S * 100 in 2 bytes
mess_buffer[13] = tempint & 0xff;
mess_buffer[14] = (tempint >> 8) & 0xff;
tempint = dcm.yaw_sensor; // Ground Course in degreees * 100 in 2 bytes
mess_buffer[15] = tempint & 0xff;
mess_buffer[16] = (tempint >> 8) & 0xff;
templong = g_gps->time; // Time of Week (milliseconds) in 4 bytes
mess_buffer[17] = templong & 0xff;
mess_buffer[18] = (templong >> 8) & 0xff;
mess_buffer[19] = (templong >> 16) & 0xff;
mess_buffer[20] = (templong >> 24) & 0xff;
break;
case MSG_PRESSURE: // ** Pressure message
mess_buffer[0] = 0x04;
ck = 4;
tempint = current_loc.alt / 10; // Altitude MSL in meters * 10 in 2 bytes
mess_buffer[3] = tempint & 0xff;
mess_buffer[4] = (tempint >> 8) & 0xff;
tempint = (int)airspeed / 100; // Airspeed pressure
mess_buffer[5] = tempint & 0xff;
mess_buffer[6] = (tempint >> 8) & 0xff;
break;
// case 0xMSG_STATUS_TEXT: // ** Status Text message
// mess_buffer[0]=sizeof(status_message[0])+1;
// ck=mess_buffer[0];
// mess_buffer[2] = param&0xff;
// for (int i=3;i<ck+2;i++) mess_buffer[i] = status_message[i-3];
// break;
case MSG_PERF_REPORT: // ** Performance Monitoring message
mess_buffer[0] = 0x10;
ck = 16;
templong = millis() - perf_mon_timer; // Report interval (milliseconds) in 4 bytes
mess_buffer[3] = templong & 0xff;
mess_buffer[4] = (templong >> 8) & 0xff;
mess_buffer[5] = (templong >> 16) & 0xff;
mess_buffer[6] = (templong >> 24) & 0xff;
tempint = mainLoop_count; // Main Loop cycles
mess_buffer[7] = tempint & 0xff;
mess_buffer[8] = (tempint >> 8) & 0xff;
mess_buffer[9] = 0 & 0xff;
mess_buffer[10] = gyro_sat_count; // Problem counts
mess_buffer[11] = adc_constraints;
mess_buffer[12] = renorm_sqrt_count;
mess_buffer[13] = renorm_blowup_count;
mess_buffer[14] = gps_fix_count;
tempint = (int)(imu_health * 1000); // IMU health metric
mess_buffer[15] = tempint & 0xff;
mess_buffer[16] = (tempint >> 8) & 0xff;
tempint = gcs_messages_sent; // GCS messages sent
mess_buffer[17] = tempint & 0xff;
mess_buffer[18] = (tempint >> 8) & 0xff;
break;
case MSG_VALUE: // ** Requested Value message
mess_buffer[0] = 0x06;
ck = 6;
templong = param; // Parameter being sent
mess_buffer[3] = templong & 0xff;
mess_buffer[4] = (templong >> 8) & 0xff;
switch(param) {
/*
case 0x10: templong = integrator[0] * 1000; break;
case 0x11: templong = integrator[1] * 1000; break;
case 0x12: templong = integrator[2] * 1000; break;
case 0x13: templong = integrator[3] * 1000; break;
case 0x14: templong = integrator[4] * 1000; break;
case 0x15: templong = integrator[5] * 1000; break;
case 0x16: templong = integrator[6] * 1000; break;
case 0x17: templong = integrator[7] * 1000; break;
case 0x20: templong = target_bearing; break;
case 0x21: templong = nav_bearing; break;
case 0x22: templong = bearing_error; break;
case 0x23: templong = crosstrack_bearing; break;
case 0x24: templong = crosstrack_correction; break;
case 0x25: templong = altitude_error; break;
case 0x26: templong = wp_radius; break;
case 0x27: templong = loiter_radius; break;
// case 0x28: templong = wp_mode; break;
// case 0x29: templong = loop_commands; break;
*/
}
mess_buffer[5] = templong & 0xff;
mess_buffer[6] = (templong >> 8) & 0xff;
mess_buffer[7] = (templong >> 16) & 0xff;
mess_buffer[8] = (templong >> 24) & 0xff;
break;
case MSG_COMMAND: // Command list item message
mess_buffer[0] = 0x10;
ck = 16;
tempint = param; // item number
mess_buffer[3] = tempint & 0xff;
mess_buffer[4] = (tempint >> 8) & 0xff;
tempint = g.waypoint_total; // list length (# of commands in mission)
mess_buffer[5] = tempint & 0xff;
mess_buffer[6] = (tempint >> 8) & 0xff;
tell_command = get_command_with_index((int)param);
mess_buffer[7] = tell_command.id; // command id
mess_buffer[8] = tell_command.p1; // P1
tempint = tell_command.alt; // P2
mess_buffer[9] = tempint & 0xff;
mess_buffer[10] = (tempint >> 8) & 0xff;
templong = tell_command.lat; // P3
mess_buffer[11] = templong & 0xff;
mess_buffer[12] = (templong >> 8) & 0xff;
mess_buffer[13] = (templong >> 16) & 0xff;
mess_buffer[14] = (templong >> 24) & 0xff;
templong = tell_command.lng; // P4
mess_buffer[15] = templong & 0xff;
mess_buffer[16] = (templong >> 8) & 0xff;
mess_buffer[17] = (templong >> 16) & 0xff;
mess_buffer[18] = (templong >> 24) & 0xff;
break;
case MSG_TRIMS: // Radio Trims message
//mess_buffer[0] = 0x10;
//ck = 16;
//for(int i = 0; i < 8; i++) {
// tempint = radio_trim[i]; // trim values
// mess_buffer[3 + 2 * i] = tempint & 0xff;
// mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff;
//}
break;
case MSG_MINS: // Radio Mins message
/*mess_buffer[0] = 0x10;
ck = 16;
for(int i = 0; i < 8; i++) {
tempint = radio_min[i]; // min values
mess_buffer[3 + 2 * i] = tempint & 0xff;
mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff;
}*/
break;
case MSG_MAXS: // Radio Maxs message
/*mess_buffer[0] = 0x10;
ck = 16;
for(int i = 0; i < 8; i++) {
tempint = radio_max[i]; // max values
mess_buffer[3 + 2 * i] = tempint & 0xff;
mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff;
}*/
break;
case MSG_PID: // PID Gains message
/*
mess_buffer[0] = 0x0f;
ck = 15;
mess_buffer[3] = param & 0xff; // PID set #
templong = (kp[param] * 1000000); // P gain
mess_buffer[4] = templong & 0xff;
mess_buffer[5] = (templong >> 8) & 0xff;
mess_buffer[6] = (templong >> 16) & 0xff;
mess_buffer[7] = (templong >> 24) & 0xff;
templong = (ki[param] * 1000000); // I gain
mess_buffer[8] = templong & 0xff;
mess_buffer[9] = (templong >> 8) & 0xff;
mess_buffer[10] = (templong >> 16) & 0xff;
mess_buffer[11] = (templong >> 24) & 0xff;
templong = (kd[param] * 1000000); // D gain
mess_buffer[12] = templong & 0xff;
mess_buffer[13] = (templong >> 8) & 0xff;
mess_buffer[14] = (templong >> 16) & 0xff;
mess_buffer[15] = (templong >> 24) & 0xff;
tempint = integrator_max[param]; // Integrator max value
mess_buffer[16] = tempint & 0xff;
mess_buffer[17] = (tempint >> 8) & 0xff;
*/
break;
}
//mess_buffer[0] = length; // Message length
mess_buffer[1] = id; // Message ID
mess_buffer[2] = 0x01; // Message version
for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]);
for (int i = 0; i < ck + 3; i++) {
mess_ck_a += mess_buffer[i]; // Calculates checksums
mess_ck_b += mess_ck_a;
}
SendSer(mess_ck_a);
SendSer(mess_ck_b);
gcs_messages_sent++;
}
static void send_message(byte severity, const char *str) // This is the instance of send_message for message MSG_STATUS_TEXT
{
if(severity >= DEBUG_LEVEL){
byte length = strlen(str) + 1;
byte mess_buffer[54];
byte mess_ck_a = 0;
byte mess_ck_b = 0;
int ck;
SendSer("4D"); // This is the message preamble
if(length > 50) length = 50;
mess_buffer[0] = length;
ck = length;
mess_buffer[1] = 0x05; // Message ID
mess_buffer[2] = 0x01; // Message version
mess_buffer[3] = severity;
for (int i = 3; i < ck + 2; i++)
mess_buffer[i] = str[i - 3]; // places the text into mess_buffer at locations 3+
for (int i = 0; i < ck + 3; i++)
SendSer(mess_buffer[i]);
for (int i = 0; i < ck + 3; i++) {
mess_ck_a += mess_buffer[i]; // Calculates checksums
mess_ck_b += mess_ck_a;
}
SendSer(mess_ck_a);
SendSer(mess_ck_b);
}
}
static void print_current_waypoints()
{
}
static void print_waypoint(struct Location *cmd, byte index)
{
Serial.print("command #: ");
Serial.print(index, DEC);
Serial.print(" id: ");
Serial.print(cmd->id, DEC);
Serial.print(" p1: ");
Serial.print(cmd->p1, DEC);
Serial.print(" p2: ");
Serial.print(cmd->alt, DEC);
Serial.print(" p3: ");
Serial.print(cmd->lat, DEC);
Serial.print(" p4: ");
Serial.println(cmd->lng, DEC);
}
static void print_waypoints()
{
}
#endif
#if GCS_PROTOCOL == GCS_PROTOCOL_NONE
static void acknowledge(byte id, byte check1, byte check2) {}
static void send_message(byte id) {}
static void send_message(byte id, long param) {}
static void send_message(byte severity, const char *str) {
Serial.println(str);
}
static void print_current_waypoints(){}
static void print_waypoint(struct Location *cmd, byte index){}
static void print_waypoints(){}
#endif

127
ArduCopter/GCS_Xplane.pde
View File

@ -1,127 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE
void acknowledge(byte id, byte check1, byte check2)
{
}
void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
{
if(severity >= DEBUG_LEVEL){
Serial.print("MSG: ");
Serial.println(str);
}
}
void send_message(byte id) {
send_message(id,0l);
}
void send_message(byte id, long param) {
switch(id) {
case MSG_HEARTBEAT:
print_control_mode();
break;
case MSG_ATTITUDE:
//print_attitude();
break;
case MSG_LOCATION:
//print_position();
break;
case MSG_COMMAND:
struct Location cmd = get_command_with_index(param);
print_waypoint(&cmd, param);
break;
}
}
// required by Groundstation to plot lateral tracking course
void print_new_wp_info()
{
}
void print_control_mode(void)
{
Serial.print("MSG: ");
Serial.print(flight_mode_strings[control_mode]);
}
void print_current_waypoints()
{
Serial.print("MSG: ");
Serial.print("CUR:");
Serial.print("\t");
Serial.print(current_loc.lat,DEC);
Serial.print(",\t");
Serial.print(current_loc.lng,DEC);
Serial.print(",\t");
Serial.println(current_loc.alt,DEC);
Serial.print("MSG: ");
Serial.print("NWP:");
Serial.print(g.waypoint_index,DEC);
Serial.print(",\t");
Serial.print(next_WP.lat,DEC);
Serial.print(",\t");
Serial.print(next_WP.lng,DEC);
Serial.print(",\t");
Serial.println(next_WP.alt,DEC);
}
void print_position(void)
{
}
void printPerfData(void)
{
}
void print_attitude(void)
{
}
void print_tuning(void) {
}
void print_waypoint(struct Location *cmd,byte index)
{
Serial.print("MSG: command #: ");
Serial.print(index, DEC);
Serial.print(" id: ");
Serial.print(cmd->id,DEC);
Serial.print(" p1: ");
Serial.print(cmd->p1,DEC);
Serial.print(" p2: ");
Serial.print(cmd->alt,DEC);
Serial.print(" p3: ");
Serial.print(cmd->lat,DEC);
Serial.print(" p4: ");
Serial.println(cmd->lng,DEC);
}
void print_waypoints()
{
Serial.println("Commands in memory");
Serial.print("commands total: ");
Serial.println(g.waypoint_total, DEC);
// create a location struct to hold the temp Waypoints for printing
//Location tmp;
Serial.println("Home: ");
struct Location cmd = get_command_with_index(0);
print_waypoint(&cmd, 0);
Serial.println("Commands: ");
for (int i=1; i < g.waypoint_total; i++){
cmd = get_command_with_index(i);
print_waypoint(&cmd, i);
}
}
#endif

135
ArduCopter/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 <BetterStream.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(BetterStream *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
BetterStream *_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(BetterStream *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

131
ArduCopter/HIL_Xplane.pde
View File

@ -1,131 +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.print("AAA"); // Message preamble
output_int((int)(g.rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(g.rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(g.rc_4.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(BetterStream * 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_IMU1:
case MSG_RAW_IMU2:
case MSG_RAW_IMU3:
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

2
ArduCopter/Log.pde
View File

@ -285,7 +285,7 @@ static void start_new_log()
DataFlash.StartWrite(start_pages[num_existing_logs - 1]);
}else{
gcs.send_text_P(SEVERITY_LOW,PSTR("Logs full"));
gcs_send_text_P(SEVERITY_LOW,PSTR("Logs full"));
}
}

456
ArduCopter/Mavlink_Common.h
View File

@ -1,456 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef Mavlink_Common_H
#define Mavlink_Common_H
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
byte mavdelay = 0;
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
/*
!!NOTE!!
the use of NOINLINE separate functions for each message type avoids
a compiler bug in gcc that would cause it to use far more stack
space than is needed. Without the NOINLINE we use the sum of the
stack needed for each message type. Please be careful to follow the
pattern below when adding any new messages
*/
#define NOINLINE __attribute__((noinline))
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
default:
mode = control_mode + 100;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
0,
battery_voltage * 1000,
battery_remaining,
packet_drops);
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
{
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
}
static void NOINLINE send_location(mavlink_channel_t chan)
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
target_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
0,
0);
}
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
mavlink_msg_gps_raw_send(
chan,
micros(),
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
}
static void NOINLINE send_servo_out(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
#if FRAME_CONFIG == HELI_FRAME
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out,
0,
0,
0,
0,
rssi);
#else
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out,
10000 * g.rc_1.norm_output(),
10000 * g.rc_2.norm_output(),
10000 * g.rc_3.norm_output(),
10000 * g.rc_4.norm_output(),
rssi);
#endif
}
static void NOINLINE send_radio_in(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
g.rc_4.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
}
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
mavlink_msg_servo_output_raw_send(
chan,
motor_out[0],
motor_out[1],
motor_out[2],
motor_out[3],
motor_out[4],
motor_out[5],
motor_out[6],
motor_out[7]);
}
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10,
current_loc.alt / 100.0,
climb_rate);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
mavlink_msg_raw_imu_send(
chan,
micros(),
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
mavlink_msg_scaled_pressure_send(
chan,
micros(),
(float)barometer.Press/100.0,
(float)(barometer.Press-ground_pressure)/100.0,
(int)(barometer.Temp*10));
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_gps_status(mavlink_channel_t chan)
{
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
return false;
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
break;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
break;
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW);
send_gps_raw(chan);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(chan);
break;
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
break;
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
break;
#endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_GPS_STATUS:
CHECK_PAYLOAD_SIZE(GPS_STATUS);
send_gps_status(chan);
break;
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
send_current_waypoint(chan);
break;
default:
break;
}
return true;
}
#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of
// switch types in mavlink_try_send_message()
static struct mavlink_queue {
uint8_t deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// don't send status MAVLink messages for 1 second after
// bootup, to try to prevent Xbee bricking
return;
}
mavlink_msg_statustext_send(
chan,
severity,
(const int8_t*) str);
}
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
{
}
#endif // mavlink in use
#endif // inclusion guard

3
ArduCopter/commands.pde
View File

@ -150,7 +150,6 @@ static void set_next_WP(struct Location *wp)
{
//SendDebug("MSG <set_next_wp> wp_index: ");
//SendDebugln(g.waypoint_index, DEC);
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// copy the current WP into the OldWP slot
// ---------------------------------------
@ -182,8 +181,6 @@ static void set_next_WP(struct Location *wp)
// reset speed governer
// --------------------
waypoint_speed_gov = 0;
gcs.print_current_waypoints();
}

12
ArduCopter/commands_logic.pde
View File

@ -154,7 +154,7 @@ static bool verify_must()
break;
default:
//gcs.send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current Must commands"));
//gcs_send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current Must commands"));
return false;
break;
}
@ -181,7 +181,7 @@ static bool verify_may()
break;
default:
//gcs.send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current May commands"));
//gcs_send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current May commands"));
return false;
break;
}
@ -207,7 +207,7 @@ static void do_RTL(void)
// output control mode to the ground station
// -----------------------------------------
gcs.send_message(MSG_HEARTBEAT);
gcs_send_message(MSG_HEARTBEAT);
}
/********************************************************************************/
@ -412,7 +412,7 @@ static bool verify_nav_wp()
if ((millis() - loiter_time) > loiter_time_max) {
wp_verify_byte |= NAV_DELAY;
//gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//Serial.println("vlt done");
}
}
@ -421,7 +421,7 @@ static bool verify_nav_wp()
//if(wp_verify_byte & NAV_LOCATION){
char message[30];
sprintf(message,"Reached Command #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
gcs_send_text(SEVERITY_LOW,message);
wp_verify_byte = 0;
return true;
}else{
@ -467,7 +467,7 @@ static bool verify_loiter_turns()
static bool verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
//gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home"));
//gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
return true;
}else{
return false;

10
ArduCopter/commands_process.pde
View File

@ -7,7 +7,7 @@ static void change_command(uint8_t index)
struct Location temp = get_command_with_index(index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs.send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
} else {
command_must_index = NO_COMMAND;
next_command.id = NO_COMMAND;
@ -39,7 +39,7 @@ static void update_commands(void)
// And we have no nav commands
// --------------------------------------------
if (command_must_ID == NO_COMMAND){
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
}
}
@ -142,8 +142,7 @@ process_next_command()
/**************************************************/
static void process_must()
{
//gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
//gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
//Serial.printf("pmst %d\n", (int)next_command.id);
// clear May indexes to force loading of more commands
@ -161,8 +160,7 @@ static void process_must()
static void process_may()
{
//gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
//gcs_send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
//Serial.print("pmay");
command_may_ID = next_command.id;

44
ArduCopter/config.h
View File

@ -80,22 +80,12 @@
//////////////////////////////////////////////////////////////////////////////
// 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
@ -104,48 +94,14 @@
#endif
// If we are in XPlane, diasble the mag
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
// check xplane settings
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
// MAGNETOMETER not supported by XPLANE
# undef MAGNETOMETER
# define MAGNETOMETER DISABLED
# if HIL_MODE != HIL_MODE_ATTITUDE
# error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE
# endif
#endif
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL
// GCS_PORT
//
#ifndef GCS_PROTOCOL
# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#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 MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif

113
ArduCopter/defines.h
View File

@ -1,5 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef _DEFINES_H
#define _DEFINES_H
// Just so that it's completely clear...
#define ENABLED 1
#define DISABLED 0
@ -99,21 +102,10 @@
#define RC_CHANNEL_ANGLE_RAW 2
// HIL enumerations
#define HIL_PROTOCOL_XPLANE 1
#define HIL_PROTOCOL_MAVLINK 2
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
// GCS enumeration
#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol
#define GCS_PROTOCOL_LEGACY 1 // legacy ArduPilot protocol
#define GCS_PROTOCOL_XPLANE 2 // X-Plane HIL simulation
#define GCS_PROTOCOL_DEBUGTERMINAL 3 //Human-readable debug interface for use with a dumb terminal
#define GCS_PROTOCOL_MAVLINK 4 // binary protocol for qgroundcontrol
#define GCS_PROTOCOL_NONE -1 // No GCS output
// Auto Pilot modes
// ----------------
#define STABILIZE 0 // hold level position
@ -193,69 +185,39 @@
//#define MAV_CMD_CONDITION_YAW 23
// GCS Message ID's
#define MSG_ACKNOWLEDGE 0x00
#define MSG_HEARTBEAT 0x01
#define MSG_ATTITUDE 0x02
#define MSG_LOCATION 0x03
#define MSG_PRESSURE 0x04
#define MSG_STATUS_TEXT 0x05
#define MSG_PERF_REPORT 0x06
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
#define MSG_VERSION_REQUEST 0x08
#define MSG_VERSION 0x09
#define MSG_EXTENDED_STATUS1 0x0a
#define MSG_EXTENDED_STATUS2 0x0b
#define MSG_CPU_LOAD 0x0c
#define MSG_NAV_CONTROLLER_OUTPUT 0x0d
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
/// create new MSG_ IDs for additional messages on the same
/// stream
enum ap_message {
MSG_HEARTBEAT,
MSG_ATTITUDE,
MSG_LOCATION,
MSG_EXTENDED_STATUS1,
MSG_EXTENDED_STATUS2,
MSG_NAV_CONTROLLER_OUTPUT,
MSG_CURRENT_WAYPOINT,
MSG_VFR_HUD,
MSG_RADIO_OUT,
MSG_RADIO_IN,
MSG_RAW_IMU1,
MSG_RAW_IMU2,
MSG_RAW_IMU3,
MSG_GPS_STATUS,
MSG_GPS_RAW,
MSG_SERVO_OUT,
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
MSG_STATUSTEXT,
MSG_RETRY_DEFERRED // this must be last
};
#define MSG_COMMAND_REQUEST 0x20
#define MSG_COMMAND_UPLOAD 0x21
#define MSG_COMMAND_LIST 0x22
#define MSG_COMMAND_MODE_CHANGE 0x23
#define MSG_CURRENT_WAYPOINT 0x24
#define MSG_VALUE_REQUEST 0x30
#define MSG_VALUE_SET 0x31
#define MSG_VALUE 0x32
#define MSG_PID_REQUEST 0x40
#define MSG_PID_SET 0x41
#define MSG_PID 0x42
#define MSG_VFR_HUD 0x4a
#define MSG_TRIM_STARTUP 0x50
#define MSG_TRIM_MIN 0x51
#define MSG_TRIM_MAX 0x52
#define MSG_RADIO_OUT 0x53
#define MSG_RADIO_IN 0x54
#define MSG_RAW_IMU1 0x60
#define MSG_RAW_IMU2 0x61
#define MSG_RAW_IMU3 0x62
#define MSG_GPS_STATUS 0x63
#define MSG_GPS_RAW 0x64
#define MSG_SERVO_OUT 0x70
#define MSG_PIN_REQUEST 0x80
#define MSG_PIN_SET 0x81
#define MSG_DATAFLASH_REQUEST 0x90
#define MSG_DATAFLASH_SET 0x91
#define MSG_EEPROM_REQUEST 0xa0
#define MSG_EEPROM_SET 0xa1
#define MSG_POSITION_CORRECT 0xb0
#define MSG_ATTITUDE_CORRECT 0xb1
#define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3
#define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1
#define SEVERITY_MEDIUM 2
#define SEVERITY_HIGH 3
#define SEVERITY_CRITICAL 4
enum gcs_severity {
SEVERITY_LOW=1,
SEVERITY_MEDIUM,
SEVERITY_HIGH,
SEVERITY_CRITICAL
};
// Logging parameters
#define TYPE_AIRSTART_MSG 0x00
@ -377,3 +339,8 @@
#define ONBOARD_PARAM_NAME_LENGTH 15
#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
// mark a function as not to be inlined
#define NOINLINE __attribute__((noinline))
#endif // _DEFINES_H

2
ArduCopter/events.pde
View File

@ -45,7 +45,7 @@ static void failsafe_off_event()
static void low_battery_event(void)
{
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
low_batt = true;
// if we are in Auto mode, come home

4
ArduCopter/motors.pde
View File

@ -26,7 +26,7 @@ static void arm_motors()
}else if (arming_counter == ARM_DELAY){
#if HIL_MODE != HIL_MODE_DISABLED
hil.send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif
motor_armed = true;
arming_counter = ARM_DELAY;
@ -90,7 +90,7 @@ static void arm_motors()
}else if (arming_counter == DISARM_DELAY){
#if HIL_MODE != HIL_MODE_DISABLED
hil.send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif
motor_armed = false;

2
ArduCopter/navigation.pde
View File

@ -14,7 +14,7 @@ static byte navigate()
wp_distance = get_distance(&current_loc, &next_WP);
if (wp_distance < 0){
//gcs.send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
//gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return 0;

22
ArduCopter/planner.pde
View File

@ -26,7 +26,7 @@ planner_mode(uint8_t argc, const Menu::arg *argv)
static int8_t
planner_gcs(uint8_t argc, const Menu::arg *argv)
{
gcs.init(&Serial);
gcs0.init(&Serial);
int loopcount = 0;
@ -34,21 +34,17 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
if (millis()-fast_loopTimer > 19) {
fast_loopTimer = millis();
gcs.update();
gcs_update();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs_data_stream_send(45, 1000);
gcs.data_stream_send(45, 1000);
if ((loopcount % 5) == 0) // 10 hz
gcs_data_stream_send(5, 45);
if ((loopcount % 5) == 0) // 10 hz
gcs.data_stream_send(5, 45);
if ((loopcount % 16) == 0) { // 3 hz
gcs.data_stream_send(1, 5);
gcs.send_message(MSG_HEARTBEAT);
}
#endif
if ((loopcount % 16) == 0) { // 3 hz
gcs_data_stream_send(1, 5);
gcs_send_message(MSG_HEARTBEAT);
}
loopcount++;
}

4
ArduCopter/sensors.pde
View File

@ -8,7 +8,7 @@ static void ReadSCP1000(void) {}
static void init_barometer(void)
{
#if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets for initialization
gcs_update(); // look for inbound hil packets for initialization
#endif
ground_temperature = barometer.Temp;
@ -28,7 +28,7 @@ static void init_barometer(void)
delay(20);
#if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets
gcs_update(); // look for inbound hil packets
#endif
// Get initial data from absolute pressure sensor

28
ArduCopter/system.pde
View File

@ -187,27 +187,8 @@ static void init_ardupilot()
g_gps->callback = mavlink_delay;
// init the GCS
#if GCS_PORT == 3
gcs.init(&Serial3);
#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
gcs0.init(&Serial);
gcs3.init(&Serial3);
if(g.compass_enabled)
init_compass();
@ -309,7 +290,7 @@ static void init_ardupilot()
//********************************************************************************
static void startup_ground(void)
{
gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
@ -453,9 +434,6 @@ static void set_mode(byte mode)
}
Log_Write_Mode(control_mode);
// output control mode to the ground station
gcs.send_message(MSG_MODE_CHANGE);
}
static void set_failsafe(boolean mode)