diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 4f5574ab86..34c6017c03 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -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) diff --git a/ArduCopter/APM_Config_mavlink_hil.h b/ArduCopter/APM_Config_mavlink_hil.h index 6901e2a29b..bd713d9bea 100644 --- a/ArduCopter/APM_Config_mavlink_hil.h +++ b/ArduCopter/APM_Config_mavlink_hil.h @@ -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 diff --git a/ArduCopter/APM_Config_xplane.h b/ArduCopter/APM_Config_xplane.h deleted file mode 100644 index 84ef95f106..0000000000 --- a/ArduCopter/APM_Config_xplane.h +++ /dev/null @@ -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 diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 49e0fc3f46..ec3895e9a3 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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 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(); diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index 8dff8c6475..21aa40d7d1 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -6,7 +6,7 @@ #ifndef __GCS_H #define __GCS_H -#include +#include #include #include #include @@ -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 diff --git a/ArduCopter/GCS_Ardupilot.pde b/ArduCopter/GCS_Ardupilot.pde deleted file mode 100644 index 13d8a2d3f1..0000000000 --- a/ArduCopter/GCS_Ardupilot.pde +++ /dev/null @@ -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 diff --git a/ArduCopter/GCS_IMU_ouput.pde b/ArduCopter/GCS_IMU_ouput.pde deleted file mode 100644 index 0316591913..0000000000 --- a/ArduCopter/GCS_IMU_ouput.pde +++ /dev/null @@ -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 diff --git a/ArduCopter/GCS_Jason_text.pde b/ArduCopter/GCS_Jason_text.pde deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8e471c9bc9..4730ea57f4 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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; isysid; 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> 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> 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 diff --git a/ArduCopter/GCS_Xplane.pde b/ArduCopter/GCS_Xplane.pde deleted file mode 100644 index ff8872bb64..0000000000 --- a/ArduCopter/GCS_Xplane.pde +++ /dev/null @@ -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 - diff --git a/ArduCopter/HIL.h b/ArduCopter/HIL.h deleted file mode 100644 index 85b65fa1be..0000000000 --- a/ArduCopter/HIL.h +++ /dev/null @@ -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 -#include -#include -#include -#include - -/// -/// @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 - diff --git a/ArduCopter/HIL_Xplane.pde b/ArduCopter/HIL_Xplane.pde deleted file mode 100644 index 9ecf47ab84..0000000000 --- a/ArduCopter/HIL_Xplane.pde +++ /dev/null @@ -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 diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index ed60a69aa9..4cceb2a7ed 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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")); } } diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h deleted file mode 100644 index e2f4531fb4..0000000000 --- a/ArduCopter/Mavlink_Common.h +++ /dev/null @@ -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 diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 01b60c22bb..c9d1660bf6 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -150,7 +150,6 @@ static void set_next_WP(struct Location *wp) { //SendDebug("MSG 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(); } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index c8a0cb1628..eeff0e6112 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -154,7 +154,7 @@ static bool verify_must() break; default: - //gcs.send_text_P(SEVERITY_HIGH,PSTR(" No current Must commands")); + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current Must commands")); return false; break; } @@ -181,7 +181,7 @@ static bool verify_may() break; default: - //gcs.send_text_P(SEVERITY_HIGH,PSTR(" No current May commands")); + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" 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; diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 89797c486e..2e46ac1e4c 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -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: ")); - //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); + //gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); //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("")); - //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); + //gcs_send_text_P(SEVERITY_LOW,PSTR("")); //Serial.print("pmay"); command_may_ID = next_command.id; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 67585a6bfc..894ec2f60d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 9408dea354..19d14c4580 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index d6ae523c54..2b892280bc 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -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 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index d03710efd5..0e961c092c 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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; diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 9b83625e8e..a102bbcd38 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -14,7 +14,7 @@ static byte navigate() wp_distance = get_distance(¤t_loc, &next_WP); if (wp_distance < 0){ - //gcs.send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); //Serial.println(wp_distance,DEC); //print_current_waypoints(); return 0; diff --git a/ArduCopter/planner.pde b/ArduCopter/planner.pde index e8df9820dd..04c7a3a284 100644 --- a/ArduCopter/planner.pde +++ b/ArduCopter/planner.pde @@ -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++; } diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index a55a1b60ea..12e0647aba 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -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 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index bc8d48cad3..496fdceccf 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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)