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

This is the real HEAD of the APM_Camera branch. Seams that lots of changes got lost in the SVN to GIT port

This commit is contained in:
Amilcar Lucas 2011-09-09 16:18:38 +02:00
parent f40c85a601
commit b7a0d8836a
32 changed files with 2253 additions and 1559 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduPlane/APM_Config.h
View File

@ -23,35 +23,13 @@
#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
// - Options to reduce code size -
// -------------------------------
// Disable text based terminal configuration
#define CLI_ENABLED DISABLED

23
ArduPlane/APM_Config.h.reference
View File

@ -210,7 +210,7 @@
// INPUT_VOLTAGE OPTIONAL
//
// In order to have accurate pressure and battery voltage readings, this
// value should be set to the voltage measured on the 5V rail on the oilpan.
// value should be set to the voltage measured at the processor.
//
// See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail.
//
@ -304,6 +304,19 @@
//#define FLIGHT_MODE_6 MANUAL
//
//////////////////////////////////////////////////////////////////////////////
// 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_FS_VALUE OPTIONAL
@ -361,7 +374,7 @@
// 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
// If XX_FAILSAFE_ACTION is 2 and the applicable failsafe occurs while in AUTO or LOITER
// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER
// 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
@ -372,8 +385,8 @@
// The default behaviour is to ignore failsafes in AUTO and LOITER modes.
//
//#define GCS_HEARTBEAT_FAILSAFE DISABLED
//#define SHORT_FAILSAFE_ACTION 2
//#define LONG_FAILSAFE_ACTION 2
//#define SHORT_FAILSAFE_ACTION 0
//#define LONG_FAILSAFE_ACTION 0
//////////////////////////////////////////////////////////////////////////////
@ -774,7 +787,7 @@
//
// Limits the slew rate of the throttle, in percent per second. Helps
// avoid sudden throttle changes, which can destabilise the aircraft.
// A setting of zero disables the feature.
// A setting of zero disables the feature. Range 1 to 100.
// Default is zero (disabled).
//
// P_TO_T OPTIONAL

373
ArduPlane/ArduPlane.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduPilotMega 2.2.0"
#define THISFIRMWARE "ArduPilotMega V2.23"
/*
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
@ -29,6 +29,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib
#include <SPI.h> // Arduino SPI lib
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
@ -38,10 +39,13 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID 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>
#include <AP_Camera.h> // Photo or video camera
#include <AP_Mount.h> // Camera mount
#define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <memcheck.h>
// Configuration
#include "config.h"
@ -70,12 +74,12 @@ FastSerialPort3(Serial3); // Telemetry port
//
// Global parameters are all contained within the 'g' class.
//
Parameters g;
static Parameters g;
////////////////////////////////////////////////////////////////////////////////
// prototypes
void update_events(void);
static void update_events(void);
////////////////////////////////////////////////////////////////////////////////
@ -92,23 +96,17 @@ void update_events(void);
//
// All GPS access should be through this pointer.
GPS *g_gps;
static GPS *g_gps;
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
#if HIL_MODE == HIL_MODE_DISABLED
// real sensors
AP_ADC_ADS7844 adc;
APM_BMP085_Class barometer;
// 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
static AP_ADC_ADS7844 adc;
static APM_BMP085_Class barometer;
static AP_Compass_HMC5843 compass(Parameters::k_param_compass);
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
@ -144,6 +142,7 @@ AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL);
#elif HIL_MODE == HIL_MODE_ATTITUDE
AP_ADC_HIL adc;
AP_DCM_HIL dcm;
AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used
@ -189,18 +188,31 @@ AP_IMU_Shim imu; // never used
GCS_Class gcs;
#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
////////////////////////////////////////////////////////////////////////////////
byte control_mode = MANUAL;
byte control_mode = INITIALISING;
byte oldSwitchPosition; // for remembering the control mode switch
bool inverted_flight = false;
const char *comma = ",";
static const char *comma = ",";
const char* flight_mode_strings[] = {
static const char* flight_mode_strings[] = {
"Manual",
"Circle",
"Stabilize",
@ -232,221 +244,196 @@ const char* flight_mode_strings[] = {
// Failsafe
// --------
int failsafe; // track which type of failsafe is being processed
bool ch3_failsafe;
byte crash_timer;
static int failsafe; // track which type of failsafe is being processed
static bool ch3_failsafe;
static byte crash_timer;
// Radio
// -----
uint16_t elevon1_trim = 1500; // TODO: handle in EEProm
uint16_t elevon2_trim = 1500;
uint16_t ch1_temp = 1500; // Used for elevon mixing
uint16_t ch2_temp = 1500;
int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
bool rc_override_active = false;
uint32_t rc_override_fs_timer = 0;
uint32_t ch3_failsafe_timer = 0;
static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm
static uint16_t elevon2_trim = 1500;
static uint16_t ch1_temp = 1500; // Used for elevon mixing
static uint16_t ch2_temp = 1500;
static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
static bool rc_override_active = false;
static uint32_t rc_override_fs_timer = 0;
static 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
// LED output
// ----------
bool GPS_light; // status of the GPS light
static bool GPS_light; // status of the GPS light
// GPS variables
// -------------
const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
float scaleLongUp = 1; // used to reverse longtitude scaling
float scaleLongDown = 1; // used to reverse longtitude scaling
byte ground_start_count = 5; // have we achieved first lock and set Home?
int ground_start_avg; // 5 samples to avg speed for ground start
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
static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
static float scaleLongUp = 1; // used to reverse longitude scaling
static float scaleLongDown = 1; // used to reverse longitude scaling
static 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
static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present
// Location & Navigation
// ---------------------
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
long hold_course = -1; // deg * 100 dir of plane
static 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
static 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
static 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
byte command_must_index; // current command memory location
byte command_may_index; // current command memory location
byte command_must_ID; // current command ID
byte command_may_ID; // current command ID
static byte command_must_index; // current command memory location
static byte command_may_index; // current command memory location
static byte command_must_ID; // current command ID
static byte command_may_ID; // current command ID
// Airspeed
// --------
int airspeed; // m/s * 100
int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
float airspeed_error; // m/s * 100
long energy_error; // energy state error (kinetic + potential) for altitude hold
long airspeed_energy_error; // kinetic portion of energy error
bool airspeed_enabled = false;
static 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
static float airspeed_error; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error
// Location Errors
// ---------------
long bearing_error; // deg * 100 : 0 to 36000
long altitude_error; // meters * 100 we are off in altitude
float crosstrack_error; // meters we are off trackline
static long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude
static float crosstrack_error; // meters we are off trackline
// Battery Sensors
// ---------------
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter
float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, 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 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
static 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
static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
float current_amps;
float current_total;
static float current_amps;
static float current_total;
// Airspeed Sensors
// ----------------
float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering
int airspeed_offset; // analog air pressure sensor while still
int airspeed_pressure; // airspeed as a pressure value
static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering
static int airspeed_pressure; // airspeed as a pressure value
// Barometer Sensor variables
// --------------------------
unsigned long abs_pressure;
static unsigned long abs_pressure;
// Altitude Sensor variables
// ----------------------
//byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int sonar_alt;
// flight mode specific
// --------------------
bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes.
bool land_complete;
long takeoff_altitude;
int landing_distance; // meters;
int landing_pitch; // pitch for landing set by commands
int takeoff_pitch;
static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes.
static bool land_complete;
static long takeoff_altitude;
// static int landing_distance; // meters;
static int landing_pitch; // pitch for landing set by commands
static int takeoff_pitch;
// Loiter management
// -----------------
long old_target_bearing; // deg * 100
int loiter_total; // deg : how many times to loiter * 360
int loiter_delta; // deg : how far we just turned
int loiter_sum; // deg : how far we have turned around a waypoint
long loiter_time; // millis : when we started LOITER mode
int loiter_time_max; // millis : how long to stay in LOITER mode
static long old_target_bearing; // deg * 100
static int loiter_total; // deg : how many times to loiter * 360
static int loiter_delta; // deg : how far we just turned
static int loiter_sum; // deg : how far we have turned around a waypoint
static long loiter_time; // millis : when we started LOITER mode
static int loiter_time_max; // millis : how long to stay in LOITER mode
// these are the values for navigation control functions
// ----------------------------------------------------
long nav_roll; // deg * 100 : target roll angle
long nav_pitch; // deg * 100 : target pitch angle
int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
static long nav_roll; // deg * 100 : target roll angle
static 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
// Waypoints
// ---------
long wp_distance; // meters - distance between plane and next waypoint
long wp_totalDistance; // meters - distance between old and next waypoint
byte next_wp_index; // Current active command index
static long wp_distance; // meters - distance between plane and next waypoint
static long wp_totalDistance; // meters - distance between old and next waypoint
// repeating event control
// -----------------------
byte event_id; // what to do - see defines
long event_timer; // when the event was asked for in ms
uint16_t event_delay; // how long to delay the next firing of event in millis
int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
int event_value; // per command value, such as PWM for servos
int event_undo_value; // the value used to cycle events (alternate value to event_value)
static byte event_id; // what to do - see defines
static 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
static 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
static int event_undo_value; // the value used to cycle events (alternate value to event_value)
// delay command
// --------------
long condition_value; // used in condition commands (eg delay, change alt, etc.)
long condition_start;
int condition_rate;
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start;
static int condition_rate;
// 3D Location vectors
// -------------------
struct Location home; // home location
struct Location prev_WP; // last waypoint
struct Location current_loc; // current location
struct Location next_WP; // next waypoint
struct Location next_command; // command preloaded
long target_altitude; // used for altitude management between waypoints
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 struct Location home; // home location
static struct Location prev_WP; // last waypoint
static struct Location current_loc; // current location
static struct Location next_WP; // next waypoint
static struct Location next_command; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static long target_altitude; // used for altitude management between waypoints
static long offset_altitude; // used for altitude management between waypoints
static bool home_is_set; // Flag for if we have g_gps lock and have set the home location
// IMU variables
// -------------
float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
// Performance monitoring
// ----------------------
long perf_mon_timer; // Metric based on accel gain deweighting
int G_Dt_max; // Max main loop cycle time in milliseconds
int gps_fix_count;
byte gcs_messages_sent;
static long perf_mon_timer; // Metric based on accel gain deweighting
static int G_Dt_max = 0; // Max main loop cycle time in milliseconds
static int gps_fix_count = 0;
static int pmTest1 = 0;
// 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
// --------------
unsigned long fast_loopTimer; // Time in miliseconds of main control loop
unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
int mainLoop_count;
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
static int mainLoop_count;
unsigned long medium_loopTimer; // Time in miliseconds of medium loop
byte medium_loopCounter; // Counters for branching from main control loop to slower loops
uint8_t delta_ms_medium_loop;
static unsigned long medium_loopTimer; // Time in miliseconds of medium loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
static uint8_t delta_ms_medium_loop;
byte slow_loopCounter;
byte superslow_loopCounter;
byte counter_one_herz;
static byte slow_loopCounter;
static byte superslow_loopCounter;
static byte counter_one_herz;
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
unsigned long elapsedTime; // for doing custom events
float load; // % MCU cycles used
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static float load; // % MCU cycles used
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
//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)
#if CAMERA == ENABLED
AP_Mount camera_mount(g_gps, &dcm);
//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)
#endif
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
void setup() {
memcheck_init();
init_ardupilot();
}
@ -491,7 +478,7 @@ void loop()
}
// Main loop 50Hz
void fast_loop()
static void fast_loop()
{
// This is the fast loop - we want it to execute at 50Hz if possible
// -----------------------------------------------------------------
@ -502,15 +489,22 @@ void fast_loop()
// ----------
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_short_failsafe();
// Read Airspeed
// -------------
if (airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) {
if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) {
read_airspeed();
} else if (airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
} else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
calc_airspeed_errors();
}
@ -550,7 +544,7 @@ void fast_loop()
// write out the servo PWM values
// ------------------------------
set_servos_4();
set_servos();
// XXX is it appropriate to be doing the comms below on the fast loop?
@ -579,9 +573,14 @@ void fast_loop()
// or be a "GCS fast loop" interface
}
void medium_loop()
static void medium_loop()
{
camera();
#if CAMERA == ENABLED
// TODO replace home with a POI coming from a MavLink message or command
camera_mount.set_GPS_target(home);
g.camera.trigger_pic_cleanup();
#endif
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
@ -614,7 +613,6 @@ Serial.println(tempaccel.z, DEC);
// This case performs some navigation computations
//------------------------------------------------
case 1:
medium_loopCounter++;
@ -638,6 +636,7 @@ Serial.println(tempaccel.z, DEC);
// Read altitude from sensors
// ------------------
update_alt();
if(g.sonar_enabled) sonar_alt = sonar.read();
// altitude smoothing
// ------------------
@ -698,7 +697,7 @@ Serial.println(tempaccel.z, DEC);
}
}
void slow_loop()
static void slow_loop()
{
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
@ -727,9 +726,9 @@ void slow_loop()
// Read Control Surfaces/Mix switches
// ----------------------------------
if (g.switch_enable) {
update_servo_switches();
}
update_servo_switches();
update_aux_servo_function();
break;
@ -737,19 +736,18 @@ void slow_loop()
slow_loopCounter = 0;
update_events();
// XXX this should be a "GCS slow loop" interface
#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
gcs.data_stream_send(1,5);
gcs.data_stream_send(3,5);
// send all requested output streams with rates requested
// between 1 and 5 Hz
// between 3 and 5 Hz
#else
gcs.send_message(MSG_LOCATION);
gcs.send_message(MSG_CPU_LOAD, load*100);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(1,5);
hil.data_stream_send(3,5);
#endif
@ -757,20 +755,26 @@ void slow_loop()
}
}
void one_second_loop()
static void one_second_loop()
{
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
// send a 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)
hil.send_message(MSG_HEARTBEAT);
hil.data_stream_send(1,3);
#endif
}
void update_GPS(void)
static void update_GPS(void)
{
g_gps->update();
update_GPS_light();
@ -822,7 +826,7 @@ void update_GPS(void)
}
}
void update_current_flight_mode(void)
static void update_current_flight_mode(void)
{
if(control_mode == AUTO){
crash_checker();
@ -835,7 +839,7 @@ void update_current_flight_mode(void)
nav_roll = 0;
}
if (airspeed_enabled == true)
if (g.airspeed_enabled == true)
{
calc_nav_pitch();
if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch;
@ -853,7 +857,7 @@ void update_current_flight_mode(void)
case MAV_CMD_NAV_LAND:
calc_nav_roll();
if (airspeed_enabled == true){
if (g.airspeed_enabled == true){
calc_nav_pitch();
calc_throttle();
}else{
@ -878,6 +882,7 @@ void update_current_flight_mode(void)
switch(control_mode){
case RTL:
case LOITER:
case GUIDED:
hold_course = -1;
crash_checker();
calc_nav_roll();
@ -891,6 +896,7 @@ void update_current_flight_mode(void)
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.
nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority
if (inverted_flight) nav_pitch = -nav_pitch;
break;
case FLY_BY_WIRE_B:
@ -900,7 +906,7 @@ void update_current_flight_mode(void)
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
if (airspeed_enabled == true)
if (g.airspeed_enabled == true)
{
airspeed_error = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) *
@ -949,7 +955,7 @@ void update_current_flight_mode(void)
}
}
void update_navigation()
static void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
@ -961,23 +967,18 @@ void update_navigation()
switch(control_mode){
case LOITER:
case RTL:
case GUIDED:
update_loiter();
calc_bearing_error();
break;
case RTL:
if(wp_distance <= ( g.loiter_radius + LOITER_RANGE) ) {
do_RTL();
}else{
update_crosstrack();
}
break;
}
}
}
void update_alt()
static void update_alt()
{
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
@ -990,6 +991,6 @@ void update_alt()
#endif
// Calculate new climb rate
if(medium_loopCounter == 0 && slow_loopCounter == 0)
add_altitude_data(millis() / 100, g_gps->altitude / 10);
//if(medium_loopCounter == 0 && slow_loopCounter == 0)
// add_altitude_data(millis() / 100, g_gps->altitude / 10);
}

133
ArduPlane/Attitude.pde
View File

@ -4,15 +4,14 @@
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************
void stabilize()
static void stabilize()
{
static byte temp = 0;
float ch1_inf = 1.0;
float ch2_inf = 1.0;
float ch4_inf = 1.0;
float speed_scaler;
if (airspeed_enabled == true){
if (g.airspeed_enabled == true){
if(airspeed > 0)
speed_scaler = (STANDARD_SPEED * 100) / airspeed;
else
@ -20,7 +19,7 @@ void stabilize()
speed_scaler = constrain(speed_scaler, 0.5, 2.0);
} else {
if (g.channel_throttle.servo_out > 0){
speed_scaler = 0.5 + (THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root
speed_scaler = 0.5 + ((float)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...
}else{
speed_scaler = 1.67;
@ -32,6 +31,16 @@ void stabilize()
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
// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10;
// Serial.printf_P(PSTR(" roll_sensor "));
@ -44,6 +53,10 @@ void stabilize()
fabs(dcm.roll_sensor * g.kff_pitch_compensation) +
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(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);
// Mix Stick input to allow users to override control surfaces
@ -99,7 +112,7 @@ void stabilize()
//#endif
}
void crash_checker()
static void crash_checker()
{
if(dcm.pitch_sensor < -4500){
crash_timer = 255;
@ -109,9 +122,9 @@ void crash_checker()
}
void calc_throttle()
static void calc_throttle()
{
if (airspeed_enabled == false) {
if (g.airspeed_enabled == false) {
int throttle_target = g.throttle_cruise + throttle_nudge;
// no airspeed sensor, we use nav pitch to determine the proper throttle output
@ -145,7 +158,7 @@ void calc_throttle()
// Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ----------------------------------------------------------------------------------------
void calc_nav_yaw(float speed_scaler)
static void calc_nav_yaw(float speed_scaler)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
Vector3f temp = imu.get_accel();
@ -160,11 +173,11 @@ void calc_nav_yaw(float speed_scaler)
}
void calc_nav_pitch()
static void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
if (airspeed_enabled == true) {
if (g.airspeed_enabled == true) {
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
} else {
nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav);
@ -175,7 +188,7 @@ void calc_nav_pitch()
#define YAW_DAMPENER 0
void calc_nav_roll()
static void calc_nav_roll()
{
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
@ -219,18 +232,22 @@ float roll_slew_limit(float servo)
/*****************************************
* Throttle slew limit
*****************************************/
/*float throttle_slew_limit(float throttle)
static void throttle_slew_limit()
{
static float last;
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;
return temp;
static int last = 1000;
if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit
float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000
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.
// Keeps outdated data out of our calculations
void reset_I(void)
static void reset_I(void)
{
g.pidNavRoll.reset_I();
g.pidNavPitchAirspeed.reset_I();
@ -242,11 +259,13 @@ void reset_I(void)
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void set_servos_4(void)
static void set_servos(void)
{
int flapSpeedSource = 0;
if(control_mode == MANUAL){
// do a direct pass through of radio values
if (mix_mode == 0){
if (g.mix_mode == 0){
g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
} else {
@ -255,21 +274,26 @@ void set_servos_4(void)
}
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in;
} else {
if (mix_mode == 0){
if (g.mix_mode == 0) {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) {
g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out;
g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm();
}
}else{
/*Elevon mode*/
float ch1;
float ch2;
ch1 = reverse_elevons * (g.channel_pitch.servo_out - g.channel_roll.servo_out);
ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (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 + (reverse_ch1_elevon * (ch1 * 500.0/ ROLL_SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (reverse_ch2_elevon * (ch2 * 500.0/ PITCH_SERVO_MAX));
g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
}
#if THROTTLE_OUT == 0
@ -281,55 +305,60 @@ void set_servos_4(void)
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
#if THROTTLE_REVERSE == 1
radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
#endif
*/
throttle_slew_limit();
}
if(control_mode <= FLY_BY_WIRE_B) {
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->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_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = 0;
} else if (flapSpeedSource > g.flap_2_speed) {
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_1_percent;
} else {
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_2_percent;
}
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// 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_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_4, g.channel_rudder.radio_out); // send to Servos
// Route configurable aux. functions to their respective servos
aux_servo_out(&g.rc_5, CH_5);
aux_servo_out(&g.rc_6, CH_6);
aux_servo_out(&g.rc_7, CH_7);
aux_servo_out(&g.rc_8, CH_8);
#endif
}
void demo_servos(byte i) {
static void demo_servos(byte i) {
while(i > 0){
gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
APM_RC.OutputCh(1, 1400);
delay(400);
mavlink_delay(400);
APM_RC.OutputCh(1, 1600);
delay(200);
mavlink_delay(200);
APM_RC.OutputCh(1, 1500);
delay(400);
#endif
mavlink_delay(400);
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;
}

8
ArduPlane/GCS.h
View File

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

486
ArduPlane/GCS_Mavlink.pde
View File

@ -4,6 +4,10 @@
#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) :
packet_drops(0),
@ -14,21 +18,21 @@ waypoint_receive_timeout(1000), // 1 second
// stream rates
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
{
// AP_VAR //ref //index, default, name
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
{
}
void
GCS_MAVLINK::init(BetterStream * port)
GCS_MAVLINK::init(FastSerial * port)
{
GCS_Class::init(port);
if (port == &Serial) { // to split hil vs gcs
@ -53,8 +57,6 @@ GCS_MAVLINK::update(void)
{
uint8_t c = comm_receive_ch(chan);
// Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
}
@ -62,11 +64,6 @@ GCS_MAVLINK::update(void)
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
_queued_send();
@ -87,41 +84,55 @@ void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
{
if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax))
send_message(MSG_RAW_IMU);
if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) {
send_message(MSG_EXTENDED_STATUS);
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
send_message(MSG_RAW_IMU1);
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_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
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);
//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
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_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);
//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);
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
}
if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
}
}
}
@ -129,7 +140,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
void
GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
{
mavlink_send_message(chan,id,param,packet_drops);
mavlink_send_message(chan,id, packet_drops);
}
void
@ -158,9 +169,9 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
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...)
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@ -168,13 +179,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_request_data_stream_t 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
if (packet.start_stop == 0) freq = 0; // stop sending
else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending
else break;
if (packet.start_stop == 0)
freq = 0; // stop sending
else if (packet.start_stop == 1)
freq = packet.req_message_rate; // start sending
else
break;
switch(packet.req_stream_id){
@ -188,6 +204,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
streamRateExtra2 = freq;
streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group.
break;
case MAV_DATA_STREAM_RAW_SENSORS:
streamRateRawSensors = freq; // 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.
@ -195,27 +212,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRateExtendedStatus.set_and_save(freq);
break;
case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels.set_and_save(freq);
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController.set_and_save(freq);
break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion.set_and_save(freq);
// break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion.set_and_save(freq);
// break;
case MAV_DATA_STREAM_POSITION:
streamRatePosition.set_and_save(freq);
break;
case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1.set_and_save(freq);
break;
case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2.set_and_save(freq);
break;
case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3.set_and_save(freq);
break;
default:
break;
}
@ -228,11 +253,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (mavlink_check_target(packet.target,packet.target_component)) break;
uint8_t result = 0;
// do action
send_text_P(SEVERITY_LOW,PSTR("action received: "));
send_text_P(SEVERITY_LOW,PSTR("action received: "));
//Serial.println(packet.action);
switch(packet.action){
@ -332,13 +357,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
default: break;
}
mavlink_msg_action_ack_send(
chan,
packet.action,
result
);
break;
}
@ -347,35 +372,38 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg, &packet);
switch(packet.mode){
case MAV_MODE_MANUAL:
set_mode(MANUAL);
break;
case MAV_MODE_GUIDED: //For future use
case MAV_MODE_GUIDED:
set_mode(GUIDED);
break;
case MAV_MODE_AUTO:
if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO);
if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL);
if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER);
mav_nav = 255;
break;
case MAV_MODE_TEST1:
set_mode(STABILIZE);
break;
case MAV_MODE_TEST2:
set_mode(FLY_BY_WIRE_A);
break;
case MAV_MODE_TEST3:
set_mode(FLY_BY_WIRE_B);
if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A);
if(mav_nav == 2) set_mode(FLY_BY_WIRE_B);
//if(mav_nav == 3) set_mode(FLY_BY_WIRE_C);
mav_nav = 255;
break;
}
}
case MAVLINK_MSG_ID_SET_NAV_MODE:
{
// decode
@ -385,7 +413,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mav_nav = packet.nav_mode;
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint request list"));
@ -393,7 +421,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_waypoint_request_list_t packet;
mavlink_msg_waypoint_request_list_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;
// Start sending waypoints
mavlink_msg_waypoint_count_send(
@ -410,6 +439,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
@ -421,28 +451,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_waypoint_request_t 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
tell_command = get_wp_with_index(packet.seq);
// set frame of waypoint
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
} else {
frame = MAV_FRAME_GLOBAL; // reference frame
}
float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
// time that the mav should loiter in milliseconds
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)
float x = 0, y = 0, z = 0;
if (tell_command.id < MAV_CMD_NAV_LAST) {
if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) {
// command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude)
@ -452,50 +489,63 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
}
}
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1*10; // APM loiter time is in ten second increments
break;
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1*10; // APM loiter time is in ten second increments
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
param1 = tell_command.p1;
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
x=0; // Clear fields loaded above that we don't want sent for this command
y=0;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command.lng;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command.lat;
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
param1 = tell_command.p1;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
}
case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command.lng;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command.lat;
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
mavlink_msg_waypoint_send(chan,msg->sysid,
msg->compid,packet.seq,frame,tell_command.id,current,autocontinue,
param1,param2,param3,param4,x,y,z);
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
}
mavlink_msg_waypoint_send(chan,msg->sysid,
msg->compid,
packet.seq,
frame,
tell_command.id,
current,
autocontinue,
param1,
param2,
param3,
param4,
x,
y,
z);
// update last waypoint comm stamp
waypoint_timelast_send = millis();
@ -511,9 +561,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_waypoint_ack_decode(msg, &packet);
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
waypoint_sending = false;
break;
@ -544,14 +591,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_waypoint_clear_all_t 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
uint8_t type = 0; // ok (0), error(1)
g.waypoint_total.set_and_save(0);
// send acknowledgement 3 times to makes sure it is received
for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
for (int i=0;i<3;i++)
mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type);
break;
}
@ -586,6 +634,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.count = MAX_WAYPOINTS;
}
g.waypoint_total.set_and_save(packet.count - 1);
waypoint_timelast_receive = millis();
waypoint_receiving = true;
waypoint_sending = false;
@ -593,55 +642,58 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS
case MAVLINK_MSG_ID_SET_MAG_OFFSETS:
{
mavlink_set_mag_offsets_t packet;
mavlink_msg_set_mag_offsets_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z));
break;
}
#endif
// XXX receive a WP from GCS and store in EEPROM
case MAVLINK_MSG_ID_WAYPOINT:
{
// Check if receiving waypiont
if (!waypoint_receiving) break;
// decode
mavlink_waypoint_t packet;
mavlink_msg_waypoint_decode(msg, &packet);
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
tell_command.id = packet.command;
switch (packet.frame)
{
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
tell_command.options = 0;
break;
}
switch (packet.frame)
{
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
tell_command.options = 0; // absolute altitude
break;
}
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2;
tell_command.options = 1;
break;
}
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.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2;
tell_command.options = 1;
break;
}
}
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
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.lng = 1.0e7 * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break;
}
}
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
@ -651,7 +703,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.p1 = packet.param1 * 100;
tell_command.lat = packet.param1;
break;
case MAV_CMD_NAV_LOITER_TIME:
@ -685,27 +737,53 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
set_wp_with_index(tell_command, packet.seq);
if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
guided_WP = tell_command;
// update waypoint receiving state machine
waypoint_timelast_receive = millis();
waypoint_request_i++;
// add home alt if needed
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){
guided_WP.alt += home.alt;
}
if (waypoint_request_i > g.waypoint_total)
{
uint8_t type = 0; // ok (0), error(1)
set_mode(GUIDED);
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
type);
// make any new wp uploaded instant (in case we are already in Guided mode)
set_guided_WP();
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
}
// verify we recevied the command
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
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;
}
@ -717,7 +795,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_param_set_t 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
@ -734,7 +814,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.
const float rounding_addition = 0.01;
float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
@ -747,12 +827,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
((AP_Float16 *)vp)->set_and_save(packet.param_value);
} 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);
} 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);
} 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);
} else {
// we don't support mavlink set on this parameter
@ -783,8 +864,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_rc_channels_override_t packet;
int16_t v[8];
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[1] = packet.chan2_raw;
v[2] = packet.chan3_raw;
@ -797,19 +880,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
rc_override_fs_timer = millis();
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// 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;
rc_override_fs_timer = millis();
pmTest1++;
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
// in HIL_ATTITUDE mode.
case MAVLINK_MSG_ID_GPS_RAW:
case MAVLINK_MSG_ID_GPS_RAW:
{
// decode
mavlink_gps_raw_t packet;
@ -951,7 +1035,7 @@ void
GCS_MAVLINK::_queued_send()
{
// Check to see if we are sending parameters
if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) {
if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) {
AP_Var *vp;
float value;
@ -984,9 +1068,10 @@ GCS_MAVLINK::_queued_send()
// request waypoints one by one
// XXX note that this is pan-interface
if (waypoint_receiving &&
(requested_interface == chan) &&
waypoint_request_i <= g.waypoint_total &&
(requested_interface == (unsigned)chan) &&
waypoint_request_i <= (unsigned)g.waypoint_total &&
mavdelay > 15) { // limits to 3.33 hz
mavlink_msg_waypoint_request_send(
chan,
waypoint_dest_sysid,
@ -997,5 +1082,66 @@ GCS_MAVLINK::_queued_send()
}
}
#endif
#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
static void send_rate(uint16_t low, uint16_t high) {
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(low, high);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(low,high);
#endif
}
/*
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
#include <BetterStream.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <GPS.h>
#include <Stream.h>
@ -41,7 +41,7 @@ public:
///
/// @param port The stream over which messages are exchanged.
///
void init(BetterStream *port) { _port = port; }
void init(FastSerial *port) { _port = port; }
/// Update HIL state.
///
@ -83,7 +83,7 @@ public:
protected:
/// The stream we are communicating over
BetterStream *_port;
FastSerial *_port;
};
//
@ -111,7 +111,7 @@ class HIL_XPLANE : public HIL_Class
public:
HIL_XPLANE();
void update(void);
void init(BetterStream *port);
void init(FastSerial *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str);

2
ArduPlane/HIL_Xplane.pde
View File

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

107
ArduPlane/Log.pde
View File

@ -1,5 +1,7 @@
// -*- 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 interact with the user to dump or erase logs
@ -10,7 +12,6 @@
// These are function definitions so the Menu can be constructed before the functions
// 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 erase_logs(uint8_t argc, const Menu::arg *argv);
static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
@ -27,13 +28,14 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv)
" enable <name>|all enable logging <name> or everything\n"
" disable <name>|all disable logging <name> or everything\n"
"\n"));
return 0;
}
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
const struct Menu::command log_menu_commands[] PROGMEM = {
static const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
@ -44,6 +46,8 @@ const struct Menu::command log_menu_commands[] PROGMEM = {
// A Macro to create the 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
print_log_menu(void)
{
@ -80,7 +84,7 @@ print_log_menu(void)
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
for(int i=1;i<last_log_num+1;i++) {
get_log_boundaries(last_log_num, i, log_start, log_end);
get_log_boundaries(i, log_start, log_end);
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"),
i, log_start, log_end);
}
@ -105,7 +109,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
return(-1);
}
get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
get_log_boundaries(dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
dump_log,
dump_log_start,
@ -113,6 +117,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Log read complete\n"));
return 0;
}
static int8_t
@ -133,6 +138,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
Serial.printf_P(PSTR("\nLog erased.\n"));
return 0;
}
static int8_t
@ -154,7 +160,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
// bits accordingly.
//
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~(bits = 0);
bits = ~0;
} else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST);
@ -178,14 +184,15 @@ select_logs(uint8_t argc, const Menu::arg *argv)
return(0);
}
int8_t
static int8_t
process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
byte get_num_logs(void)
static byte get_num_logs(void)
{
int page = 1;
byte data;
@ -224,14 +231,14 @@ byte get_num_logs(void)
}
// send the number of the last log?
void start_new_log(byte num_existing_logs)
static void start_new_log(byte num_existing_logs)
{
int start_pages[50] = {0,0,0};
int end_pages[50] = {0,0,0};
if(num_existing_logs > 0) {
for(int i=0;i<num_existing_logs;i++) {
get_log_boundaries(num_existing_logs, i+1,start_pages[i],end_pages[i]);
get_log_boundaries(i+1,start_pages[i],end_pages[i]);
}
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
}
@ -259,7 +266,7 @@ void start_new_log(byte num_existing_logs)
}
}
void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end_page)
static void get_log_boundaries(byte log_num, int & start_page, int & end_page)
{
int page = 1;
byte data;
@ -301,7 +308,7 @@ void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end
// Error condition if we reach here with page = 2 TO DO - report condition
}
int find_last_log_page(int bottom_page)
static int find_last_log_page(int bottom_page)
{
int top_page = 4096;
int look_page;
@ -311,7 +318,7 @@ int find_last_log_page(int bottom_page)
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
check = DataFlash.ReadLong();
if(check == 0xFFFFFFFF)
if(check == (long)0xFFFFFFFF)
top_page = look_page;
else
bottom_page = look_page;
@ -321,7 +328,7 @@ int find_last_log_page(int bottom_page)
// Write an attitude packet. Total length : 10 bytes
void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -333,7 +340,7 @@ void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
}
// Write a performance monitoring packet. Total length : 19 bytes
void Log_Write_Performance()
static void Log_Write_Performance()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -347,12 +354,13 @@ void Log_Write_Performance()
DataFlash.WriteByte(dcm.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(dcm.get_health() * 1000));
DataFlash.WriteInt(pmTest1);
DataFlash.WriteByte(END_BYTE);
}
// 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, struct Location *wp)
static void Log_Write_Cmd(byte num, struct Location *wp)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -366,7 +374,7 @@ void Log_Write_Cmd(byte num, struct Location *wp)
DataFlash.WriteByte(END_BYTE);
}
void Log_Write_Startup(byte type)
static void Log_Write_Startup(byte type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -388,7 +396,7 @@ void Log_Write_Startup(byte type)
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Control_Tuning()
static void Log_Write_Control_Tuning()
{
Vector3f accel = imu.get_accel();
@ -409,7 +417,7 @@ void Log_Write_Control_Tuning()
#endif
// Write a navigation tuning packet. Total length : 18 bytes
void Log_Write_Nav_Tuning()
static void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -425,7 +433,7 @@ void Log_Write_Nav_Tuning()
}
// Write a mode packet. Total length : 5 bytes
void Log_Write_Mode(byte mode)
static void Log_Write_Mode(byte mode)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -435,8 +443,8 @@ void Log_Write_Mode(byte mode)
}
// Write an GPS packet. Total length : 30 bytes
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_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)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -446,6 +454,7 @@ void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long
DataFlash.WriteByte(log_NumSats);
DataFlash.WriteLong(log_Lattitude);
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_gps_alt);
DataFlash.WriteLong(log_Ground_Speed);
@ -455,7 +464,7 @@ void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long
// Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Raw()
static void Log_Write_Raw()
{
Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel();
@ -476,7 +485,7 @@ void Log_Write_Raw()
}
#endif
void Log_Write_Current()
static void Log_Write_Current()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -489,7 +498,7 @@ void Log_Write_Current()
}
// Read a Current packet
void Log_Read_Current()
static void Log_Read_Current()
{
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
@ -499,7 +508,7 @@ void Log_Read_Current()
}
// Read an control tuning packet
void Log_Read_Control_Tuning()
static void Log_Read_Control_Tuning()
{
float logvar;
@ -515,7 +524,7 @@ void Log_Read_Control_Tuning()
}
// Read a nav tuning packet
void Log_Read_Nav_Tuning()
static 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
(float)((uint16_t)DataFlash.ReadInt())/100.0,
@ -528,7 +537,7 @@ void Log_Read_Nav_Tuning()
}
// Read a performance packet
void Log_Read_Performance()
static void Log_Read_Performance()
{
long pm_time;
int logvar;
@ -537,7 +546,7 @@ void Log_Read_Performance()
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
Serial.print(comma);
for (int y = 1; y < 9; y++) {
for (int y = 1; y <= 9; y++) {
if(y < 3 || y > 7){
logvar = DataFlash.ReadInt();
}else{
@ -550,7 +559,7 @@ void Log_Read_Performance()
}
// Read a command processing packet
void Log_Read_Cmd()
static void Log_Read_Cmd()
{
byte logvarb;
long logvarl;
@ -569,7 +578,7 @@ void Log_Read_Cmd()
Serial.println(" ");
}
void Log_Read_Startup()
static void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
@ -584,7 +593,7 @@ void Log_Read_Startup()
}
// Read an attitude packet
void Log_Read_Attitude()
static void Log_Read_Attitude()
{
Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
DataFlash.ReadInt(),
@ -593,21 +602,22 @@ void Log_Read_Attitude()
}
// Read a mode packet
void Log_Read_Mode()
static void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD:"));
Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
}
// Read a GPS packet
void Log_Read_GPS()
static void Log_Read_GPS()
{
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %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, %4.4f\n"),
DataFlash.ReadLong(),
(int)DataFlash.ReadByte(),
(int)DataFlash.ReadByte(),
(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,
@ -616,7 +626,7 @@ void Log_Read_GPS()
}
// Read a raw accel/gyro packet
void Log_Read_Raw()
static void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW:"));
@ -629,13 +639,19 @@ void Log_Read_Raw()
}
// Read the DataFlash log memory : Packet Parser
void Log_Read(int start_page, int end_page)
static void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step = 0;
int packet_count = 0;
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);
while (page < end_page && page != -1){
@ -712,4 +728,23 @@ void Log_Read(int start_page, int end_page)
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,21 +25,36 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
}
}
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
// 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)
{
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) {
case MSG_HEARTBEAT:
{
CHECK_PAYLOAD_SIZE(HEARTBEAT);
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
}
case MSG_EXTENDED_STATUS:
case MSG_EXTENDED_STATUS1:
{
CHECK_PAYLOAD_SIZE(SYS_STATUS);
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
@ -52,9 +67,14 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST2;
nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST3;
mode = MAV_MODE_TEST2;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
case AUTO:
mode = MAV_MODE_AUTO;
@ -68,6 +88,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER;
break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
@ -82,11 +106,20 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
battery_voltage * 1000,
battery_remaining,
packet_drops);
break;
break;
}
case MSG_EXTENDED_STATUS2:
{
CHECK_PAYLOAD_SIZE(MEMINFO);
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
break;
}
case MSG_ATTITUDE:
{
CHECK_PAYLOAD_SIZE(ATTITUDE);
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(
chan,
@ -102,6 +135,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_LOCATION:
{
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
@ -117,6 +151,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_NAV_CONTROLLER_OUTPUT:
{
if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
@ -133,6 +168,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_LOCAL_LOCATION:
{
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(
chan,
@ -148,6 +184,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_GPS_RAW:
{
CHECK_PAYLOAD_SIZE(GPS_RAW);
mavlink_msg_gps_raw_send(
chan,
timeStamp,
@ -164,6 +201,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_SERVO_OUT:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
@ -183,6 +221,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_RADIO_IN:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
@ -200,6 +239,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_RADIO_OUT:
{
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
@ -215,6 +255,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_VFR_HUD:
{
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
@ -227,10 +268,12 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU:
case MSG_RAW_IMU1:
{
CHECK_PAYLOAD_SIZE(RAW_IMU);
Vector3f accel = imu.get_accel();
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 gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(
@ -245,21 +288,42 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
compass.mag_x,
compass.mag_y,
compass.mag_z);
break;
}
/* This message is not working. Why?
case MSG_RAW_IMU2:
{
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
mavlink_msg_scaled_pressure_send(
chan,
timeStamp,
(float)barometer.Press/100.,
(float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw
(int)(barometer.Temp*100));
*/
chan,
timeStamp,
(float)barometer.Press/100.0,
(float)(barometer.Press-g.ground_pressure)/100.0,
(int)(barometer.Temp*10));
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;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
CHECK_PAYLOAD_SIZE(GPS_STATUS);
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
@ -273,6 +337,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_CURRENT_WAYPOINT:
{
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
@ -282,10 +347,77 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
default:
break;
}
return true;
}
#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of
// switch types in mavlink_try_send_message()
static struct mavlink_queue {
uint8_t deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// don't send status MAVLink messages for 1 second after
// bootup, to try to prevent Xbee bricking
return;
}
mavlink_msg_statustext_send(
chan,
severity,

847
ArduPlane/Parameters.h
View File

@ -1,381 +1,468 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 6;
//
// Parameter identities.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
// Misc
//
k_param_auto_trim,
k_param_switch_enable,
k_param_log_bitmask,
k_param_pitch_trim,
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
// 120: Fly-by-wire control
//
k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max,
//
// 140: Sensor parameters
//
k_param_IMU_calibration = 140,
k_param_altitude_mix,
k_param_airspeed_ratio,
k_param_ground_temperature,
k_param_ground_pressure,
k_param_compass_enabled,
k_param_compass,
k_param_battery_monitoring,
k_param_pack_capacity,
//
// 160: Navigation parameters
//
k_param_crosstrack_gain = 160,
k_param_crosstrack_entry_angle,
k_param_roll_limit,
k_param_pitch_limit_max,
k_param_pitch_limit_min,
k_param_airspeed_cruise,
k_param_RTL_altitude,
//
// 180: Radio settings
//
k_param_channel_roll = 180,
k_param_channel_pitch,
k_param_channel_throttle,
k_param_channel_yaw,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled,
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_flight_mode_channel,
k_param_flight_modes,
k_param_short_fs_action,
k_param_long_fs_action,
k_param_gcs_heartbeat_fs_enabled,
//
// 200: Feed-forward gains
//
k_param_kff_pitch_compensation = 200,
k_param_kff_rudder_mix,
k_param_kff_pitch_to_throttle,
k_param_kff_throttle_to_pitch,
//
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_waypoint_total,
k_param_waypoint_index,
k_param_waypoint_radius,
k_param_loiter_radius,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from commnd to roll command deviation from trim
// (bank to turn strategy)
//
k_param_heading_to_roll_PID = 240,
// Roll-to-servo PID:
// roll error from command to roll servo deviation from trim
// (tracks commanded bank angle)
//
k_param_roll_to_servo_PID,
//
// Pitch control
//
// Pitch-to-servo PID:
// pitch error from command to pitch servo deviation from trim
// (front-side strategy)
//
k_param_pitch_to_servo_PID,
// Airspeed-to-pitch PID:
// airspeed error from commnd to pitch servo deviation from trim
// (back-side strategy)
//
k_param_airspeed_to_pitch_PID,
//
// Yaw control
//
// Yaw-to-servo PID:
// yaw rate error from commnd to yaw servo deviation from trim
// (stabilizes dutch roll)
//
k_param_yaw_to_servo_PID,
//
// Throttle control
//
// Energy-to-throttle PID:
// total energy error from command to throttle servo deviation from trim
// (throttle back-side strategy alternative)
//
k_param_energy_to_throttle_PID,
// Altitude-to-pitch PID:
// altitude error from commnd to pitch servo deviation from trim
// (throttle front-side strategy alternative)
//
k_param_altitude_to_pitch_PID,
// 255: reserved
};
AP_Int16 format_version;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
// Feed-forward gains
//
AP_Float kff_pitch_compensation;
AP_Float kff_rudder_mix;
AP_Float kff_pitch_to_throttle;
AP_Float kff_throttle_to_pitch;
// Crosstrack navigation
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
// Estimation
//
AP_Float altitude_mix;
AP_Float airspeed_ratio;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 waypoint_total;
AP_Int8 waypoint_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
// Fly-by-wire
//
AP_Int8 flybywire_airspeed_min;
AP_Int8 flybywire_airspeed_max;
// Throttle
//
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value;
AP_Int8 throttle_cruise;
// Failsafe
AP_Int8 short_fs_action;
AP_Int8 long_fs_action;
AP_Int8 gcs_heartbeat_fs_enabled;
// Flight modes
//
AP_Int8 flight_mode_channel;
AP_VarA<uint8_t,6> flight_modes;
// Navigational maneuvering limits
//
AP_Int16 roll_limit;
AP_Int16 pitch_limit_max;
AP_Int16 pitch_limit_min;
// Misc
//
AP_Int8 auto_trim;
AP_Int8 switch_enable;
AP_Int16 log_bitmask;
AP_Int16 airspeed_cruise;
AP_Int16 pitch_trim;
AP_Int16 RTL_altitude;
AP_Int16 ground_temperature;
AP_Int32 ground_pressure;
AP_Int8 compass_enabled;
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_Int16 pack_capacity; // Battery pack capacity less reserve
// RC channels
RC_Channel channel_roll;
RC_Channel channel_pitch;
RC_Channel channel_throttle;
RC_Channel channel_rudder;
RC_Channel rc_5;
RC_Channel rc_6;
RC_Channel rc_7;
RC_Channel rc_8;
RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll;
// PID controllers
//
PID pidNavRoll;
PID pidServoRoll;
PID pidServoPitch;
PID pidNavPitchAirspeed;
PID pidServoRudder;
PID pidTeThrottle;
PID pidNavPitchAltitude;
uint8_t junk;
// Note: keep initializers here in the same order as they are declared above.
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, NULL),
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")),
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_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")),
kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")),
crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")),
altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")),
airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")),
/* XXX waypoint_mode missing here */
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")),
flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")),
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
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_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")),
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")),
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")),
flight_modes (k_param_flight_modes, PSTR("FLTMODE")),
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_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")),
auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")),
switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")),
log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")),
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")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
// RC channel group key name
//----------------------------------------------------------------------
channel_roll (k_param_channel_roll, PSTR("RC1_")),
channel_pitch (k_param_channel_pitch, PSTR("RC2_")),
channel_throttle (k_param_channel_throttle, PSTR("RC3_")),
channel_rudder (k_param_channel_yaw, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
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
//---------------------------------------------------------------------------------------------------------------------------------------
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),
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("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),
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),
junk(0) // XXX just so that we can add things without worrying about the trailing comma
{
}
};
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 11;
// 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.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
// Misc
//
k_param_auto_trim,
k_param_switch_enable,
k_param_log_bitmask,
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
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
// 120: Fly-by-wire control
//
k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max,
//
// 130: Sensor parameters
//
k_param_IMU_calibration = 130,
k_param_altitude_mix,
k_param_airspeed_ratio,
k_param_ground_temperature,
k_param_ground_pressure,
k_param_compass_enabled,
k_param_compass,
k_param_battery_monitoring,
k_param_pack_capacity,
k_param_airspeed_offset,
k_param_sonar_enabled,
k_param_airspeed_enabled,
//
// 150: Navigation parameters
//
k_param_crosstrack_gain = 150,
k_param_crosstrack_entry_angle,
k_param_roll_limit,
k_param_pitch_limit_max,
k_param_pitch_limit_min,
k_param_airspeed_cruise,
k_param_RTL_altitude,
k_param_inverted_flight_ch,
//
// Camera parameters
//
#if CAMERA == ENABLED
k_param_camera,
#endif
//
// 170: Radio settings
//
k_param_channel_roll = 170,
k_param_channel_pitch,
k_param_channel_throttle,
k_param_channel_yaw,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled,
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_short_fs_action,
k_param_long_fs_action,
k_param_gcs_heartbeat_fs_enabled,
k_param_throttle_slewrate,
//
// 200: Feed-forward gains
//
k_param_kff_pitch_compensation = 200,
k_param_kff_rudder_mix,
k_param_kff_pitch_to_throttle,
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
//
k_param_waypoint_mode = 220,
k_param_waypoint_total,
k_param_waypoint_index,
k_param_waypoint_radius,
k_param_loiter_radius,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from command to roll command deviation from trim
// (bank to turn strategy)
//
k_param_heading_to_roll_PID = 240,
// Roll-to-servo PID:
// roll error from command to roll servo deviation from trim
// (tracks commanded bank angle)
//
k_param_roll_to_servo_PID,
//
// Pitch control
//
// Pitch-to-servo PID:
// pitch error from command to pitch servo deviation from trim
// (front-side strategy)
//
k_param_pitch_to_servo_PID,
// Airspeed-to-pitch PID:
// airspeed error from command to pitch servo deviation from trim
// (back-side strategy)
//
k_param_airspeed_to_pitch_PID,
//
// Yaw control
//
// Yaw-to-servo PID:
// yaw rate error from command to yaw servo deviation from trim
// (stabilizes dutch roll)
//
k_param_yaw_to_servo_PID,
//
// Throttle control
//
// Energy-to-throttle PID:
// total energy error from command to throttle servo deviation from trim
// (throttle back-side strategy alternative)
//
k_param_energy_to_throttle_PID,
// Altitude-to-pitch PID:
// altitude error from command to pitch servo deviation from trim
// (throttle front-side strategy alternative)
//
k_param_altitude_to_pitch_PID,
// 254,255: reserved
};
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
// Feed-forward gains
//
AP_Float kff_pitch_compensation;
AP_Float kff_rudder_mix;
AP_Float kff_pitch_to_throttle;
AP_Float kff_throttle_to_pitch;
// Crosstrack navigation
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
// Estimation
//
AP_Float altitude_mix;
AP_Float airspeed_ratio;
AP_Int16 airspeed_offset;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 waypoint_total;
AP_Int8 waypoint_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
// Fly-by-wire
//
AP_Int8 flybywire_airspeed_min;
AP_Int8 flybywire_airspeed_max;
// Throttle
//
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_slewrate;
AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value;
AP_Int8 throttle_cruise;
// Failsafe
AP_Int8 short_fs_action;
AP_Int8 long_fs_action;
AP_Int8 gcs_heartbeat_fs_enabled;
// Flight modes
//
AP_Int8 flight_mode_channel;
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
// Navigational maneuvering limits
//
AP_Int16 roll_limit;
AP_Int16 pitch_limit_max;
AP_Int16 pitch_limit_min;
// Misc
//
AP_Int8 auto_trim;
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 airspeed_cruise;
AP_Int16 pitch_trim;
AP_Int16 RTL_altitude;
AP_Int16 ground_temperature;
AP_Int32 ground_pressure;
AP_Int8 compass_enabled;
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_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;
// Camera
#if CAMERA == ENABLED
AP_Camera camera;
#endif
// RC channels
RC_Channel channel_roll;
RC_Channel channel_pitch;
RC_Channel channel_throttle;
RC_Channel channel_rudder;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
// PID controllers
//
PID pidNavRoll;
PID pidServoRoll;
PID pidServoPitch;
PID pidNavPitchAirspeed;
PID pidServoRudder;
PID pidTeThrottle;
PID pidNavPitchAltitude;
uint8_t junk;
// Note: keep initializers here in the same order as they are declared above.
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")),
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_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_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")),
kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")),
kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")),
crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")),
altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")),
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 */
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")),
flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")),
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
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_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
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")),
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")),
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_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")),
pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")),
pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")),
auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")),
switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")),
mix_mode (ELEVON_MIXING, k_param_mix_mode, PSTR("ELEVON_MIXING")),
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")),
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")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
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")),
flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")),
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")),
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
#if CAMERA == ENABLED
camera (k_param_camera, PSTR("CAM_")),
#endif
// RC channel group key name
//----------------------------------------------------------------------
channel_roll (k_param_channel_roll, PSTR("RC1_")),
channel_pitch (k_param_channel_pitch, PSTR("RC2_")),
channel_throttle (k_param_channel_throttle, PSTR("RC3_")),
channel_rudder (k_param_channel_yaw, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
// 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),
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),
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),
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),
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),
junk(0) // XXX just so that we can add things without worrying about the trailing comma
{
}
};
#endif // PARAMETERS_H

39
ArduPlane/WP_activity.pde
View File

@ -1,49 +1,56 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CAMERA == ENABLED
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();
if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you
g.camera.trigger_pic();
} // DO SOMETHIMNG
}
if(g.waypoint_index == 20){ // When here do whats underneath
servo_pic();
g.camera.trigger_pic();
}
}
void picture_time_check()
{
if (picture_time == 1){
if (wp_distance < 10){
servo_pic(); // or any camera activation command
if (g.camera.picture_time == 1){
if (wp_distance < g.camera.wp_distance_min){
g.camera.trigger_pic(); // or any camera activation command
}
}
}
#endif
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_rc_function[RC_Channel_aux::k_egg_drop])
{
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));
if(g.waypoint_index == 3){
if(wp_distance < egg_dist){
g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (-45*10.31);
}
}else{
g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (45*10.31);
}
}else{
APM_RC.OutputCh(CH_RUDDER,1500 + (45*10.31));
}
}
#if CAMERA == ENABLED
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(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you
g.camera.trigger_pic();
}
}
if(g.waypoint_index == 20){ // When here do whats underneath
servo_pic();
g.camera.trigger_pic();
}
}
#endif

44
ArduPlane/climb_rate.pde
View File

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

103
ArduPlane/commands.pde
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
void init_commands()
static void init_commands()
{
//read_EEPROM_waypoint_info();
g.waypoint_index.set_and_save(0);
@ -9,15 +9,19 @@ void init_commands()
next_command.id = CMD_BLANK;
}
void update_auto()
static void update_auto()
{
if (g.waypoint_index == g.waypoint_total) {
do_RTL();
if (g.waypoint_index >= g.waypoint_total) {
handle_no_commands();
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
void reload_commands_airstart()
static void reload_commands_airstart()
{
init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
@ -26,7 +30,7 @@ void reload_commands_airstart()
// Getters
// -------
struct Location get_wp_with_index(int i)
static struct Location get_wp_with_index(int i)
{
struct Location temp;
long mem;
@ -66,12 +70,13 @@ struct Location get_wp_with_index(int i)
// Setters
// -------
void set_wp_with_index(struct Location temp, int i)
static void set_wp_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
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){
temp.options = MASK_OPTIONS_RELATIVE_ALT;
} else {
@ -96,26 +101,26 @@ void set_wp_with_index(struct Location temp, int i)
eeprom_write_dword((uint32_t *) mem, temp.lng);
}
void increment_WP_index()
static 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);
SendDebug_P("MSG <increment_WP_index> WP index is incremented to ");
SendDebug_P("MSG - WP index is incremented to ");
}else{
//SendDebug_P("MSG <increment_WP_index> Failed to increment WP index of ");
// This message is used excessively at the end of a mission
}
SendDebugln(g.waypoint_index,DEC);
//SendDebugln(g.waypoint_index,DEC);
}
void decrement_WP_index()
static void decrement_WP_index()
{
if (g.waypoint_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1);
}
}
long read_alt_to_hold()
static long read_alt_to_hold()
{
if(g.RTL_altitude < 0)
return current_loc.alt;
@ -124,30 +129,14 @@ 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
It looks to see what the next command type is and finds the last command.
*/
void set_next_WP(struct Location *wp)
static void set_next_WP(struct Location *wp)
{
//gcs.send_text_P(SEVERITY_LOW,PSTR("load WP"));
SendDebug_P("MSG <set_next_wp> wp_index: ");
SendDebug_P("MSG - wp_index: ");
SendDebugln(g.waypoint_index, DEC);
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -173,6 +162,44 @@ void set_next_WP(struct Location *wp)
loiter_sum = 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
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
@ -190,15 +217,13 @@ void set_next_WP(struct Location *wp)
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
gcs.print_current_waypoints();
}
// run this at setup on the ground
// -------------------------------
void init_home()
{
SendDebugln("MSG: <init_home> init home");
SendDebugln("MSG: init home");
// block until we get a good fix
// -----------------------------
@ -221,6 +246,12 @@ void init_home()
// Save prev loc
// -------------
next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
}

108
ArduPlane/commands_logic.pde
View File

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

20
ArduPlane/commands_process.pde
View File

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

104
ArduPlane/config.h
View File

@ -46,13 +46,31 @@
// AIRSPEED_SENSOR
// AIRSPEED_RATIO
//
//#ifndef AIRSPEED_SENSOR
//# define AIRSPEED_SENSOR DISABLED
//#endif
#ifndef AIRSPEED_SENSOR
# define AIRSPEED_SENSOR DISABLED
#endif
#ifndef AIRSPEED_RATIO
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
#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_MODE OPTIONAL
@ -169,9 +187,6 @@
#ifndef MAG_ORIENTATION
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#endif
#ifndef MAG_PROTOCOL
# define MAG_PROTOCOL MAG_PROTOCOL_5843
#endif
//////////////////////////////////////////////////////////////////////////////
@ -184,8 +199,7 @@
//////////////////////////////////////////////////////////////////////////////
// Radio channel limits
//
// Note that these are not called out in APM_Config.h.example as there
// is currently no configured behaviour for these channels.
// Note that these are not called out in APM_Config.h.reference.
//
#ifndef CH5_MIN
# define CH5_MIN 1000
@ -213,6 +227,18 @@
#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_CHANNEL
@ -256,14 +282,14 @@
#ifndef THROTTLE_FAILSAFE
# define THROTTLE_FAILSAFE ENABLED
#endif
#ifndef THROTTE_FS_VALUE
#ifndef THROTTLE_FS_VALUE
# define THROTTLE_FS_VALUE 950
#endif
#ifndef SHORT_FAILSAFE_ACTION
# define SHORT_FAILSAFE_ACTION 2
# define SHORT_FAILSAFE_ACTION 0
#endif
#ifndef LONG_FAILSAFE_ACTION
# define LONG_FAILSAFE_ACTION 2
# define LONG_FAILSAFE_ACTION 0
#endif
#ifndef GCS_HEARTBEAT_FAILSAFE
# define GCS_HEARTBEAT_FAILSAFE DISABLED
@ -330,6 +356,22 @@
# define REVERSE_SWITCH ENABLED
#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
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
@ -556,6 +598,12 @@
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
#ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED
#endif
@ -587,6 +635,22 @@
# define LOG_CUR DISABLED
#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
# define DEBUG_PORT 0
#endif
@ -633,6 +697,10 @@
# define USE_CURRENT_ALT FALSE
#endif
#ifndef INVERTED_FLIGHT_PWM
# define INVERTED_FLIGHT_PWM 1750
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
@ -643,3 +711,17 @@
#endif
#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,9 +1,11 @@
void read_control_switch()
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void read_control_switch()
{
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
set_mode(g.flight_modes[switchPosition]);
set_mode(flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
prev_WP = current_loc;
@ -12,9 +14,15 @@ void read_control_switch()
// -------------------------
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);
}
}
byte readSwitch(void){
static byte readSwitch(void){
uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
@ -24,7 +32,7 @@ byte readSwitch(void){
return 0;
}
void reset_control_switch()
static void reset_control_switch()
{
oldSwitchPosition = 0;
read_control_switch();
@ -32,52 +40,23 @@ void reset_control_switch()
SendDebugln(oldSwitchPosition , DEC);
}
void update_servo_switches()
static 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 elevon
mix_mode = (PINL & 128) ? 1 : 0;
if (mix_mode == 0) {
reverse_roll = (PINE & 128) ? true : false;
reverse_pitch = (PINE & 64) ? true : false;
reverse_rudder = (PINL & 64) ? true : false;
g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode
if (g.mix_mode == 0) {
g.channel_roll.set_reverse((PINE & 128) ? true : false);
g.channel_pitch.set_reverse((PINE & 64) ? true : false);
g.channel_rudder.set_reverse((PINL & 64) ? true : false);
} else {
reverse_elevons = (PINE & 128) ? -1 : 1;
reverse_ch1_elevon = (PINE & 64) ? -1 : 1;
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;
}
g.reverse_elevons = (PINE & 128) ? true : false;
g.reverse_ch1_elevon = (PINE & 64) ? true : false;
g.reverse_ch2_elevon = (PINL & 64) ? true : false;
}
}

47
ArduPlane/defines.h
View File

@ -10,11 +10,9 @@
#define DEBUG 0
#define LOITER_RANGE 60 // for calculating power outside of loiter radius
#define ROLL_SERVO_MAX 4500
#define PITCH_SERVO_MAX 4500
#define RUDDER_SERVO_MAX 4500
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
// failsafe
// failsafe
// ----------------------
#define FAILSAFE_NONE 0
#define FAILSAFE_SHORT 1
@ -32,10 +30,6 @@
#define T6 1000000
#define T7 10000000
//MAGNETOMETER
#define MAG_PROTOCOL_5843 0
#define MAG_PROTOCOL_5883L 1
// GPS type codes - use the names, not the numbers
#define GPS_PROTOCOL_NONE -1
#define GPS_PROTOCOL_NMEA 0
@ -90,12 +84,15 @@
#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
// Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0
#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 and Fly By Wire C require airspeed sensor
#define AUTO 10
#define RTL 11
#define LOITER 12
#define TAKEOFF 13
#define LAND 14
//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM
//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM
#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
@ -118,6 +115,10 @@
#define MAV_CMD_CONDITION_YAW 23
// 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_HEARTBEAT 0x01
#define MSG_ATTITUDE 0x02
@ -128,9 +129,10 @@
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
#define MSG_VERSION_REQUEST 0x08
#define MSG_VERSION 0x09
#define MSG_EXTENDED_STATUS 0x0a
#define MSG_CPU_LOAD 0x0b
#define MSG_NAV_CONTROLLER_OUTPUT 0x0c
#define MSG_EXTENDED_STATUS1 0x0a
#define MSG_EXTENDED_STATUS2 0x0b
#define MSG_CPU_LOAD 0x0c
#define MSG_NAV_CONTROLLER_OUTPUT 0x0d
#define MSG_COMMAND_REQUEST 0x20
#define MSG_COMMAND_UPLOAD 0x21
@ -153,9 +155,11 @@
#define MSG_RADIO_OUT 0x53
#define MSG_RADIO_IN 0x54
#define MSG_RAW_IMU 0x60
#define MSG_GPS_STATUS 0x61
#define MSG_GPS_RAW 0x62
#define MSG_RAW_IMU1 0x60
#define MSG_RAW_IMU2 0x61
#define MSG_RAW_IMU3 0x62
#define MSG_GPS_STATUS 0x63
#define MSG_GPS_RAW 0x64
#define MSG_SERVO_OUT 0x70
@ -173,6 +177,7 @@
#define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4
#define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1
#define SEVERITY_MEDIUM 2
@ -205,7 +210,6 @@
#define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
#define MASK_LOG_SET_DEFAULTS (1<<15)
// Waypoint Modes
// ----------------
@ -246,7 +250,11 @@
// sonar
#define MAX_SONAR_XL 0
#define MAX_SONAR_LV 1
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
#define AN4 4
#define AN5 5
// Hardware Parameters
#define SLIDE_SWITCH_PIN 40
@ -267,3 +275,6 @@
#define ONBOARD_PARAM_NAME_LENGTH 15
#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 -*-
void failsafe_short_on_event()
static void failsafe_short_on_event()
{
// This is how to handle a short loss of control signal failsafe.
failsafe = FAILSAFE_SHORT;
@ -18,7 +18,7 @@ void failsafe_short_on_event()
case AUTO:
case LOITER:
if(g.short_fs_action != 1) {
if(g.short_fs_action == 1) {
set_mode(RTL);
}
break;
@ -32,10 +32,11 @@ void failsafe_short_on_event()
SendDebugln(control_mode, DEC);
}
void failsafe_long_on_event()
static void failsafe_long_on_event()
{
// This is how to handle a short loss of control signal failsafe.
// This is how to handle a long loss of control signal failsafe.
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)
{
case MANUAL:
@ -48,7 +49,7 @@ void failsafe_long_on_event()
case AUTO:
case LOITER:
case CIRCLE:
if(g.long_fs_action != 1) {
if(g.long_fs_action == 1) {
set_mode(RTL);
}
break;
@ -59,7 +60,7 @@ void failsafe_long_on_event()
}
}
void failsafe_short_off_event()
static void failsafe_short_off_event()
{
// We're back in radio contact
SendDebug_P("Failsafe - Short event off");
@ -74,15 +75,16 @@ void failsafe_short_off_event()
reset_I();
}
void low_battery_event(void)
#if BATTERY_EVENT == ENABLED
static void low_battery_event(void)
{
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
}
#endif
void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
static 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)
return;
@ -108,17 +110,17 @@ void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPE
}
}
void relay_on()
static void relay_on()
{
PORTL |= B00000100;
}
void relay_off()
static void relay_off()
{
PORTL &= ~B00000100;
}
void relay_toggle()
static void relay_toggle()
{
PORTL ^= B00000100;
}

57
ArduPlane/navigation.pde
View File

@ -3,7 +3,7 @@
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
void navigate()
static void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
@ -63,7 +63,7 @@ void calc_distance_error()
}
#endif
void calc_airspeed_errors()
static void calc_airspeed_errors()
{
// XXX excess casting here
if(control_mode>=AUTO && airspeed_nudge > 0) {
@ -75,7 +75,7 @@ void calc_airspeed_errors()
}
}
void calc_bearing_error()
static void calc_bearing_error()
{
if(takeoff_complete == true || g.compass_enabled == true) {
bearing_error = nav_bearing - dcm.yaw_sensor;
@ -89,7 +89,7 @@ void calc_bearing_error()
bearing_error = wrap_180(bearing_error);
}
void calc_altitude_error()
static void calc_altitude_error()
{
if(control_mode == AUTO && offset_altitude != 0) {
// limit climb rates
@ -127,38 +127,39 @@ void calc_altitude_error()
altitude_error = target_altitude - current_loc.alt;
}
long wrap_360(long error)
static long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
long wrap_180(long error)
static long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
void update_loiter()
static void update_loiter()
{
float power;
if(wp_distance <= 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));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0, 1);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
/*
if (wp_distance < g.loiter_radius){
nav_bearing += 9000;
}else{
@ -166,35 +167,27 @@ void update_loiter()
}
update_crosstrack();
*/
nav_bearing = wrap_360(nav_bearing);
}
void update_crosstrack(void)
static void update_crosstrack(void)
{
// 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
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)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 = wrap_360(nav_bearing);
}
}
void reset_crosstrack()
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
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)
static long get_distance(struct Location *loc1, struct Location *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
@ -205,12 +198,7 @@ long get_distance(struct Location *loc1, struct Location *loc2)
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
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)
static long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
@ -218,14 +206,3 @@ long get_bearing(struct Location *loc1, struct Location *loc2)
if (bearing < 0) bearing += 36000;
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;
}

9
ArduPlane/planner.pde
View File

@ -8,18 +8,19 @@ static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv);
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
const struct Menu::command planner_menu_commands[] PROGMEM = {
static const struct Menu::command planner_menu_commands[] PROGMEM = {
{"gcs", planner_gcs},
};
// A Macro to create the Menu
MENU(planner_menu, "planner", planner_menu_commands);
int8_t
static int8_t
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"));
planner_menu.run();
return 0;
}
static int8_t
planner_gcs(uint8_t argc, const Menu::arg *argv)
@ -44,4 +45,6 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
loopcount++;
}
}
}
return 0;
}

139
ArduPlane/radio.pde
View File

@ -2,21 +2,18 @@
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
void init_rc_in()
static void init_rc_in()
{
// set rc reversing
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
g.channel_roll.set_angle(ROLL_SERVO_MAX);
g.channel_pitch.set_angle(PITCH_SERVO_MAX);
g.channel_rudder.set_angle(RUDDER_SERVO_MAX);
g.channel_roll.set_angle(SERVO_MAX);
g.channel_pitch.set_angle(SERVO_MAX);
g.channel_rudder.set_angle(SERVO_MAX);
g.channel_throttle.set_range(0, 100);
// set rc dead zones
@ -26,13 +23,49 @@ void init_rc_in()
g.channel_throttle.dead_zone = 6;
//set auxiliary ranges
g.rc_5.set_range(0,1000);
g.rc_6.set_range(0,1000);
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
if (g_rc_function[RC_Channel_aux::k_mount_yaw]) {
g_rc_function[RC_Channel_aux::k_mount_yaw]->set_range(
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10,
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10);
}
if (g_rc_function[RC_Channel_aux::k_mount_pitch]) {
g_rc_function[RC_Channel_aux::k_mount_pitch]->set_range(
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10,
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10);
}
if (g_rc_function[RC_Channel_aux::k_mount_roll]) {
g_rc_function[RC_Channel_aux::k_mount_roll]->set_range(
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10,
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10);
}
if (g_rc_function[RC_Channel_aux::k_cam_trigger]) {
g_rc_function[RC_Channel_aux::k_cam_trigger]->set_range(
g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10,
g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10);
}
if (g_rc_function[RC_Channel_aux::k_cam_open]) {
g_rc_function[RC_Channel_aux::k_cam_open]->set_range(
g_rc_function[RC_Channel_aux::k_cam_open]->angle_min / 10,
g_rc_function[RC_Channel_aux::k_cam_open]->angle_max / 10);
}
if (g_rc_function[RC_Channel_aux::k_flap]) {
g_rc_function[RC_Channel_aux::k_flap]->set_range(0,100);
}
if (g_rc_function[RC_Channel_aux::k_flap_auto]) {
g_rc_function[RC_Channel_aux::k_flap_auto]->set_range(0,100);
}
if (g_rc_function[RC_Channel_aux::k_aileron]) {
g_rc_function[RC_Channel_aux::k_aileron]->set_angle(SERVO_MAX);
}
if (g_rc_function[RC_Channel_aux::k_flaperon]) {
g_rc_function[RC_Channel_aux::k_flaperon]->set_range(0,100);
}
if (g_rc_function[RC_Channel_aux::k_egg_drop]) {
g_rc_function[RC_Channel_aux::k_egg_drop]->set_range(0,100);
}
}
void init_rc_out()
static void init_rc_out()
{
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
@ -57,17 +90,17 @@ void init_rc_out()
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
}
void read_radio()
static void read_radio()
{
ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH);
if(mix_mode == 0){
if(g.mix_mode == 0){
g.channel_roll.set_pwm(ch1_temp);
g.channel_pitch.set_pwm(ch2_temp);
}else{
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((reverse_ch2_elevon * int(ch2_temp - elevon2_trim) + reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500);
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_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_throttle.set_pwm(APM_RC.InputCh(CH_3));
@ -87,7 +120,7 @@ void read_radio()
g.channel_throttle.servo_out = g.channel_throttle.control_in;
if (g.channel_throttle.servo_out > 50) {
if(airspeed_enabled == true) {
if(g.airspeed_enabled == true) {
airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
} else {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
@ -106,7 +139,7 @@ void read_radio()
*/
}
void control_failsafe(uint16_t pwm)
static void control_failsafe(uint16_t pwm)
{
if(g.throttle_fs_enabled == 0)
return;
@ -121,7 +154,7 @@ void control_failsafe(uint16_t pwm)
//Check for failsafe and debounce funky reads
} else if (g.throttle_fs_enabled) {
if (pwm < g.throttle_fs_value){
if (pwm < (unsigned)g.throttle_fs_value){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
@ -153,15 +186,17 @@ void control_failsafe(uint16_t pwm)
}
}
void trim_control_surfaces()
static void trim_control_surfaces()
{
read_radio();
// Store control surface trim values
// ---------------------------------
if(mix_mode == 0){
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
}else{
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
@ -177,9 +212,10 @@ void trim_control_surfaces()
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom();
}
void trim_radio()
static void trim_radio()
{
for (int y = 0; y < 30; y++) {
read_radio();
@ -187,11 +223,12 @@ void trim_radio()
// Store the trim values
// ---------------------
if(mix_mode == 0){
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else {
elevon1_trim = ch1_temp;
@ -207,5 +244,59 @@ void trim_radio()
g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom();
}
// map a function to a servo channel
static void aux_servo_out(RC_Channel_aux* ch, unsigned char ch_nr)
{
switch(ch->function)
{
case RC_Channel_aux::k_none: // disabled
return;
break;
case RC_Channel_aux::k_mount_yaw: // mount yaw (pan)
case RC_Channel_aux::k_mount_pitch: // mount pitch (tilt)
case RC_Channel_aux::k_mount_roll: // mount roll
case RC_Channel_aux::k_cam_trigger: // camera trigger
case RC_Channel_aux::k_cam_open: // camera open
case RC_Channel_aux::k_flap: // flaps
case RC_Channel_aux::k_flap_auto: // flaps automated
case RC_Channel_aux::k_aileron: // aileron
case RC_Channel_aux::k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing)
case RC_Channel_aux::k_egg_drop: // egg drop
case RC_Channel_aux::k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning
break;
case RC_Channel_aux::k_manual: // manual
ch->radio_out = ch->radio_in;
break;
}
APM_RC.OutputCh(ch_nr, ch->radio_out);
}
// update the g_rc_function array from pointers to rc_x channels
static void update_aux_servo_function()
{
RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos
aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)g.rc_5.function.get();
aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)g.rc_6.function.get();
aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)g.rc_7.function.get();
aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)g.rc_8.function.get();
// Assume that no auxiliary function is used
for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++)
{
g_rc_function[i] = NULL;
}
// assign the RC channel to each function
g_rc_function[aux_servo_function[CH_5]] = &g.rc_5;
g_rc_function[aux_servo_function[CH_6]] = &g.rc_6;
g_rc_function[aux_servo_function[CH_7]] = &g.rc_7;
g_rc_function[aux_servo_function[CH_8]] = &g.rc_8;
#if CAMERA == ENABLED
camera_mount.update_mount_type();
#endif
}

40
ArduPlane/sensors.pde
View File

@ -5,21 +5,17 @@
void ReadSCP1000(void) {}
void init_barometer(void)
static void init_barometer(void)
{
int flashcount;
int flashcount = 0;
long ground_pressure = 0;
int ground_temperature;
#if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets for initialization
#endif
while(ground_pressure == 0){
barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = barometer.Press;
ground_temperature = barometer.Temp;
delay(20);
mavlink_delay(20);
//Serial.printf("barometer.Press %ld\n", barometer.Press);
}
@ -33,7 +29,7 @@ void init_barometer(void)
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
delay(20);
mavlink_delay(20);
if(flashcount == 5) {
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
@ -53,11 +49,11 @@ void init_barometer(void)
g.ground_temperature.set_and_save(ground_temperature / 10.0f);
abs_pressure = ground_pressure;
Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
SendDebugln_P("barometer calibration complete.");
Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete."));
}
long read_barometer(void)
static long read_barometer(void)
{
float x, scaling, temp;
@ -73,30 +69,38 @@ long read_barometer(void)
}
// in M/S * 100
void read_airspeed(void)
static void read_airspeed(void)
{
#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_pressure = max(((int)airspeed_raw - airspeed_offset), 0);
airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0);
airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100;
#endif
calc_airspeed_errors();
}
void zero_airspeed(void)
static void zero_airspeed(void)
{
airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
for(int c = 0; c < 50; c++){
for(int c = 0; c < 10; c++){
delay(20);
airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10);
}
airspeed_offset = airspeed_raw;
g.airspeed_offset.set_and_save(airspeed_raw);
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
void read_battery(void)
static void read_battery(void)
{
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
@ -114,7 +118,7 @@ void read_battery(void)
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
}
#if BATTERY_EVENT == 1
#if BATTERY_EVENT == ENABLED
if(battery_voltage < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
#endif

145
ArduPlane/setup.pde
View File

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

170
ArduPlane/system.pde
View File

@ -6,11 +6,13 @@ The init_ardupilot function processes everything we need for an in - air restart
*****************************************************************************/
#if CLI_ENABLED == ENABLED
// Functions called from the top-level menu
extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
extern int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
static 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
static 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
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
@ -28,7 +30,7 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
}
// Command/function table for the top-level menu.
const struct Menu::command main_menu_commands[] PROGMEM = {
static const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"logs", process_logs},
@ -39,9 +41,11 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "ArduPilotMega", main_menu_commands);
MENU(main_menu, "APM trunk", main_menu_commands);
void init_ardupilot()
#endif // CLI_ENABLED
static void init_ardupilot()
{
// Console serial port
//
@ -73,16 +77,6 @@ void init_ardupilot()
Serial1.begin(38400, 128, 16);
#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
"\n\nFree RAM: %lu\n"),
freeRAM());
@ -92,7 +86,10 @@ void init_ardupilot()
//
if (!g.format_version.load()) {
Serial.println("\n\nEEPROM blank - all parameters are reset to default values.\n");
Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n"));
delay(100); // wait for serial msg to flush
AP_Var::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
@ -102,6 +99,7 @@ void init_ardupilot()
Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
"\n\nForcing complete parameter reset..."),
g.format_version.get(), Parameters::k_format_version);
delay(100); // wait for serial msg to flush
// erase all parameters
AP_Var::erase_all();
@ -116,45 +114,48 @@ void init_ardupilot()
AP_Var::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"),
AP_Var::get_memory_use(), (unsigned)g.num_resets);
}
if (!g.flight_modes.load()) {
default_flight_modes();
}
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
default_log_bitmask();
}
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// 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(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
mavlink_system.sysid = g.sysid_this_mav;
#if CAMERA == ENABLED
init_camera();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library 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) {
dcm.set_compass(&compass);
bool junkbool = compass.init();
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
//compass.set_declination(ToRad(get(PARAM_DECLINATION))); // TODO fix this to have a UI // set local difference between magnetic north and true north
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
dcm.set_compass(&compass);
compass.get_offsets(); // load offsets to account for airframe magnetic interference
}
}
#else
airspeed_enabled = true;
/*
Init is depricated - Jason
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC);
}
*/
#endif
DataFlash.Init(); // DataFlash log initialization
@ -162,6 +163,7 @@ void init_ardupilot()
// Do GPS init
g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization
g_gps->callback = mavlink_delay;
// init the GCS
#if GCS_PORT == 3
@ -208,6 +210,7 @@ void init_ardupilot()
// the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly.
//
#if CLI_ENABLED == ENABLED
if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
Serial.printf_P(PSTR("\n"
@ -221,6 +224,7 @@ void init_ardupilot()
main_menu.run();
}
}
#endif // CLI_ENABLED
if(g.log_bitmask != 0){
@ -268,6 +272,8 @@ void init_ardupilot()
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
}
set_mode(MANUAL);
// set the correct flight mode
// ---------------------------
reset_control_switch();
@ -276,8 +282,10 @@ void init_ardupilot()
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
void startup_ground(void)
static void startup_ground(void)
{
set_mode(INITIALISING);
gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
#if(GROUND_START_DELAY > 0)
@ -306,7 +314,7 @@ void startup_ground(void)
trim_radio(); // This was commented out as a HACK. Why? I don't find a problem.
#if HIL_MODE != HIL_MODE_ATTITUDE
if (airspeed_enabled == true)
if (g.airspeed_enabled == true)
{
// initialize airspeed sensor
// --------------------------
@ -349,7 +357,7 @@ else
gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}
void set_mode(byte mode)
static void set_mode(byte mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
@ -363,18 +371,11 @@ void set_mode(byte mode)
switch(control_mode)
{
case INITIALISING:
case MANUAL:
break;
case CIRCLE:
break;
case STABILIZE:
break;
case FLY_BY_WIRE_A:
break;
case FLY_BY_WIRE_B:
break;
@ -390,10 +391,8 @@ void set_mode(byte mode)
do_loiter_at_location();
break;
case TAKEOFF:
break;
case LAND:
case GUIDED:
set_guided_WP();
break;
default:
@ -408,7 +407,7 @@ void set_mode(byte mode)
Log_Write_Mode(control_mode);
}
void check_long_failsafe()
static void check_long_failsafe()
{
// only act on changes
// -------------------
@ -433,7 +432,7 @@ void check_long_failsafe()
}
}
void check_short_failsafe()
static void check_short_failsafe()
{
// only act on changes
// -------------------
@ -442,7 +441,7 @@ void check_short_failsafe()
failsafe_short_on_event();
}
}
if(failsafe == FAILSAFE_SHORT){
if(!ch3_failsafe) {
failsafe_short_off_event();
@ -451,20 +450,19 @@ void check_short_failsafe()
}
void startup_IMU_ground(void)
static void startup_IMU_ground(void)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
uint16_t store = 0;
SendDebugln("<startup_IMU_ground> Warming up ADC...");
delay(500);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
// -----------------------
demo_servos(2);
SendDebugln("<startup_IMU_ground> Beginning IMU calibration; do not move plane");
delay(1000);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane"));
mavlink_delay(1000);
imu.init(IMU::COLD_START);
imu.init(IMU::COLD_START, mavlink_delay);
dcm.set_centripetal(1);
// read Baro pressure at ground
@ -479,7 +477,7 @@ void startup_IMU_ground(void)
}
void update_GPS_light(void)
static void update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
@ -507,15 +505,16 @@ void update_GPS_light(void)
}
void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
dcm.gyro_sat_count = 0;
imu.adc_constraints = 0;
dcm.renorm_sqrt_count = 0;
dcm.renorm_blowup_count = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
gps_fix_count = 0;
pmTest1 = 0;
perf_mon_timer = millis();
}
@ -526,7 +525,7 @@ void resetPerfData(void) {
* 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.
*/
unsigned long freeRAM() {
static unsigned long freeRAM() {
uint8_t * heapptr, * stackptr;
stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
heapptr = stackptr; // save value of heap pointer
@ -535,3 +534,20 @@ unsigned long freeRAM() {
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,4 +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 -*-
#if CLI_ENABLED == ENABLED
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
@ -26,7 +28,7 @@ static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
const struct Menu::command test_menu_commands[] PROGMEM = {
static const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"failsafe", test_failsafe},
@ -64,14 +66,15 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
int8_t
static int8_t
test_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run();
return 0;
}
void print_hit_enter()
static void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
}
@ -104,7 +107,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
// ----------------------------------------------------------
read_radio();
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:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
@ -147,7 +150,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
// write out the servo PWM values
// ------------------------------
set_servos_4();
set_servos();
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,
@ -255,7 +258,7 @@ if (g.battery_monitoring == 4) {
// write out the servo PWM values
// ------------------------------
set_servos_4();
set_servos();
if(Serial.available() > 0){
return (0);
@ -315,11 +318,11 @@ test_wp(uint8_t argc, const Menu::arg *argv)
return (0);
}
void
test_wp_print(struct Location *cmd, byte index)
static void
test_wp_print(struct Location *cmd, byte wp_index)
{
Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)index,
(int)wp_index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
@ -376,20 +379,24 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
delay(1000);
if (!g.switch_enable) {
Serial.println_P(PSTR("dip switches disabled, using EEPROM"));
}
while(1){
delay(100);
update_servo_switches();
if (mix_mode == 0) {
if (g.mix_mode == 0) {
Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
(int)reverse_roll,
(int)reverse_pitch,
(int)reverse_rudder);
(int)g.channel_roll.get_reverse(),
(int)g.channel_pitch.get_reverse(),
(int)g.channel_rudder.get_reverse());
} else {
Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
(int)reverse_elevons,
(int)reverse_ch1_elevon,
(int)reverse_ch2_elevon);
(int)g.reverse_elevons,
(int)g.reverse_ch1_elevon,
(int)g.reverse_ch2_elevon);
}
if(Serial.available() > 0){
return (0);
@ -525,13 +532,29 @@ test_gyro(uint8_t argc, const Menu::arg *argv)
static int8_t
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;
if(g.compass_enabled) {
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter();
print_hit_enter();
while(1){
while(1) {
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
@ -542,40 +565,38 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// ---
dcm.update_DCM(G_Dt);
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
medium_loopCounter = 0;
}
}
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
Vector3f maggy = compass.get_offsets();
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,
compass.mag_x,
compass.mag_y,
compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
counter=0;
}
Vector3f maggy = compass.get_offsets();
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,
compass.mag_x,
compass.mag_y,
compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
counter=0;
}
}
if (Serial.available() > 0) {
break;
}
}
}
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
Serial.println_P(PSTR("saving offsets"));
compass.save_offsets();
return (0);
}
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
@ -588,14 +609,11 @@ test_mag(uint8_t argc, const Menu::arg *argv)
static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
unsigned airspeed_ch = adc.Ch(AIRSPEED_CH);
// Serial.println(adc.Ch(AIRSPEED_CH));
if ((adc.Ch(AIRSPEED_CH) > 2000) && (adc.Ch(AIRSPEED_CH) < 2900)){
airspeed_enabled = false;
}else{
airspeed_enabled = true;
}
Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch);
if (airspeed_enabled == false){
if (g.airspeed_enabled == false){
Serial.printf_P(PSTR("airspeed: "));
print_enabled(false);
return (0);
@ -614,11 +632,8 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
if(Serial.available() > 0){
return (0);
}
if(Serial.available() > 0){
return (0);
}
}
}
}
@ -669,3 +684,5 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED

213
libraries/AP_Mount/AP_Mount.cpp
View File

@ -1,184 +1,197 @@
#include "AP_Mount.h"
#include <AP_Mount.h>
#include <RC_Channel.h>
AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name):
_group(key, name),
_mountMode(&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name
_mountType(&_group, 0, 0, name ? PSTR("TYPE") : 0),
_dcm(dcm);
_gps(gps);
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm)
{
_dcm=dcm;
_gps=gps;
}
void AP_Mount::SetPitchYaw(Location targetGPSLocation)
void AP_Mount::set_pitch_yaw(int pitchCh, int yawCh)
{
_targetGPSLocation=targetGPSLocation;
}
void AP_Mount::set_GPS_target(Location targetGPSLocation)
{
_target_GPS_location=targetGPSLocation;
//set mode
_mountMode=gps;
_mount_mode=k_gps_target;
//update mount position
UpDateMount();
update_mount();
}
void AP_Mount::SetGPSTarget(Location targetGPSLocation)
void AP_Mount::set_assisted(int roll, int pitch, int yaw)
{
_targetGPSLocation=targetGPSLocation;
//set mode
_mountMode=gps;
//update mount position
UpDateMount();
}
void AP_Mount::SetAssisted(int roll, int pitch, int yaw)
{
_AssistAngles.x = roll;
_AssistAngles.y = pitch;
_AssistAngles.z = yaw;
_assist_angles.x = roll;
_assist_angles.y = pitch;
_assist_angles.z = yaw;
//set mode
_mountMode=assisted;
_mount_mode=k_assisted;
//update mount position
UpDateMount();
update_mount();
}
//sets the servo angles for FPV, note angles are * 100
void AP_Mount::SetMountFreeRoam(int roll, int pitch, int yaw)
void AP_Mount::set_mount_free_roam(int roll, int pitch, int yaw)
{
_RoamAngles.x=roll;
_RoamAngles.y=pitch;
_RoamAngles.z=yaw;
_roam_angles.x=roll;
_roam_angles.y=pitch;
_roam_angles.z=yaw;
//set mode
_mountMode=roam;
_mount_mode=k_roam;
//now update mount position
UpDateMount();
update_mount();
}
//sets the servo angles for landing, note angles are * 100
void AP_Mount::SetMountLanding(int roll, int pitch, int yaw)
void AP_Mount::set_mount_landing(int roll, int pitch, int yaw)
{
_LandingAngles.x=roll;
_LandingAngles.y=pitch;
_LandingAngles.z=yaw;
_landing_angles.x=roll;
_landing_angles.y=pitch;
_landing_angles.z=yaw;
//set mode
_mountMode=landing;
_mount_mode=k_landing;
//now update mount position
UpDateMount();
update_mount();
}
void AP_Mount::SetNone()
void AP_Mount::set_none()
{
//set mode
_mountMode=none;
_mount_mode=k_none;
//now update mount position
UpDateMount();
update_mount();
}
void AP_Mount::UpDateMount()
void AP_Mount::update_mount()
{
switch(_mountMode)
Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs
Vector3f targ; //holds target vector, var is used as temp in calcs
switch(_mount_mode)
{
case gps:
case k_gps_target:
{
if(_gps->fix)
{
CalcGPSTargetVector(&_targetGPSLocation);
calc_GPS_target_vector(&_target_GPS_location);
}
m = _dcm->get_dcm_transposed();
targ = m*_GPSVector;
this->CalcMountAnglesFromVector(*targ)
targ = m*_GPS_vector;
roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll
pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch
break;
}
case stabilise:
case k_stabilise:
{
//to do
// TODO replace this simplified implementation with a proper one
roll_angle = -_dcm->roll_sensor;
pitch_angle = -_dcm->pitch_sensor;
yaw_angle = -_dcm->yaw_sensor;
break;
}
case roam:
case k_roam:
{
pitchAngle=100*_RoamAngles.y;
rollAngle=100*_RoamAngles.x;
yawAngle=100*_RoamAngles.z;
roll_angle=100*_roam_angles.x;
pitch_angle=100*_roam_angles.y;
yaw_angle=100*_roam_angles.z;
break;
}
case assisted:
case k_assisted:
{
m = _dcm->get_dcm_transposed();
targ = m*_AssistVector;
this->CalcMountAnglesFromVector(*targ)
//rotate vector
targ = m*_assist_vector;
roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll
pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch
break;
}
case landing:
case k_landing:
{
pitchAngle=100*_RoamAngles.y;
rollAngle=100*_RoamAngles.x;
yawAngle=100*_RoamAngles.z;
roll_angle=100*_roam_angles.x;
pitch_angle=100*_roam_angles.y;
yaw_angle=100*_roam_angles.z;
break;
}
case none:
case k_none:
{
//do nothing
break;
}
case k_manual: // radio manual control
if (g_rc_function[RC_Channel_aux::k_mount_roll])
roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in,
g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min,
g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max,
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min,
g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max);
if (g_rc_function[RC_Channel_aux::k_mount_pitch])
pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in,
g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min,
g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max,
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min,
g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max);
if (g_rc_function[RC_Channel_aux::k_mount_yaw])
yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in,
g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min,
g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max,
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min,
g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max);
break;
default:
{
//do nothing
break;
}
}
// write the results to the servos
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
if (g_rc_function[RC_Channel_aux::k_mount_roll])
g_rc_function[RC_Channel_aux::k_mount_roll]->closest_limit(roll_angle/10);
if (g_rc_function[RC_Channel_aux::k_mount_pitch])
g_rc_function[RC_Channel_aux::k_mount_pitch]->closest_limit(pitch_angle/10);
if (g_rc_function[RC_Channel_aux::k_mount_yaw])
g_rc_function[RC_Channel_aux::k_mount_yaw]->closest_limit(yaw_angle/10);
}
void AP_Mount::SetMode(MountMode mode)
void AP_Mount::set_mode(MountMode mode)
{
_mountMode=mode;
_mount_mode=mode;
}
void AP_Mount::CalcGPSTargetVector(struct Location *target)
void AP_Mount::calc_GPS_target_vector(struct Location *target)
{
_targetVector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195;
_targetVector.y = (target->lat-_gps->latitude)*.01113195;
_targetVector.z = (_gps->altitude-target->alt);
_GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195;
_GPS_vector.y = (target->lat-_gps->latitude)*.01113195;
_GPS_vector.z = (_gps->altitude-target->alt);
}
void AP_Mount::CalcMountAnglesFromVector(Vector3f *targ)
void
AP_Mount::update_mount_type()
{
switch(_mountType)
// Auto-detect the mount gimbal type depending on the functions assigned to the servos
if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
{
case pitch_yaw:
{
//need to tidy up maths for below
pitchAngle = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1;
yawAngle = 9000 + atan2(-targ.y, targ.x) * 5729.57795;
break;
}
case pitch_roll:
{
pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch
rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll
break;
}
case pitch_roll_yaw:
{
//to do
break;
}
case none:
{
//do nothing
break;
}
default:
{
//do nothing
break;
}
_mount_type = k_pan_tilt;
}
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL))
{
_mount_type = k_tilt_roll;
}
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
{
_mount_type = k_pan_tilt_roll;
}
}

83
libraries/AP_Mount/AP_Mount.h
View File

@ -5,7 +5,7 @@
* *
* Author: Joe Holdsworth; *
* Ritchie Wilson; *
* Amiclair Lucus; *
* Amilcar Lucas; *
* *
* Purpose: Move a 2 or 3 axis mount attached to vehicle, *
* Used for mount to track targets or stabilise *
@ -13,8 +13,6 @@
* *
* Usage: Use in main code to control mounts attached to *
* vehicle. *
* 1. initialise class *
* 2. setMounttype * *
* *
*Comments: All angles in degrees * 100, distances in meters*
* unless otherwise stated. *
@ -22,78 +20,71 @@
#ifndef AP_Mount_H
#define AP_Mount_H
#include <AP_Common.h>
//#include <AP_Common.h>
#include <AP_Math.h>
#include <AP_GPS.h>
#include <AP_DCM.h>
class AP_Mount
{
protected:
AP_Var_group _group; // must be before all vars to keep ctor init order correct
public:
//Constructors
AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name);
AP_Mount(GPS *gps, AP_DCM *dcm);
//enums
enum MountMode{
gps = 0,
stabilise = 1, //note the correct english spelling :)
roam = 2,
assisted = 3,
landing = 4,
none = 5
k_gps_target = 0,
k_stabilise = 1, //note the correct English spelling :)
k_roam = 2,
k_assisted = 3,
k_landing = 4,
k_none = 5,
k_manual = 6
};
enum MountType{
pitch_yaw = 0,
pitch_roll = 1,
pitch_roll_yaw = 2,
none = 3;
k_pan_tilt = 0, //yaw-pitch
k_tilt_roll = 1, //pitch-roll
k_pan_tilt_roll = 2, //yaw-pitch-roll
};
//Accessors
//void SetPitchYaw(int pitchCh, int yawCh);
//void SetPitchRoll(int pitchCh, int rollCh);
//void SetPitchRollYaw(int pitchCh, int rollCh, int yawCh);
void set_pitch_yaw(int pitchCh, int yawCh);
void set_pitch_roll(int pitchCh, int rollCh);
void set_pitch_roll_yaw(int pitchCh, int rollCh, int yawCh);
void SetGPSTarget(Location targetGPSLocation); //used to tell the mount to track GPS location
void SetAssisted(int roll, int pitch, int yaw);
void SetMountFreeRoam(int roll, int pitch, int yaw);//used in the FPV for example,
void SetMountLanding(int roll, int pitch, int yaw); //set mount landing position
void SetNone();
void set_GPS_target(Location targetGPSLocation); //used to tell the mount to track GPS location
void set_assisted(int roll, int pitch, int yaw);
void set_mount_free_roam(int roll, int pitch, int yaw);//used in the FPV for example,
void set_mount_landing(int roll, int pitch, int yaw); //set mount landing position
void set_none();
//methods
void UpDateMount();
void SetMode(MountMode mode);
void update_mount();
void update_mount_type(); //Auto-detect the mount gimbal type depending on the functions assigned to the servos
void set_mode(MountMode mode);
int pitchAngle; //degrees*100
int rollAngle; //degrees*100
int yawAngle; //degrees*100
private:
int pitch_angle; //degrees*100
int roll_angle; //degrees*100
int yaw_angle; //degrees*100
protected:
//methods
void CalcGPSTargetVector(struct Location *target);
void CalcMountAnglesFromVector(Vector3f *targ);
void calc_GPS_target_vector(struct Location *target);
//void CalculateDCM(int roll, int pitch, int yaw);
//members
AP_DCM *_dcm;
GPS *_gps;
MountMode _mountMode;
MountType _mountType;
MountMode _mount_mode;
MountType _mount_type;
struct Location _targetGPSLocation;
Vector3f _GPSVector; //target vector calculated stored in meters
struct Location _target_GPS_location;
Vector3f _GPS_vector; //target vector calculated stored in meters
Vector3i _RoamAngles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw
Vector3i _LandingAngles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
Vector3i _roam_angles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw
Vector3i _landing_angles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
Vector3i _AssistAngles; //used to keep angles that user has supplied from assisted targetting
Vector3f _AssistVector; //used to keep vector calculated from _AssistAngles
Matrix3f _m; //holds 3 x 3 matrix, var is used as temp in calcs
Vector3f _targ; //holds target vector, var is used as temp in calcs
Vector3i _assist_angles; //used to keep angles that user has supplied from assisted targeting
Vector3f _assist_vector; //used to keep vector calculated from _AssistAngles
};
#endif

35
libraries/RC_Channel/RC_Channel.cpp
View File

@ -218,3 +218,38 @@ RC_Channel::norm_output()
else
return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim);
}
int16_t
RC_Channel_aux::closest_limit(int16_t angle)
{
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
int16_t min = angle_min / 10;
int16_t max = angle_max / 10;
// Make sure the angle lies in the interval [-180 .. 180[ degrees
while (angle < -1800) angle += 3600;
while (angle >= 1800) angle -= 3600;
// Make sure the angle limits lie in the interval [-180 .. 180[ degrees
while (min < -1800) min += 3600;
while (min >= 1800) min -= 3600;
while (max < -1800) max += 3600;
while (max >= 1800) max -= 3600;
set_range(min, max);
// If the angle is outside servo limits, saturate the angle to the closest limit
// On a circle the closest angular position must be carefully calculated to account for wrap-around
if ((angle < min) && (angle > max)){
// angle error if min limit is used
int16_t err_min = min - angle + (angle<min?0:3600); // add 360 degrees if on the "wrong side"
// angle error if max limit is used
int16_t err_max = angle - max + (angle>max?0:3600); // add 360 degrees if on the "wrong side"
angle = err_min<err_max?min:max;
}
servo_out = angle;
// convert angle to PWM using a linear transformation (ignores trimming because the camera limits might not be symmetric)
calc_pwm();
return angle;
}

45
libraries/RC_Channel/RC_Channel.h
View File

@ -7,12 +7,13 @@
#define RC_Channel_h
#include <AP_Common.h>
// TODO is this include really necessary ?
#include <stdint.h>
/// @class RC_Channel
/// @brief Object managing one RC channel
class RC_Channel{
private:
protected:
AP_Var_group _group; // must be before all vars to keep ctor init order correct
public:
@ -103,6 +104,48 @@ class RC_Channel{
int16_t _low;
};
/// @class RC_Channel_aux
/// @brief Object managing one aux. RC channel (CH5-8), with information about its function
class RC_Channel_aux : public RC_Channel{
public:
/// Constructor
///
/// @param key EEPROM storage key for the channel trim parameters.
/// @param name Optional name for the group.
///
RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) :
RC_Channel(key, name),
function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name
angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection
angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection
{}
typedef enum
{
k_none = 0, // 0=disabled
k_mount_yaw = 1, // 1=mount yaw (pan)
k_mount_pitch = 2, // 2=mount pitch (tilt)
k_mount_roll = 3, // 3=mount roll
k_cam_trigger = 4, // 4=camera trigger
k_cam_open = 5, // 5=camera open
k_flap = 6, // 6=flap
k_flap_auto = 7, // 7=flap automated
k_aileron = 8, // 8=aileron
k_flaperon = 9, // 9=flaperon (flaps and aileron combined, needs two independent servos one for each wing)
k_egg_drop = 10, // 10=egg drop
k_manual = 11, // 11=manual, just pass-thru the RC in signal
k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one)
} Aux_servo_function_t;
AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func
AP_Aux_srv_func function; // 0=disabled, 1=mount yaw (pan), 2=mount pitch (tilt), 3=mount roll, 4=camera trigger, 5=camera open, 6=flap, 7=flap auto, 8=aileron, 9=flaperon, 10=egg drop, 11=manual
AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units
AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units
int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval
};
#endif