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

import APM_Camera branch from SVN

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

36
ArduPlane/APM_Config.h
View File

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

36
ArduPlane/APM_Config.h.reference
View File

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

361
ArduPlane/ArduPilotMega.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduPilotMega V2.23"
#define THISFIRMWARE "ArduPilotMega 2.2.0"
/*
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,7 +29,6 @@ version 2.1 of the License, or (at your option) any later version.
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <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
@ -39,10 +38,10 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <ModeFilter.h>
//#include <AP_RangeFinder.h> // Range finder library
#define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <memcheck.h>
// Configuration
#include "config.h"
@ -71,12 +70,12 @@ FastSerialPort3(Serial3); // Telemetry port
//
// Global parameters are all contained within the 'g' class.
//
static Parameters g;
Parameters g;
////////////////////////////////////////////////////////////////////////////////
// prototypes
static void update_events(void);
void update_events(void);
////////////////////////////////////////////////////////////////////////////////
@ -93,17 +92,23 @@ static void update_events(void);
//
// All GPS access should be through this pointer.
static GPS *g_gps;
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
GPS *g_gps;
#if HIL_MODE == HIL_MODE_DISABLED
// real sensors
static AP_ADC_ADS7844 adc;
static APM_BMP085_Class barometer;
static AP_Compass_HMC5843 compass(Parameters::k_param_compass);
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
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
@ -139,7 +144,6 @@ 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
@ -185,31 +189,18 @@ AP_IMU_Shim imu; // never used
GCS_Class gcs;
#endif
////////////////////////////////////////////////////////////////////////////////
// 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
//AP_RangeFinder_MaxsonarXL sonar;
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
byte control_mode = INITIALISING;
byte control_mode = MANUAL;
byte oldSwitchPosition; // for remembering the control mode switch
bool inverted_flight = false;
static const char *comma = ",";
const char *comma = ",";
static const char* flight_mode_strings[] = {
const char* flight_mode_strings[] = {
"Manual",
"Circle",
"Stabilize",
@ -241,179 +232,214 @@ static const char* flight_mode_strings[] = {
// Failsafe
// --------
static int failsafe; // track which type of failsafe is being processed
static bool ch3_failsafe;
static byte crash_timer;
int failsafe; // track which type of failsafe is being processed
bool ch3_failsafe;
byte crash_timer;
// Radio
// -----
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;
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;
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
// ----------
static bool GPS_light; // status of the GPS light
bool GPS_light; // status of the GPS light
// GPS variables
// -------------
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
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
// Location & Navigation
// ---------------------
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
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
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 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
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
// Airspeed
// --------
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
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;
// Location Errors
// ---------------
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
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
// Battery Sensors
// ---------------
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 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 current_amps;
static float current_total;
float current_amps;
float current_total;
// Airspeed Sensors
// ----------------
static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering
static int airspeed_pressure; // airspeed as a pressure value
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
// Barometer Sensor variables
// --------------------------
static unsigned long abs_pressure;
unsigned long abs_pressure;
// Altitude Sensor variables
// ----------------------
static int sonar_alt;
//byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific
// --------------------
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;
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;
// Loiter management
// -----------------
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
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
// these are the values for navigation control functions
// ----------------------------------------------------
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
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
// Waypoints
// ---------
static long wp_distance; // meters - distance between plane and next waypoint
static long wp_totalDistance; // meters - distance between old and next waypoint
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
// repeating event control
// -----------------------
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)
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)
// delay command
// --------------
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start;
static int condition_rate;
long condition_value; // used in condition commands (eg delay, change alt, etc.)
long condition_start;
int condition_rate;
// 3D Location vectors
// -------------------
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
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
// IMU variables
// -------------
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
// Performance monitoring
// ----------------------
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;
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;
// 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
// --------------
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 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 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;
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 byte slow_loopCounter;
static byte superslow_loopCounter;
static byte counter_one_herz;
byte slow_loopCounter;
byte superslow_loopCounter;
byte counter_one_herz;
static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static float load; // % MCU cycles used
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
unsigned long elapsedTime; // for doing custom events
float load; // % MCU cycles used
//Camera tracking and stabilisation stuff
// --------------------------------------
byte camera_mode = 1; //0 is do nothing, 1 is stabilize, 2 is track target
byte gimbal_mode = 0; // 0 - pitch & roll, 1 - pitch and yaw (pan & tilt), 2 - pitch, roll and yaw (to be added)
struct Location camera_target; //point of iterest for the camera to track
Vector3<float> target_vector(0,0,1); //x, y, z to target before rotating to planes axis, values are in meters
float cam_pitch;
float cam_roll;
float cam_tilt;
float cam_pan;
struct Location GPS_mark; // GPS POI for position based triggering
int picture_time = 0; // waypoint trigger variable
int thr_pic = 0; // timer variable for throttle_pic
int camtrig = 83; // PK6 chosen as it not near anything so safer for soldering
pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82)
////////////////////////////////////////////////////////////////////////////////
@ -421,7 +447,6 @@ static float load; // % MCU cycles used
////////////////////////////////////////////////////////////////////////////////
void setup() {
memcheck_init();
init_ardupilot();
}
@ -466,7 +491,7 @@ void loop()
}
// Main loop 50Hz
static void fast_loop()
void fast_loop()
{
// This is the fast loop - we want it to execute at 50Hz if possible
// -----------------------------------------------------------------
@ -477,22 +502,15 @@ static 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 (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) {
if (airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) {
read_airspeed();
} else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
} else if (airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
calc_airspeed_errors();
}
@ -532,7 +550,7 @@ static void fast_loop()
// write out the servo PWM values
// ------------------------------
set_servos();
set_servos_4();
// XXX is it appropriate to be doing the comms below on the fast loop?
@ -561,8 +579,9 @@ static void fast_loop()
// or be a "GCS fast loop" interface
}
static void medium_loop()
void medium_loop()
{
camera();
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
@ -595,6 +614,7 @@ Serial.println(tempaccel.z, DEC);
// This case performs some navigation computations
//------------------------------------------------
case 1:
medium_loopCounter++;
@ -618,7 +638,6 @@ Serial.println(tempaccel.z, DEC);
// Read altitude from sensors
// ------------------
update_alt();
if(g.sonar_enabled) sonar_alt = sonar.read();
// altitude smoothing
// ------------------
@ -679,7 +698,7 @@ Serial.println(tempaccel.z, DEC);
}
}
static void slow_loop()
void slow_loop()
{
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
@ -708,7 +727,9 @@ static void slow_loop()
// Read Control Surfaces/Mix switches
// ----------------------------------
update_servo_switches();
if (g.switch_enable) {
update_servo_switches();
}
break;
@ -716,18 +737,19 @@ static 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(3,5);
gcs.data_stream_send(1,5);
// send all requested output streams with rates requested
// between 3 and 5 Hz
// between 1 and 5 Hz
#else
gcs.send_message(MSG_LOCATION);
gcs.send_message(MSG_CPU_LOAD, load*100);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(3,5);
hil.data_stream_send(1,5);
#endif
@ -735,26 +757,20 @@ static void slow_loop()
}
}
static void one_second_loop()
void one_second_loop()
{
if (g.log_bitmask & MASK_LOG_CUR)
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
}
static void update_GPS(void)
void update_GPS(void)
{
g_gps->update();
update_GPS_light();
@ -806,7 +822,7 @@ static void update_GPS(void)
}
}
static void update_current_flight_mode(void)
void update_current_flight_mode(void)
{
if(control_mode == AUTO){
crash_checker();
@ -819,7 +835,7 @@ static void update_current_flight_mode(void)
nav_roll = 0;
}
if (g.airspeed_enabled == true)
if (airspeed_enabled == true)
{
calc_nav_pitch();
if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch;
@ -837,7 +853,7 @@ static void update_current_flight_mode(void)
case MAV_CMD_NAV_LAND:
calc_nav_roll();
if (g.airspeed_enabled == true){
if (airspeed_enabled == true){
calc_nav_pitch();
calc_throttle();
}else{
@ -862,7 +878,6 @@ static void update_current_flight_mode(void)
switch(control_mode){
case RTL:
case LOITER:
case GUIDED:
hold_course = -1;
crash_checker();
calc_nav_roll();
@ -876,7 +891,6 @@ static void update_current_flight_mode(void)
nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min;
// 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:
@ -886,7 +900,7 @@ static 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 (g.airspeed_enabled == true)
if (airspeed_enabled == true)
{
airspeed_error = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) *
@ -935,7 +949,7 @@ static void update_current_flight_mode(void)
}
}
static void update_navigation()
void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
@ -947,18 +961,23 @@ static 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;
}
}
}
static void update_alt()
void update_alt()
{
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
@ -971,6 +990,6 @@ static 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);
}

135
ArduPlane/Attitude.pde
View File

@ -4,14 +4,15 @@
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************
static void stabilize()
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 (g.airspeed_enabled == true){
if (airspeed_enabled == true){
if(airspeed > 0)
speed_scaler = (STANDARD_SPEED * 100) / airspeed;
else
@ -19,7 +20,7 @@ static void stabilize()
speed_scaler = constrain(speed_scaler, 0.5, 2.0);
} else {
if (g.channel_throttle.servo_out > 0){
speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root
speed_scaler = 0.5 + (THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root
// Should maybe be to the 2/7 power, but we aren't goint to implement that...
}else{
speed_scaler = 1.67;
@ -31,16 +32,6 @@ static 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 "));
@ -53,10 +44,6 @@ static 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
@ -112,7 +99,7 @@ static void stabilize()
//#endif
}
static void crash_checker()
void crash_checker()
{
if(dcm.pitch_sensor < -4500){
crash_timer = 255;
@ -122,9 +109,9 @@ static void crash_checker()
}
static void calc_throttle()
void calc_throttle()
{
if (g.airspeed_enabled == false) {
if (airspeed_enabled == false) {
int throttle_target = g.throttle_cruise + throttle_nudge;
// no airspeed sensor, we use nav pitch to determine the proper throttle output
@ -158,7 +145,7 @@ static void calc_throttle()
// Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ----------------------------------------------------------------------------------------
static void calc_nav_yaw(float speed_scaler)
void calc_nav_yaw(float speed_scaler)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
Vector3f temp = imu.get_accel();
@ -173,11 +160,11 @@ static void calc_nav_yaw(float speed_scaler)
}
static void calc_nav_pitch()
void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
if (g.airspeed_enabled == true) {
if (airspeed_enabled == true) {
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
} else {
nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav);
@ -188,7 +175,7 @@ static void calc_nav_pitch()
#define YAW_DAMPENER 0
static void calc_nav_roll()
void calc_nav_roll()
{
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
@ -232,22 +219,18 @@ float roll_slew_limit(float servo)
/*****************************************
* Throttle slew limit
*****************************************/
static void throttle_slew_limit()
/*float throttle_slew_limit(float throttle)
{
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;
}
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;
}
*/
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_I(void)
void reset_I(void)
{
g.pidNavRoll.reset_I();
g.pidNavPitchAirspeed.reset_I();
@ -259,13 +242,11 @@ static void reset_I(void)
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
static void set_servos(void)
void set_servos_4(void)
{
int flapSpeedSource = 0;
if(control_mode == MANUAL){
// do a direct pass through of radio values
if (g.mix_mode == 0){
if (mix_mode == 0){
g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
} else {
@ -274,31 +255,21 @@ static void set_servos(void)
}
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in;
} else {
if (g.mix_mode == 0) {
if (mix_mode == 0){
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
g.rc_5.servo_out = g.channel_roll.servo_out;
g.rc_5.calc_pwm();
}
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
g.rc_6.servo_out = g.channel_roll.servo_out;
g.rc_6.calc_pwm();
}
}else{
/*Elevon mode*/
float ch1;
float ch2;
ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out);
ch1 = reverse_elevons * (g.channel_pitch.servo_out - g.channel_roll.servo_out);
ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out;
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));
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));
}
#if THROTTLE_OUT == 0
@ -310,61 +281,55 @@ static void set_servos(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_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in;
} else if (control_mode >= FLY_BY_WIRE_C) {
if (g.airspeed_enabled == true) {
flapSpeedSource = g.airspeed_cruise;
} else {
flapSpeedSource = g.throttle_cruise;
}
if ( flapSpeedSource > g.flap_1_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0;
} else if (flapSpeedSource > g.flap_2_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent;
} else {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent;
}
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
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
APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos
APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos
#endif
}
static void demo_servos(byte i) {
void demo_servos(byte i) {
while(i > 0){
gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
APM_RC.OutputCh(1, 1400);
mavlink_delay(400);
delay(400);
APM_RC.OutputCh(1, 1600);
mavlink_delay(200);
delay(200);
APM_RC.OutputCh(1, 1500);
#endif
mavlink_delay(400);
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;
}

210
ArduPlane/Cam_move.pde Normal file
View File

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

41
ArduPlane/Cam_trigger.pde Normal file
View File

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

8
ArduPlane/GCS.h
View File

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

452
ArduPlane/GCS_Mavlink.pde
View File

@ -4,10 +4,6 @@
#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),
@ -18,21 +14,21 @@ waypoint_receive_timeout(1000), // 1 second
// stream rates
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
// AP_VAR //ref //index, default, name
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
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"))
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(FastSerial * port)
GCS_MAVLINK::init(BetterStream * port)
{
GCS_Class::init(port);
if (port == &Serial) { // to split hil vs gcs
@ -57,6 +53,8 @@ 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);
}
@ -64,6 +62,11 @@ GCS_MAVLINK::update(void)
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
_queued_send();
@ -84,55 +87,41 @@ 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(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);
if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) {
send_message(MSG_EXTENDED_STATUS);
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)) {
// sent with GPS read
if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) {
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());
}
}
}
@ -140,7 +129,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, packet_drops);
mavlink_send_message(chan,id,param,packet_drops);
}
void
@ -169,7 +158,7 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
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) {
@ -179,18 +168,13 @@ 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){
@ -204,7 +188,6 @@ 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.
@ -212,35 +195,27 @@ 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;
}
@ -379,8 +354,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
set_mode(MANUAL);
break;
case MAV_MODE_GUIDED:
set_mode(GUIDED);
case MAV_MODE_GUIDED: //For future use
break;
case MAV_MODE_AUTO:
@ -393,14 +367,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_MODE_TEST1:
set_mode(STABILIZE);
break;
case MAV_MODE_TEST2:
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;
set_mode(FLY_BY_WIRE_A);
break;
case MAV_MODE_TEST3:
set_mode(FLY_BY_WIRE_B);
break;
}
}
@ -421,8 +393,7 @@ 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(
@ -439,7 +410,6 @@ 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"));
@ -451,17 +421,14 @@ 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
@ -471,15 +438,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == (uint16_t)g.waypoint_index)
current = 1;
if (packet.seq == 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 || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) {
if (tell_command.id < MAV_CMD_NAV_LAST) {
// command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude)
@ -490,62 +453,49 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
}
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
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_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
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_TIME:
param1 = tell_command.p1*10; // APM loiter time is in ten second increments
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_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
param1 = tell_command.p1;
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
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;
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_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_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);
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();
@ -561,6 +511,9 @@ 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;
@ -591,15 +544,14 @@ 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;
}
@ -634,7 +586,6 @@ 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;
@ -642,58 +593,55 @@ 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; // absolute altitude
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;
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;
}
}
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;
}
}
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
@ -703,7 +651,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.lat = packet.param1;
tell_command.p1 = packet.param1 * 100;
break;
case MAV_CMD_NAV_LOITER_TIME:
@ -737,53 +685,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
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;
set_wp_with_index(tell_command, packet.seq);
// add home alt if needed
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){
guided_WP.alt += home.alt;
}
// update waypoint receiving state machine
waypoint_timelast_receive = millis();
waypoint_request_i++;
set_mode(GUIDED);
if (waypoint_request_i > g.waypoint_total)
{
uint8_t type = 0; // ok (0), error(1)
// make any new wp uploaded instant (in case we are already in Guided mode)
set_guided_WP();
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
type);
// 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
}
}
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;
}
@ -795,9 +717,7 @@ 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
@ -814,7 +734,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.
float rounding_addition = 0.01;
const float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
@ -827,13 +747,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
((AP_Float16 *)vp)->set_and_save(packet.param_value);
} 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
@ -864,9 +783,7 @@ 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;
@ -886,14 +803,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
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;
@ -1035,7 +951,7 @@ void
GCS_MAVLINK::_queued_send()
{
// Check to see if we are sending parameters
if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) {
if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) {
AP_Var *vp;
float value;
@ -1068,10 +984,9 @@ GCS_MAVLINK::_queued_send()
// request waypoints one by one
// XXX note that this is pan-interface
if (waypoint_receiving &&
(requested_interface == (unsigned)chan) &&
waypoint_request_i <= (unsigned)g.waypoint_total &&
(requested_interface == chan) &&
waypoint_request_i <= g.waypoint_total &&
mavdelay > 15) { // limits to 3.33 hz
mavlink_msg_waypoint_request_send(
chan,
waypoint_dest_sysid,
@ -1082,66 +997,5 @@ GCS_MAVLINK::_queued_send()
}
}
#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
static void send_rate(uint16_t low, uint16_t high) {
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(low, high);
#endif
#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 <FastSerial.h>
#include <BetterStream.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(FastSerial *port) { _port = port; }
void init(BetterStream *port) { _port = port; }
/// Update HIL state.
///
@ -83,7 +83,7 @@ public:
protected:
/// The stream we are communicating over
FastSerial *_port;
BetterStream *_port;
};
//
@ -111,7 +111,7 @@ class HIL_XPLANE : public HIL_Class
public:
HIL_XPLANE();
void update(void);
void init(FastSerial *port);
void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str);

2
ArduPlane/HIL_Xplane.pde
View File

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

107
ArduPlane/Log.pde
View File

@ -1,7 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#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
@ -12,6 +10,7 @@
// 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);
@ -28,14 +27,13 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv)
" enable <name>|all enable logging <name> or everything\n"
" 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
static const struct Menu::command log_menu_commands[] PROGMEM = {
const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
@ -46,8 +44,6 @@ static 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)
{
@ -84,7 +80,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(i, log_start, log_end);
get_log_boundaries(last_log_num, i, log_start, log_end);
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"),
i, log_start, log_end);
}
@ -109,7 +105,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
return(-1);
}
get_log_boundaries(dump_log, dump_log_start, dump_log_end);
get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
dump_log,
dump_log_start,
@ -117,7 +113,6 @@ 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
@ -138,7 +133,6 @@ 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
@ -160,7 +154,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
// bits accordingly.
//
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~0;
bits = ~(bits = 0);
} else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST);
@ -184,15 +178,14 @@ select_logs(uint8_t argc, const Menu::arg *argv)
return(0);
}
static int8_t
int8_t
process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
static byte get_num_logs(void)
byte get_num_logs(void)
{
int page = 1;
byte data;
@ -231,14 +224,14 @@ static byte get_num_logs(void)
}
// send the number of the last log?
static void start_new_log(byte num_existing_logs)
void start_new_log(byte num_existing_logs)
{
int start_pages[50] = {0,0,0};
int end_pages[50] = {0,0,0};
if(num_existing_logs > 0) {
for(int i=0;i<num_existing_logs;i++) {
get_log_boundaries(i+1,start_pages[i],end_pages[i]);
get_log_boundaries(num_existing_logs, i+1,start_pages[i],end_pages[i]);
}
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
}
@ -266,7 +259,7 @@ static void start_new_log(byte num_existing_logs)
}
}
static void get_log_boundaries(byte log_num, int & start_page, int & end_page)
void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end_page)
{
int page = 1;
byte data;
@ -308,7 +301,7 @@ static void get_log_boundaries(byte log_num, int & start_page, int & end_page)
// Error condition if we reach here with page = 2 TO DO - report condition
}
static int find_last_log_page(int bottom_page)
int find_last_log_page(int bottom_page)
{
int top_page = 4096;
int look_page;
@ -318,7 +311,7 @@ static int find_last_log_page(int bottom_page)
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
check = DataFlash.ReadLong();
if(check == (long)0xFFFFFFFF)
if(check == 0xFFFFFFFF)
top_page = look_page;
else
bottom_page = look_page;
@ -328,7 +321,7 @@ static int find_last_log_page(int bottom_page)
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -340,7 +333,7 @@ static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
}
// Write a performance monitoring packet. Total length : 19 bytes
static void Log_Write_Performance()
void Log_Write_Performance()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -354,13 +347,12 @@ static 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)
static void Log_Write_Cmd(byte num, struct Location *wp)
void Log_Write_Cmd(byte num, struct Location *wp)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -374,7 +366,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp)
DataFlash.WriteByte(END_BYTE);
}
static void Log_Write_Startup(byte type)
void Log_Write_Startup(byte type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -396,7 +388,7 @@ static void Log_Write_Startup(byte type)
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning()
void Log_Write_Control_Tuning()
{
Vector3f accel = imu.get_accel();
@ -417,7 +409,7 @@ static void Log_Write_Control_Tuning()
#endif
// Write a navigation tuning packet. Total length : 18 bytes
static void Log_Write_Nav_Tuning()
void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -433,7 +425,7 @@ static void Log_Write_Nav_Tuning()
}
// Write a mode packet. Total length : 5 bytes
static void Log_Write_Mode(byte mode)
void Log_Write_Mode(byte mode)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -443,8 +435,8 @@ static void Log_Write_Mode(byte mode)
}
// Write an GPS packet. Total length : 30 bytes
static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
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);
@ -454,7 +446,6 @@ static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude
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);
@ -464,7 +455,7 @@ static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude
// Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Raw()
void Log_Write_Raw()
{
Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel();
@ -485,7 +476,7 @@ static void Log_Write_Raw()
}
#endif
static void Log_Write_Current()
void Log_Write_Current()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -498,7 +489,7 @@ static void Log_Write_Current()
}
// Read a Current packet
static void Log_Read_Current()
void Log_Read_Current()
{
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
@ -508,7 +499,7 @@ static void Log_Read_Current()
}
// Read an control tuning packet
static void Log_Read_Control_Tuning()
void Log_Read_Control_Tuning()
{
float logvar;
@ -524,7 +515,7 @@ static void Log_Read_Control_Tuning()
}
// Read a nav tuning packet
static void Log_Read_Nav_Tuning()
void Log_Read_Nav_Tuning()
{
Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
(float)((uint16_t)DataFlash.ReadInt())/100.0,
@ -537,7 +528,7 @@ static void Log_Read_Nav_Tuning()
}
// Read a performance packet
static void Log_Read_Performance()
void Log_Read_Performance()
{
long pm_time;
int logvar;
@ -546,7 +537,7 @@ static 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{
@ -559,7 +550,7 @@ static void Log_Read_Performance()
}
// Read a command processing packet
static void Log_Read_Cmd()
void Log_Read_Cmd()
{
byte logvarb;
long logvarl;
@ -578,7 +569,7 @@ static void Log_Read_Cmd()
Serial.println(" ");
}
static void Log_Read_Startup()
void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
@ -593,7 +584,7 @@ static void Log_Read_Startup()
}
// Read an attitude packet
static void Log_Read_Attitude()
void Log_Read_Attitude()
{
Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
DataFlash.ReadInt(),
@ -602,22 +593,21 @@ static void Log_Read_Attitude()
}
// Read a mode packet
static void Log_Read_Mode()
void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD:"));
Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
}
// Read a GPS packet
static void Log_Read_GPS()
void Log_Read_GPS()
{
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
DataFlash.ReadLong(),
(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,
@ -626,7 +616,7 @@ static void Log_Read_GPS()
}
// Read a raw accel/gyro packet
static void Log_Read_Raw()
void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW:"));
@ -639,19 +629,13 @@ static void Log_Read_Raw()
}
// Read the DataFlash log memory : Packet Parser
static void Log_Read(int start_page, int end_page)
void Log_Read(int start_page, int end_page)
{
byte data;
byte 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){
@ -728,23 +712,4 @@ static void Log_Read(int start_page, int end_page)
Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count);
}
#else // LOGGING_ENABLED
// dummy functions
static void Log_Write_Mode(byte mode) {}
static void Log_Write_Startup(byte type) {}
static void Log_Write_Cmd(byte num, struct Location *wp) {}
static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {}
static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {}
static void Log_Write_Performance() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
static byte get_num_logs(void) { return 0; }
static void start_new_log(byte num_existing_logs) {}
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {}
static void Log_Write_Control_Tuning() {}
static void Log_Write_Raw() {}
#endif // LOGGING_ENABLED

158
ArduPlane/Mavlink_Common.h
View File

@ -25,36 +25,21 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
}
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
{
uint64_t timeStamp = micros();
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_STATUS1:
case MSG_EXTENDED_STATUS:
{
CHECK_PAYLOAD_SIZE(SYS_STATUS);
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
@ -67,14 +52,9 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
break;
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_TEST2;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
mode = MAV_MODE_TEST3;
break;
case AUTO:
mode = MAV_MODE_AUTO;
@ -88,10 +68,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER;
break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
@ -106,20 +82,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
battery_voltage * 1000,
battery_remaining,
packet_drops);
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,
@ -135,7 +102,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
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,
@ -151,7 +117,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
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,
@ -168,7 +133,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
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,
@ -184,7 +148,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_GPS_RAW:
{
CHECK_PAYLOAD_SIZE(GPS_RAW);
mavlink_msg_gps_raw_send(
chan,
timeStamp,
@ -201,7 +164,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
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
@ -221,7 +183,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_RADIO_IN:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
@ -239,7 +200,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_RADIO_OUT:
{
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
@ -255,7 +215,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_VFR_HUD:
{
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
@ -268,12 +227,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
case MSG_RAW_IMU:
{
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(
@ -288,42 +245,21 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
compass.mag_x,
compass.mag_y,
compass.mag_z);
break;
}
case MSG_RAW_IMU2:
{
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
/* This message is not working. Why?
mavlink_msg_scaled_pressure_send(
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());
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));
*/
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,
@ -337,7 +273,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
case MSG_CURRENT_WAYPOINT:
{
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
@ -347,77 +282,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
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,

158
ArduPlane/Parameters.h
View File

@ -17,14 +17,7 @@ public:
// 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
static const uint16_t k_format_version = 6;
//
// Parameter identities.
@ -50,7 +43,6 @@ public:
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
// Misc
@ -60,16 +52,6 @@ public:
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
//
@ -77,7 +59,6 @@ public:
k_param_streamrates_port3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
// 120: Fly-by-wire control
//
@ -85,9 +66,9 @@ public:
k_param_flybywire_airspeed_max,
//
// 130: Sensor parameters
// 140: Sensor parameters
//
k_param_IMU_calibration = 130,
k_param_IMU_calibration = 140,
k_param_altitude_mix,
k_param_airspeed_ratio,
k_param_ground_temperature,
@ -96,26 +77,22 @@ public:
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
// 160: Navigation parameters
//
k_param_crosstrack_gain = 150,
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,
k_param_inverted_flight_ch,
//
// 170: Radio settings
// 180: Radio settings
//
k_param_channel_roll = 170,
k_param_channel_roll = 180,
k_param_channel_pitch,
k_param_channel_throttle,
k_param_channel_yaw,
@ -123,22 +100,20 @@ public:
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,
k_param_throttle_slewrate,
k_param_rc_5_funct,
k_param_rc_6_funct,
k_param_rc_7_funct,
k_param_rc_8_funct,
//
// 200: Feed-forward gains
@ -148,17 +123,6 @@ public:
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
//
@ -172,7 +136,7 @@ public:
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from command to roll command deviation from trim
// heading error from commnd to roll command deviation from trim
// (bank to turn strategy)
//
k_param_heading_to_roll_PID = 240,
@ -193,7 +157,7 @@ public:
k_param_pitch_to_servo_PID,
// Airspeed-to-pitch PID:
// airspeed error from command to pitch servo deviation from trim
// airspeed error from commnd to pitch servo deviation from trim
// (back-side strategy)
//
k_param_airspeed_to_pitch_PID,
@ -202,7 +166,7 @@ public:
// Yaw control
//
// Yaw-to-servo PID:
// yaw rate error from command to yaw servo deviation from trim
// yaw rate error from commnd to yaw servo deviation from trim
// (stabilizes dutch roll)
//
k_param_yaw_to_servo_PID,
@ -217,22 +181,20 @@ public:
k_param_energy_to_throttle_PID,
// Altitude-to-pitch PID:
// altitude error from command to pitch servo deviation from trim
// altitude error from commnd to pitch servo deviation from trim
// (throttle front-side strategy alternative)
//
k_param_altitude_to_pitch_PID,
// 254,255: reserved
// 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
//
@ -250,7 +212,6 @@ public:
//
AP_Float altitude_mix;
AP_Float airspeed_ratio;
AP_Int16 airspeed_offset;
// Waypoints
//
@ -269,7 +230,6 @@ public:
//
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;
@ -282,12 +242,7 @@ public:
// 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;
AP_VarA<uint8_t,6> flight_modes;
// Navigational maneuvering limits
//
@ -299,11 +254,6 @@ public:
//
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;
@ -314,13 +264,6 @@ public:
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;
// RC channels
RC_Channel channel_roll;
@ -331,10 +274,8 @@ public:
RC_Channel rc_6;
RC_Channel rc_7;
RC_Channel rc_8;
AP_Int8 rc_5_funct;
AP_Int8 rc_6_funct;
AP_Int8 rc_7_funct;
AP_Int8 rc_8_funct;
RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll;
// PID controllers
//
@ -352,12 +293,10 @@ public:
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")),
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")),
serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")),
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")),
@ -369,7 +308,6 @@ public:
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")),
@ -382,22 +320,16 @@ public:
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_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")),
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")),
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")),
@ -405,33 +337,15 @@ public:
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")),
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")),
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")),
rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")),
rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")),
rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")),
rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")),
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!
@ -441,17 +355,19 @@ public:
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_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("ARSP2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSPD2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
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),

49
ArduPlane/WP_activity.pde Normal file
View File

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

44
ArduPlane/climb_rate.pde
View File

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

99
ArduPlane/commands.pde
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void init_commands()
void init_commands()
{
//read_EEPROM_waypoint_info();
g.waypoint_index.set_and_save(0);
@ -9,19 +9,15 @@ static void init_commands()
next_command.id = CMD_BLANK;
}
static void update_auto()
void update_auto()
{
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
}
if (g.waypoint_index == g.waypoint_total) {
do_RTL();
}
}
// this is only used by an air-start
static void reload_commands_airstart()
void reload_commands_airstart()
{
init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
@ -30,7 +26,7 @@ static void reload_commands_airstart()
// Getters
// -------
static struct Location get_wp_with_index(int i)
struct Location get_wp_with_index(int i)
{
struct Location temp;
long mem;
@ -70,13 +66,12 @@ static struct Location get_wp_with_index(int i)
// Setters
// -------
static void set_wp_with_index(struct Location temp, int i)
void set_wp_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
// 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 {
@ -101,26 +96,26 @@ static void set_wp_with_index(struct Location temp, int i)
eeprom_write_dword((uint32_t *) mem, temp.lng);
}
static void increment_WP_index()
void increment_WP_index()
{
if (g.waypoint_index <= g.waypoint_total) {
if (g.waypoint_index < g.waypoint_total) {
g.waypoint_index.set_and_save(g.waypoint_index + 1);
SendDebug_P("MSG - WP index is incremented to ");
SendDebug_P("MSG <increment_WP_index> 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);
}
static void decrement_WP_index()
void decrement_WP_index()
{
if (g.waypoint_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1);
}
}
static long read_alt_to_hold()
long read_alt_to_hold()
{
if(g.RTL_altitude < 0)
return current_loc.alt;
@ -129,14 +124,30 @@ static long read_alt_to_hold()
}
//********************************************************************************
//This function sets the waypoint and modes for Return to Launch
//********************************************************************************
Location get_LOITER_home_wp()
{
//so we know where we are navigating from
next_WP = current_loc;
// read home position
struct Location temp = get_wp_with_index(0);
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold();
return temp;
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
static void set_next_WP(struct Location *wp)
void set_next_WP(struct Location *wp)
{
//gcs.send_text_P(SEVERITY_LOW,PSTR("load WP"));
SendDebug_P("MSG - wp_index: ");
SendDebug_P("MSG <set_next_wp> wp_index: ");
SendDebugln(g.waypoint_index, DEC);
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -162,44 +173,6 @@ static void set_next_WP(struct Location *wp)
loiter_sum = 0;
loiter_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);
@ -217,13 +190,15 @@ static void set_guided_WP(void)
// 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");
SendDebugln("MSG: <init_home> init home");
// block until we get a good fix
// -----------------------------
@ -246,12 +221,6 @@ 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;
}

104
ArduPlane/commands_logic.pde
View File

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

20
ArduPlane/commands_process.pde
View File

@ -2,15 +2,15 @@
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t cmd_index)
void change_command(uint8_t index)
{
struct Location temp = get_wp_with_index(cmd_index);
struct Location temp = get_wp_with_index(index);
if (temp.id > MAV_CMD_NAV_LAST ){
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(cmd_index - 1);
g.waypoint_index.set_and_save(index - 1);
load_next_command_from_EEPROM();
process_next_command();
}
@ -19,7 +19,7 @@ static void change_command(uint8_t cmd_index)
// called by 10 Hz loop
// --------------------
static void update_commands(void)
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 @@ static void update_commands(void)
} // Other (eg GCS_Auto) modes may be implemented here
}
static void verify_commands(void)
void verify_commands(void)
{
if(verify_must()){
command_must_index = NO_COMMAND;
@ -46,7 +46,7 @@ static void verify_commands(void)
}
}
static void load_next_command_from_EEPROM()
void load_next_command_from_EEPROM()
{
// fetch next command if the next command queue is empty
// -----------------------------------------------------
@ -64,7 +64,7 @@ static void load_next_command_from_EEPROM()
}
}
static void process_next_command()
void process_next_command()
{
// these are Navigation/Must commands
// ---------------------------------
@ -115,7 +115,7 @@ static void process_next_command()
/**************************************************/
// These functions implement the commands.
/**************************************************/
static void process_must()
void process_must()
{
gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -132,7 +132,7 @@ static void process_must()
next_command.id = NO_COMMAND;
}
static void process_may()
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 @@ static void process_may()
next_command.id = NO_COMMAND;
}
static void process_now()
void process_now()
{
handle_process_now();

117
ArduPlane/config.h
View File

@ -46,31 +46,13 @@
// 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
@ -187,6 +169,9 @@
#ifndef MAG_ORIENTATION
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#endif
#ifndef MAG_PROTOCOL
# define MAG_PROTOCOL MAG_PROTOCOL_5843
#endif
//////////////////////////////////////////////////////////////////////////////
@ -199,7 +184,8 @@
//////////////////////////////////////////////////////////////////////////////
// Radio channel limits
//
// Note that these are not called out in APM_Config.h.reference.
// Note that these are not called out in APM_Config.h.example as there
// is currently no configured behaviour for these channels.
//
#ifndef CH5_MIN
# define CH5_MIN 1000
@ -226,32 +212,7 @@
# define CH8_MAX 2000
#endif
//////////////////////////////////////////////////////////////////////////////
#ifndef RC_5_FUNCT
# define RC_5_FUNCT RC_5_FUNCT_NONE
#endif
#ifndef RC_6_FUNCT
# define RC_6_FUNCT RC_6_FUNCT_NONE
#endif
#ifndef RC_7_FUNCT
# define RC_7_FUNCT RC_7_FUNCT_NONE
#endif
#ifndef RC_8_FUNCT
# define RC_8_FUNCT RC_8_FUNCT_NONE
#endif
#ifndef FLAP_1_PERCENT
# define FLAP_1_PERCENT 0
#endif
#ifndef FLAP_1_SPEED
# define FLAP_1_SPEED 255
#endif
#ifndef FLAP_2_PERCENT
# define FLAP_2_PERCENT 0
#endif
#ifndef FLAP_2_SPEED
# define FLAP_2_SPEED 255
#endif
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE
// FLIGHT_MODE_CHANNEL
@ -295,14 +256,14 @@
#ifndef THROTTLE_FAILSAFE
# define THROTTLE_FAILSAFE ENABLED
#endif
#ifndef THROTTLE_FS_VALUE
#ifndef THROTTE_FS_VALUE
# define THROTTLE_FS_VALUE 950
#endif
#ifndef SHORT_FAILSAFE_ACTION
# define SHORT_FAILSAFE_ACTION 0
# define SHORT_FAILSAFE_ACTION 2
#endif
#ifndef LONG_FAILSAFE_ACTION
# define LONG_FAILSAFE_ACTION 0
# define LONG_FAILSAFE_ACTION 2
#endif
#ifndef GCS_HEARTBEAT_FAILSAFE
# define GCS_HEARTBEAT_FAILSAFE DISABLED
@ -369,22 +330,6 @@
# 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
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
@ -611,12 +556,6 @@
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
#ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED
#endif
@ -648,22 +587,6 @@
# 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
@ -710,10 +633,6 @@
# define USE_CURRENT_ALT FALSE
#endif
#ifndef INVERTED_FLIGHT_PWM
# define INVERTED_FLIGHT_PWM 1750
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
@ -724,17 +643,3 @@
#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,11 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void read_control_switch()
void read_control_switch()
{
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
set_mode(flight_modes[switchPosition]);
set_mode(g.flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
prev_WP = current_loc;
@ -14,15 +12,9 @@ static 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);
}
}
static byte readSwitch(void){
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;
@ -32,7 +24,7 @@ static byte readSwitch(void){
return 0;
}
static void reset_control_switch()
void reset_control_switch()
{
oldSwitchPosition = 0;
read_control_switch();
@ -40,23 +32,52 @@ static void reset_control_switch()
SendDebugln(oldSwitchPosition , DEC);
}
static void update_servo_switches()
void update_servo_switches()
{
if (!g.switch_enable) {
// switches are disabled in EEPROM (see SWITCH_ENABLE option)
// this means the EEPROM control of all channel reversal is enabled
return;
}
// up is reverse
// up is elevon
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);
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;
} else {
g.reverse_elevons = (PINE & 128) ? true : false;
g.reverse_ch1_elevon = (PINE & 64) ? true : false;
g.reverse_ch2_elevon = (PINL & 64) ? true : false;
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;
}
}
}

59
ArduPlane/defines.h
View File

@ -10,7 +10,9 @@
#define DEBUG 0
#define LOITER_RANGE 60 // for calculating power outside of loiter radius
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
#define ROLL_SERVO_MAX 4500
#define PITCH_SERVO_MAX 4500
#define RUDDER_SERVO_MAX 4500
// failsafe
// ----------------------
@ -30,6 +32,10 @@
#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
@ -60,20 +66,6 @@
#define CH_RUDDER CH_4
#define CH_YAW CH_4
#define RC_5_FUNCT_NONE 0
#define RC_5_FUNCT_AILERON 1
#define RC_5_FUNCT_FLAP_AUTO 2
#define RC_5_FUNCT_FLAPERON 3
#define RC_6_FUNCT_NONE 0
#define RC_6_FUNCT_AILERON 1
#define RC_6_FUNCT_FLAP_AUTO 2
#define RC_6_FUNCT_FLAPERON 3
#define RC_7_FUNCT_NONE 0
#define RC_8_FUNCT_NONE 0
// HIL enumerations
#define HIL_PROTOCOL_XPLANE 1
#define HIL_PROTOCOL_MAVLINK 2
@ -98,15 +90,12 @@
#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle
#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed
#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed
// Fly By Wire B and Fly By Wire C require airspeed sensor
// Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0
#define AUTO 10
#define RTL 11
#define LOITER 12
//#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
#define TAKEOFF 13
#define LAND 14
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
@ -129,10 +118,6 @@
#define MAV_CMD_CONDITION_YAW 23
// 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
@ -143,10 +128,9 @@
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
#define MSG_VERSION_REQUEST 0x08
#define MSG_VERSION 0x09
#define MSG_EXTENDED_STATUS1 0x0a
#define MSG_EXTENDED_STATUS2 0x0b
#define MSG_CPU_LOAD 0x0c
#define MSG_NAV_CONTROLLER_OUTPUT 0x0d
#define MSG_EXTENDED_STATUS 0x0a
#define MSG_CPU_LOAD 0x0b
#define MSG_NAV_CONTROLLER_OUTPUT 0x0c
#define MSG_COMMAND_REQUEST 0x20
#define MSG_COMMAND_UPLOAD 0x21
@ -169,11 +153,9 @@
#define MSG_RADIO_OUT 0x53
#define MSG_RADIO_IN 0x54
#define MSG_RAW_IMU1 0x60
#define MSG_RAW_IMU2 0x61
#define MSG_RAW_IMU3 0x62
#define MSG_GPS_STATUS 0x63
#define MSG_GPS_RAW 0x64
#define MSG_RAW_IMU 0x60
#define MSG_GPS_STATUS 0x61
#define MSG_GPS_RAW 0x62
#define MSG_SERVO_OUT 0x70
@ -191,7 +173,6 @@
#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
@ -224,6 +205,7 @@
#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
// ----------------
@ -264,11 +246,7 @@
// 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
@ -289,6 +267,3 @@
#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 -*-
static void failsafe_short_on_event()
void failsafe_short_on_event()
{
// This is how to handle a short loss of control signal failsafe.
failsafe = FAILSAFE_SHORT;
@ -18,7 +18,7 @@ static 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,11 +32,10 @@ static void failsafe_short_on_event()
SendDebugln(control_mode, DEC);
}
static void failsafe_long_on_event()
void failsafe_long_on_event()
{
// This is how to handle a long loss of control signal failsafe.
// This is how to handle a short loss of control signal failsafe.
SendDebug_P("Failsafe - Long event on");
APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC
switch(control_mode)
{
case MANUAL:
@ -49,7 +48,7 @@ static 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;
@ -60,7 +59,7 @@ static void failsafe_long_on_event()
}
}
static void failsafe_short_off_event()
void failsafe_short_off_event()
{
// We're back in radio contact
SendDebug_P("Failsafe - Short event off");
@ -75,16 +74,15 @@ static void failsafe_short_off_event()
reset_I();
}
#if BATTERY_EVENT == ENABLED
static void low_battery_event(void)
void low_battery_event(void)
{
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
}
#endif
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
@ -110,17 +108,17 @@ static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_
}
}
static void relay_on()
void relay_on()
{
PORTL |= B00000100;
}
static void relay_off()
void relay_off()
{
PORTL &= ~B00000100;
}
static void relay_toggle()
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
//****************************************************************
static void navigate()
void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
@ -63,7 +63,7 @@ void calc_distance_error()
}
#endif
static void calc_airspeed_errors()
void calc_airspeed_errors()
{
// XXX excess casting here
if(control_mode>=AUTO && airspeed_nudge > 0) {
@ -75,7 +75,7 @@ static void calc_airspeed_errors()
}
}
static void calc_bearing_error()
void calc_bearing_error()
{
if(takeoff_complete == true || g.compass_enabled == true) {
bearing_error = nav_bearing - dcm.yaw_sensor;
@ -89,7 +89,7 @@ static void calc_bearing_error()
bearing_error = wrap_180(bearing_error);
}
static void calc_altitude_error()
void calc_altitude_error()
{
if(control_mode == AUTO && offset_altitude != 0) {
// limit climb rates
@ -127,39 +127,38 @@ static void calc_altitude_error()
altitude_error = target_altitude - current_loc.alt;
}
static long wrap_360(long error)
long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
static long wrap_180(long error)
long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
static void update_loiter()
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.5, 1); //power = constrain(power, 0, 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{
@ -167,27 +166,35 @@ static void update_loiter()
}
update_crosstrack();
*/
nav_bearing = wrap_360(nav_bearing);
}
static void update_crosstrack(void)
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) / (float)100)) * (float)wp_distance; // Meters we are off track line
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
}
}
static void reset_crosstrack()
void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
static long get_distance(struct Location *loc1, struct Location *loc2)
long get_altitude_above_home(void) // TO DO - this function has been deprecated - check if any useage vs removal
{
// This is the altitude above the home location
// The GPS gives us altitude at Sea Level
// if you slope soar, you should see a negative number sometimes
// -------------------------------------------------------------
return current_loc.alt - home.alt;
}
long get_distance(struct Location *loc1, struct Location *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
@ -198,7 +205,12 @@ static long get_distance(struct Location *loc1, struct Location *loc2)
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
static long get_bearing(struct Location *loc1, struct Location *loc2)
long get_alt_distance(struct Location *loc1, struct Location *loc2)
{
return abs(loc1->alt - loc2->alt);
}
long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
@ -206,3 +218,14 @@ static long get_bearing(struct Location *loc1, struct Location *loc2)
if (bearing < 0) bearing += 36000;
return bearing;
}
Vector3f get_location_vector(struct Location *uav, struct Location *target)
{
Vector3<float> v;
v.x = (target->lng-uav->lng) * cos((uav->lat+target->lat)/2)*.01113195;
v.y = (target->lat-uav->lat)*.01113195;
v.z = (uav->alt-target->alt);
return v;
}

7
ArduPlane/planner.pde
View File

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

66
ArduPlane/radio.pde
View File

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

40
ArduPlane/sensors.pde
View File

@ -5,17 +5,21 @@
void ReadSCP1000(void) {}
static void init_barometer(void)
void init_barometer(void)
{
int flashcount = 0;
int flashcount;
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;
mavlink_delay(20);
delay(20);
//Serial.printf("barometer.Press %ld\n", barometer.Press);
}
@ -29,7 +33,7 @@ static void init_barometer(void)
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
mavlink_delay(20);
delay(20);
if(flashcount == 5) {
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
@ -49,11 +53,11 @@ static 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);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete."));
Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
SendDebugln_P("barometer calibration complete.");
}
static long read_barometer(void)
long read_barometer(void)
{
float x, scaling, temp;
@ -69,38 +73,30 @@ static long read_barometer(void)
}
// in M/S * 100
static void read_airspeed(void)
void read_airspeed(void)
{
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed
if (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 - g.airspeed_offset), 0);
airspeed_pressure = max(((int)airspeed_raw - airspeed_offset), 0);
airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100;
#endif
calc_airspeed_errors();
}
static void zero_airspeed(void)
void zero_airspeed(void)
{
airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
for(int c = 0; c < 10; c++){
for(int c = 0; c < 50; c++){
delay(20);
airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10);
}
g.airspeed_offset.set_and_save(airspeed_raw);
airspeed_offset = airspeed_raw;
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void read_battery(void)
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;
@ -118,7 +114,7 @@ static void read_battery(void)
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
}
#if BATTERY_EVENT == ENABLED
#if BATTERY_EVENT == 1
if(battery_voltage < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
#endif

145
ArduPlane/setup.pde
View File

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

168
ArduPlane/system.pde
View File

@ -6,13 +6,11 @@ The init_ardupilot function processes everything we need for an in - air restart
*****************************************************************************/
#if CLI_ENABLED == ENABLED
// Functions called from the top-level menu
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
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
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
@ -30,7 +28,7 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
}
// Command/function table for the top-level menu.
static const struct Menu::command main_menu_commands[] PROGMEM = {
const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"logs", process_logs},
@ -41,11 +39,9 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "APM trunk", main_menu_commands);
MENU(main_menu, "ArduPilotMega", main_menu_commands);
#endif // CLI_ENABLED
static void init_ardupilot()
void init_ardupilot()
{
// Console serial port
//
@ -77,6 +73,16 @@ static 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());
@ -86,10 +92,7 @@ static void init_ardupilot()
//
if (!g.format_version.load()) {
Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n"));
delay(100); // wait for serial msg to flush
AP_Var::erase_all();
Serial.println("\n\nEEPROM blank - all parameters are reset to default values.\n");
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
@ -99,7 +102,6 @@ static void init_ardupilot()
Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
"\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();
@ -114,48 +116,45 @@ static 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 (%u resets)\n"),
AP_Var::get_memory_use(), (unsigned)g.num_resets);
Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
}
// 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);
if (!g.flight_modes.load()) {
default_flight_modes();
}
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
default_log_bitmask();
}
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) {
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
}
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
}
/*
Init is depricated - Jason
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC);
}
*/
#else
airspeed_enabled = true;
#endif
DataFlash.Init(); // DataFlash log initialization
@ -163,7 +162,6 @@ static 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
@ -210,7 +208,6 @@ static 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"
@ -224,7 +221,6 @@ static void init_ardupilot()
main_menu.run();
}
}
#endif // CLI_ENABLED
if(g.log_bitmask != 0){
@ -272,8 +268,6 @@ static void init_ardupilot()
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
}
set_mode(MANUAL);
// set the correct flight mode
// ---------------------------
reset_control_switch();
@ -282,10 +276,8 @@ static void init_ardupilot()
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
static void startup_ground(void)
void startup_ground(void)
{
set_mode(INITIALISING);
gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
#if(GROUND_START_DELAY > 0)
@ -314,7 +306,7 @@ static void startup_ground(void)
trim_radio(); // This was commented out as a HACK. Why? I don't find a problem.
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.airspeed_enabled == true)
if (airspeed_enabled == true)
{
// initialize airspeed sensor
// --------------------------
@ -357,7 +349,7 @@ else
gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}
static void set_mode(byte mode)
void set_mode(byte mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
@ -371,11 +363,18 @@ static void set_mode(byte mode)
switch(control_mode)
{
case INITIALISING:
case MANUAL:
break;
case CIRCLE:
break;
case STABILIZE:
break;
case FLY_BY_WIRE_A:
break;
case FLY_BY_WIRE_B:
break;
@ -391,8 +390,10 @@ static void set_mode(byte mode)
do_loiter_at_location();
break;
case GUIDED:
set_guided_WP();
case TAKEOFF:
break;
case LAND:
break;
default:
@ -407,7 +408,7 @@ static void set_mode(byte mode)
Log_Write_Mode(control_mode);
}
static void check_long_failsafe()
void check_long_failsafe()
{
// only act on changes
// -------------------
@ -432,7 +433,7 @@ static void check_long_failsafe()
}
}
static void check_short_failsafe()
void check_short_failsafe()
{
// only act on changes
// -------------------
@ -450,19 +451,20 @@ static void check_short_failsafe()
}
static void startup_IMU_ground(void)
void startup_IMU_ground(void)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
uint16_t store = 0;
SendDebugln("<startup_IMU_ground> Warming up ADC...");
delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
// -----------------------
demo_servos(2);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane"));
mavlink_delay(1000);
SendDebugln("<startup_IMU_ground> Beginning IMU calibration; do not move plane");
delay(1000);
imu.init(IMU::COLD_START, mavlink_delay);
imu.init(IMU::COLD_START);
dcm.set_centripetal(1);
// read Baro pressure at ground
@ -477,7 +479,7 @@ static void startup_IMU_ground(void)
}
static void update_GPS_light(void)
void update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
@ -505,16 +507,15 @@ static void update_GPS_light(void)
}
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
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;
pmTest1 = 0;
perf_mon_timer = millis();
gps_fix_count = 0;
perf_mon_timer = millis();
}
@ -525,7 +526,7 @@ static void resetPerfData(void) {
* be larger than HP or you'll be in big trouble! The smaller the gap, the more
* careful you need to be. Julian Gall 6 - Feb - 2009.
*/
static unsigned long freeRAM() {
unsigned long freeRAM() {
uint8_t * heapptr, * stackptr;
stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
heapptr = stackptr; // save value of heap pointer
@ -534,20 +535,3 @@ static 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,6 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
@ -28,7 +26,7 @@ static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] PROGMEM = {
const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"failsafe", test_failsafe},
@ -66,15 +64,14 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
static int8_t
int8_t
test_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run();
return 0;
}
static void print_hit_enter()
void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
}
@ -107,7 +104,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
// ----------------------------------------------------------
read_radio();
Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
@ -150,7 +147,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
// write out the servo PWM values
// ------------------------------
set_servos();
set_servos_4();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.control_in,
@ -258,7 +255,7 @@ if (g.battery_monitoring == 4) {
// write out the servo PWM values
// ------------------------------
set_servos();
set_servos_4();
if(Serial.available() > 0){
return (0);
@ -318,11 +315,11 @@ test_wp(uint8_t argc, const Menu::arg *argv)
return (0);
}
static void
test_wp_print(struct Location *cmd, byte wp_index)
void
test_wp_print(struct Location *cmd, byte index)
{
Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index,
(int)index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
@ -379,24 +376,20 @@ 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 (g.mix_mode == 0) {
if (mix_mode == 0) {
Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
(int)g.channel_roll.get_reverse(),
(int)g.channel_pitch.get_reverse(),
(int)g.channel_rudder.get_reverse());
(int)reverse_roll,
(int)reverse_pitch,
(int)reverse_rudder);
} else {
Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
(int)g.reverse_elevons,
(int)g.reverse_ch1_elevon,
(int)g.reverse_ch2_elevon);
(int)reverse_elevons,
(int)reverse_ch1_elevon,
(int)reverse_ch2_elevon);
}
if(Serial.available() > 0){
return (0);
@ -532,29 +525,13 @@ 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;
@ -565,38 +542,40 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// ---
dcm.update_DCM(G_Dt);
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;
}
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;
}
}
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;
}
}
if (Serial.available() > 0) {
break;
}
}
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;
}
// 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);
}
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
@ -609,11 +588,14 @@ test_mag(uint8_t argc, const Menu::arg *argv)
static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
unsigned airspeed_ch = adc.Ch(AIRSPEED_CH);
// Serial.println(adc.Ch(AIRSPEED_CH));
Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch);
if ((adc.Ch(AIRSPEED_CH) > 2000) && (adc.Ch(AIRSPEED_CH) < 2900)){
airspeed_enabled = false;
}else{
airspeed_enabled = true;
}
if (g.airspeed_enabled == false){
if (airspeed_enabled == false){
Serial.printf_P(PSTR("airspeed: "));
print_enabled(false);
return (0);
@ -632,8 +614,11 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
if(Serial.available() > 0){
return (0);
}
if(Serial.available() > 0){
return (0);
}
}
}
}
@ -684,5 +669,3 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED