Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

import APM_Camera branch from SVN

This commit is contained in:
Andrew Tridgell 2011-09-09 11:45:13 +10:00
parent 0d5ea17f63
commit 609ae8359d
30 changed files with 1707 additions and 1954 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduPlane/APM_Config.h
View File

@ -1,5 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
// their default values, place the appropriate #define statements here. // their default values, place the appropriate #define statements here.
@ -23,3 +22,36 @@
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PORT 3 #define GCS_PORT 3
*/ */
#define MAGNETOMETER ENABLED
// ----- Camera definitions ------
// ------------------------------
#define CAMERA ENABLED
#define CAM_DEBUG DISABLED
// Comment out servos that you do not have
//#define CAM_SERVO 8 // Camera servo channel
#define CAM_ANGLE 30 // Set angle in degrees
//#define CAM_CLICK 45 // This is the position of the servo arm when it actually clicks
//#define OPEN_SERVO 5 // Retraction servo channel - my camera retracts yours might not.
// Camera yaw (left-right)
#define YAW_SERVO 7 // Pan servo channel (can be pitch in stabilization)
#define YAW_REV 1 // output is normal = 1 output is reversed = -1
#define YAW_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg
#define YAW_RANGE 90 // Pan Servo sweep in degrees
#define YAW_RATIO 10.31 // match this to the swing of your pan servo
// Camera pitch (up-down)
#define PITCH_SERVO 6 // Tilt servo channel (can be roll in stabilization)
#define PITCH_REV 1 // output is normal = 1 output is reversed = -1
#define PITCH_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg
#define PITCH_RANGE 90 // Tilt Servo sweep in degrees
#define PITCH_RATIO 10.31 // match this to the swing of your tilt servo
// Camera roll (up-down)
#define ROLL_SERVO 6 // Tilt servo channel (can be roll in stabilization)
#define ROLL_REV 1 // output is normal = 1 output is reversed = -1
#define ROLL_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg
#define ROLL_RANGE 90 // Tilt Servo sweep in degrees
#define ROLL_RATIO 10.31 // match this to the swing of your tilt servo

36
ArduPlane/APM_Config.h.reference
View File

@ -210,7 +210,7 @@
// INPUT_VOLTAGE OPTIONAL // INPUT_VOLTAGE OPTIONAL
// //
// In order to have accurate pressure and battery voltage readings, this // In order to have accurate pressure and battery voltage readings, this
// value should be set to the voltage measured at the processor. // value should be set to the voltage measured on the 5V rail on the oilpan.
// //
// See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail. // See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail.
// //
@ -304,32 +304,6 @@
//#define FLIGHT_MODE_6 MANUAL //#define FLIGHT_MODE_6 MANUAL
// //
//////////////////////////////////////////////////////////////////////////////
// RC_5_FUNCT OPTIONAL
// RC_6_FUNCT OPTIONAL
// RC_7_FUNCT OPTIONAL
// RC_8_FUNCT OPTIONAL
//
// The channel 5 through 8 function assignments allow configuration of those
// channels for use with differential ailerons, flaps, flaperons, or camera
// or intrument mounts
//
//#define RC_5_FUNCT RC_5_FUNCT_NONE
//etc.
//////////////////////////////////////////////////////////////////////////////
// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed
// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed
// then the flap position shall be flap_1_percent. If the speed setpoint is below flap_2_speed
// then the flap position shall be flap_2_percent. Speed setpoint is the current value of
// airspeed_cruise (if airspeed sensor enabled) or throttle_cruise (if no airspeed sensor)
// FLAP_1_PERCENT OPTIONAL
// FLAP_1_SPEED OPTIONAL
// FLAP_2_PERCENT OPTIONAL
// FLAP_2_SPEED OPTIONAL
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE OPTIONAL // THROTTLE_FAILSAFE OPTIONAL
// THROTTLE_FS_VALUE OPTIONAL // THROTTLE_FS_VALUE OPTIONAL
@ -387,7 +361,7 @@
// the aircraft will head for home in RTL mode. If the failsafe condition is // the aircraft will head for home in RTL mode. If the failsafe condition is
// resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home // resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home
// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER // If XX_FAILSAFE_ACTION is 2 and the applicable failsafe occurs while in AUTO or LOITER
// mode the aircraft will continue in that mode ignoring the failsafe condition. // mode the aircraft will continue in that mode ignoring the failsafe condition.
// Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always // Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always
@ -398,8 +372,8 @@
// The default behaviour is to ignore failsafes in AUTO and LOITER modes. // The default behaviour is to ignore failsafes in AUTO and LOITER modes.
// //
//#define GCS_HEARTBEAT_FAILSAFE DISABLED //#define GCS_HEARTBEAT_FAILSAFE DISABLED
//#define SHORT_FAILSAFE_ACTION 0 //#define SHORT_FAILSAFE_ACTION 2
//#define LONG_FAILSAFE_ACTION 0 //#define LONG_FAILSAFE_ACTION 2
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -800,7 +774,7 @@
// //
// Limits the slew rate of the throttle, in percent per second. Helps // Limits the slew rate of the throttle, in percent per second. Helps
// avoid sudden throttle changes, which can destabilise the aircraft. // avoid sudden throttle changes, which can destabilise the aircraft.
// A setting of zero disables the feature. Range 1 to 100. // A setting of zero disables the feature.
// Default is zero (disabled). // Default is zero (disabled).
// //
// P_TO_T OPTIONAL // P_TO_T OPTIONAL

361
ArduPlane/ArduPilotMega.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduPilotMega V2.23" #define THISFIRMWARE "ArduPilotMega 2.2.0"
/* /*
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short
Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi
@ -29,7 +29,6 @@ version 2.1 of the License, or (at your option) any later version.
#include <APM_RC.h> // ArduPilot Mega RC Library #include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library #include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib #include <Wire.h> // Arduino I2C lib
#include <SPI.h> // Arduino SPI lib
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library #include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library #include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library #include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
@ -39,10 +38,10 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_DCM.h> // ArduPilot Mega DCM Library #include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID library #include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library //#include <AP_RangeFinder.h> // Range finder library
#include <ModeFilter.h>
#define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // MAVLink GCS definitions #include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <memcheck.h>
// Configuration // Configuration
#include "config.h" #include "config.h"
@ -71,12 +70,12 @@ FastSerialPort3(Serial3); // Telemetry port
// //
// Global parameters are all contained within the 'g' class. // Global parameters are all contained within the 'g' class.
// //
static Parameters g; Parameters g;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// prototypes // prototypes
static void update_events(void); void update_events(void);
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -93,17 +92,23 @@ static void update_events(void);
// //
// All GPS access should be through this pointer. // All GPS access should be through this pointer.
static GPS *g_gps; GPS *g_gps;
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
#if HIL_MODE == HIL_MODE_DISABLED #if HIL_MODE == HIL_MODE_DISABLED
// real sensors // real sensors
static AP_ADC_ADS7844 adc; AP_ADC_ADS7844 adc;
static APM_BMP085_Class barometer; APM_BMP085_Class barometer;
static AP_Compass_HMC5843 compass(Parameters::k_param_compass); // MAG PROTOCOL
#if MAG_PROTOCOL == MAG_PROTOCOL_5843
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
#elif MAG_PROTOCOL == MAG_PROTOCOL_5883L
AP_Compass_HMC5883L compass(Parameters::k_param_compass);
#else
#error Unrecognised MAG_PROTOCOL setting.
#endif // MAG PROTOCOL
// real GPS selection // real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
@ -139,7 +144,6 @@ AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL); AP_GPS_HIL g_gps_driver(NULL);
#elif HIL_MODE == HIL_MODE_ATTITUDE #elif HIL_MODE == HIL_MODE_ATTITUDE
AP_ADC_HIL adc;
AP_DCM_HIL dcm; AP_DCM_HIL dcm;
AP_GPS_HIL g_gps_driver(NULL); AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used AP_Compass_HIL compass; // never used
@ -185,31 +189,18 @@ AP_IMU_Shim imu; // never used
GCS_Class gcs; GCS_Class gcs;
#endif #endif
//////////////////////////////////////////////////////////////////////////////// //AP_RangeFinder_MaxsonarXL sonar;
// SONAR selection
////////////////////////////////////////////////////////////////////////////////
//
ModeFilter sonar_mode_filter;
#if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#elif SONAR_TYPE == MAX_SONAR_LV
// XXX honestly I think these output the same values
// If someone knows, can they confirm it?
AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Global variables // Global variables
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
byte control_mode = INITIALISING; byte control_mode = MANUAL;
byte oldSwitchPosition; // for remembering the control mode switch byte oldSwitchPosition; // for remembering the control mode switch
bool inverted_flight = false;
static const char *comma = ","; const char *comma = ",";
static const char* flight_mode_strings[] = { const char* flight_mode_strings[] = {
"Manual", "Manual",
"Circle", "Circle",
"Stabilize", "Stabilize",
@ -241,179 +232,214 @@ static const char* flight_mode_strings[] = {
// Failsafe // Failsafe
// -------- // --------
static int failsafe; // track which type of failsafe is being processed int failsafe; // track which type of failsafe is being processed
static bool ch3_failsafe; bool ch3_failsafe;
static byte crash_timer; byte crash_timer;
// Radio // Radio
// ----- // -----
static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm uint16_t elevon1_trim = 1500; // TODO: handle in EEProm
static uint16_t elevon2_trim = 1500; uint16_t elevon2_trim = 1500;
static uint16_t ch1_temp = 1500; // Used for elevon mixing uint16_t ch1_temp = 1500; // Used for elevon mixing
static uint16_t ch2_temp = 1500; uint16_t ch2_temp = 1500;
static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
static bool rc_override_active = false; bool rc_override_active = false;
static uint32_t rc_override_fs_timer = 0; uint32_t rc_override_fs_timer = 0;
static uint32_t ch3_failsafe_timer = 0; uint32_t ch3_failsafe_timer = 0;
bool reverse_roll;
bool reverse_pitch;
bool reverse_rudder;
byte mix_mode; // 0 = normal , 1 = elevons
// TODO: switch these reverses to true/false, after they are handled by RC_Channel
int reverse_elevons = 1;
int reverse_ch1_elevon = 1;
int reverse_ch2_elevon = 1;
// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon // for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon
// LED output // LED output
// ---------- // ----------
static bool GPS_light; // status of the GPS light bool GPS_light; // status of the GPS light
// GPS variables // GPS variables
// ------------- // -------------
static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
static float scaleLongUp = 1; // used to reverse longitude scaling float scaleLongUp = 1; // used to reverse longtitude scaling
static float scaleLongDown = 1; // used to reverse longitude scaling float scaleLongDown = 1; // used to reverse longtitude scaling
static byte ground_start_count = 5; // have we achieved first lock and set Home? byte ground_start_count = 5; // have we achieved first lock and set Home?
static int ground_start_avg; // 5 samples to avg speed for ground start int ground_start_avg; // 5 samples to avg speed for ground start
static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present bool ground_start; // have we started on the ground?
bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present
// Location & Navigation // Location & Navigation
// --------------------- // ---------------------
const float radius_of_earth = 6378100; // meters const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2 const float gravity = 9.81; // meters/ sec^2
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
static long hold_course = -1; // deg * 100 dir of plane long hold_course = -1; // deg * 100 dir of plane
static byte command_must_index; // current command memory location byte command_must_index; // current command memory location
static byte command_may_index; // current command memory location byte command_may_index; // current command memory location
static byte command_must_ID; // current command ID byte command_must_ID; // current command ID
static byte command_may_ID; // current command ID byte command_may_ID; // current command ID
// Airspeed // Airspeed
// -------- // --------
static int airspeed; // m/s * 100 int airspeed; // m/s * 100
static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
static float airspeed_error; // m/s * 100 float airspeed_error; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error long airspeed_energy_error; // kinetic portion of energy error
bool airspeed_enabled = false;
// Location Errors // Location Errors
// --------------- // ---------------
static long bearing_error; // deg * 100 : 0 to 36000 long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude long altitude_error; // meters * 100 we are off in altitude
static float crosstrack_error; // meters we are off trackline float crosstrack_error; // meters we are off trackline
// Battery Sensors // Battery Sensors
// --------------- // ---------------
static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
static float current_amps; float current_amps;
static float current_total; float current_total;
// Airspeed Sensors // Airspeed Sensors
// ---------------- // ----------------
static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering
static int airspeed_pressure; // airspeed as a pressure value int airspeed_offset; // analog air pressure sensor while still
int airspeed_pressure; // airspeed as a pressure value
// Barometer Sensor variables // Barometer Sensor variables
// -------------------------- // --------------------------
static unsigned long abs_pressure; unsigned long abs_pressure;
// Altitude Sensor variables // Altitude Sensor variables
// ---------------------- // ----------------------
static int sonar_alt; //byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific // flight mode specific
// -------------------- // --------------------
static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes.
static bool land_complete; bool land_complete;
static long takeoff_altitude; long takeoff_altitude;
// static int landing_distance; // meters; int landing_distance; // meters;
static int landing_pitch; // pitch for landing set by commands int landing_pitch; // pitch for landing set by commands
static int takeoff_pitch; int takeoff_pitch;
// Loiter management // Loiter management
// ----------------- // -----------------
static long old_target_bearing; // deg * 100 long old_target_bearing; // deg * 100
static int loiter_total; // deg : how many times to loiter * 360 int loiter_total; // deg : how many times to loiter * 360
static int loiter_delta; // deg : how far we just turned int loiter_delta; // deg : how far we just turned
static int loiter_sum; // deg : how far we have turned around a waypoint int loiter_sum; // deg : how far we have turned around a waypoint
static long loiter_time; // millis : when we started LOITER mode long loiter_time; // millis : when we started LOITER mode
static int loiter_time_max; // millis : how long to stay in LOITER mode int loiter_time_max; // millis : how long to stay in LOITER mode
// these are the values for navigation control functions // these are the values for navigation control functions
// ---------------------------------------------------- // ----------------------------------------------------
static long nav_roll; // deg * 100 : target roll angle long nav_roll; // deg * 100 : target roll angle
static long nav_pitch; // deg * 100 : target pitch angle long nav_pitch; // deg * 100 : target pitch angle
static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
// Waypoints // Waypoints
// --------- // ---------
static long wp_distance; // meters - distance between plane and next waypoint long wp_distance; // meters - distance between plane and next waypoint
static long wp_totalDistance; // meters - distance between old and next waypoint long wp_totalDistance; // meters - distance between old and next waypoint
byte next_wp_index; // Current active command index
// repeating event control // repeating event control
// ----------------------- // -----------------------
static byte event_id; // what to do - see defines byte event_id; // what to do - see defines
static long event_timer; // when the event was asked for in ms long event_timer; // when the event was asked for in ms
static uint16_t event_delay; // how long to delay the next firing of event in millis uint16_t event_delay; // how long to delay the next firing of event in millis
static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
static int event_value; // per command value, such as PWM for servos int event_value; // per command value, such as PWM for servos
static int event_undo_value; // the value used to cycle events (alternate value to event_value) int event_undo_value; // the value used to cycle events (alternate value to event_value)
// delay command // delay command
// -------------- // --------------
static long condition_value; // used in condition commands (eg delay, change alt, etc.) long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start; long condition_start;
static int condition_rate; int condition_rate;
// 3D Location vectors // 3D Location vectors
// ------------------- // -------------------
static struct Location home; // home location struct Location home; // home location
static struct Location prev_WP; // last waypoint struct Location prev_WP; // last waypoint
static struct Location current_loc; // current location struct Location current_loc; // current location
static struct Location next_WP; // next waypoint struct Location next_WP; // next waypoint
static struct Location next_command; // command preloaded struct Location next_command; // command preloaded
static struct Location guided_WP; // guided mode waypoint long target_altitude; // used for altitude management between waypoints
static long target_altitude; // used for altitude management between waypoints long offset_altitude; // used for altitude management between waypoints
static long offset_altitude; // used for altitude management between waypoints bool home_is_set; // Flag for if we have g_gps lock and have set the home location
static bool home_is_set; // Flag for if we have g_gps lock and have set the home location
// IMU variables // IMU variables
// ------------- // -------------
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
// Performance monitoring // Performance monitoring
// ---------------------- // ----------------------
static long perf_mon_timer; // Metric based on accel gain deweighting long perf_mon_timer; // Metric based on accel gain deweighting
static int G_Dt_max = 0; // Max main loop cycle time in milliseconds int G_Dt_max; // Max main loop cycle time in milliseconds
static int gps_fix_count = 0; int gps_fix_count;
static int pmTest1 = 0; byte gcs_messages_sent;
// GCS
// ---
char GCS_buffer[53];
char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
// System Timers // System Timers
// -------------- // --------------
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
static int mainLoop_count; int mainLoop_count;
static unsigned long medium_loopTimer; // Time in miliseconds of medium loop unsigned long medium_loopTimer; // Time in miliseconds of medium loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops byte medium_loopCounter; // Counters for branching from main control loop to slower loops
static uint8_t delta_ms_medium_loop; uint8_t delta_ms_medium_loop;
static byte slow_loopCounter; byte slow_loopCounter;
static byte superslow_loopCounter; byte superslow_loopCounter;
static byte counter_one_herz; byte counter_one_herz;
static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static float load; // % MCU cycles used unsigned long elapsedTime; // for doing custom events
float load; // % MCU cycles used
//Camera tracking and stabilisation stuff
// --------------------------------------
byte camera_mode = 1; //0 is do nothing, 1 is stabilize, 2 is track target
byte gimbal_mode = 0; // 0 - pitch & roll, 1 - pitch and yaw (pan & tilt), 2 - pitch, roll and yaw (to be added)
struct Location camera_target; //point of iterest for the camera to track
Vector3<float> target_vector(0,0,1); //x, y, z to target before rotating to planes axis, values are in meters
float cam_pitch;
float cam_roll;
float cam_tilt;
float cam_pan;
struct Location GPS_mark; // GPS POI for position based triggering
int picture_time = 0; // waypoint trigger variable
int thr_pic = 0; // timer variable for throttle_pic
int camtrig = 83; // PK6 chosen as it not near anything so safer for soldering
pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82)
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -421,7 +447,6 @@ static float load; // % MCU cycles used
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
void setup() { void setup() {
memcheck_init();
init_ardupilot(); init_ardupilot();
} }
@ -466,7 +491,7 @@ void loop()
} }
// Main loop 50Hz // Main loop 50Hz
static void fast_loop() void fast_loop()
{ {
// This is the fast loop - we want it to execute at 50Hz if possible // This is the fast loop - we want it to execute at 50Hz if possible
// ----------------------------------------------------------------- // -----------------------------------------------------------------
@ -477,22 +502,15 @@ static void fast_loop()
// ---------- // ----------
read_radio(); read_radio();
// 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
// check for loss of control signal failsafe condition // check for loss of control signal failsafe condition
// ------------------------------------ // ------------------------------------
check_short_failsafe(); check_short_failsafe();
// Read Airspeed // Read Airspeed
// ------------- // -------------
if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { if (airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) {
read_airspeed(); read_airspeed();
} else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { } else if (airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
calc_airspeed_errors(); calc_airspeed_errors();
} }
@ -532,7 +550,7 @@ static void fast_loop()
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
set_servos(); set_servos_4();
// XXX is it appropriate to be doing the comms below on the fast loop? // XXX is it appropriate to be doing the comms below on the fast loop?
@ -561,8 +579,9 @@ static void fast_loop()
// or be a "GCS fast loop" interface // or be a "GCS fast loop" interface
} }
static void medium_loop() void medium_loop()
{ {
camera();
// This is the start of the medium (10 Hz) loop pieces // This is the start of the medium (10 Hz) loop pieces
// ----------------------------------------- // -----------------------------------------
switch(medium_loopCounter) { switch(medium_loopCounter) {
@ -595,6 +614,7 @@ Serial.println(tempaccel.z, DEC);
// This case performs some navigation computations // This case performs some navigation computations
//------------------------------------------------ //------------------------------------------------
case 1: case 1:
medium_loopCounter++; medium_loopCounter++;
@ -618,7 +638,6 @@ Serial.println(tempaccel.z, DEC);
// Read altitude from sensors // Read altitude from sensors
// ------------------ // ------------------
update_alt(); update_alt();
if(g.sonar_enabled) sonar_alt = sonar.read();
// altitude smoothing // altitude smoothing
// ------------------ // ------------------
@ -679,7 +698,7 @@ Serial.println(tempaccel.z, DEC);
} }
} }
static void slow_loop() void slow_loop()
{ {
// This is the slow (3 1/3 Hz) loop pieces // This is the slow (3 1/3 Hz) loop pieces
//---------------------------------------- //----------------------------------------
@ -708,7 +727,9 @@ static void slow_loop()
// Read Control Surfaces/Mix switches // Read Control Surfaces/Mix switches
// ---------------------------------- // ----------------------------------
update_servo_switches(); if (g.switch_enable) {
update_servo_switches();
}
break; break;
@ -716,18 +737,19 @@ static void slow_loop()
slow_loopCounter = 0; slow_loopCounter = 0;
update_events(); update_events();
// XXX this should be a "GCS slow loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
gcs.data_stream_send(3,5); gcs.data_stream_send(1,5);
// send all requested output streams with rates requested // send all requested output streams with rates requested
// between 3 and 5 Hz // between 1 and 5 Hz
#else #else
gcs.send_message(MSG_LOCATION); gcs.send_message(MSG_LOCATION);
gcs.send_message(MSG_CPU_LOAD, load*100); gcs.send_message(MSG_CPU_LOAD, load*100);
#endif #endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(3,5); hil.data_stream_send(1,5);
#endif #endif
@ -735,26 +757,20 @@ static void slow_loop()
} }
} }
static void one_second_loop() void one_second_loop()
{ {
if (g.log_bitmask & MASK_LOG_CUR) if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current(); Log_Write_Current();
// send a heartbeat // send a heartbeat
gcs.send_message(MSG_HEARTBEAT); gcs.send_message(MSG_HEARTBEAT);
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,3);
// send all requested output streams with rates requested
// between 1 and 3 Hz
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT); hil.send_message(MSG_HEARTBEAT);
hil.data_stream_send(1,3);
#endif #endif
} }
static void update_GPS(void) void update_GPS(void)
{ {
g_gps->update(); g_gps->update();
update_GPS_light(); update_GPS_light();
@ -806,7 +822,7 @@ static void update_GPS(void)
} }
} }
static void update_current_flight_mode(void) void update_current_flight_mode(void)
{ {
if(control_mode == AUTO){ if(control_mode == AUTO){
crash_checker(); crash_checker();
@ -819,7 +835,7 @@ static void update_current_flight_mode(void)
nav_roll = 0; nav_roll = 0;
} }
if (g.airspeed_enabled == true) if (airspeed_enabled == true)
{ {
calc_nav_pitch(); calc_nav_pitch();
if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch;
@ -837,7 +853,7 @@ static void update_current_flight_mode(void)
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
calc_nav_roll(); calc_nav_roll();
if (g.airspeed_enabled == true){ if (airspeed_enabled == true){
calc_nav_pitch(); calc_nav_pitch();
calc_throttle(); calc_throttle();
}else{ }else{
@ -862,7 +878,6 @@ static void update_current_flight_mode(void)
switch(control_mode){ switch(control_mode){
case RTL: case RTL:
case LOITER: case LOITER:
case GUIDED:
hold_course = -1; hold_course = -1;
crash_checker(); crash_checker();
calc_nav_roll(); calc_nav_roll();
@ -876,7 +891,6 @@ static void update_current_flight_mode(void)
nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min; nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min;
// We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign.
nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority
if (inverted_flight) nav_pitch = -nav_pitch;
break; break;
case FLY_BY_WIRE_B: case FLY_BY_WIRE_B:
@ -886,7 +900,7 @@ static void update_current_flight_mode(void)
nav_roll = g.channel_roll.norm_input() * g.roll_limit; nav_roll = g.channel_roll.norm_input() * g.roll_limit;
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
if (g.airspeed_enabled == true) if (airspeed_enabled == true)
{ {
airspeed_error = ((int)(g.flybywire_airspeed_max - airspeed_error = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) * g.flybywire_airspeed_min) *
@ -935,7 +949,7 @@ static void update_current_flight_mode(void)
} }
} }
static void update_navigation() void update_navigation()
{ {
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------ // ------------------------------------------------------------------------
@ -947,18 +961,23 @@ static void update_navigation()
switch(control_mode){ switch(control_mode){
case LOITER: case LOITER:
case RTL:
case GUIDED:
update_loiter(); update_loiter();
calc_bearing_error(); calc_bearing_error();
break; break;
case RTL:
if(wp_distance <= ( g.loiter_radius + LOITER_RANGE) ) {
do_RTL();
}else{
update_crosstrack();
}
break;
} }
} }
} }
static void update_alt() void update_alt()
{ {
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude; current_loc.alt = g_gps->altitude;
@ -971,6 +990,6 @@ static void update_alt()
#endif #endif
// Calculate new climb rate // Calculate new climb rate
//if(medium_loopCounter == 0 && slow_loopCounter == 0) if(medium_loopCounter == 0 && slow_loopCounter == 0)
// add_altitude_data(millis() / 100, g_gps->altitude / 10); add_altitude_data(millis() / 100, g_gps->altitude / 10);
} }

135
ArduPlane/Attitude.pde
View File

@ -4,14 +4,15 @@
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. // Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//**************************************************************** //****************************************************************
static void stabilize() void stabilize()
{ {
static byte temp = 0;
float ch1_inf = 1.0; float ch1_inf = 1.0;
float ch2_inf = 1.0; float ch2_inf = 1.0;
float ch4_inf = 1.0; float ch4_inf = 1.0;
float speed_scaler; float speed_scaler;
if (g.airspeed_enabled == true){ if (airspeed_enabled == true){
if(airspeed > 0) if(airspeed > 0)
speed_scaler = (STANDARD_SPEED * 100) / airspeed; speed_scaler = (STANDARD_SPEED * 100) / airspeed;
else else
@ -19,7 +20,7 @@ static void stabilize()
speed_scaler = constrain(speed_scaler, 0.5, 2.0); speed_scaler = constrain(speed_scaler, 0.5, 2.0);
} else { } else {
if (g.channel_throttle.servo_out > 0){ if (g.channel_throttle.servo_out > 0){
speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root speed_scaler = 0.5 + (THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root
// Should maybe be to the 2/7 power, but we aren't goint to implement that... // Should maybe be to the 2/7 power, but we aren't goint to implement that...
}else{ }else{
speed_scaler = 1.67; speed_scaler = 1.67;
@ -31,16 +32,6 @@ static void stabilize()
nav_roll = 0; nav_roll = 0;
} }
if (inverted_flight) {
// we want to fly upside down. We need to cope with wrap of
// the roll_sensor interfering with wrap of nav_roll, which
// would really confuse the PID code. The easiest way to
// handle this is to ensure both go in the same direction from
// zero
nav_roll += 18000;
if (dcm.roll_sensor < 0) nav_roll -= 36000;
}
// For Testing Only // For Testing Only
// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10;
// Serial.printf_P(PSTR(" roll_sensor ")); // Serial.printf_P(PSTR(" roll_sensor "));
@ -53,10 +44,6 @@ static void stabilize()
fabs(dcm.roll_sensor * g.kff_pitch_compensation) + fabs(dcm.roll_sensor * g.kff_pitch_compensation) +
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(dcm.pitch_sensor - g.pitch_trim); (dcm.pitch_sensor - g.pitch_trim);
if (inverted_flight) {
// when flying upside down the elevator control is inverted
tempcalc = -tempcalc;
}
g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler);
// Mix Stick input to allow users to override control surfaces // Mix Stick input to allow users to override control surfaces
@ -112,7 +99,7 @@ static void stabilize()
//#endif //#endif
} }
static void crash_checker() void crash_checker()
{ {
if(dcm.pitch_sensor < -4500){ if(dcm.pitch_sensor < -4500){
crash_timer = 255; crash_timer = 255;
@ -122,9 +109,9 @@ static void crash_checker()
} }
static void calc_throttle() void calc_throttle()
{ {
if (g.airspeed_enabled == false) { if (airspeed_enabled == false) {
int throttle_target = g.throttle_cruise + throttle_nudge; int throttle_target = g.throttle_cruise + throttle_nudge;
// no airspeed sensor, we use nav pitch to determine the proper throttle output // no airspeed sensor, we use nav pitch to determine the proper throttle output
@ -158,7 +145,7 @@ static void calc_throttle()
// Yaw is separated into a function for future implementation of heading hold on rolling take-off // Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ---------------------------------------------------------------------------------------- // ----------------------------------------------------------------------------------------
static void calc_nav_yaw(float speed_scaler) void calc_nav_yaw(float speed_scaler)
{ {
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
Vector3f temp = imu.get_accel(); Vector3f temp = imu.get_accel();
@ -173,11 +160,11 @@ static void calc_nav_yaw(float speed_scaler)
} }
static void calc_nav_pitch() void calc_nav_pitch()
{ {
// Calculate the Pitch of the plane // Calculate the Pitch of the plane
// -------------------------------- // --------------------------------
if (g.airspeed_enabled == true) { if (airspeed_enabled == true) {
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
} else { } else {
nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav);
@ -188,7 +175,7 @@ static void calc_nav_pitch()
#define YAW_DAMPENER 0 #define YAW_DAMPENER 0
static void calc_nav_roll() void calc_nav_roll()
{ {
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
@ -232,22 +219,18 @@ float roll_slew_limit(float servo)
/***************************************** /*****************************************
* Throttle slew limit * Throttle slew limit
*****************************************/ *****************************************/
static void throttle_slew_limit() /*float throttle_slew_limit(float throttle)
{ {
static int last = 1000; static float last;
if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f);
last = throttle;
float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000 return temp;
Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last);
g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
last = g.channel_throttle.radio_out;
}
} }
*/
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations // Keeps outdated data out of our calculations
static void reset_I(void) void reset_I(void)
{ {
g.pidNavRoll.reset_I(); g.pidNavRoll.reset_I();
g.pidNavPitchAirspeed.reset_I(); g.pidNavPitchAirspeed.reset_I();
@ -259,13 +242,11 @@ static void reset_I(void)
/***************************************** /*****************************************
* Set the flight control servos based on the current calculated values * Set the flight control servos based on the current calculated values
*****************************************/ *****************************************/
static void set_servos(void) void set_servos_4(void)
{ {
int flapSpeedSource = 0;
if(control_mode == MANUAL){ if(control_mode == MANUAL){
// do a direct pass through of radio values // do a direct pass through of radio values
if (g.mix_mode == 0){ if (mix_mode == 0){
g.channel_roll.radio_out = g.channel_roll.radio_in; g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in; g.channel_pitch.radio_out = g.channel_pitch.radio_in;
} else { } else {
@ -274,31 +255,21 @@ static void set_servos(void)
} }
g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in; g.channel_rudder.radio_out = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in;
} else { } else {
if (g.mix_mode == 0) { if (mix_mode == 0){
g.channel_roll.calc_pwm(); g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm(); g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm(); g.channel_rudder.calc_pwm();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
g.rc_5.servo_out = g.channel_roll.servo_out;
g.rc_5.calc_pwm();
}
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
g.rc_6.servo_out = g.channel_roll.servo_out;
g.rc_6.calc_pwm();
}
}else{ }else{
/*Elevon mode*/ /*Elevon mode*/
float ch1; float ch1;
float ch2; float ch2;
ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out); ch1 = reverse_elevons * (g.channel_pitch.servo_out - g.channel_roll.servo_out);
ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out; ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out;
g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); g.channel_roll.radio_out = elevon1_trim + (reverse_ch1_elevon * (ch1 * 500.0/ ROLL_SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); g.channel_pitch.radio_out = elevon2_trim + (reverse_ch2_elevon * (ch2 * 500.0/ PITCH_SERVO_MAX));
} }
#if THROTTLE_OUT == 0 #if THROTTLE_OUT == 0
@ -310,61 +281,55 @@ static void set_servos(void)
g.channel_throttle.calc_pwm(); g.channel_throttle.calc_pwm();
//Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938
/* TO DO - fix this for RC_Channel library /* TO DO - fix this for RC_Channel library
#if THROTTLE_REVERSE == 1 #if THROTTLE_REVERSE == 1
radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE]; radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
#endif #endif
*/ */
throttle_slew_limit();
} }
if(control_mode <= FLY_BY_WIRE_B) {
if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in;
} else if (control_mode >= FLY_BY_WIRE_C) {
if (g.airspeed_enabled == true) {
flapSpeedSource = g.airspeed_cruise;
} else {
flapSpeedSource = g.throttle_cruise;
}
if ( flapSpeedSource > g.flap_1_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0;
} else if (flapSpeedSource > g.flap_2_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent;
} else {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent;
}
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output // send values to the PWM timers for output
// ---------------------------------------- // ----------------------------------------
APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos
APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos
#endif
} }
static void demo_servos(byte i) { void demo_servos(byte i) {
while(i > 0){ while(i > 0){
gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
APM_RC.OutputCh(1, 1400); APM_RC.OutputCh(1, 1400);
mavlink_delay(400); delay(400);
APM_RC.OutputCh(1, 1600); APM_RC.OutputCh(1, 1600);
mavlink_delay(200); delay(200);
APM_RC.OutputCh(1, 1500); APM_RC.OutputCh(1, 1500);
#endif delay(400);
mavlink_delay(400);
i--; i--;
} }
} }
int readOutputCh(unsigned char ch)
{
int pwm;
switch(ch)
{
case 0: pwm = OCR5B; break; // ch0
case 1: pwm = OCR5C; break; // ch1
case 2: pwm = OCR1B; break; // ch2
case 3: pwm = OCR1C; break; // ch3
case 4: pwm = OCR4C; break; // ch4
case 5: pwm = OCR4B; break; // ch5
case 6: pwm = OCR3C; break; // ch6
case 7: pwm = OCR3B; break; // ch7
case 8: pwm = OCR5A; break; // ch8, PL3
case 9: pwm = OCR1A; break; // ch9, PB5
case 10: pwm = OCR3A; break; // ch10, PE3
}
pwm >>= 1; // pwm / 2;
return pwm;
}

210
ArduPlane/Cam_move.pde Normal file
View File

@ -0,0 +1,210 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CAMERA == ENABLED
void init_camera()
{
g.rc_camera_pitch.set_angle(4500); // throw of servo?
g.rc_camera_pitch.radio_min = 1000; // lowest radio input
g.rc_camera_pitch.radio_trim = 1500; // middle radio input
g.rc_camera_pitch.radio_max = 2000; // highest radio input
g.rc_camera_roll.set_angle(4500);
g.rc_camera_roll.radio_min = 1000;
g.rc_camera_roll.radio_trim = 1500;
g.rc_camera_roll.radio_max = 2000;
//use test target for now
camera_target = home;
}
void camera()
{
//decide what happens to camera depending on camera mode
switch(camera_mode)
{
case 0:
//do nothing, i.e lock camera in place
break;
case 1:
//stabilize
target_vector.x=0; //east to west gives +tive value (i.e. longitude)
target_vector.y=0; //south to north gives +tive value (i.e. latitude)
target_vector.z=100; //downwards is +tive
camera_move();
break;
case 2:
//track target
if(g_gps->fix)
{
target_vector=get_location_vector(&current_loc,&camera_target);
camera_move();
}
break;
}
}
void camera_move()
{
Matrix3f m = dcm.get_dcm_transposed();
Vector3<float> targ = m*target_vector; //to do: find out notion of x y convention
switch(gimbal_mode)
{
case 0: // pitch & roll
cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch
cam_roll = degrees(atan2(targ.y, targ.z)); //roll
// check_limits(pitch);
// check_limits(roll);
// camera_out();
break;
case 1: // pitch and yaw
cam_tilt = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1;
cam_pan = 9000 + atan2(-targ.y, targ.x) * 5729.57795;
if (cam_pan < 0) cam_pan += 36000;
// check_limits(pitch);
// check_limits(yaw);
// camera_out();
break;
/* case 2: // pitch, roll & yaw - not started
float cam_ritch = 100;
float cam_yoll = 100;
float cam_paw = 100;
break; */
}
}
/* void check_limits(axis,variable) // Use servo definitions to calculate for all servo throws - TO DO
{
// find limits of servo range in deg
track_pan_right = PAN_CENTER + (PAN_RANGE/2);
track_pan_left = track_pan_right + (360 - PAN_RANGE);
if (track_pan_left > 360){
track_pan_left = track_pan_left - 360;
}
// check track_bearing is "safe" - not outside pan servo limits
// if the bearing lies in the servo dead zone change bearing to closet edge
if (track_bearing < track_pan_left && track_bearing > track_pan_right){
track_oor_l = abs(track_bearing - track_pan_left);
track_oor_r = abs(track_bearing - track_pan_right);
if (track_oor_r > track_oor_l){
track_bearing = track_pan_right;
}
if (track_oor_l > track_oor_r){
track_bearing = track_pan_left;
}
}
// center bearing to cam_servo center
track_pan_deg = track_bearing - PAN_CENTER;
// make negative is left rotation
if (track_pan_deg > 180){
track_pan_deg = (180 - (track_pan_deg - 180)) * -1;
}
} */
void camera_out()
{
switch(gimbal_mode)
{
case 0: // pitch & roll
g.rc_camera_pitch.servo_out = cam_pitch;
g.rc_camera_pitch.calc_pwm();
g.rc_camera_roll.servo_out = cam_roll;
g.rc_camera_roll.calc_pwm();
break;
case 1: // pitch and yaw
g.rc_camera_pitch.servo_out = cam_tilt;
g.rc_camera_pitch.calc_pwm();
g.rc_camera_roll.servo_out = cam_pan; // borrowing roll servo output for pan/yaw
g.rc_camera_roll.calc_pwm();
break;
/*case 2: // pitch, roll & yaw
g.rc_camera_pitch.servo_out = cam_ritch;
g.rc_camera_pitch.calc_pwm();
g.rc_camera_roll.servo_out = cam_yoll;
g.rc_camera_roll.calc_pwm();
g.rc_camera_yaw.servo_out = cam_paw; // camera_yaw doesn't exist it should unless we use another channel
g.rc_camera_yaw.calc_pwm();
break; */
}
#if defined PITCH_SERVO
APM_RC.OutputCh(PITCH_SERVO, g.rc_camera_pitch.radio_out);
#endif
#if defined ROLL_SERVO
APM_RC.OutputCh(ROLL_SERVO, g.rc_camera_roll.radio_out);
#endif
/*#if defined YAW_SERVO
APM_RC.OutputCh(YAW_SERVO, g.rc_camera_yaw.radio_out);
#endif */
#if CAM_DEBUG == ENABLED
//for debugging purposes
Serial.println();
Serial.print("current_loc: lat: ");
Serial.print(current_loc.lat);
Serial.print(", lng: ");
Serial.print(current_loc.lng);
Serial.print(", alt: ");
Serial.print(current_loc.alt);
Serial.println();
Serial.print("target_loc: lat: ");
Serial.print(camera_target.lat);
Serial.print(", lng: ");
Serial.print(camera_target.lng);
Serial.print(", alt: ");
Serial.print(camera_target.alt);
Serial.print(", distance: ");
Serial.print(get_distance(&current_loc,&camera_target));
Serial.print(", bearing: ");
Serial.print(get_bearing(&current_loc,&camera_target));
Serial.println();
Serial.print("dcm_angles: roll: ");
Serial.print(degrees(dcm.roll));
Serial.print(", pitch: ");
Serial.print(degrees(dcm.pitch));
Serial.print(", yaw: ");
Serial.print(degrees(dcm.yaw));
Serial.println();
Serial.print("target_vector: x: ");
Serial.print(target_vector.x,2);
Serial.print(", y: ");
Serial.print(target_vector.y,2);
Serial.print(", z: ");
Serial.print(target_vector.z,2);
Serial.println();
Serial.print("rotated_target_vector: x: ");
Serial.print(targ.x,2);
Serial.print(", y: ");
Serial.print(targ.y,2);
Serial.print(", z: ");
Serial.print(targ.z,2);
Serial.println();
Serial.print("gimbal type 0: roll: ");
Serial.print(roll);
Serial.print(", pitch: ");
Serial.print(pitch);
Serial.println();
/* Serial.print("gimbal type 1: pitch: ");
Serial.print(pan);
Serial.print(", roll: ");
Serial.print(tilt);
Serial.println(); */
/* Serial.print("gimbal type 2: pitch: ");
Serial.print(ritch);
Serial.print(", roll: ");
Serial.print(yoll);
Serial.print(", yaw: ");
Serial.print(paw);
Serial.println(); */
#endif
}
#endif

41
ArduPlane/Cam_trigger.pde Normal file
View File

@ -0,0 +1,41 @@
void servo_pic() // Servo operated camera
{
APM_RC.OutputCh(CAM_SERVO,1500 + (333)); // Camera click, not enough - add more, wring way - put a minus before bracket number (-300)
delay(250); // delay
APM_RC.OutputCh(CAM_SERVO,1500); // Return servo to mid position
}
void relay_picture() // basic relay activation
{
relay_on();
delay(250); // 0.25 seconds delay
relay_off();
}
void throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle.
{
g.channel_throttle.radio_out = g.throttle_min;
if (thr_pic = 10){
servo_pic(); // triggering method
thr_pic = 0;
g.channel_throttle.radio_out = g.throttle_cruise;
}
thr_pic++;
}
void distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle.
{
g.channel_throttle.radio_out = g.throttle_min;
if (wp_distance < 3){
servo_pic(); // triggering method
g.channel_throttle.radio_out = g.throttle_cruise;
}
}
void NPN_trigger() // hacked the circuit to run a transistor? use this trigger to send output.
{
// To Do: Assign pin spare pin for output
digitalWrite(camtrig, HIGH);
delay(50);
digitalWrite(camtrig, LOW);
}

8
ArduPlane/GCS.h
View File

@ -6,7 +6,7 @@
#ifndef __GCS_H #ifndef __GCS_H
#define __GCS_H #define __GCS_H
#include <FastSerial.h> #include <BetterStream.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#include <GPS.h> #include <GPS.h>
@ -40,7 +40,7 @@ public:
/// ///
/// @param port The stream over which messages are exchanged. /// @param port The stream over which messages are exchanged.
/// ///
void init(FastSerial *port) { _port = port; } void init(BetterStream *port) { _port = port; }
/// Update GCS state. /// Update GCS state.
/// ///
@ -119,7 +119,7 @@ public:
protected: protected:
/// The stream we are communicating over /// The stream we are communicating over
FastSerial *_port; BetterStream *_port;
}; };
// //
@ -139,7 +139,7 @@ class GCS_MAVLINK : public GCS_Class
public: public:
GCS_MAVLINK(AP_Var::Key key); GCS_MAVLINK(AP_Var::Key key);
void update(void); void update(void);
void init(FastSerial *port); void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0); void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str); void send_text(uint8_t severity, const prog_char_t *str);

452
ArduPlane/GCS_Mavlink.pde
View File

@ -4,10 +4,6 @@
#include "Mavlink_Common.h" #include "Mavlink_Common.h"
// use this to prevent recursion during sensor init
static bool in_mavlink_delay;
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0), packet_drops(0),
@ -18,21 +14,21 @@ waypoint_receive_timeout(1000), // 1 second
// stream rates // stream rates
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
// AP_VAR //ref //index, default, name streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")), streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
{ {
} }
void void
GCS_MAVLINK::init(FastSerial * port) GCS_MAVLINK::init(BetterStream * port)
{ {
GCS_Class::init(port); GCS_Class::init(port);
if (port == &Serial) { // to split hil vs gcs if (port == &Serial) { // to split hil vs gcs
@ -57,6 +53,8 @@ GCS_MAVLINK::update(void)
{ {
uint8_t c = comm_receive_ch(chan); uint8_t c = comm_receive_ch(chan);
// Try to get a new message // Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
} }
@ -64,6 +62,11 @@ GCS_MAVLINK::update(void)
// Update packet drops counter // Update packet drops counter
packet_drops += status.packet_rx_drop_count; packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints // send out queued params/ waypoints
_queued_send(); _queued_send();
@ -84,55 +87,41 @@ void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
{ {
if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax))
send_message(MSG_RAW_IMU);
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) {
send_message(MSG_RAW_IMU1); send_message(MSG_EXTENDED_STATUS);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_GPS_STATUS); send_message(MSG_GPS_STATUS);
send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT); send_message(MSG_NAV_CONTROLLER_OUTPUT);
//Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get());
} }
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) {
// sent with GPS read
send_message(MSG_LOCATION); send_message(MSG_LOCATION);
//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
} }
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers // This is used for HIL. Do not change without discussing with HIL maintainers
send_message(MSG_SERVO_OUT); send_message(MSG_SERVO_OUT);
//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
} }
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) {
send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN); send_message(MSG_RADIO_IN);
//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
} }
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE); send_message(MSG_ATTITUDE);
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
} }
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD); send_message(MSG_VFR_HUD);
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
} }
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){
// Available datastream // Available datastream
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
} }
} }
} }
@ -140,7 +129,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
void void
GCS_MAVLINK::send_message(uint8_t id, uint32_t param) GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
{ {
mavlink_send_message(chan,id, packet_drops); mavlink_send_message(chan,id,param,packet_drops);
} }
void void
@ -169,7 +158,7 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{ {
struct Location tell_command = {}; // command for telemetry struct Location tell_command; // command for telemetry
static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...) static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...)
switch (msg->msgid) { switch (msg->msgid) {
@ -179,18 +168,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode // decode
mavlink_request_data_stream_t packet; mavlink_request_data_stream_t packet;
mavlink_msg_request_data_stream_decode(msg, &packet); mavlink_msg_request_data_stream_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
int freq = 0; // packet frequency int freq = 0; // packet frequency
if (packet.start_stop == 0) if (packet.start_stop == 0) freq = 0; // stop sending
freq = 0; // stop sending else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending
else if (packet.start_stop == 1) else break;
freq = packet.req_message_rate; // start sending
else
break;
switch(packet.req_stream_id){ switch(packet.req_stream_id){
@ -204,7 +188,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
streamRateExtra2 = freq; streamRateExtra2 = freq;
streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group.
break; break;
case MAV_DATA_STREAM_RAW_SENSORS: case MAV_DATA_STREAM_RAW_SENSORS:
streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly
// we will not continue to broadcast raw sensor data at 50Hz. // we will not continue to broadcast raw sensor data at 50Hz.
@ -212,35 +195,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_DATA_STREAM_EXTENDED_STATUS: case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRateExtendedStatus.set_and_save(freq); streamRateExtendedStatus.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_RC_CHANNELS: case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels.set_and_save(freq); streamRateRCChannels.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_RAW_CONTROLLER: case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController.set_and_save(freq); streamRateRawController.set_and_save(freq);
break; break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION: // streamRateRawSensorFusion.set_and_save(freq);
// streamRateRawSensorFusion.set_and_save(freq); // break;
// break;
case MAV_DATA_STREAM_POSITION: case MAV_DATA_STREAM_POSITION:
streamRatePosition.set_and_save(freq); streamRatePosition.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA1: case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1.set_and_save(freq); streamRateExtra1.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA2: case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2.set_and_save(freq); streamRateExtra2.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA3: case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3.set_and_save(freq); streamRateExtra3.set_and_save(freq);
break; break;
default: default:
break; break;
} }
@ -379,8 +354,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
set_mode(MANUAL); set_mode(MANUAL);
break; break;
case MAV_MODE_GUIDED: case MAV_MODE_GUIDED: //For future use
set_mode(GUIDED);
break; break;
case MAV_MODE_AUTO: case MAV_MODE_AUTO:
@ -393,14 +367,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_MODE_TEST1: case MAV_MODE_TEST1:
set_mode(STABILIZE); set_mode(STABILIZE);
break; break;
case MAV_MODE_TEST2: case MAV_MODE_TEST2:
if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A); set_mode(FLY_BY_WIRE_A);
if(mav_nav == 2) set_mode(FLY_BY_WIRE_B); break;
//if(mav_nav == 3) set_mode(FLY_BY_WIRE_C); case MAV_MODE_TEST3:
mav_nav = 255; set_mode(FLY_BY_WIRE_B);
break; break;
} }
} }
@ -421,8 +393,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode // decode
mavlink_waypoint_request_list_t packet; mavlink_waypoint_request_list_t packet;
mavlink_msg_waypoint_request_list_decode(msg, &packet); mavlink_msg_waypoint_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) if (mavlink_check_target(packet.target_system,packet.target_component)) break;
break;
// Start sending waypoints // Start sending waypoints
mavlink_msg_waypoint_count_send( mavlink_msg_waypoint_count_send(
@ -439,7 +410,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
} }
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_WAYPOINT_REQUEST: case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{ {
//send_text_P(SEVERITY_LOW,PSTR("waypoint request")); //send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
@ -451,17 +421,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode // decode
mavlink_waypoint_request_t packet; mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &packet); mavlink_msg_waypoint_request_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
// send waypoint // send waypoint
tell_command = get_wp_with_index(packet.seq); tell_command = get_wp_with_index(packet.seq);
// set frame of waypoint // set frame of waypoint
uint8_t frame; uint8_t frame;
if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
} else { } else {
frame = MAV_FRAME_GLOBAL; // reference frame frame = MAV_FRAME_GLOBAL; // reference frame
@ -471,15 +438,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// time that the mav should loiter in milliseconds // time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false) uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == g.waypoint_index) current = 1;
if (packet.seq == (uint16_t)g.waypoint_index)
current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false) uint8_t autocontinue = 1; // 1 (true), 0 (false)
float x = 0, y = 0, z = 0; float x = 0, y = 0, z = 0;
if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { if (tell_command.id < MAV_CMD_NAV_LAST) {
// command needs scaling // command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (latitude) x = tell_command.lat/1.0e7; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude) y = tell_command.lng/1.0e7; // local (y), global (longitude)
@ -490,62 +453,49 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
} }
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS: switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_DO_SET_HOME: case MAV_CMD_NAV_TAKEOFF:
param1 = tell_command.p1; case MAV_CMD_CONDITION_CHANGE_ALT:
break; case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1*10; // APM loiter time is in ten second increments param1 = tell_command.p1*10; // APM loiter time is in ten second increments
break; break;
case MAV_CMD_CONDITION_CHANGE_ALT: case MAV_CMD_CONDITION_DELAY:
x=0; // Clear fields loaded above that we don't want sent for this command case MAV_CMD_CONDITION_DISTANCE:
y=0; param1 = tell_command.lat;
case MAV_CMD_CONDITION_DELAY: break;
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_DO_JUMP: case MAV_CMD_DO_JUMP:
param2 = tell_command.lat; param2 = tell_command.lat;
param1 = tell_command.p1; param1 = tell_command.p1;
break; break;
case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command.lng; param4 = tell_command.lng;
case MAV_CMD_DO_REPEAT_RELAY: case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command.lat; param3 = tell_command.lat;
param2 = tell_command.alt; param2 = tell_command.alt;
param1 = tell_command.p1; param1 = tell_command.p1;
break; break;
case MAV_CMD_DO_SET_PARAMETER: case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_SET_SERVO:
param2 = tell_command.alt; param2 = tell_command.alt;
param1 = tell_command.p1; param1 = tell_command.p1;
break; break;
} }
mavlink_msg_waypoint_send(chan,msg->sysid, mavlink_msg_waypoint_send(chan,msg->sysid,
msg->compid, msg->compid,packet.seq,frame,tell_command.id,current,autocontinue,
packet.seq, param1,param2,param3,param4,x,y,z);
frame,
tell_command.id,
current,
autocontinue,
param1,
param2,
param3,
param4,
x,
y,
z);
// update last waypoint comm stamp // update last waypoint comm stamp
waypoint_timelast_send = millis(); waypoint_timelast_send = millis();
@ -561,6 +511,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_waypoint_ack_decode(msg, &packet); mavlink_msg_waypoint_ack_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break; if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// check for error
uint8_t type = packet.type; // ok (0), error(1)
// turn off waypoint send // turn off waypoint send
waypoint_sending = false; waypoint_sending = false;
break; break;
@ -591,15 +544,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode // decode
mavlink_waypoint_clear_all_t packet; mavlink_waypoint_clear_all_t packet;
mavlink_msg_waypoint_clear_all_decode(msg, &packet); mavlink_msg_waypoint_clear_all_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) break; if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// clear all waypoints // clear all waypoints
uint8_t type = 0; // ok (0), error(1) uint8_t type = 0; // ok (0), error(1)
g.waypoint_total.set_and_save(0); g.waypoint_total.set_and_save(0);
// send acknowledgement 3 times to makes sure it is received // send acknowledgement 3 times to makes sure it is received
for (int i=0;i<3;i++) for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type);
break; break;
} }
@ -634,7 +586,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.count = MAX_WAYPOINTS; packet.count = MAX_WAYPOINTS;
} }
g.waypoint_total.set_and_save(packet.count - 1); g.waypoint_total.set_and_save(packet.count - 1);
waypoint_timelast_receive = millis(); waypoint_timelast_receive = millis();
waypoint_receiving = true; waypoint_receiving = true;
waypoint_sending = false; waypoint_sending = false;
@ -642,58 +593,55 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; 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: case MAVLINK_MSG_ID_WAYPOINT:
{ {
// Check if receiving waypiont
if (!waypoint_receiving) break;
// decode // decode
mavlink_waypoint_t packet; mavlink_waypoint_t packet;
mavlink_msg_waypoint_decode(msg, &packet); mavlink_msg_waypoint_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break; if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) break;
// store waypoint
uint8_t loadAction = 0; // 0 insert in list, 1 exec now
// defaults // defaults
tell_command.id = packet.command; tell_command.id = packet.command;
switch (packet.frame) switch (packet.frame)
{ {
case MAV_FRAME_MISSION: case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL:
{ {
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 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.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.alt = packet.z*1.0e2; // in as m converted to cm
tell_command.options = 0; // absolute altitude tell_command.options = 0;
break; break;
} }
case MAV_FRAME_LOCAL: // local (relative to home position) case MAV_FRAME_LOCAL: // local (relative to home position)
{ {
tell_command.lat = 1.0e7*ToDeg(packet.x/ tell_command.lat = 1.0e7*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; (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.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2; tell_command.alt = packet.z*1.0e2;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; tell_command.options = 1;
break; break;
} }
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{ {
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 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.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2; tell_command.alt = packet.z*1.0e2;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! tell_command.options = 1;
break; break;
} }
} }
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_LOITER_TURNS:
@ -703,7 +651,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_CONDITION_CHANGE_ALT: case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.lat = packet.param1; tell_command.p1 = packet.param1 * 100;
break; break;
case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TIME:
@ -737,53 +685,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
} }
if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission set_wp_with_index(tell_command, packet.seq);
guided_WP = tell_command;
// add home alt if needed // update waypoint receiving state machine
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ waypoint_timelast_receive = millis();
guided_WP.alt += home.alt; waypoint_request_i++;
}
set_mode(GUIDED); if (waypoint_request_i > g.waypoint_total)
{
uint8_t type = 0; // ok (0), error(1)
// make any new wp uploaded instant (in case we are already in Guided mode) mavlink_msg_waypoint_ack_send(
set_guided_WP(); chan,
msg->sysid,
msg->compid,
type);
// verify we recevied the command send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
mavlink_msg_waypoint_ack_send( waypoint_receiving = false;
chan, // XXX ignores waypoint radius for individual waypoints, can
msg->sysid, // only set WP_RADIUS parameter
msg->compid, }
0);
} else {
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) break;
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) break;
set_wp_with_index(tell_command, packet.seq);
// update waypoint receiving state machine
waypoint_timelast_receive = millis();
waypoint_request_i++;
if (waypoint_request_i > (uint16_t)g.waypoint_total){
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
type);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
}
}
break; break;
} }
@ -795,9 +717,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode // decode
mavlink_param_set_t packet; mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet); mavlink_msg_param_set_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
// set parameter // set parameter
@ -814,7 +734,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// add a small amount before casting parameter values // add a small amount before casting parameter values
// from float to integer to avoid truncating to the // from float to integer to avoid truncating to the
// next lower integer value. // next lower integer value.
float rounding_addition = 0.01; const float rounding_addition = 0.01;
// fetch the variable type ID // fetch the variable type ID
var_type = vp->meta_type_id(); var_type = vp->meta_type_id();
@ -827,13 +747,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
((AP_Float16 *)vp)->set_and_save(packet.param_value); ((AP_Float16 *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_int32) { } else if (var_type == AP_Var::k_typeid_int32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_Var::k_typeid_int16) { } else if (var_type == AP_Var::k_typeid_int16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else if (var_type == AP_Var::k_typeid_int8) { } else if (var_type == AP_Var::k_typeid_int8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
} else { } else {
// we don't support mavlink set on this parameter // we don't support mavlink set on this parameter
@ -864,9 +783,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_rc_channels_override_t packet; mavlink_rc_channels_override_t packet;
int16_t v[8]; int16_t v[8];
mavlink_msg_rc_channels_override_decode(msg, &packet); mavlink_msg_rc_channels_override_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system,packet.target_component))
break;
v[0] = packet.chan1_raw; v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw; v[1] = packet.chan2_raw;
@ -886,14 +803,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if(msg->sysid != g.sysid_my_gcs) break; if(msg->sysid != g.sysid_my_gcs) break;
rc_override_fs_timer = millis(); rc_override_fs_timer = millis();
pmTest1++;
break; break;
} }
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
// This is used both as a sensor and to pass the location // This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode. // in HIL_ATTITUDE mode.
case MAVLINK_MSG_ID_GPS_RAW: case MAVLINK_MSG_ID_GPS_RAW:
{ {
// decode // decode
mavlink_gps_raw_t packet; mavlink_gps_raw_t packet;
@ -1035,7 +951,7 @@ void
GCS_MAVLINK::_queued_send() GCS_MAVLINK::_queued_send()
{ {
// Check to see if we are sending parameters // Check to see if we are sending parameters
if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) { if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) {
AP_Var *vp; AP_Var *vp;
float value; float value;
@ -1068,10 +984,9 @@ GCS_MAVLINK::_queued_send()
// request waypoints one by one // request waypoints one by one
// XXX note that this is pan-interface // XXX note that this is pan-interface
if (waypoint_receiving && if (waypoint_receiving &&
(requested_interface == (unsigned)chan) && (requested_interface == chan) &&
waypoint_request_i <= (unsigned)g.waypoint_total && waypoint_request_i <= g.waypoint_total &&
mavdelay > 15) { // limits to 3.33 hz mavdelay > 15) { // limits to 3.33 hz
mavlink_msg_waypoint_request_send( mavlink_msg_waypoint_request_send(
chan, chan,
waypoint_dest_sysid, waypoint_dest_sysid,
@ -1082,66 +997,5 @@ GCS_MAVLINK::_queued_send()
} }
} }
#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 #endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(low,high);
#endif
}
/*
a delay() callback that processes MAVLink packets. We set this as the
callback in long running library initialisation routines to allow
MAVLink to process packets while waiting for the initialisation to
complete
*/
static void mavlink_delay(unsigned long t)
{
unsigned long tstart;
static unsigned long last_1hz, last_3hz, last_10hz, last_50hz;
if (in_mavlink_delay) {
// this should never happen, but let's not tempt fate by
// letting the stack grow too much
delay(t);
return;
}
in_mavlink_delay = true;
tstart = millis();
do {
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);
}
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);
}
delay(1);
} while (millis() - tstart < t);
in_mavlink_delay = false;
}

8
ArduPlane/HIL.h
View File

@ -8,7 +8,7 @@
# if HIL_MODE != HIL_MODE_DISABLED # if HIL_MODE != HIL_MODE_DISABLED
#include <FastSerial.h> #include <BetterStream.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <GPS.h> #include <GPS.h>
#include <Stream.h> #include <Stream.h>
@ -41,7 +41,7 @@ public:
/// ///
/// @param port The stream over which messages are exchanged. /// @param port The stream over which messages are exchanged.
/// ///
void init(FastSerial *port) { _port = port; } void init(BetterStream *port) { _port = port; }
/// Update HIL state. /// Update HIL state.
/// ///
@ -83,7 +83,7 @@ public:
protected: protected:
/// The stream we are communicating over /// The stream we are communicating over
FastSerial *_port; BetterStream *_port;
}; };
// //
@ -111,7 +111,7 @@ class HIL_XPLANE : public HIL_Class
public: public:
HIL_XPLANE(); HIL_XPLANE();
void update(void); void update(void);
void init(FastSerial *port); void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0); void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str); void send_text(uint8_t severity, const prog_char_t *str);

2
ArduPlane/HIL_Xplane.pde
View File

@ -55,7 +55,7 @@ HIL_XPLANE::HIL_XPLANE() :
} }
void void
HIL_XPLANE::init(FastSerial * port) HIL_XPLANE::init(BetterStream * port)
{ {
HIL_Class::init(port); HIL_Class::init(port);
hilPacketDecoder = new AP_GPS_IMU(port); hilPacketDecoder = new AP_GPS_IMU(port);

107
ArduPlane/Log.pde
View File

@ -1,7 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory // Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs // Code to interact with the user to dump or erase logs
@ -12,6 +10,7 @@
// These are function definitions so the Menu can be constructed before the functions // These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler. // are defined below. Order matters to the compiler.
static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv);
static int8_t dump_log(uint8_t argc, const Menu::arg *argv); static int8_t dump_log(uint8_t argc, const Menu::arg *argv);
static int8_t erase_logs(uint8_t argc, const Menu::arg *argv); static int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
static int8_t select_logs(uint8_t argc, const Menu::arg *argv); static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
@ -28,14 +27,13 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv)
" enable <name>|all enable logging <name> or everything\n" " enable <name>|all enable logging <name> or everything\n"
" disable <name>|all disable logging <name> or everything\n" " disable <name>|all disable logging <name> or everything\n"
"\n")); "\n"));
return 0;
} }
// Creates a constant array of structs representing menu options // Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM. // and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right. // User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details // See class Menu in AP_Coommon for implementation details
static const struct Menu::command log_menu_commands[] PROGMEM = { const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log}, {"dump", dump_log},
{"erase", erase_logs}, {"erase", erase_logs},
{"enable", select_logs}, {"enable", select_logs},
@ -46,8 +44,6 @@ static const struct Menu::command log_menu_commands[] PROGMEM = {
// A Macro to create the Menu // A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, print_log_menu); MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
static void get_log_boundaries(byte log_num, int & start_page, int & end_page);
static bool static bool
print_log_menu(void) print_log_menu(void)
{ {
@ -84,7 +80,7 @@ print_log_menu(void)
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
for(int i=1;i<last_log_num+1;i++) { for(int i=1;i<last_log_num+1;i++) {
get_log_boundaries(i, log_start, log_end); get_log_boundaries(last_log_num, i, log_start, log_end);
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"), Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"),
i, log_start, log_end); i, log_start, log_end);
} }
@ -109,7 +105,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
return(-1); return(-1);
} }
get_log_boundaries(dump_log, dump_log_start, dump_log_end); get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
dump_log, dump_log,
dump_log_start, dump_log_start,
@ -117,7 +113,6 @@ dump_log(uint8_t argc, const Menu::arg *argv)
Log_Read(dump_log_start, dump_log_end); Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Log read complete\n")); Serial.printf_P(PSTR("Log read complete\n"));
return 0;
} }
static int8_t static int8_t
@ -138,7 +133,6 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite(); DataFlash.FinishWrite();
Serial.printf_P(PSTR("\nLog erased.\n")); Serial.printf_P(PSTR("\nLog erased.\n"));
return 0;
} }
static int8_t static int8_t
@ -160,7 +154,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
// bits accordingly. // bits accordingly.
// //
if (!strcasecmp_P(argv[1].str, PSTR("all"))) { if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~0; bits = ~(bits = 0);
} else { } else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST); TARG(ATTITUDE_FAST);
@ -184,15 +178,14 @@ select_logs(uint8_t argc, const Menu::arg *argv)
return(0); return(0);
} }
static int8_t int8_t
process_logs(uint8_t argc, const Menu::arg *argv) process_logs(uint8_t argc, const Menu::arg *argv)
{ {
log_menu.run(); log_menu.run();
return 0;
} }
static byte get_num_logs(void) byte get_num_logs(void)
{ {
int page = 1; int page = 1;
byte data; byte data;
@ -231,14 +224,14 @@ static byte get_num_logs(void)
} }
// send the number of the last log? // send the number of the last log?
static void start_new_log(byte num_existing_logs) void start_new_log(byte num_existing_logs)
{ {
int start_pages[50] = {0,0,0}; int start_pages[50] = {0,0,0};
int end_pages[50] = {0,0,0}; int end_pages[50] = {0,0,0};
if(num_existing_logs > 0) { if(num_existing_logs > 0) {
for(int i=0;i<num_existing_logs;i++) { for(int i=0;i<num_existing_logs;i++) {
get_log_boundaries(i+1,start_pages[i],end_pages[i]); get_log_boundaries(num_existing_logs, i+1,start_pages[i],end_pages[i]);
} }
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]); end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
} }
@ -266,7 +259,7 @@ static void start_new_log(byte num_existing_logs)
} }
} }
static void get_log_boundaries(byte log_num, int & start_page, int & end_page) void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end_page)
{ {
int page = 1; int page = 1;
byte data; byte data;
@ -308,7 +301,7 @@ static void get_log_boundaries(byte log_num, int & start_page, int & end_page)
// Error condition if we reach here with page = 2 TO DO - report condition // Error condition if we reach here with page = 2 TO DO - report condition
} }
static int find_last_log_page(int bottom_page) int find_last_log_page(int bottom_page)
{ {
int top_page = 4096; int top_page = 4096;
int look_page; int look_page;
@ -318,7 +311,7 @@ static int find_last_log_page(int bottom_page)
look_page = (top_page + bottom_page) / 2; look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page); DataFlash.StartRead(look_page);
check = DataFlash.ReadLong(); check = DataFlash.ReadLong();
if(check == (long)0xFFFFFFFF) if(check == 0xFFFFFFFF)
top_page = look_page; top_page = look_page;
else else
bottom_page = look_page; bottom_page = look_page;
@ -328,7 +321,7 @@ static int find_last_log_page(int bottom_page)
// Write an attitude packet. Total length : 10 bytes // Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -340,7 +333,7 @@ static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
} }
// Write a performance monitoring packet. Total length : 19 bytes // Write a performance monitoring packet. Total length : 19 bytes
static void Log_Write_Performance() void Log_Write_Performance()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -354,13 +347,12 @@ static void Log_Write_Performance()
DataFlash.WriteByte(dcm.renorm_blowup_count); DataFlash.WriteByte(dcm.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count); DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(dcm.get_health() * 1000)); DataFlash.WriteInt((int)(dcm.get_health() * 1000));
DataFlash.WriteInt(pmTest1);
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
// Write a command processing packet. Total length : 19 bytes // Write a command processing packet. Total length : 19 bytes
//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng) //void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
static void Log_Write_Cmd(byte num, struct Location *wp) void Log_Write_Cmd(byte num, struct Location *wp)
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -374,7 +366,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp)
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
static void Log_Write_Startup(byte type) void Log_Write_Startup(byte type)
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -396,7 +388,7 @@ static void Log_Write_Startup(byte type)
// Write a control tuning packet. Total length : 22 bytes // Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning() void Log_Write_Control_Tuning()
{ {
Vector3f accel = imu.get_accel(); Vector3f accel = imu.get_accel();
@ -417,7 +409,7 @@ static void Log_Write_Control_Tuning()
#endif #endif
// Write a navigation tuning packet. Total length : 18 bytes // Write a navigation tuning packet. Total length : 18 bytes
static void Log_Write_Nav_Tuning() void Log_Write_Nav_Tuning()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -433,7 +425,7 @@ static void Log_Write_Nav_Tuning()
} }
// Write a mode packet. Total length : 5 bytes // Write a mode packet. Total length : 5 bytes
static void Log_Write_Mode(byte mode) void Log_Write_Mode(byte mode)
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -443,8 +435,8 @@ static void Log_Write_Mode(byte mode)
} }
// Write an GPS packet. Total length : 30 bytes // Write an GPS packet. Total length : 30 bytes
static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -454,7 +446,6 @@ static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude
DataFlash.WriteByte(log_NumSats); DataFlash.WriteByte(log_NumSats);
DataFlash.WriteLong(log_Lattitude); DataFlash.WriteLong(log_Lattitude);
DataFlash.WriteLong(log_Longitude); DataFlash.WriteLong(log_Longitude);
DataFlash.WriteInt(sonar_alt); // This one is just temporary for testing out sonar in fixed wing
DataFlash.WriteLong(log_mix_alt); DataFlash.WriteLong(log_mix_alt);
DataFlash.WriteLong(log_gps_alt); DataFlash.WriteLong(log_gps_alt);
DataFlash.WriteLong(log_Ground_Speed); DataFlash.WriteLong(log_Ground_Speed);
@ -464,7 +455,7 @@ static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude
// Write an raw accel/gyro data packet. Total length : 28 bytes // Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Raw() void Log_Write_Raw()
{ {
Vector3f gyro = imu.get_gyro(); Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel(); Vector3f accel = imu.get_accel();
@ -485,7 +476,7 @@ static void Log_Write_Raw()
} }
#endif #endif
static void Log_Write_Current() void Log_Write_Current()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -498,7 +489,7 @@ static void Log_Write_Current()
} }
// Read a Current packet // Read a Current packet
static void Log_Read_Current() void Log_Read_Current()
{ {
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"), Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(), DataFlash.ReadInt(),
@ -508,7 +499,7 @@ static void Log_Read_Current()
} }
// Read an control tuning packet // Read an control tuning packet
static void Log_Read_Control_Tuning() void Log_Read_Control_Tuning()
{ {
float logvar; float logvar;
@ -524,7 +515,7 @@ static void Log_Read_Control_Tuning()
} }
// Read a nav tuning packet // Read a nav tuning packet
static void Log_Read_Nav_Tuning() void Log_Read_Nav_Tuning()
{ {
Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
(float)((uint16_t)DataFlash.ReadInt())/100.0, (float)((uint16_t)DataFlash.ReadInt())/100.0,
@ -537,7 +528,7 @@ static void Log_Read_Nav_Tuning()
} }
// Read a performance packet // Read a performance packet
static void Log_Read_Performance() void Log_Read_Performance()
{ {
long pm_time; long pm_time;
int logvar; int logvar;
@ -546,7 +537,7 @@ static void Log_Read_Performance()
pm_time = DataFlash.ReadLong(); pm_time = DataFlash.ReadLong();
Serial.print(pm_time); Serial.print(pm_time);
Serial.print(comma); Serial.print(comma);
for (int y = 1; y <= 9; y++) { for (int y = 1; y < 9; y++) {
if(y < 3 || y > 7){ if(y < 3 || y > 7){
logvar = DataFlash.ReadInt(); logvar = DataFlash.ReadInt();
}else{ }else{
@ -559,7 +550,7 @@ static void Log_Read_Performance()
} }
// Read a command processing packet // Read a command processing packet
static void Log_Read_Cmd() void Log_Read_Cmd()
{ {
byte logvarb; byte logvarb;
long logvarl; long logvarl;
@ -578,7 +569,7 @@ static void Log_Read_Cmd()
Serial.println(" "); Serial.println(" ");
} }
static void Log_Read_Startup() void Log_Read_Startup()
{ {
byte logbyte = DataFlash.ReadByte(); byte logbyte = DataFlash.ReadByte();
@ -593,7 +584,7 @@ static void Log_Read_Startup()
} }
// Read an attitude packet // Read an attitude packet
static void Log_Read_Attitude() void Log_Read_Attitude()
{ {
Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
DataFlash.ReadInt(), DataFlash.ReadInt(),
@ -602,22 +593,21 @@ static void Log_Read_Attitude()
} }
// Read a mode packet // Read a mode packet
static void Log_Read_Mode() void Log_Read_Mode()
{ {
Serial.printf_P(PSTR("MOD:")); Serial.printf_P(PSTR("MOD:"));
Serial.println(flight_mode_strings[DataFlash.ReadByte()]); Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
} }
// Read a GPS packet // Read a GPS packet
static void Log_Read_GPS() void Log_Read_GPS()
{ {
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"), Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
DataFlash.ReadLong(), DataFlash.ReadLong(),
(int)DataFlash.ReadByte(), (int)DataFlash.ReadByte(),
(int)DataFlash.ReadByte(), (int)DataFlash.ReadByte(),
(float)DataFlash.ReadLong() / t7, (float)DataFlash.ReadLong() / t7,
(float)DataFlash.ReadLong() / t7, (float)DataFlash.ReadLong() / t7,
(float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing
(float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0,
@ -626,7 +616,7 @@ static void Log_Read_GPS()
} }
// Read a raw accel/gyro packet // Read a raw accel/gyro packet
static void Log_Read_Raw() void Log_Read_Raw()
{ {
float logvar; float logvar;
Serial.printf_P(PSTR("RAW:")); Serial.printf_P(PSTR("RAW:"));
@ -639,19 +629,13 @@ static void Log_Read_Raw()
} }
// Read the DataFlash log memory : Packet Parser // Read the DataFlash log memory : Packet Parser
static void Log_Read(int start_page, int end_page) void Log_Read(int start_page, int end_page)
{ {
byte data; byte data;
byte log_step = 0; byte log_step = 0;
int packet_count = 0; int packet_count = 0;
int page = start_page; int page = start_page;
#ifdef AIRFRAME_NAME
Serial.printf_P(PSTR((AIRFRAME_NAME)
#endif
Serial.printf_P(PSTR("\n" THISFIRMWARE
"\nFree RAM: %lu\n"),
freeRAM());
DataFlash.StartRead(start_page); DataFlash.StartRead(start_page);
while (page < end_page && page != -1){ while (page < end_page && page != -1){
@ -728,23 +712,4 @@ static void Log_Read(int start_page, int end_page)
Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count);
} }
#else // LOGGING_ENABLED
// dummy functions
static void Log_Write_Mode(byte mode) {}
static void Log_Write_Startup(byte type) {}
static void Log_Write_Cmd(byte num, struct Location *wp) {}
static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {}
static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {}
static void Log_Write_Performance() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
static byte get_num_logs(void) { return 0; }
static void start_new_log(byte num_existing_logs) {}
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {}
static void Log_Write_Control_Tuning() {}
static void Log_Write_Raw() {}
#endif // LOGGING_ENABLED

158
ArduPlane/Mavlink_Common.h
View File

@ -25,36 +25,21 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
} }
} }
// 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) void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
{ {
uint64_t timeStamp = micros(); uint64_t timeStamp = micros();
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
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) { switch(id) {
case MSG_HEARTBEAT: case MSG_HEARTBEAT:
{
CHECK_PAYLOAD_SIZE(HEARTBEAT);
mavlink_msg_heartbeat_send( mavlink_msg_heartbeat_send(
chan, chan,
mavlink_system.type, mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA); MAV_AUTOPILOT_ARDUPILOTMEGA);
break; break;
}
case MSG_EXTENDED_STATUS1: case MSG_EXTENDED_STATUS:
{ {
CHECK_PAYLOAD_SIZE(SYS_STATUS);
uint8_t mode = MAV_MODE_UNINIT; uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR; uint8_t nav_mode = MAV_NAV_VECTOR;
@ -67,14 +52,9 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
break; break;
case FLY_BY_WIRE_A: case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST2; mode = MAV_MODE_TEST2;
nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break; break;
case FLY_BY_WIRE_B: case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2; mode = MAV_MODE_TEST3;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break; break;
case AUTO: case AUTO:
mode = MAV_MODE_AUTO; mode = MAV_MODE_AUTO;
@ -88,10 +68,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
mode = MAV_MODE_AUTO; mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER; nav_mode = MAV_NAV_LOITER;
break; break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
} }
uint8_t status = MAV_STATE_ACTIVE; uint8_t status = MAV_STATE_ACTIVE;
@ -106,20 +82,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
battery_voltage * 1000, battery_voltage * 1000,
battery_remaining, battery_remaining,
packet_drops); packet_drops);
break;
}
case MSG_EXTENDED_STATUS2:
{
CHECK_PAYLOAD_SIZE(MEMINFO);
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
break; break;
} }
case MSG_ATTITUDE: case MSG_ATTITUDE:
{ {
CHECK_PAYLOAD_SIZE(ATTITUDE);
Vector3f omega = dcm.get_gyro(); Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send( mavlink_msg_attitude_send(
chan, chan,
@ -135,7 +102,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_LOCATION: case MSG_LOCATION:
{ {
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send( mavlink_msg_global_position_int_send(
chan, chan,
@ -151,7 +117,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_NAV_CONTROLLER_OUTPUT: case MSG_NAV_CONTROLLER_OUTPUT:
{ {
if (control_mode != MANUAL) { if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
mavlink_msg_nav_controller_output_send( mavlink_msg_nav_controller_output_send(
chan, chan,
nav_roll / 1.0e2, nav_roll / 1.0e2,
@ -168,7 +133,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_LOCAL_LOCATION: case MSG_LOCAL_LOCATION:
{ {
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send( mavlink_msg_local_position_send(
chan, chan,
@ -184,7 +148,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_GPS_RAW: case MSG_GPS_RAW:
{ {
CHECK_PAYLOAD_SIZE(GPS_RAW);
mavlink_msg_gps_raw_send( mavlink_msg_gps_raw_send(
chan, chan,
timeStamp, timeStamp,
@ -201,7 +164,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_SERVO_OUT: case MSG_SERVO_OUT:
{ {
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
uint8_t rssi = 1; uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000 // normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers // This is used for HIL. Do not change without discussing with HIL maintainers
@ -221,7 +183,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_RADIO_IN: case MSG_RADIO_IN:
{ {
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
uint8_t rssi = 1; uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send( mavlink_msg_rc_channels_raw_send(
chan, chan,
@ -239,7 +200,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_RADIO_OUT: case MSG_RADIO_OUT:
{ {
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send( mavlink_msg_servo_output_raw_send(
chan, chan,
g.channel_roll.radio_out, g.channel_roll.radio_out,
@ -255,7 +215,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_VFR_HUD: case MSG_VFR_HUD:
{ {
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send( mavlink_msg_vfr_hud_send(
chan, chan,
(float)airspeed / 100.0, (float)airspeed / 100.0,
@ -268,12 +227,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
} }
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1: case MSG_RAW_IMU:
{ {
CHECK_PAYLOAD_SIZE(RAW_IMU);
Vector3f accel = imu.get_accel(); Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro(); Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send( mavlink_msg_raw_imu_send(
@ -288,42 +245,21 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
compass.mag_x, compass.mag_x,
compass.mag_y, compass.mag_y,
compass.mag_z); compass.mag_z);
break;
}
case MSG_RAW_IMU2: /* This message is not working. Why?
{
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
mavlink_msg_scaled_pressure_send( mavlink_msg_scaled_pressure_send(
chan, chan,
timeStamp, timeStamp,
(float)barometer.Press/100.0, (float)barometer.Press/100.,
(float)(barometer.Press-g.ground_pressure)/100.0, (float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw
(int)(barometer.Temp*10)); (int)(barometer.Temp*100));
break; */
}
case MSG_RAW_IMU3:
{
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
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());
break; break;
} }
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS: case MSG_GPS_STATUS:
{ {
CHECK_PAYLOAD_SIZE(GPS_STATUS);
mavlink_msg_gps_status_send( mavlink_msg_gps_status_send(
chan, chan,
g_gps->num_sats, g_gps->num_sats,
@ -337,7 +273,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_CURRENT_WAYPOINT: case MSG_CURRENT_WAYPOINT:
{ {
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send( mavlink_msg_waypoint_current_send(
chan, chan,
g.waypoint_index); g.waypoint_index);
@ -347,77 +282,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
default: default:
break; 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) 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( mavlink_msg_statustext_send(
chan, chan,
severity, severity,

158
ArduPlane/Parameters.h
View File

@ -17,14 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly // The increment will prevent old parameters from being used incorrectly
// by newer code. // by newer code.
// //
static const uint16_t k_format_version = 11; static const uint16_t k_format_version = 6;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches.
//
static const uint16_t k_software_type = 0; // 0 for APM trunk
// //
// Parameter identities. // Parameter identities.
@ -50,7 +43,6 @@ public:
// Layout version number, always key zero. // Layout version number, always key zero.
// //
k_param_format_version = 0, k_param_format_version = 0,
k_param_software_type,
// Misc // Misc
@ -60,16 +52,6 @@ public:
k_param_switch_enable, k_param_switch_enable,
k_param_log_bitmask, k_param_log_bitmask,
k_param_pitch_trim, k_param_pitch_trim,
k_param_mix_mode,
k_param_reverse_elevons,
k_param_reverse_ch1_elevon,
k_param_reverse_ch2_elevon,
k_param_flap_1_percent,
k_param_flap_1_speed,
k_param_flap_2_percent,
k_param_flap_2_speed,
k_param_num_resets,
// 110: Telemetry control // 110: Telemetry control
// //
@ -77,7 +59,6 @@ public:
k_param_streamrates_port3, k_param_streamrates_port3,
k_param_sysid_this_mav, k_param_sysid_this_mav,
k_param_sysid_my_gcs, k_param_sysid_my_gcs,
k_param_serial3_baud,
// 120: Fly-by-wire control // 120: Fly-by-wire control
// //
@ -85,9 +66,9 @@ public:
k_param_flybywire_airspeed_max, k_param_flybywire_airspeed_max,
// //
// 130: Sensor parameters // 140: Sensor parameters
// //
k_param_IMU_calibration = 130, k_param_IMU_calibration = 140,
k_param_altitude_mix, k_param_altitude_mix,
k_param_airspeed_ratio, k_param_airspeed_ratio,
k_param_ground_temperature, k_param_ground_temperature,
@ -96,26 +77,22 @@ public:
k_param_compass, k_param_compass,
k_param_battery_monitoring, k_param_battery_monitoring,
k_param_pack_capacity, k_param_pack_capacity,
k_param_airspeed_offset,
k_param_sonar_enabled,
k_param_airspeed_enabled,
// //
// 150: Navigation parameters // 160: Navigation parameters
// //
k_param_crosstrack_gain = 150, k_param_crosstrack_gain = 160,
k_param_crosstrack_entry_angle, k_param_crosstrack_entry_angle,
k_param_roll_limit, k_param_roll_limit,
k_param_pitch_limit_max, k_param_pitch_limit_max,
k_param_pitch_limit_min, k_param_pitch_limit_min,
k_param_airspeed_cruise, k_param_airspeed_cruise,
k_param_RTL_altitude, k_param_RTL_altitude,
k_param_inverted_flight_ch,
// //
// 170: Radio settings // 180: Radio settings
// //
k_param_channel_roll = 170, k_param_channel_roll = 180,
k_param_channel_pitch, k_param_channel_pitch,
k_param_channel_throttle, k_param_channel_throttle,
k_param_channel_yaw, k_param_channel_yaw,
@ -123,22 +100,20 @@ public:
k_param_rc_6, k_param_rc_6,
k_param_rc_7, k_param_rc_7,
k_param_rc_8, k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_throttle_min, k_param_throttle_min,
k_param_throttle_max, k_param_throttle_max,
k_param_throttle_fs_enabled, k_param_throttle_fs_enabled,
k_param_throttle_fs_value, k_param_throttle_fs_value,
k_param_throttle_cruise, k_param_throttle_cruise,
k_param_flight_mode_channel,
k_param_flight_modes,
k_param_short_fs_action, k_param_short_fs_action,
k_param_long_fs_action, k_param_long_fs_action,
k_param_gcs_heartbeat_fs_enabled, k_param_gcs_heartbeat_fs_enabled,
k_param_throttle_slewrate,
k_param_rc_5_funct,
k_param_rc_6_funct,
k_param_rc_7_funct,
k_param_rc_8_funct,
// //
// 200: Feed-forward gains // 200: Feed-forward gains
@ -148,17 +123,6 @@ public:
k_param_kff_pitch_to_throttle, k_param_kff_pitch_to_throttle,
k_param_kff_throttle_to_pitch, k_param_kff_throttle_to_pitch,
//
// 210: flight modes
//
k_param_flight_mode_channel = 210,
k_param_flight_mode1,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
// //
// 220: Waypoint data // 220: Waypoint data
// //
@ -172,7 +136,7 @@ public:
// 240: PID Controllers // 240: PID Controllers
// //
// Heading-to-roll PID: // Heading-to-roll PID:
// heading error from command to roll command deviation from trim // heading error from commnd to roll command deviation from trim
// (bank to turn strategy) // (bank to turn strategy)
// //
k_param_heading_to_roll_PID = 240, k_param_heading_to_roll_PID = 240,
@ -193,7 +157,7 @@ public:
k_param_pitch_to_servo_PID, k_param_pitch_to_servo_PID,
// Airspeed-to-pitch PID: // Airspeed-to-pitch PID:
// airspeed error from command to pitch servo deviation from trim // airspeed error from commnd to pitch servo deviation from trim
// (back-side strategy) // (back-side strategy)
// //
k_param_airspeed_to_pitch_PID, k_param_airspeed_to_pitch_PID,
@ -202,7 +166,7 @@ public:
// Yaw control // Yaw control
// //
// Yaw-to-servo PID: // Yaw-to-servo PID:
// yaw rate error from command to yaw servo deviation from trim // yaw rate error from commnd to yaw servo deviation from trim
// (stabilizes dutch roll) // (stabilizes dutch roll)
// //
k_param_yaw_to_servo_PID, k_param_yaw_to_servo_PID,
@ -217,22 +181,20 @@ public:
k_param_energy_to_throttle_PID, k_param_energy_to_throttle_PID,
// Altitude-to-pitch PID: // Altitude-to-pitch PID:
// altitude error from command to pitch servo deviation from trim // altitude error from commnd to pitch servo deviation from trim
// (throttle front-side strategy alternative) // (throttle front-side strategy alternative)
// //
k_param_altitude_to_pitch_PID, k_param_altitude_to_pitch_PID,
// 254,255: reserved // 255: reserved
}; };
AP_Int16 format_version; AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control // Telemetry control
// //
AP_Int16 sysid_this_mav; AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs; AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
// Feed-forward gains // Feed-forward gains
// //
@ -250,7 +212,6 @@ public:
// //
AP_Float altitude_mix; AP_Float altitude_mix;
AP_Float airspeed_ratio; AP_Float airspeed_ratio;
AP_Int16 airspeed_offset;
// Waypoints // Waypoints
// //
@ -269,7 +230,6 @@ public:
// //
AP_Int8 throttle_min; AP_Int8 throttle_min;
AP_Int8 throttle_max; AP_Int8 throttle_max;
AP_Int8 throttle_slewrate;
AP_Int8 throttle_fs_enabled; AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value; AP_Int16 throttle_fs_value;
AP_Int8 throttle_cruise; AP_Int8 throttle_cruise;
@ -282,12 +242,7 @@ public:
// Flight modes // Flight modes
// //
AP_Int8 flight_mode_channel; AP_Int8 flight_mode_channel;
AP_Int8 flight_mode1; AP_VarA<uint8_t,6> flight_modes;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
// Navigational maneuvering limits // Navigational maneuvering limits
// //
@ -299,11 +254,6 @@ public:
// //
AP_Int8 auto_trim; AP_Int8 auto_trim;
AP_Int8 switch_enable; AP_Int8 switch_enable;
AP_Int8 mix_mode;
AP_Int8 reverse_elevons;
AP_Int8 reverse_ch1_elevon;
AP_Int8 reverse_ch2_elevon;
AP_Int16 num_resets;
AP_Int16 log_bitmask; AP_Int16 log_bitmask;
AP_Int16 airspeed_cruise; AP_Int16 airspeed_cruise;
AP_Int16 pitch_trim; AP_Int16 pitch_trim;
@ -314,13 +264,6 @@ public:
AP_Int16 angle_of_attack; AP_Int16 angle_of_attack;
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
AP_Int8 sonar_enabled;
AP_Int8 airspeed_enabled;
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;
AP_Int8 flap_2_percent;
AP_Int8 flap_2_speed;
// RC channels // RC channels
RC_Channel channel_roll; RC_Channel channel_roll;
@ -331,10 +274,8 @@ public:
RC_Channel rc_6; RC_Channel rc_6;
RC_Channel rc_7; RC_Channel rc_7;
RC_Channel rc_8; RC_Channel rc_8;
AP_Int8 rc_5_funct; RC_Channel rc_camera_pitch;
AP_Int8 rc_6_funct; RC_Channel rc_camera_roll;
AP_Int8 rc_7_funct;
AP_Int8 rc_8_funct;
// PID controllers // PID controllers
// //
@ -352,12 +293,10 @@ public:
Parameters() : Parameters() :
// variable default key name // variable default key name
//------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")), format_version (k_format_version, k_param_format_version, NULL),
software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")),
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")),
kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")), kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")),
kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")), kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")),
@ -369,7 +308,6 @@ public:
altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")), altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")),
airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")), airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")),
airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")),
/* XXX waypoint_mode missing here */ /* XXX waypoint_mode missing here */
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
@ -382,22 +320,16 @@ public:
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
throttle_slewrate (THROTTLE_SLEW_LIMIT, k_param_throttle_slewrate, PSTR("THR_SLEWRATE")),
throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")),
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")),
long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")),
gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")),
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")), flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")),
flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")), flight_modes (k_param_flight_modes, PSTR("FLTMODE")),
flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")),
flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")),
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")), roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")),
pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")), pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")),
@ -405,33 +337,15 @@ public:
auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")), auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")),
switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")), switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")),
mix_mode (ELEVON_MIXING, k_param_mix_mode, PSTR("ELEVON_MIXING")), log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
reverse_elevons (ELEVON_REVERSE, k_param_reverse_elevons, PSTR("ELEVON_REVERSE")),
reverse_ch1_elevon (ELEVON_CH1_REVERSE, k_param_reverse_ch1_elevon, PSTR("ELEVON_CH1_REV")),
reverse_ch2_elevon (ELEVON_CH2_REVERSE, k_param_reverse_ch2_elevon, PSTR("ELEVON_CH2_REV")),
num_resets (0, k_param_num_resets, PSTR("SYS_NUM_RESETS")),
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")),
pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")),
RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
flap_1_percent (FLAP_1_PERCENT, k_param_flap_1_percent, PSTR("FLAP_1_PERCNT")), battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")), pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
flap_2_percent (FLAP_2_PERCENT, k_param_flap_2_percent, PSTR("FLAP_2_PERCNT")),
flap_2_speed (FLAP_2_SPEED, k_param_flap_2_speed, PSTR("FLAP_2_SPEED")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")),
rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")),
rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")),
rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")),
rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")),
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility! // Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
@ -441,17 +355,19 @@ public:
channel_pitch (k_param_channel_pitch, PSTR("RC2_")), channel_pitch (k_param_channel_pitch, PSTR("RC2_")),
channel_throttle (k_param_channel_throttle, PSTR("RC3_")), channel_throttle (k_param_channel_throttle, PSTR("RC3_")),
channel_rudder (k_param_channel_yaw, PSTR("RC4_")), channel_rudder (k_param_channel_yaw, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")), rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")), rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")), rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")), rc_8 (k_param_rc_8, PSTR("RC8_")),
rc_camera_pitch (k_param_rc_9, NULL),
rc_camera_roll (k_param_rc_10, NULL),
// PID controller group key name initial P initial I initial D initial imax // PID controller group key name initial P initial I initial D initial imax
//--------------------------------------------------------------------------------------------------------------------------------------- //---------------------------------------------------------------------------------------------------------------------------------------
pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSP2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSPD2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),

49
ArduPlane/WP_activity.pde Normal file
View File

@ -0,0 +1,49 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
void waypoint_check()
{
if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want
if(wp_distance < 10){ // Get as close as it can for you
servo_pic();
} // DO SOMETHIMNG
}
if(g.waypoint_index == 20){ // When here do whats underneath
servo_pic();
}
}
void picture_time_check()
{
if (picture_time == 1){
if (wp_distance < 10){
servo_pic(); // or any camera activation command
}
}
}
void egg_waypoint()
{
float temp = (float)(current_loc.alt - home.alt) * .01;
float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01;
if(g.waypoint_index == 3){
if(wp_distance < egg_dist){
APM_RC.OutputCh(CH_RUDDER,1500 + (-45*10.31));
}
}else{
APM_RC.OutputCh(CH_RUDDER,1500 + (45*10.31));
}
}
void johann_check() // if you aren't Johann it doesn't really matter :D
{
APM_RC.OutputCh(CH_7,1500 + (350));
if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want
if(wp_distance < 10){ // Get as close as it can for you
servo_pic();
}
}
if(g.waypoint_index == 20){ // When here do whats underneath
servo_pic();
}
}

44
ArduPlane/climb_rate.pde
View File

@ -1,12 +1,10 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
struct DataPoint { struct DataPoint {
unsigned long x; unsigned long x;
long y; long y;
}; };
DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from
unsigned char hindex; // Index in history for the current data point unsigned char index; // Index in history for the current data point
unsigned long xoffset; unsigned long xoffset;
unsigned char n; unsigned char n;
@ -17,9 +15,12 @@ long yi;
long xiyi; long xiyi;
unsigned long xi2; unsigned long xi2;
#if 0 // currently unused
static void add_altitude_data(unsigned long xl, long y) void add_altitude_data(unsigned long xl, long y)
{ {
unsigned char i;
int dx;
//Reset the regression if our X variable overflowed //Reset the regression if our X variable overflowed
if (xl < xoffset) if (xl < xoffset)
n = 0; n = 0;
@ -29,10 +30,10 @@ static void add_altitude_data(unsigned long xl, long y)
n = 0; n = 0;
if (n == ALTITUDE_HISTORY_LENGTH) { if (n == ALTITUDE_HISTORY_LENGTH) {
xi -= history[hindex].x; xi -= history[index].x;
yi -= history[hindex].y; yi -= history[index].y;
xiyi -= (long)history[hindex].x * history[hindex].y; xiyi -= (long)history[index].x * history[index].y;
xi2 -= history[hindex].x * history[hindex].x; xi2 -= history[index].x * history[index].x;
} else { } else {
if (n == 0) { if (n == 0) {
xoffset = xl; xoffset = xl;
@ -44,33 +45,31 @@ static void add_altitude_data(unsigned long xl, long y)
n++; n++;
} }
history[hindex].x = xl - xoffset; history[index].x = xl - xoffset;
history[hindex].y = y; history[index].y = y;
xi += history[hindex].x; xi += history[index].x;
yi += history[hindex].y; yi += history[index].y;
xiyi += (long)history[hindex].x * history[hindex].y; xiyi += (long)history[index].x * history[index].y;
xi2 += history[hindex].x * history[hindex].x; xi2 += history[index].x * history[index].x;
if (++hindex >= ALTITUDE_HISTORY_LENGTH) if (++index >= ALTITUDE_HISTORY_LENGTH)
hindex = 0; index = 0;
} }
#endif
#if 0 // unused void recalc_climb_rate()
static void recalc_climb_rate()
{ {
float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2); float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2);
climb_rate = (int)(slope*100); climb_rate = (int)(slope*100);
} }
static void print_climb_debug_info() void print_climb_debug_info()
{ {
unsigned char i, j; unsigned char i, j;
recalc_climb_rate(); recalc_climb_rate();
//SendDebugln_P("Climb rate:"); //SendDebugln_P("Climb rate:");
for (i=0; i<ALTITUDE_HISTORY_LENGTH; i++) { for (i=0; i<ALTITUDE_HISTORY_LENGTH; i++) {
j = i + hindex; j = i + index;
if (j >= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH; if (j >= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH;
//SendDebug_P(" "); //SendDebug_P(" ");
//SendDebug(j,DEC); //SendDebug(j,DEC);
@ -91,4 +90,3 @@ static void print_climb_debug_info()
//SendDebug((float)climb_rate/100,2); //SendDebug((float)climb_rate/100,2);
//SendDebugln_P(" m/s"); //SendDebugln_P(" m/s");
} }
#endif

2
ArduPlane/command description.txt
View File

@ -45,7 +45,7 @@ May Commands - these commands are optional to finish
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) - 0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) -
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT - alt (finish) rate (cm/sec) - 0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - -
Note: rate must be > 10 cm/sec due to integer math Note: rate must be > 10 cm/sec due to integer math
MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM) MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)

99
ArduPlane/commands.pde
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void init_commands() void init_commands()
{ {
//read_EEPROM_waypoint_info(); //read_EEPROM_waypoint_info();
g.waypoint_index.set_and_save(0); g.waypoint_index.set_and_save(0);
@ -9,19 +9,15 @@ static void init_commands()
next_command.id = CMD_BLANK; next_command.id = CMD_BLANK;
} }
static void update_auto() void update_auto()
{ {
if (g.waypoint_index >= g.waypoint_total) { if (g.waypoint_index == g.waypoint_total) {
handle_no_commands(); do_RTL();
if(g.waypoint_total == 0) {
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
}
} }
} }
// this is only used by an air-start // this is only used by an air-start
static void reload_commands_airstart() void reload_commands_airstart()
{ {
init_commands(); init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
@ -30,7 +26,7 @@ static void reload_commands_airstart()
// Getters // Getters
// ------- // -------
static struct Location get_wp_with_index(int i) struct Location get_wp_with_index(int i)
{ {
struct Location temp; struct Location temp;
long mem; long mem;
@ -70,13 +66,12 @@ static struct Location get_wp_with_index(int i)
// Setters // Setters
// ------- // -------
static void set_wp_with_index(struct Location temp, int i) void set_wp_with_index(struct Location temp, int i)
{ {
i = constrain(i, 0, g.waypoint_total.get()); i = constrain(i, 0, g.waypoint_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE); uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask // Set altitude options bitmask
// XXX What is this trying to do?
if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){ if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){
temp.options = MASK_OPTIONS_RELATIVE_ALT; temp.options = MASK_OPTIONS_RELATIVE_ALT;
} else { } else {
@ -101,26 +96,26 @@ static void set_wp_with_index(struct Location temp, int i)
eeprom_write_dword((uint32_t *) mem, temp.lng); eeprom_write_dword((uint32_t *) mem, temp.lng);
} }
static void increment_WP_index() void increment_WP_index()
{ {
if (g.waypoint_index <= g.waypoint_total) { if (g.waypoint_index < g.waypoint_total) {
g.waypoint_index.set_and_save(g.waypoint_index + 1); g.waypoint_index.set_and_save(g.waypoint_index + 1);
SendDebug_P("MSG - WP index is incremented to "); SendDebug_P("MSG <increment_WP_index> WP index is incremented to ");
}else{ }else{
//SendDebug_P("MSG <increment_WP_index> Failed to increment WP index of "); //SendDebug_P("MSG <increment_WP_index> Failed to increment WP index of ");
// This message is used excessively at the end of a mission // This message is used excessively at the end of a mission
} }
//SendDebugln(g.waypoint_index,DEC); SendDebugln(g.waypoint_index,DEC);
} }
static void decrement_WP_index() void decrement_WP_index()
{ {
if (g.waypoint_index > 0) { if (g.waypoint_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1); g.waypoint_index.set_and_save(g.waypoint_index - 1);
} }
} }
static long read_alt_to_hold() long read_alt_to_hold()
{ {
if(g.RTL_altitude < 0) if(g.RTL_altitude < 0)
return current_loc.alt; return current_loc.alt;
@ -129,14 +124,30 @@ static long read_alt_to_hold()
} }
//********************************************************************************
//This function sets the waypoint and modes for Return to Launch
//********************************************************************************
Location get_LOITER_home_wp()
{
//so we know where we are navigating from
next_WP = current_loc;
// read home position
struct Location temp = get_wp_with_index(0);
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold();
return temp;
}
/* /*
This function stores waypoint commands This function stores waypoint commands
It looks to see what the next command type is and finds the last command. It looks to see what the next command type is and finds the last command.
*/ */
static void set_next_WP(struct Location *wp) void set_next_WP(struct Location *wp)
{ {
//gcs.send_text_P(SEVERITY_LOW,PSTR("load WP")); //gcs.send_text_P(SEVERITY_LOW,PSTR("load WP"));
SendDebug_P("MSG - wp_index: "); SendDebug_P("MSG <set_next_wp> wp_index: ");
SendDebugln(g.waypoint_index, DEC); SendDebugln(g.waypoint_index, DEC);
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -162,44 +173,6 @@ static void set_next_WP(struct Location *wp)
loiter_sum = 0; loiter_sum = 0;
loiter_total = 0; loiter_total = 0;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
gcs.print_current_waypoints();
}
static void set_guided_WP(void)
{
SendDebug_P("MSG - set_guided_wp");
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
// Load the next_WP slot
// ---------------------
next_WP = guided_WP;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
offset_altitude = next_WP.alt - prev_WP.alt;
// this is used to offset the shrinking longitude as we go towards the poles // this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925; float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads); scaleLongDown = cos(rads);
@ -217,13 +190,15 @@ static void set_guided_WP(void)
// set a new crosstrack bearing // set a new crosstrack bearing
// ---------------------------- // ----------------------------
reset_crosstrack(); reset_crosstrack();
gcs.print_current_waypoints();
} }
// run this at setup on the ground // run this at setup on the ground
// ------------------------------- // -------------------------------
void init_home() void init_home()
{ {
SendDebugln("MSG: init home"); SendDebugln("MSG: <init_home> init home");
// block until we get a good fix // block until we get a good fix
// ----------------------------- // -----------------------------
@ -246,12 +221,6 @@ void init_home()
// Save prev loc // Save prev loc
// ------------- // -------------
next_WP = prev_WP = home; next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
} }

104
ArduPlane/commands_logic.pde
View File

@ -3,7 +3,7 @@
/********************************************************************************/ /********************************************************************************/
// Command Event Handlers // Command Event Handlers
/********************************************************************************/ /********************************************************************************/
static void void
handle_process_must() handle_process_must()
{ {
// reset navigation integrators // reset navigation integrators
@ -45,7 +45,7 @@ handle_process_must()
} }
} }
static void void
handle_process_may() handle_process_may()
{ {
switch(next_command.id){ switch(next_command.id){
@ -82,7 +82,7 @@ handle_process_may()
} }
} }
static void handle_process_now() void handle_process_now()
{ {
switch(next_command.id){ switch(next_command.id){
@ -116,18 +116,27 @@ static void handle_process_now()
} }
} }
static void handle_no_commands() void handle_no_commands()
{ {
next_command = home; if (command_must_ID)
next_command.alt = read_alt_to_hold(); return;
next_command.id = MAV_CMD_NAV_LOITER_UNLIM;
switch (control_mode){
case LAND:
// don't get a new command
break;
default:
set_mode(RTL);
break;
}
} }
/********************************************************************************/ /********************************************************************************/
// Verify command Handlers // Verify command Handlers
/********************************************************************************/ /********************************************************************************/
static bool verify_must() bool verify_must()
{ {
switch(command_must_ID) { switch(command_must_ID) {
@ -166,7 +175,7 @@ static bool verify_must()
} }
} }
static bool verify_may() bool verify_may()
{ {
switch(command_may_ID) { switch(command_may_ID) {
case NO_COMMAND: case NO_COMMAND:
@ -186,18 +195,18 @@ static bool verify_may()
default: default:
gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd"));
return false;
break; break;
} }
return false;
} }
/********************************************************************************/ /********************************************************************************/
// Nav (Must) commands // Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
static void do_RTL(void) void do_RTL(void)
{ {
control_mode = RTL; control_mode = LOITER;
crash_timer = 0; crash_timer = 0;
next_WP = home; next_WP = home;
@ -213,7 +222,7 @@ static void do_RTL(void)
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);
} }
static void do_takeoff() void do_takeoff()
{ {
set_next_WP(&next_command); set_next_WP(&next_command);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
@ -227,28 +236,28 @@ static void do_takeoff()
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
} }
static void do_nav_wp() void do_nav_wp()
{ {
set_next_WP(&next_command); set_next_WP(&next_command);
} }
static void do_land() void do_land()
{ {
set_next_WP(&next_command); set_next_WP(&next_command);
} }
static void do_loiter_unlimited() void do_loiter_unlimited()
{ {
set_next_WP(&next_command); set_next_WP(&next_command);
} }
static void do_loiter_turns() void do_loiter_turns()
{ {
set_next_WP(&next_command); set_next_WP(&next_command);
loiter_total = next_command.p1 * 360; loiter_total = next_command.p1 * 360;
} }
static void do_loiter_time() void do_loiter_time()
{ {
set_next_WP(&next_command); set_next_WP(&next_command);
loiter_time = millis(); loiter_time = millis();
@ -258,7 +267,7 @@ static void do_loiter_time()
/********************************************************************************/ /********************************************************************************/
// Verify Nav (Must) commands // Verify Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
static bool verify_takeoff() bool verify_takeoff()
{ {
if (g_gps->ground_speed > 300){ if (g_gps->ground_speed > 300){
if(hold_course == -1){ if(hold_course == -1){
@ -287,7 +296,7 @@ static bool verify_takeoff()
} }
} }
static bool verify_land() bool verify_land()
{ {
// we don't verify landing - we never go to a new Must command after Land // we don't verify landing - we never go to a new Must command after Land
if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
@ -314,7 +323,7 @@ static bool verify_land()
return false; return false;
} }
static bool verify_nav_wp() bool verify_nav_wp()
{ {
hold_course = -1; hold_course = -1;
update_crosstrack(); update_crosstrack();
@ -335,25 +344,25 @@ static bool verify_nav_wp()
return false; return false;
} }
static bool verify_loiter_unlim() bool verify_loiter_unlim()
{ {
update_loiter(); update_loiter();
calc_bearing_error(); calc_bearing_error();
return false; return false;
} }
static bool verify_loiter_time() bool verify_loiter_time()
{ {
update_loiter(); update_loiter();
calc_bearing_error(); calc_bearing_error();
if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds if ((millis() - loiter_time) > (long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
return true; return true;
} }
return false; return false;
} }
static bool verify_loiter_turns() bool verify_loiter_turns()
{ {
update_loiter(); update_loiter();
calc_bearing_error(); calc_bearing_error();
@ -366,7 +375,7 @@ static bool verify_loiter_turns()
return false; return false;
} }
static bool verify_RTL() bool verify_RTL()
{ {
if (wp_distance <= g.waypoint_radius) { if (wp_distance <= g.waypoint_radius) {
gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home")); gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home"));
@ -380,22 +389,22 @@ static bool verify_RTL()
// Condition (May) commands // Condition (May) commands
/********************************************************************************/ /********************************************************************************/
static void do_wait_delay() void do_wait_delay()
{ {
condition_start = millis(); condition_start = millis();
condition_value = next_command.lat * 1000; // convert to milliseconds condition_value = next_command.lat * 1000; // convert to milliseconds
} }
static void do_change_alt() void do_change_alt()
{ {
condition_rate = next_command.lat; condition_rate = next_command.p1;
condition_value = next_command.alt; condition_value = next_command.alt;
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations next_WP.alt = condition_value; // For future nav calculations
offset_altitude = 0; // For future nav calculations offset_altitude = 0; // For future nav calculations
} }
static void do_within_distance() void do_within_distance()
{ {
condition_value = next_command.lat; condition_value = next_command.lat;
} }
@ -404,18 +413,23 @@ static void do_within_distance()
// Verify Condition (May) commands // Verify Condition (May) commands
/********************************************************************************/ /********************************************************************************/
static bool verify_wait_delay() bool verify_wait_delay()
{ {
if ((unsigned)(millis() - condition_start) > condition_value){ if ((millis() - condition_start) > condition_value){
condition_value = 0; condition_value = 0;
return true; return true;
} }
return false; return false;
} }
static bool verify_change_alt() bool verify_change_alt()
{ {
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) { //XXX this doesn't look right. How do you descend?
if(current_loc.alt >= condition_value) {
//Serial.printf_P(PSTR("alt, top:"));
//Serial.print(current_loc.alt, DEC);
//Serial.printf_P(PSTR("\t"));
//Serial.println(condition_value, DEC);
condition_value = 0; condition_value = 0;
return true; return true;
} }
@ -423,7 +437,7 @@ static bool verify_change_alt()
return false; return false;
} }
static bool verify_within_distance() bool verify_within_distance()
{ {
if (wp_distance < condition_value){ if (wp_distance < condition_value){
condition_value = 0; condition_value = 0;
@ -436,12 +450,12 @@ static bool verify_within_distance()
// Do (Now) commands // Do (Now) commands
/********************************************************************************/ /********************************************************************************/
static void do_loiter_at_location() void do_loiter_at_location()
{ {
next_WP = current_loc; next_WP = current_loc;
} }
static void do_jump() void do_jump()
{ {
struct Location temp; struct Location temp;
if(next_command.lat > 0) { if(next_command.lat > 0) {
@ -453,22 +467,20 @@ static void do_jump()
set_wp_with_index(temp, g.waypoint_index); set_wp_with_index(temp, g.waypoint_index);
g.waypoint_index.set_and_save(next_command.p1 - 1); g.waypoint_index.set_and_save(next_command.p1 - 1);
} else if (next_command.lat == -1) {
g.waypoint_index.set_and_save(next_command.p1 - 1);
} }
} }
static void do_change_speed() void do_change_speed()
{ {
// Note: we have no implementation for commanded ground speed, only air speed and throttle // Note: we have no implementation for commanded ground speed, only air speed and throttle
if(next_command.alt > 0) if(next_command.alt > 0)
g.airspeed_cruise.set_and_save(next_command.alt * 100); g.airspeed_cruise.set_and_save(next_command.alt * 100);
if(next_command.lat > 0) if(next_command.lat > 0)
g.throttle_cruise.set_and_save(next_command.lat); g.throttle_cruise.set_and_save(next_command.lat * 100);
} }
static void do_set_home() void do_set_home()
{ {
if(next_command.p1 == 1 && GPS_enabled) { if(next_command.p1 == 1 && GPS_enabled) {
init_home(); init_home();
@ -481,12 +493,12 @@ static void do_set_home()
} }
} }
static void do_set_servo() void do_set_servo()
{ {
APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); APM_RC.OutputCh(next_command.p1 - 1, next_command.alt);
} }
static void do_set_relay() void do_set_relay()
{ {
if (next_command.p1 == 1) { if (next_command.p1 == 1) {
relay_on(); relay_on();
@ -497,7 +509,7 @@ static void do_set_relay()
} }
} }
static void do_repeat_servo() void do_repeat_servo()
{ {
event_id = next_command.p1 - 1; event_id = next_command.p1 - 1;
@ -526,7 +538,7 @@ static void do_repeat_servo()
} }
} }
static void do_repeat_relay() void do_repeat_relay()
{ {
event_id = RELAY_TOGGLE; event_id = RELAY_TOGGLE;
event_timer = 0; event_timer = 0;

20
ArduPlane/commands_process.pde
View File

@ -2,15 +2,15 @@
// For changing active command mid-mission // For changing active command mid-mission
//---------------------------------------- //----------------------------------------
static void change_command(uint8_t cmd_index) void change_command(uint8_t index)
{ {
struct Location temp = get_wp_with_index(cmd_index); struct Location temp = get_wp_with_index(index);
if (temp.id > MAV_CMD_NAV_LAST ){ if (temp.id > MAV_CMD_NAV_LAST ){
gcs.send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); gcs.send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else { } else {
command_must_index = NO_COMMAND; command_must_index = NO_COMMAND;
next_command.id = NO_COMMAND; next_command.id = NO_COMMAND;
g.waypoint_index.set_and_save(cmd_index - 1); g.waypoint_index.set_and_save(index - 1);
load_next_command_from_EEPROM(); load_next_command_from_EEPROM();
process_next_command(); process_next_command();
} }
@ -19,7 +19,7 @@ static void change_command(uint8_t cmd_index)
// called by 10 Hz loop // called by 10 Hz loop
// -------------------- // --------------------
static void update_commands(void) void update_commands(void)
{ {
// This function loads commands into three buffers // This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX() // when a new command is loaded, it is processed with process_XXX()
@ -34,7 +34,7 @@ static void update_commands(void)
} // Other (eg GCS_Auto) modes may be implemented here } // Other (eg GCS_Auto) modes may be implemented here
} }
static void verify_commands(void) void verify_commands(void)
{ {
if(verify_must()){ if(verify_must()){
command_must_index = NO_COMMAND; command_must_index = NO_COMMAND;
@ -46,7 +46,7 @@ static void verify_commands(void)
} }
} }
static void load_next_command_from_EEPROM() void load_next_command_from_EEPROM()
{ {
// fetch next command if the next command queue is empty // fetch next command if the next command queue is empty
// ----------------------------------------------------- // -----------------------------------------------------
@ -64,7 +64,7 @@ static void load_next_command_from_EEPROM()
} }
} }
static void process_next_command() void process_next_command()
{ {
// these are Navigation/Must commands // these are Navigation/Must commands
// --------------------------------- // ---------------------------------
@ -115,7 +115,7 @@ static void process_next_command()
/**************************************************/ /**************************************************/
// These functions implement the commands. // These functions implement the commands.
/**************************************************/ /**************************************************/
static void process_must() void process_must()
{ {
gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>")); gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -132,7 +132,7 @@ static void process_must()
next_command.id = NO_COMMAND; next_command.id = NO_COMMAND;
} }
static void process_may() void process_may()
{ {
gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>")); gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -145,7 +145,7 @@ static void process_may()
next_command.id = NO_COMMAND; next_command.id = NO_COMMAND;
} }
static void process_now() void process_now()
{ {
handle_process_now(); handle_process_now();

117
ArduPlane/config.h
View File

@ -46,31 +46,13 @@
// AIRSPEED_SENSOR // AIRSPEED_SENSOR
// AIRSPEED_RATIO // AIRSPEED_RATIO
// //
#ifndef AIRSPEED_SENSOR //#ifndef AIRSPEED_SENSOR
# define AIRSPEED_SENSOR DISABLED //# define AIRSPEED_SENSOR DISABLED
#endif //#endif
#ifndef AIRSPEED_RATIO #ifndef AIRSPEED_RATIO
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution # define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// Sonar
//
#ifndef SONAR_ENABLED
# define SONAR_ENABLED DISABLED
#endif
#ifndef SONAR_PIN
# define SONAR_PIN AN4 // AN5, AP_RANGEFINDER_PITOT_TUBE
#endif
#ifndef SONAR_TYPE
# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// HIL_PROTOCOL OPTIONAL // HIL_PROTOCOL OPTIONAL
// HIL_MODE OPTIONAL // HIL_MODE OPTIONAL
@ -187,6 +169,9 @@
#ifndef MAG_ORIENTATION #ifndef MAG_ORIENTATION
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD # define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#endif #endif
#ifndef MAG_PROTOCOL
# define MAG_PROTOCOL MAG_PROTOCOL_5843
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -199,7 +184,8 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Radio channel limits // Radio channel limits
// //
// Note that these are not called out in APM_Config.h.reference. // Note that these are not called out in APM_Config.h.example as there
// is currently no configured behaviour for these channels.
// //
#ifndef CH5_MIN #ifndef CH5_MIN
# define CH5_MIN 1000 # define CH5_MIN 1000
@ -226,32 +212,7 @@
# define CH8_MAX 2000 # define CH8_MAX 2000
#endif #endif
//////////////////////////////////////////////////////////////////////////////
#ifndef RC_5_FUNCT
# define RC_5_FUNCT RC_5_FUNCT_NONE
#endif
#ifndef RC_6_FUNCT
# define RC_6_FUNCT RC_6_FUNCT_NONE
#endif
#ifndef RC_7_FUNCT
# define RC_7_FUNCT RC_7_FUNCT_NONE
#endif
#ifndef RC_8_FUNCT
# define RC_8_FUNCT RC_8_FUNCT_NONE
#endif
#ifndef FLAP_1_PERCENT
# define FLAP_1_PERCENT 0
#endif
#ifndef FLAP_1_SPEED
# define FLAP_1_SPEED 255
#endif
#ifndef FLAP_2_PERCENT
# define FLAP_2_PERCENT 0
#endif
#ifndef FLAP_2_SPEED
# define FLAP_2_SPEED 255
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE // FLIGHT_MODE
// FLIGHT_MODE_CHANNEL // FLIGHT_MODE_CHANNEL
@ -295,14 +256,14 @@
#ifndef THROTTLE_FAILSAFE #ifndef THROTTLE_FAILSAFE
# define THROTTLE_FAILSAFE ENABLED # define THROTTLE_FAILSAFE ENABLED
#endif #endif
#ifndef THROTTLE_FS_VALUE #ifndef THROTTE_FS_VALUE
# define THROTTLE_FS_VALUE 950 # define THROTTLE_FS_VALUE 950
#endif #endif
#ifndef SHORT_FAILSAFE_ACTION #ifndef SHORT_FAILSAFE_ACTION
# define SHORT_FAILSAFE_ACTION 0 # define SHORT_FAILSAFE_ACTION 2
#endif #endif
#ifndef LONG_FAILSAFE_ACTION #ifndef LONG_FAILSAFE_ACTION
# define LONG_FAILSAFE_ACTION 0 # define LONG_FAILSAFE_ACTION 2
#endif #endif
#ifndef GCS_HEARTBEAT_FAILSAFE #ifndef GCS_HEARTBEAT_FAILSAFE
# define GCS_HEARTBEAT_FAILSAFE DISABLED # define GCS_HEARTBEAT_FAILSAFE DISABLED
@ -369,22 +330,6 @@
# define REVERSE_SWITCH ENABLED # define REVERSE_SWITCH ENABLED
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE ELEVON_MIXING
//
#ifndef ELEVON_MIXING
# define ELEVON_MIXING DISABLED
#endif
#ifndef ELEVON_REVERSE
# define ELEVON_REVERSE DISABLED
#endif
#ifndef ELEVON_CH1_REVERSE
# define ELEVON_CH1_REVERSE DISABLED
#endif
#ifndef ELEVON_CH2_REVERSE
# define ELEVON_CH2_REVERSE DISABLED
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -611,12 +556,6 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Dataflash logging control // Dataflash logging control
// //
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
#ifndef LOG_ATTITUDE_FAST #ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED # define LOG_ATTITUDE_FAST DISABLED
#endif #endif
@ -648,22 +587,6 @@
# define LOG_CUR DISABLED # define LOG_CUR DISABLED
#endif #endif
// calculate the default log_bitmask
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
#define DEFAULT_LOG_BITMASK \
LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(RAW) | \
LOGBIT(CMD) | \
LOGBIT(CUR)
#ifndef DEBUG_PORT #ifndef DEBUG_PORT
# define DEBUG_PORT 0 # define DEBUG_PORT 0
#endif #endif
@ -710,10 +633,6 @@
# define USE_CURRENT_ALT FALSE # define USE_CURRENT_ALT FALSE
#endif #endif
#ifndef INVERTED_FLIGHT_PWM
# define INVERTED_FLIGHT_PWM 1750
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Developer Items // Developer Items
// //
@ -724,17 +643,3 @@
#endif #endif
#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE) #define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
// use this to enable servos in HIL mode
#ifndef HIL_SERVOS
# define HIL_SERVOS DISABLED
#endif
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED
#endif
// delay to prevent Xbee bricking, in milliseconds
#ifndef MAVLINK_TELEMETRY_PORT_DELAY
# define MAVLINK_TELEMETRY_PORT_DELAY 2000
#endif

73
ArduPlane/control_modes.pde
View File

@ -1,11 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- void read_control_switch()
static void read_control_switch()
{ {
byte switchPosition = readSwitch(); byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){ if (oldSwitchPosition != switchPosition){
set_mode(flight_modes[switchPosition]); set_mode(g.flight_modes[switchPosition]);
oldSwitchPosition = switchPosition; oldSwitchPosition = switchPosition;
prev_WP = current_loc; prev_WP = current_loc;
@ -14,15 +12,9 @@ static void read_control_switch()
// ------------------------- // -------------------------
reset_I(); reset_I();
} }
if (g.inverted_flight_ch != 0) {
// if the user has configured an inverted flight channel, then
// fly upside down when that channel goes above INVERTED_FLIGHT_PWM
inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM);
}
} }
static byte readSwitch(void){ byte readSwitch(void){
uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
@ -32,7 +24,7 @@ static byte readSwitch(void){
return 0; return 0;
} }
static void reset_control_switch() void reset_control_switch()
{ {
oldSwitchPosition = 0; oldSwitchPosition = 0;
read_control_switch(); read_control_switch();
@ -40,23 +32,52 @@ static void reset_control_switch()
SendDebugln(oldSwitchPosition , DEC); SendDebugln(oldSwitchPosition , DEC);
} }
static void update_servo_switches() void update_servo_switches()
{ {
if (!g.switch_enable) {
// switches are disabled in EEPROM (see SWITCH_ENABLE option)
// this means the EEPROM control of all channel reversal is enabled
return;
}
// up is reverse // up is reverse
// up is elevon // up is elevon
g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode mix_mode = (PINL & 128) ? 1 : 0;
if (g.mix_mode == 0) { if (mix_mode == 0) {
g.channel_roll.set_reverse((PINE & 128) ? true : false); reverse_roll = (PINE & 128) ? true : false;
g.channel_pitch.set_reverse((PINE & 64) ? true : false); reverse_pitch = (PINE & 64) ? true : false;
g.channel_rudder.set_reverse((PINL & 64) ? true : false); reverse_rudder = (PINL & 64) ? true : false;
} else { } else {
g.reverse_elevons = (PINE & 128) ? true : false; reverse_elevons = (PINE & 128) ? -1 : 1;
g.reverse_ch1_elevon = (PINE & 64) ? true : false; reverse_ch1_elevon = (PINE & 64) ? -1 : 1;
g.reverse_ch2_elevon = (PINL & 64) ? true : false; reverse_ch2_elevon = (PINL & 64) ? -1 : 1;
}
}
bool trim_flag;
unsigned long trim_timer;
// read at 10 hz
// set this to your trainer switch
void read_trim_switch()
{
// switch is engaged
if (g.rc_7.control_in > 800){
if(trim_flag == false){
// called once
trim_timer = millis();
}
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
// switch was just released
if((millis() - trim_timer) > 2000){
g.throttle_cruise.set_and_save(g.channel_throttle.control_in);
g.angle_of_attack.set_and_save(dcm.pitch_sensor);
g.airspeed_cruise.set_and_save(airspeed);
} else {
}
trim_flag = false;
}
} }
} }

59
ArduPlane/defines.h
View File

@ -10,7 +10,9 @@
#define DEBUG 0 #define DEBUG 0
#define LOITER_RANGE 60 // for calculating power outside of loiter radius #define LOITER_RANGE 60 // for calculating power outside of loiter radius
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. #define ROLL_SERVO_MAX 4500
#define PITCH_SERVO_MAX 4500
#define RUDDER_SERVO_MAX 4500
// failsafe // failsafe
// ---------------------- // ----------------------
@ -30,6 +32,10 @@
#define T6 1000000 #define T6 1000000
#define T7 10000000 #define T7 10000000
//MAGNETOMETER
#define MAG_PROTOCOL_5843 0
#define MAG_PROTOCOL_5883L 1
// GPS type codes - use the names, not the numbers // GPS type codes - use the names, not the numbers
#define GPS_PROTOCOL_NONE -1 #define GPS_PROTOCOL_NONE -1
#define GPS_PROTOCOL_NMEA 0 #define GPS_PROTOCOL_NMEA 0
@ -60,20 +66,6 @@
#define CH_RUDDER CH_4 #define CH_RUDDER CH_4
#define CH_YAW CH_4 #define CH_YAW CH_4
#define RC_5_FUNCT_NONE 0
#define RC_5_FUNCT_AILERON 1
#define RC_5_FUNCT_FLAP_AUTO 2
#define RC_5_FUNCT_FLAPERON 3
#define RC_6_FUNCT_NONE 0
#define RC_6_FUNCT_AILERON 1
#define RC_6_FUNCT_FLAP_AUTO 2
#define RC_6_FUNCT_FLAPERON 3
#define RC_7_FUNCT_NONE 0
#define RC_8_FUNCT_NONE 0
// HIL enumerations // HIL enumerations
#define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_XPLANE 1
#define HIL_PROTOCOL_MAVLINK 2 #define HIL_PROTOCOL_MAVLINK 2
@ -98,15 +90,12 @@
#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle #define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle
#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed #define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed
#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed // Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0
// Fly By Wire B and Fly By Wire C require airspeed sensor
#define AUTO 10 #define AUTO 10
#define RTL 11 #define RTL 11
#define LOITER 12 #define LOITER 12
//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM #define TAKEOFF 13
//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM #define LAND 14
#define GUIDED 15
#define INITIALISING 16 // in startup routines
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
@ -129,10 +118,6 @@
#define MAV_CMD_CONDITION_YAW 23 #define MAV_CMD_CONDITION_YAW 23
// GCS Message ID's // GCS Message ID's
/// 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
#define MSG_ACKNOWLEDGE 0x00 #define MSG_ACKNOWLEDGE 0x00
#define MSG_HEARTBEAT 0x01 #define MSG_HEARTBEAT 0x01
#define MSG_ATTITUDE 0x02 #define MSG_ATTITUDE 0x02
@ -143,10 +128,9 @@
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed #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_REQUEST 0x08
#define MSG_VERSION 0x09 #define MSG_VERSION 0x09
#define MSG_EXTENDED_STATUS1 0x0a #define MSG_EXTENDED_STATUS 0x0a
#define MSG_EXTENDED_STATUS2 0x0b #define MSG_CPU_LOAD 0x0b
#define MSG_CPU_LOAD 0x0c #define MSG_NAV_CONTROLLER_OUTPUT 0x0c
#define MSG_NAV_CONTROLLER_OUTPUT 0x0d
#define MSG_COMMAND_REQUEST 0x20 #define MSG_COMMAND_REQUEST 0x20
#define MSG_COMMAND_UPLOAD 0x21 #define MSG_COMMAND_UPLOAD 0x21
@ -169,11 +153,9 @@
#define MSG_RADIO_OUT 0x53 #define MSG_RADIO_OUT 0x53
#define MSG_RADIO_IN 0x54 #define MSG_RADIO_IN 0x54
#define MSG_RAW_IMU1 0x60 #define MSG_RAW_IMU 0x60
#define MSG_RAW_IMU2 0x61 #define MSG_GPS_STATUS 0x61
#define MSG_RAW_IMU3 0x62 #define MSG_GPS_RAW 0x62
#define MSG_GPS_STATUS 0x63
#define MSG_GPS_RAW 0x64
#define MSG_SERVO_OUT 0x70 #define MSG_SERVO_OUT 0x70
@ -191,7 +173,6 @@
#define MSG_POSITION_SET 0xb2 #define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3 #define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4 #define MSG_LOCAL_LOCATION 0xb4
#define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1 #define SEVERITY_LOW 1
#define SEVERITY_MEDIUM 2 #define SEVERITY_MEDIUM 2
@ -224,6 +205,7 @@
#define MASK_LOG_RAW (1<<7) #define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8) #define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9) #define MASK_LOG_CUR (1<<9)
#define MASK_LOG_SET_DEFAULTS (1<<15)
// Waypoint Modes // Waypoint Modes
// ---------------- // ----------------
@ -264,11 +246,7 @@
// sonar // sonar
#define MAX_SONAR_XL 0
#define MAX_SONAR_LV 1
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
#define AN4 4
#define AN5 5
// Hardware Parameters // Hardware Parameters
#define SLIDE_SWITCH_PIN 40 #define SLIDE_SWITCH_PIN 40
@ -289,6 +267,3 @@
#define ONBOARD_PARAM_NAME_LENGTH 15 #define ONBOARD_PARAM_NAME_LENGTH 15
#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe #define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)

26
ArduPlane/events.pde
View File

@ -1,7 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void failsafe_short_on_event() void failsafe_short_on_event()
{ {
// This is how to handle a short loss of control signal failsafe. // This is how to handle a short loss of control signal failsafe.
failsafe = FAILSAFE_SHORT; failsafe = FAILSAFE_SHORT;
@ -18,7 +18,7 @@ static void failsafe_short_on_event()
case AUTO: case AUTO:
case LOITER: case LOITER:
if(g.short_fs_action == 1) { if(g.short_fs_action != 1) {
set_mode(RTL); set_mode(RTL);
} }
break; break;
@ -32,11 +32,10 @@ static void failsafe_short_on_event()
SendDebugln(control_mode, DEC); SendDebugln(control_mode, DEC);
} }
static void failsafe_long_on_event() void failsafe_long_on_event()
{ {
// This is how to handle a long loss of control signal failsafe. // This is how to handle a short loss of control signal failsafe.
SendDebug_P("Failsafe - Long event on"); SendDebug_P("Failsafe - Long event on");
APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC
switch(control_mode) switch(control_mode)
{ {
case MANUAL: case MANUAL:
@ -49,7 +48,7 @@ static void failsafe_long_on_event()
case AUTO: case AUTO:
case LOITER: case LOITER:
case CIRCLE: case CIRCLE:
if(g.long_fs_action == 1) { if(g.long_fs_action != 1) {
set_mode(RTL); set_mode(RTL);
} }
break; break;
@ -60,7 +59,7 @@ static void failsafe_long_on_event()
} }
} }
static void failsafe_short_off_event() void failsafe_short_off_event()
{ {
// We're back in radio contact // We're back in radio contact
SendDebug_P("Failsafe - Short event off"); SendDebug_P("Failsafe - Short event off");
@ -75,16 +74,15 @@ static void failsafe_short_off_event()
reset_I(); reset_I();
} }
#if BATTERY_EVENT == ENABLED void low_battery_event(void)
static void low_battery_event(void)
{ {
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL); set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE; g.throttle_cruise = THROTTLE_CRUISE;
} }
#endif
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{ {
if(event_repeat == 0 || (millis() - event_timer) < event_delay) if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return; return;
@ -110,17 +108,17 @@ static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_
} }
} }
static void relay_on() void relay_on()
{ {
PORTL |= B00000100; PORTL |= B00000100;
} }
static void relay_off() void relay_off()
{ {
PORTL &= ~B00000100; PORTL &= ~B00000100;
} }
static void relay_toggle() void relay_toggle()
{ {
PORTL ^= B00000100; PORTL ^= B00000100;
} }

57
ArduPlane/navigation.pde
View File

@ -3,7 +3,7 @@
//**************************************************************** //****************************************************************
// Function that will calculate the desired direction to fly and distance // Function that will calculate the desired direction to fly and distance
//**************************************************************** //****************************************************************
static void navigate() void navigate()
{ {
// do not navigate with corrupt data // do not navigate with corrupt data
// --------------------------------- // ---------------------------------
@ -63,7 +63,7 @@ void calc_distance_error()
} }
#endif #endif
static void calc_airspeed_errors() void calc_airspeed_errors()
{ {
// XXX excess casting here // XXX excess casting here
if(control_mode>=AUTO && airspeed_nudge > 0) { if(control_mode>=AUTO && airspeed_nudge > 0) {
@ -75,7 +75,7 @@ static void calc_airspeed_errors()
} }
} }
static void calc_bearing_error() void calc_bearing_error()
{ {
if(takeoff_complete == true || g.compass_enabled == true) { if(takeoff_complete == true || g.compass_enabled == true) {
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
@ -89,7 +89,7 @@ static void calc_bearing_error()
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }
static void calc_altitude_error() void calc_altitude_error()
{ {
if(control_mode == AUTO && offset_altitude != 0) { if(control_mode == AUTO && offset_altitude != 0) {
// limit climb rates // limit climb rates
@ -127,39 +127,38 @@ static void calc_altitude_error()
altitude_error = target_altitude - current_loc.alt; altitude_error = target_altitude - current_loc.alt;
} }
static long wrap_360(long error) long wrap_360(long error)
{ {
if (error > 36000) error -= 36000; if (error > 36000) error -= 36000;
if (error < 0) error += 36000; if (error < 0) error += 36000;
return error; return error;
} }
static long wrap_180(long error) long wrap_180(long error)
{ {
if (error > 18000) error -= 36000; if (error > 18000) error -= 36000;
if (error < -18000) error += 36000; if (error < -18000) error += 36000;
return error; return error;
} }
static void update_loiter() void update_loiter()
{ {
float power; float power;
if(wp_distance <= g.loiter_radius){ if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius); power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1);
nav_bearing += (int)(9000.0 * (2.0 + power)); nav_bearing += (int)(9000.0 * (2.0 + power));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); power = constrain(power, 0, 1);
nav_bearing -= power * 9000; nav_bearing -= power * 9000;
}else{ }else{
update_crosstrack(); update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
} }
/*
if (wp_distance < g.loiter_radius){ if (wp_distance < g.loiter_radius){
nav_bearing += 9000; nav_bearing += 9000;
}else{ }else{
@ -167,27 +166,35 @@ static void update_loiter()
} }
update_crosstrack(); update_crosstrack();
*/
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
} }
static void update_crosstrack(void) void update_crosstrack(void)
{ {
// Crosstrack Error // Crosstrack Error
// ---------------- // ----------------
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
} }
} }
static void reset_crosstrack() void reset_crosstrack()
{ {
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
} }
static long get_distance(struct Location *loc1, struct Location *loc2) long get_altitude_above_home(void) // TO DO - this function has been deprecated - check if any useage vs removal
{
// This is the altitude above the home location
// The GPS gives us altitude at Sea Level
// if you slope soar, you should see a negative number sometimes
// -------------------------------------------------------------
return current_loc.alt - home.alt;
}
long get_distance(struct Location *loc1, struct Location *loc2)
{ {
if(loc1->lat == 0 || loc1->lng == 0) if(loc1->lat == 0 || loc1->lng == 0)
return -1; return -1;
@ -198,7 +205,12 @@ static long get_distance(struct Location *loc1, struct Location *loc2)
return sqrt(sq(dlat) + sq(dlong)) * .01113195; return sqrt(sq(dlat) + sq(dlong)) * .01113195;
} }
static long get_bearing(struct Location *loc1, struct Location *loc2) long get_alt_distance(struct Location *loc1, struct Location *loc2)
{
return abs(loc1->alt - loc2->alt);
}
long get_bearing(struct Location *loc1, struct Location *loc2)
{ {
long off_x = loc2->lng - loc1->lng; long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp; long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
@ -206,3 +218,14 @@ static long get_bearing(struct Location *loc1, struct Location *loc2)
if (bearing < 0) bearing += 36000; if (bearing < 0) bearing += 36000;
return bearing; return bearing;
} }
Vector3f get_location_vector(struct Location *uav, struct Location *target)
{
Vector3<float> v;
v.x = (target->lng-uav->lng) * cos((uav->lat+target->lat)/2)*.01113195;
v.y = (target->lat-uav->lat)*.01113195;
v.z = (uav->alt-target->alt);
return v;
}

7
ArduPlane/planner.pde
View File

@ -8,19 +8,18 @@ static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv);
// and stores them in Flash memory, not RAM. // and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right. // User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details // See class Menu in AP_Common for implementation details
static const struct Menu::command planner_menu_commands[] PROGMEM = { const struct Menu::command planner_menu_commands[] PROGMEM = {
{"gcs", planner_gcs}, {"gcs", planner_gcs},
}; };
// A Macro to create the Menu // A Macro to create the Menu
MENU(planner_menu, "planner", planner_menu_commands); MENU(planner_menu, "planner", planner_menu_commands);
static int8_t int8_t
planner_mode(uint8_t argc, const Menu::arg *argv) planner_mode(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
planner_menu.run(); planner_menu.run();
return 0;
} }
static int8_t static int8_t
planner_gcs(uint8_t argc, const Menu::arg *argv) planner_gcs(uint8_t argc, const Menu::arg *argv)
@ -45,6 +44,4 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
loopcount++; loopcount++;
} }
} }
return 0;
} }

66
ArduPlane/radio.pde
View File

@ -2,18 +2,21 @@
//Function that will read the radio data, limit servos and trigger a failsafe //Function that will read the radio data, limit servos and trigger a failsafe
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
static void init_rc_in() void init_rc_in()
{ {
// set rc reversing // set rc reversing
update_servo_switches(); update_servo_switches();
g.channel_roll.set_reverse(reverse_roll);
g.channel_pitch.set_reverse(reverse_pitch);
g.channel_rudder.set_reverse(reverse_rudder);
// set rc channel ranges // set rc channel ranges
g.channel_roll.set_angle(SERVO_MAX); g.channel_roll.set_angle(ROLL_SERVO_MAX);
g.channel_pitch.set_angle(SERVO_MAX); g.channel_pitch.set_angle(PITCH_SERVO_MAX);
g.channel_rudder.set_angle(SERVO_MAX); g.channel_rudder.set_angle(RUDDER_SERVO_MAX);
g.channel_throttle.set_range(0, 100); g.channel_throttle.set_range(0, 100);
// set rc dead zones // set rc dead zones
@ -23,27 +26,13 @@ static void init_rc_in()
g.channel_throttle.dead_zone = 6; g.channel_throttle.dead_zone = 6;
//set auxiliary ranges //set auxiliary ranges
if (g.rc_5_funct == RC_5_FUNCT_AILERON) { g.rc_5.set_range(0,1000);
g.rc_5.set_angle(SERVO_MAX); g.rc_6.set_range(0,1000);
} else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) { g.rc_7.set_range(0,1000);
g.rc_5.set_range(0,100);
} else {
g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here
}
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
g.rc_6.set_angle(SERVO_MAX);
} else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) {
g.rc_6.set_range(0,100);
} else {
g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here
}
g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here
g.rc_8.set_range(0,1000); g.rc_8.set_range(0,1000);
} }
static void init_rc_out() void init_rc_out()
{ {
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
@ -68,17 +57,17 @@ static void init_rc_out()
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
} }
static void read_radio() void read_radio()
{ {
ch1_temp = APM_RC.InputCh(CH_ROLL); ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH); ch2_temp = APM_RC.InputCh(CH_PITCH);
if(g.mix_mode == 0){ if(mix_mode == 0){
g.channel_roll.set_pwm(ch1_temp); g.channel_roll.set_pwm(ch1_temp);
g.channel_pitch.set_pwm(ch2_temp); g.channel_pitch.set_pwm(ch2_temp);
}else{ }else{
g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); g.channel_roll.set_pwm(reverse_elevons * (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) - reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500);
g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); g.channel_pitch.set_pwm((reverse_ch2_elevon * int(ch2_temp - elevon2_trim) + reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500);
} }
g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
@ -98,7 +87,7 @@ static void read_radio()
g.channel_throttle.servo_out = g.channel_throttle.control_in; g.channel_throttle.servo_out = g.channel_throttle.control_in;
if (g.channel_throttle.servo_out > 50) { if (g.channel_throttle.servo_out > 50) {
if(g.airspeed_enabled == true) { if(airspeed_enabled == true) {
airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
} else { } else {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
@ -117,7 +106,7 @@ static void read_radio()
*/ */
} }
static void control_failsafe(uint16_t pwm) void control_failsafe(uint16_t pwm)
{ {
if(g.throttle_fs_enabled == 0) if(g.throttle_fs_enabled == 0)
return; return;
@ -132,7 +121,7 @@ static void control_failsafe(uint16_t pwm)
//Check for failsafe and debounce funky reads //Check for failsafe and debounce funky reads
} else if (g.throttle_fs_enabled) { } else if (g.throttle_fs_enabled) {
if (pwm < (unsigned)g.throttle_fs_value){ if (pwm < g.throttle_fs_value){
// we detect a failsafe from radio // we detect a failsafe from radio
// throttle has dropped below the mark // throttle has dropped below the mark
failsafeCounter++; failsafeCounter++;
@ -164,18 +153,15 @@ static void control_failsafe(uint16_t pwm)
} }
} }
static void trim_control_surfaces() void trim_control_surfaces()
{ {
read_radio(); read_radio();
// Store control surface trim values // Store control surface trim values
// --------------------------------- // ---------------------------------
if(g.mix_mode == 0){ if(mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
}else{ }else{
elevon1_trim = ch1_temp; elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp; elevon2_trim = ch2_temp;
@ -191,11 +177,9 @@ static void trim_control_surfaces()
g.channel_pitch.save_eeprom(); g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom(); g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom(); g.channel_rudder.save_eeprom();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
} }
static void trim_radio() void trim_radio()
{ {
for (int y = 0; y < 30; y++) { for (int y = 0; y < 30; y++) {
read_radio(); read_radio();
@ -203,13 +187,11 @@ static void trim_radio()
// Store the trim values // Store the trim values
// --------------------- // ---------------------
if(g.mix_mode == 0){ if(mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
} else { } else {
elevon1_trim = ch1_temp; elevon1_trim = ch1_temp;
@ -225,7 +207,5 @@ static void trim_radio()
g.channel_pitch.save_eeprom(); g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom(); //g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom(); g.channel_rudder.save_eeprom();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
} }

40
ArduPlane/sensors.pde
View File

@ -5,17 +5,21 @@
void ReadSCP1000(void) {} void ReadSCP1000(void) {}
static void init_barometer(void) void init_barometer(void)
{ {
int flashcount = 0; int flashcount;
long ground_pressure = 0; long ground_pressure = 0;
int ground_temperature; int ground_temperature;
#if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets for initialization
#endif
while(ground_pressure == 0){ while(ground_pressure == 0){
barometer.Read(); // Get initial data from absolute pressure sensor barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = barometer.Press; ground_pressure = barometer.Press;
ground_temperature = barometer.Temp; ground_temperature = barometer.Temp;
mavlink_delay(20); delay(20);
//Serial.printf("barometer.Press %ld\n", barometer.Press); //Serial.printf("barometer.Press %ld\n", barometer.Press);
} }
@ -29,7 +33,7 @@ static void init_barometer(void)
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l; ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
mavlink_delay(20); delay(20);
if(flashcount == 5) { if(flashcount == 5) {
digitalWrite(C_LED_PIN, LOW); digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH); digitalWrite(A_LED_PIN, HIGH);
@ -49,11 +53,11 @@ static void init_barometer(void)
g.ground_temperature.set_and_save(ground_temperature / 10.0f); g.ground_temperature.set_and_save(ground_temperature / 10.0f);
abs_pressure = ground_pressure; abs_pressure = ground_pressure;
Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); SendDebugln_P("barometer calibration complete.");
} }
static long read_barometer(void) long read_barometer(void)
{ {
float x, scaling, temp; float x, scaling, temp;
@ -69,38 +73,30 @@ static long read_barometer(void)
} }
// in M/S * 100 // in M/S * 100
static void read_airspeed(void) void read_airspeed(void)
{ {
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed
if (g.airspeed_offset == 0) {
// runtime enabling of airspeed, we need to do instant
// calibration before we can use it. This isn't as
// accurate as the 50 point average in zero_airspeed(),
// but it is better than using it uncalibrated
airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
g.airspeed_offset.set_and_save(airspeed_raw);
}
airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75); airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75);
airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0); airspeed_pressure = max(((int)airspeed_raw - airspeed_offset), 0);
airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100; airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100;
#endif #endif
calc_airspeed_errors(); calc_airspeed_errors();
} }
static void zero_airspeed(void) void zero_airspeed(void)
{ {
airspeed_raw = (float)adc.Ch(AIRSPEED_CH); airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
for(int c = 0; c < 10; c++){ for(int c = 0; c < 50; c++){
delay(20); delay(20);
airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10); airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10);
} }
g.airspeed_offset.set_and_save(airspeed_raw); airspeed_offset = airspeed_raw;
} }
#endif // HIL_MODE != HIL_MODE_ATTITUDE #endif // HIL_MODE != HIL_MODE_ATTITUDE
static void read_battery(void) void read_battery(void)
{ {
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
@ -118,7 +114,7 @@ static void read_battery(void)
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
} }
#if BATTERY_EVENT == ENABLED #if BATTERY_EVENT == 1
if(battery_voltage < LOW_VOLTAGE) low_battery_event(); if(battery_voltage < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
#endif #endif

145
ArduPlane/setup.pde
View File

@ -1,7 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
// Functions called from the setup menu // Functions called from the setup menu
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
@ -13,7 +11,7 @@ static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
// Command/function table for the setup menu // Command/function table for the setup menu
static const struct Menu::command setup_menu_commands[] PROGMEM = { const struct Menu::command setup_menu_commands[] PROGMEM = {
// command function called // command function called
// ======= =============== // ======= ===============
{"reset", setup_factory}, {"reset", setup_factory},
@ -30,7 +28,7 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = {
MENU(setup_menu, "setup", setup_menu_commands); MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu. // Called from the top-level menu to run the setup menu.
static int8_t int8_t
setup_mode(uint8_t argc, const Menu::arg *argv) setup_mode(uint8_t argc, const Menu::arg *argv)
{ {
// Give the user some guidance // Give the user some guidance
@ -43,7 +41,6 @@ setup_mode(uint8_t argc, const Menu::arg *argv)
// Run the setup menu. When the menu exits, we will return to the main menu. // Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run(); setup_menu.run();
return 0;
} }
// Print the current configuration. // Print the current configuration.
@ -51,6 +48,7 @@ setup_mode(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
setup_show(uint8_t argc, const Menu::arg *argv) setup_show(uint8_t argc, const Menu::arg *argv)
{ {
uint8_t i;
// clear the area // clear the area
print_blanks(8); print_blanks(8);
@ -75,6 +73,8 @@ setup_show(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
setup_factory(uint8_t argc, const Menu::arg *argv) setup_factory(uint8_t argc, const Menu::arg *argv)
{ {
uint8_t i;
int c; int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
@ -185,7 +185,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv) setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{ {
byte switchPosition, mode = 0; byte switchPosition, oldSwitchPosition, mode;
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter(); print_hit_enter();
@ -201,10 +201,10 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
if (oldSwitchPosition != switchPosition){ if (oldSwitchPosition != switchPosition){
// force position 5 to MANUAL // force position 5 to MANUAL
if (switchPosition > 4) { if (switchPosition > 4) {
flight_modes[switchPosition] = MANUAL; g.flight_modes[switchPosition] = MANUAL;
} }
// update our current mode // update our current mode
mode = flight_modes[switchPosition]; mode = g.flight_modes[switchPosition];
// update the user // update the user
print_switch(switchPosition, mode); print_switch(switchPosition, mode);
@ -228,11 +228,13 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode != FLY_BY_WIRE_B && mode != FLY_BY_WIRE_B &&
mode != AUTO && mode != AUTO &&
mode != RTL && mode != RTL &&
mode != LOITER) mode != LOITER &&
mode != TAKEOFF &&
mode != LAND)
{ {
if (mode < MANUAL) if (mode < MANUAL)
mode = LOITER; mode = LAND;
else if (mode >LOITER) else if (mode >LAND)
mode = MANUAL; mode = MANUAL;
else else
mode += radioInputSwitch; mode += radioInputSwitch;
@ -243,7 +245,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode = MANUAL; mode = MANUAL;
// save new mode // save new mode
flight_modes[switchPosition] = mode; g.flight_modes[switchPosition] = mode;
// print new mode // print new mode
print_switch(switchPosition, mode); print_switch(switchPosition, mode);
@ -252,8 +254,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
// escape hatch // escape hatch
if(Serial.available() > 0){ if(Serial.available() > 0){
// save changes // save changes
for (mode=0; mode<6; mode++) g.flight_modes.save();
flight_modes[mode].save();
report_flight_modes(); report_flight_modes();
print_done(); print_done();
return (0); return (0);
@ -266,13 +267,13 @@ setup_declination(uint8_t argc, const Menu::arg *argv)
{ {
compass.set_declination(radians(argv[1].f)); compass.set_declination(radians(argv[1].f));
report_compass(); report_compass();
return 0;
} }
static int8_t static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv) setup_erase(uint8_t argc, const Menu::arg *argv)
{ {
uint8_t i;
int c; int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ")); Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
@ -291,13 +292,9 @@ static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv) setup_compass(uint8_t argc, const Menu::arg *argv)
{ {
if (!strcmp_P(argv[1].str, PSTR("on"))) { if (!strcmp_P(argv[1].str, PSTR("on"))) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft g.compass_enabled = true;
if (!compass.init()) { bool junkbool = compass.init();
Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
g.compass_enabled = true;
}
} else if (!strcmp_P(argv[1].str, PSTR("off"))) { } else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.compass_enabled = false; g.compass_enabled = false;
@ -326,11 +323,50 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
/***************************************************************************/
// CLI defaults
/***************************************************************************/
void
default_flight_modes()
{
g.flight_modes[0] = FLIGHT_MODE_1;
g.flight_modes[1] = FLIGHT_MODE_2;
g.flight_modes[2] = FLIGHT_MODE_3;
g.flight_modes[3] = FLIGHT_MODE_4;
g.flight_modes[4] = FLIGHT_MODE_5;
g.flight_modes[5] = FLIGHT_MODE_6;
g.flight_modes.save();
}
void
default_log_bitmask()
{
// convenience macro for testing LOG_* and setting LOGBIT_*
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
g.log_bitmask =
LOGBIT(ATTITUDE_FAST) |
LOGBIT(ATTITUDE_MED) |
LOGBIT(GPS) |
LOGBIT(PM) |
LOGBIT(CTUN) |
LOGBIT(NTUN) |
LOGBIT(MODE) |
LOGBIT(RAW) |
LOGBIT(CMD) |
LOGBIT(CUR);
#undef LOGBIT
g.log_bitmask.save();
}
/***************************************************************************/ /***************************************************************************/
// CLI reports // CLI reports
/***************************************************************************/ /***************************************************************************/
static void report_batt_monitor() void report_batt_monitor()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Batt Mointor\n")); Serial.printf_P(PSTR("Batt Mointor\n"));
@ -342,7 +378,7 @@ static void report_batt_monitor()
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
print_blanks(2); print_blanks(2);
} }
static void report_radio() void report_radio()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Radio\n")); Serial.printf_P(PSTR("Radio\n"));
@ -352,7 +388,7 @@ static void report_radio()
print_blanks(2); print_blanks(2);
} }
static void report_gains() void report_gains()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Gains\n")); Serial.printf_P(PSTR("Gains\n"));
@ -370,7 +406,7 @@ static void report_gains()
Serial.printf_P(PSTR("nav roll:\n")); Serial.printf_P(PSTR("nav roll:\n"));
print_PID(&g.pidNavRoll); print_PID(&g.pidNavRoll);
Serial.printf_P(PSTR("nav pitch airspeed:\n")); Serial.printf_P(PSTR("nav pitch airpseed:\n"));
print_PID(&g.pidNavPitchAirspeed); print_PID(&g.pidNavPitchAirspeed);
Serial.printf_P(PSTR("energry throttle:\n")); Serial.printf_P(PSTR("energry throttle:\n"));
@ -382,7 +418,7 @@ static void report_gains()
print_blanks(2); print_blanks(2);
} }
static void report_xtrack() void report_xtrack()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Crosstrack\n")); Serial.printf_P(PSTR("Crosstrack\n"));
@ -395,7 +431,7 @@ static void report_xtrack()
print_blanks(2); print_blanks(2);
} }
static void report_throttle() void report_throttle()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Throttle\n")); Serial.printf_P(PSTR("Throttle\n"));
@ -414,7 +450,7 @@ static void report_throttle()
print_blanks(2); print_blanks(2);
} }
static void report_imu() void report_imu()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("IMU\n")); Serial.printf_P(PSTR("IMU\n"));
@ -425,32 +461,16 @@ static void report_imu()
print_blanks(2); print_blanks(2);
} }
static void report_compass() void report_compass()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Compass: ")); Serial.printf_P(PSTR("Compass\n"));
switch (compass.product_id) {
case AP_COMPASS_TYPE_HMC5883L:
Serial.println_P(PSTR("HMC5883L"));
break;
case AP_COMPASS_TYPE_HMC5843:
Serial.println_P(PSTR("HMC5843"));
break;
case AP_COMPASS_TYPE_HIL:
Serial.println_P(PSTR("HIL"));
break;
default:
Serial.println_P(PSTR("??"));
break;
}
print_divider(); print_divider();
print_enabled(g.compass_enabled); print_enabled(g.compass_enabled);
// mag declination // mag declination
Serial.printf_P(PSTR("Mag Declination: %4.4f\n"), Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
degrees(compass.get_declination())); degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets(); Vector3f offsets = compass.get_offsets();
@ -463,14 +483,14 @@ static void report_compass()
print_blanks(2); print_blanks(2);
} }
static void report_flight_modes() void report_flight_modes()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n")); Serial.printf_P(PSTR("Flight modes\n"));
print_divider(); print_divider();
for(int i = 0; i < 6; i++ ){ for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]); print_switch(i, g.flight_modes[i]);
} }
print_blanks(2); print_blanks(2);
} }
@ -479,7 +499,7 @@ static void report_flight_modes()
// CLI utilities // CLI utilities
/***************************************************************************/ /***************************************************************************/
static void void
print_PID(PID * pid) print_PID(PID * pid)
{ {
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
@ -489,7 +509,7 @@ print_PID(PID * pid)
(long)pid->imax()); (long)pid->imax());
} }
static void void
print_radio_values() print_radio_values()
{ {
Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
@ -503,20 +523,20 @@ print_radio_values()
} }
static void void
print_switch(byte p, byte m) print_switch(byte p, byte m)
{ {
Serial.printf_P(PSTR("Pos %d: "),p); Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]); Serial.println(flight_mode_strings[m]);
} }
static void void
print_done() print_done()
{ {
Serial.printf_P(PSTR("\nSaved Settings\n\n")); Serial.printf_P(PSTR("\nSaved Settings\n\n"));
} }
static void void
print_blanks(int num) print_blanks(int num)
{ {
while(num > 0){ while(num > 0){
@ -525,7 +545,7 @@ print_blanks(int num)
} }
} }
static void void
print_divider(void) print_divider(void)
{ {
for (int i = 0; i < 40; i++) { for (int i = 0; i < 40; i++) {
@ -534,7 +554,7 @@ print_divider(void)
Serial.println(""); Serial.println("");
} }
static int8_t int8_t
radio_input_switch(void) radio_input_switch(void)
{ {
static int8_t bouncer = 0; static int8_t bouncer = 0;
@ -561,9 +581,9 @@ radio_input_switch(void)
} }
static void zero_eeprom(void) void zero_eeprom(void)
{ {
byte b = 0; byte b;
Serial.printf_P(PSTR("\nErasing EEPROM\n")); Serial.printf_P(PSTR("\nErasing EEPROM\n"));
for (int i = 0; i < EEPROM_MAX_ADDR; i++) { for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b); eeprom_write_byte((uint8_t *) i, b);
@ -571,7 +591,7 @@ static void zero_eeprom(void)
Serial.printf_P(PSTR("done\n")); Serial.printf_P(PSTR("done\n"));
} }
static void print_enabled(bool b) void print_enabled(bool b)
{ {
if(b) if(b)
Serial.printf_P(PSTR("en")); Serial.printf_P(PSTR("en"));
@ -580,7 +600,7 @@ static void print_enabled(bool b)
Serial.printf_P(PSTR("abled\n")); Serial.printf_P(PSTR("abled\n"));
} }
static void void
print_accel_offsets(void) print_accel_offsets(void)
{ {
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
@ -589,7 +609,7 @@ print_accel_offsets(void)
(float)imu.az()); (float)imu.az());
} }
static void void
print_gyro_offsets(void) print_gyro_offsets(void)
{ {
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
@ -599,4 +619,3 @@ print_gyro_offsets(void)
} }
#endif // CLI_ENABLED

168
ArduPlane/system.pde
View File

@ -6,13 +6,11 @@ The init_ardupilot function processes everything we need for an in - air restart
*****************************************************************************/ *****************************************************************************/
#if CLI_ENABLED == ENABLED
// Functions called from the top-level menu // Functions called from the top-level menu
static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde extern int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
// This is the help function // This is the help function
// PSTR is an AVR macro to read strings from flash memory // PSTR is an AVR macro to read strings from flash memory
@ -30,7 +28,7 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
} }
// Command/function table for the top-level menu. // Command/function table for the top-level menu.
static const struct Menu::command main_menu_commands[] PROGMEM = { const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called // command function called
// ======= =============== // ======= ===============
{"logs", process_logs}, {"logs", process_logs},
@ -41,11 +39,9 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "APM trunk", main_menu_commands); MENU(main_menu, "ArduPilotMega", main_menu_commands);
#endif // CLI_ENABLED void init_ardupilot()
static void init_ardupilot()
{ {
// Console serial port // Console serial port
// //
@ -77,6 +73,16 @@ static void init_ardupilot()
Serial1.begin(38400, 128, 16); Serial1.begin(38400, 128, 16);
#endif #endif
// Telemetry port.
//
// Not used if telemetry is going to the console.
//
// XXX for unidirectional protocols, we could (should) minimize
// the receive buffer, and the transmit buffer could also be
// shrunk for protocols that don't send large messages.
//
Serial3.begin(SERIAL3_BAUD, 128, 128);
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %lu\n"), "\n\nFree RAM: %lu\n"),
freeRAM()); freeRAM());
@ -86,10 +92,7 @@ static void init_ardupilot()
// //
if (!g.format_version.load()) { if (!g.format_version.load()) {
Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n")); Serial.println("\n\nEEPROM blank - all parameters are reset to default values.\n");
delay(100); // wait for serial msg to flush
AP_Var::erase_all();
// save the current format version // save the current format version
g.format_version.set_and_save(Parameters::k_format_version); g.format_version.set_and_save(Parameters::k_format_version);
@ -99,7 +102,6 @@ static void init_ardupilot()
Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
"\n\nForcing complete parameter reset..."), "\n\nForcing complete parameter reset..."),
g.format_version.get(), Parameters::k_format_version); g.format_version.get(), Parameters::k_format_version);
delay(100); // wait for serial msg to flush
// erase all parameters // erase all parameters
AP_Var::erase_all(); AP_Var::erase_all();
@ -114,48 +116,45 @@ static void init_ardupilot()
AP_Var::load_all(); AP_Var::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"), Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
AP_Var::get_memory_use(), (unsigned)g.num_resets);
} }
// keep a record of how many resets have happened. This can be if (!g.flight_modes.load()) {
// used to detect in-flight resets default_flight_modes();
g.num_resets.set_and_save(g.num_resets+1); }
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
// Telemetry port. default_log_bitmask();
// }
// Not used if telemetry is going to the console.
//
// XXX for unidirectional protocols, we could (should) minimize
// the receive buffer, and the transmit buffer could also be
// shrunk for protocols that don't send large messages.
//
Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
mavlink_system.sysid = g.sysid_this_mav; mavlink_system.sysid = g.sysid_this_mav;
#if CAMERA == ENABLED
init_camera();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization adc.Init(); // APM ADC library initialization
barometer.Init(); // APM Abs Pressure sensor initialization barometer.Init(); // APM Abs Pressure sensor initialization
// Autodetect airspeed sensor
if ((adc.Ch(AIRSPEED_CH) > 2000)&&(adc.Ch(AIRSPEED_CH) < 2900))
{
airspeed_enabled = false;
}
else
{
airspeed_enabled = true;
}
if (g.compass_enabled==true) { if (g.compass_enabled==true) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft dcm.set_compass(&compass);
if (!compass.init()) { bool junkbool = compass.init();
Serial.println_P(PSTR("Compass initialisation failed!")); compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
g.compass_enabled = false; Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
} else { //compass.set_declination(ToRad(get(PARAM_DECLINATION))); // TODO fix this to have a UI // set local difference between magnetic north and true north
dcm.set_compass(&compass);
compass.get_offsets(); // load offsets to account for airframe magnetic interference
}
} }
/* #else
Init is depricated - Jason airspeed_enabled = true;
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC);
}
*/
#endif #endif
DataFlash.Init(); // DataFlash log initialization DataFlash.Init(); // DataFlash log initialization
@ -163,7 +162,6 @@ static void init_ardupilot()
// Do GPS init // Do GPS init
g_gps = &g_gps_driver; g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization g_gps->init(); // GPS Initialization
g_gps->callback = mavlink_delay;
// init the GCS // init the GCS
#if GCS_PORT == 3 #if GCS_PORT == 3
@ -210,7 +208,6 @@ static void init_ardupilot()
// the system in an odd state, we don't let the user exit the top // the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly. // menu; they must reset in order to fly.
// //
#if CLI_ENABLED == ENABLED
if (digitalRead(SLIDE_SWITCH_PIN) == 0) { if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
Serial.printf_P(PSTR("\n" Serial.printf_P(PSTR("\n"
@ -224,7 +221,6 @@ static void init_ardupilot()
main_menu.run(); main_menu.run();
} }
} }
#endif // CLI_ENABLED
if(g.log_bitmask != 0){ if(g.log_bitmask != 0){
@ -272,8 +268,6 @@ static void init_ardupilot()
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
} }
set_mode(MANUAL);
// set the correct flight mode // set the correct flight mode
// --------------------------- // ---------------------------
reset_control_switch(); reset_control_switch();
@ -282,10 +276,8 @@ static void init_ardupilot()
//******************************************************************************** //********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start //This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************** //********************************************************************************
static void startup_ground(void) void startup_ground(void)
{ {
set_mode(INITIALISING);
gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START")); gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
#if(GROUND_START_DELAY > 0) #if(GROUND_START_DELAY > 0)
@ -314,7 +306,7 @@ static void startup_ground(void)
trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. trim_radio(); // This was commented out as a HACK. Why? I don't find a problem.
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if (g.airspeed_enabled == true) if (airspeed_enabled == true)
{ {
// initialize airspeed sensor // initialize airspeed sensor
// -------------------------- // --------------------------
@ -357,7 +349,7 @@ else
gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
} }
static void set_mode(byte mode) void set_mode(byte mode)
{ {
if(control_mode == mode){ if(control_mode == mode){
// don't switch modes if we are already in the correct mode. // don't switch modes if we are already in the correct mode.
@ -371,11 +363,18 @@ static void set_mode(byte mode)
switch(control_mode) switch(control_mode)
{ {
case INITIALISING:
case MANUAL: case MANUAL:
break;
case CIRCLE: case CIRCLE:
break;
case STABILIZE: case STABILIZE:
break;
case FLY_BY_WIRE_A: case FLY_BY_WIRE_A:
break;
case FLY_BY_WIRE_B: case FLY_BY_WIRE_B:
break; break;
@ -391,8 +390,10 @@ static void set_mode(byte mode)
do_loiter_at_location(); do_loiter_at_location();
break; break;
case GUIDED: case TAKEOFF:
set_guided_WP(); break;
case LAND:
break; break;
default: default:
@ -407,7 +408,7 @@ static void set_mode(byte mode)
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);
} }
static void check_long_failsafe() void check_long_failsafe()
{ {
// only act on changes // only act on changes
// ------------------- // -------------------
@ -432,7 +433,7 @@ static void check_long_failsafe()
} }
} }
static void check_short_failsafe() void check_short_failsafe()
{ {
// only act on changes // only act on changes
// ------------------- // -------------------
@ -450,19 +451,20 @@ static void check_short_failsafe()
} }
static void startup_IMU_ground(void) void startup_IMU_ground(void)
{ {
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); uint16_t store = 0;
mavlink_delay(500); SendDebugln("<startup_IMU_ground> Warming up ADC...");
delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
// ----------------------- // -----------------------
demo_servos(2); demo_servos(2);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); SendDebugln("<startup_IMU_ground> Beginning IMU calibration; do not move plane");
mavlink_delay(1000); delay(1000);
imu.init(IMU::COLD_START, mavlink_delay); imu.init(IMU::COLD_START);
dcm.set_centripetal(1); dcm.set_centripetal(1);
// read Baro pressure at ground // read Baro pressure at ground
@ -477,7 +479,7 @@ static void startup_IMU_ground(void)
} }
static void update_GPS_light(void) void update_GPS_light(void)
{ {
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// --------------------------------------------------------------------- // ---------------------------------------------------------------------
@ -505,16 +507,15 @@ static void update_GPS_light(void)
} }
static void resetPerfData(void) { void resetPerfData(void) {
mainLoop_count = 0; mainLoop_count = 0;
G_Dt_max = 0; G_Dt_max = 0;
dcm.gyro_sat_count = 0; dcm.gyro_sat_count = 0;
imu.adc_constraints = 0; imu.adc_constraints = 0;
dcm.renorm_sqrt_count = 0; dcm.renorm_sqrt_count = 0;
dcm.renorm_blowup_count = 0; dcm.renorm_blowup_count = 0;
gps_fix_count = 0; gps_fix_count = 0;
pmTest1 = 0; perf_mon_timer = millis();
perf_mon_timer = millis();
} }
@ -525,7 +526,7 @@ static void resetPerfData(void) {
* be larger than HP or you'll be in big trouble! The smaller the gap, the more * be larger than HP or you'll be in big trouble! The smaller the gap, the more
* careful you need to be. Julian Gall 6 - Feb - 2009. * careful you need to be. Julian Gall 6 - Feb - 2009.
*/ */
static unsigned long freeRAM() { unsigned long freeRAM() {
uint8_t * heapptr, * stackptr; uint8_t * heapptr, * stackptr;
stackptr = (uint8_t *)malloc(4); // use stackptr temporarily stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
heapptr = stackptr; // save value of heap pointer heapptr = stackptr; // save value of heap pointer
@ -534,20 +535,3 @@ static unsigned long freeRAM() {
return stackptr - heapptr; return stackptr - heapptr;
} }
/*
map from a 8 bit EEPROM baud rate to a real baud rate
*/
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
{
switch (rate) {
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 111: return 111100;
case 115: return 115200;
}
Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
return default_baud;
}

135
ArduPlane/test.pde
View File

@ -1,6 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
// These are function definitions so the Menu can be constructed before the functions // These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler. // are defined below. Order matters to the compiler.
@ -28,7 +26,7 @@ static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
// and stores them in Flash memory, not RAM. // and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right. // User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details // See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] PROGMEM = { const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm}, {"pwm", test_radio_pwm},
{"radio", test_radio}, {"radio", test_radio},
{"failsafe", test_failsafe}, {"failsafe", test_failsafe},
@ -66,15 +64,14 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
// A Macro to create the Menu // A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands); MENU(test_menu, "test", test_menu_commands);
static int8_t int8_t
test_mode(uint8_t argc, const Menu::arg *argv) test_mode(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("Test Mode\n\n")); Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run(); test_menu.run();
return 0;
} }
static void print_hit_enter() void print_hit_enter()
{ {
Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
} }
@ -107,7 +104,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
// ---------------------------------------------------------- // ----------------------------------------------------------
read_radio(); read_radio();
Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.radio_in, g.channel_roll.radio_in,
g.channel_pitch.radio_in, g.channel_pitch.radio_in,
g.channel_throttle.radio_in, g.channel_throttle.radio_in,
@ -150,7 +147,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
set_servos(); set_servos_4();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.control_in, g.channel_roll.control_in,
@ -258,7 +255,7 @@ if (g.battery_monitoring == 4) {
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
set_servos(); set_servos_4();
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -318,11 +315,11 @@ test_wp(uint8_t argc, const Menu::arg *argv)
return (0); return (0);
} }
static void void
test_wp_print(struct Location *cmd, byte wp_index) test_wp_print(struct Location *cmd, byte index)
{ {
Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index, (int)index,
(int)cmd->id, (int)cmd->id,
(int)cmd->options, (int)cmd->options,
(int)cmd->p1, (int)cmd->p1,
@ -379,24 +376,20 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
if (!g.switch_enable) {
Serial.println_P(PSTR("dip switches disabled, using EEPROM"));
}
while(1){ while(1){
delay(100); delay(100);
update_servo_switches(); update_servo_switches();
if (g.mix_mode == 0) { if (mix_mode == 0) {
Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
(int)g.channel_roll.get_reverse(), (int)reverse_roll,
(int)g.channel_pitch.get_reverse(), (int)reverse_pitch,
(int)g.channel_rudder.get_reverse()); (int)reverse_rudder);
} else { } else {
Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
(int)g.reverse_elevons, (int)reverse_elevons,
(int)g.reverse_ch1_elevon, (int)reverse_ch1_elevon,
(int)g.reverse_ch2_elevon); (int)reverse_ch2_elevon);
} }
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -532,29 +525,13 @@ test_gyro(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)
{ {
if (!g.compass_enabled) {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
compass.set_orientation(MAG_ORIENTATION);
if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
return 0;
}
dcm.set_compass(&compass);
report_compass();
// we need the DCM initialised for this test
imu.init(IMU::COLD_START);
int counter = 0; int counter = 0;
if(g.compass_enabled) {
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter(); print_hit_enter();
while(1) { while(1){
delay(20); delay(20);
if (millis() - fast_loopTimer > 19) { if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer; delta_ms_fast_loop = millis() - fast_loopTimer;
@ -565,38 +542,40 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// --- // ---
dcm.update_DCM(G_Dt); dcm.update_DCM(G_Dt);
medium_loopCounter++; if(g.compass_enabled) {
if(medium_loopCounter == 5){ medium_loopCounter++;
compass.read(); // Read magnetometer if(medium_loopCounter == 5){
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading compass.read(); // Read magnetometer
compass.null_offsets(dcm.get_dcm_matrix()); compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
medium_loopCounter = 0; compass.null_offsets(dcm.get_dcm_matrix());
} medium_loopCounter = 0;
}
}
counter++; counter++;
if (counter>20) { if (counter>20) {
Vector3f maggy = compass.get_offsets(); Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100, (wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x, compass.mag_x,
compass.mag_y, compass.mag_y,
compass.mag_z, compass.mag_z,
maggy.x, maggy.x,
maggy.y, maggy.y,
maggy.z); maggy.z);
counter=0; counter=0;
} }
}
if (Serial.available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using }
// the CLI before you go flying. if(Serial.available() > 0){
Serial.println_P(PSTR("saving offsets")); return (0);
compass.save_offsets(); }
return (0); }
} else {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
} }
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS #endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
@ -609,11 +588,14 @@ test_mag(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv) test_airspeed(uint8_t argc, const Menu::arg *argv)
{ {
unsigned airspeed_ch = adc.Ch(AIRSPEED_CH);
// Serial.println(adc.Ch(AIRSPEED_CH)); // Serial.println(adc.Ch(AIRSPEED_CH));
Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch); if ((adc.Ch(AIRSPEED_CH) > 2000) && (adc.Ch(AIRSPEED_CH) < 2900)){
airspeed_enabled = false;
}else{
airspeed_enabled = true;
}
if (g.airspeed_enabled == false){ if (airspeed_enabled == false){
Serial.printf_P(PSTR("airspeed: ")); Serial.printf_P(PSTR("airspeed: "));
print_enabled(false); print_enabled(false);
return (0); return (0);
@ -632,8 +614,11 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
} }
if(Serial.available() > 0){
return (0);
} }
} }
}
} }
@ -684,5 +669,3 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
} }
} }
#endif // HIL_MODE == HIL_MODE_DISABLED #endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED