Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 10:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity
e1f8609b78
ardupilot/ArduPlane/ArduPlane.cpp
James Goppert e1f8609b78 Cmake working for ArduCoptre/ ArduPlane 
Need to get pde processing working.
2011-10-30 01:17:51 -04:00

7974 lines
228 KiB
C++
Raw Blame History

#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/ArduPlane.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduPlane V2.24"
/*
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
Please contribute your ideas!
This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#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
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#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_Relay.h> // APM relay
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <memcheck.h>
// Configuration
#include "config.h"
// Local modules
#include "defines.h"
#include "Parameters.h"
#include "GCS.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
#line 1 "autogenerated"
#include "WProgram.h"
void setup() ;
void loop() ;
static void fast_loop() ;
static void medium_loop() ;
static void slow_loop() ;
static void one_second_loop() ;
static void update_GPS(void) ;
static void update_current_flight_mode(void) ;
static void update_navigation() ;
static void update_alt() ;
static void stabilize() ;
static void crash_checker() ;
static void calc_throttle() ;
static void calc_nav_yaw(float speed_scaler) ;
static void calc_nav_pitch() ;
static void calc_nav_roll() ;
float roll_slew_limit(float servo) ;
static void throttle_slew_limit() ;
static void reset_I(void) ;
static void set_servos(void) ;
static void demo_servos(byte i) ;
static NOINLINE void send_heartbeat(mavlink_channel_t chan) ;
static NOINLINE void send_attitude(mavlink_channel_t chan) ;
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) ;
static void NOINLINE send_meminfo(mavlink_channel_t chan) ;
static void NOINLINE send_location(mavlink_channel_t chan) ;
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) ;
static void NOINLINE send_gps_raw(mavlink_channel_t chan) ;
static void NOINLINE send_servo_out(mavlink_channel_t chan) ;
static void NOINLINE send_radio_in(mavlink_channel_t chan) ;
static void NOINLINE send_radio_out(mavlink_channel_t chan) ;
static void NOINLINE send_vfr_hud(mavlink_channel_t chan) ;
static void NOINLINE send_raw_imu1(mavlink_channel_t chan) ;
static void NOINLINE send_raw_imu2(mavlink_channel_t chan) ;
static void NOINLINE send_raw_imu3(mavlink_channel_t chan) ;
static void NOINLINE send_gps_status(mavlink_channel_t chan) ;
static void NOINLINE send_current_waypoint(mavlink_channel_t chan) ;
static void NOINLINE send_statustext(mavlink_channel_t chan) ;
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ;
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ;
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) ;
static void mavlink_delay(unsigned long t) ;
static void gcs_send_message(enum ap_message id) ;
static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) ;
static void gcs_update(void) ;
static void gcs_send_text(gcs_severity severity, const char *str) ;
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) ;
static bool print_log_menu(void) ;
static byte get_num_logs(void) ;
static void start_new_log(byte num_existing_logs) ;
static void get_log_boundaries(byte log_num, int & start_page, int & end_page) ;
static int find_last_log_page(int bottom_page) ;
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) ;
static void Log_Write_Performance() ;
static void Log_Write_Cmd(byte num, struct Location *wp) ;
static void Log_Write_Startup(byte type) ;
static void Log_Write_Control_Tuning() ;
static void Log_Write_Nav_Tuning() ;
static void Log_Write_Mode(byte mode) ;
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_Raw() ;
static void Log_Write_Current() ;
static void Log_Read_Current() ;
static void Log_Read_Control_Tuning() ;
static void Log_Read_Nav_Tuning() ;
static void Log_Read_Performance() ;
static void Log_Read_Cmd() ;
static void Log_Read_Startup() ;
static void Log_Read_Attitude() ;
static void Log_Read_Mode() ;
static void Log_Read_GPS() ;
static void Log_Read_Raw() ;
static void Log_Read(int start_page, int end_page) ;
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 byte get_num_logs(void) ;
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() ;
static void add_altitude_data(unsigned long xl, long y) ;
static void init_commands() ;
static void update_auto() ;
static void reload_commands_airstart() ;
static struct Location get_cmd_with_index(int i) ;
static void set_cmd_with_index(struct Location temp, int i) ;
static void increment_cmd_index() ;
static void decrement_cmd_index() ;
static long read_alt_to_hold() ;
static void set_next_WP(struct Location *wp) ;
static void set_guided_WP(void) ;
void init_home() ;
static void handle_process_nav_cmd() ;
static void handle_process_condition_command() ;
static void handle_process_do_command() ;
static void handle_no_commands() ;
static void do_RTL(void) ;
static void do_takeoff() ;
static void do_nav_wp() ;
static void do_land() ;
static void do_loiter_unlimited() ;
static void do_loiter_turns() ;
static void do_loiter_time() ;
static bool verify_takeoff() ;
static bool verify_land() ;
static bool verify_nav_wp() ;
static bool verify_loiter_unlim() ;
static bool verify_loiter_time() ;
static bool verify_loiter_turns() ;
static bool verify_RTL() ;
static void do_wait_delay() ;
static void do_change_alt() ;
static void do_within_distance() ;
static bool verify_wait_delay() ;
static bool verify_change_alt() ;
static bool verify_within_distance() ;
static void do_loiter_at_location() ;
static void do_jump() ;
static void do_change_speed() ;
static void do_set_home() ;
static void do_set_servo() ;
static void do_set_relay() ;
static void do_repeat_servo() ;
static void do_repeat_relay() ;
static void change_command(uint8_t cmd_index) ;
static void update_commands(void) ;
static void verify_commands(void) ;
static void process_next_command() ;
static void process_nav_cmd() ;
static void process_non_nav_command() ;
static void read_control_switch() ;
static byte readSwitch(void);
static void reset_control_switch() ;
static void update_servo_switches() ;
static void failsafe_short_on_event() ;
static void failsafe_long_on_event() ;
static void failsafe_short_off_event() ;
static void low_battery_event(void) ;
static void navigate() ;
void calc_distance_error() ;
static void calc_airspeed_errors() ;
static void calc_bearing_error() ;
static void calc_altitude_error() ;
static long wrap_360(long error) ;
static long wrap_180(long error) ;
static void update_loiter() ;
static void update_crosstrack(void) ;
static void reset_crosstrack() ;
static long get_distance(struct Location *loc1, struct Location *loc2) ;
static long get_bearing(struct Location *loc1, struct Location *loc2) ;
static void init_rc_in() ;
static void init_rc_out() ;
static void read_radio() ;
static void control_failsafe(uint16_t pwm) ;
static void trim_control_surfaces() ;
static void trim_radio() ;
void ReadSCP1000(void) ;
static void init_barometer(void) ;
static long read_barometer(void) ;
static void read_airspeed(void) ;
static void zero_airspeed(void) ;
static void read_battery(void) ;
static void report_batt_monitor() ;
static void report_radio() ;
static void report_gains() ;
static void report_xtrack() ;
static void report_throttle() ;
static void report_imu() ;
static void report_compass() ;
static void report_flight_modes() ;
static void print_PID(PID * pid) ;
static void print_radio_values() ;
static void print_switch(byte p, byte m) ;
static void print_done() ;
static void print_blanks(int num) ;
static void print_divider(void) ;
static int8_t radio_input_switch(void) ;
static void zero_eeprom(void) ;
static void print_enabled(bool b) ;
static void print_accel_offsets(void) ;
static void print_gyro_offsets(void) ;
static void run_cli(void) ;
static void init_ardupilot() ;
static void startup_ground(void) ;
static void set_mode(byte mode) ;
static void check_long_failsafe() ;
static void check_short_failsafe() ;
static void startup_IMU_ground(void) ;
static void update_GPS_light(void) ;
static void resetPerfData(void) ;
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) ;
static void print_hit_enter() ;
static void test_wp_print(struct Location *cmd, byte wp_index) ;
#line 64 "/home/jgoppert/Projects/ardupilotone/ArduPlane/ArduPlane.pde"
FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port
FastSerialPort3(Serial3); // Telemetry port
////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
//
// Global parameters are all contained within the 'g' class.
//
static Parameters g;
////////////////////////////////////////////////////////////////////////////////
// prototypes
static void update_events(void);
////////////////////////////////////////////////////////////////////////////////
// Sensors
////////////////////////////////////////////////////////////////////////////////
//
// There are three basic options related to flight sensor selection.
//
// - Normal flight mode. Real sensors are used.
// - HIL Attitude mode. Most sensors are disabled, as the HIL
// protocol supplies attitude information directly.
// - HIL Sensors mode. Synthetic sensors are configured that
// supply data from the simulation.
//
// All GPS access should be through this pointer.
static GPS *g_gps;
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
#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);
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
AP_GPS_MTK16 g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver(NULL);
#else
#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL
#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
AP_ADC_HIL adc;
APM_BMP085_HIL_Class barometer;
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
AP_IMU_Shim imu; // never used
#else
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
#if HIL_MODE != HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_SENSORS
// Normal
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration);
#else
// hil imu
AP_IMU_Shim imu;
#endif
// normal dcm
AP_DCM dcm(&imu, g_gps);
#endif
////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0);
GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
////////////////////////////////////////////////////////////////////////////////
// SONAR selection
////////////////////////////////////////////////////////////////////////////////
//
ModeFilter sonar_mode_filter;
#if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#elif SONAR_TYPE == MAX_SONAR_LV
// XXX honestly I think these output the same values
// If someone knows, can they confirm it?
AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#endif
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
byte control_mode = INITIALISING;
byte oldSwitchPosition; // for remembering the control mode switch
bool inverted_flight = false;
static const char *comma = ",";
static const char* flight_mode_strings[] = {
"Manual",
"Circle",
"Stabilize",
"",
"",
"FBW_A",
"FBW_B",
"",
"",
"",
"Auto",
"RTL",
"Loiter",
"Takeoff",
"Land"};
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
3 Throttle
4 Rudder (if we have ailerons)
5 Aux5
6 Aux6
7 Aux7
8 Aux8/Mode
Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
See libraries/RC_Channel/RC_Channel_aux.h for more information
*/
// Failsafe
// --------
static int failsafe; // track which type of failsafe is being processed
static bool ch3_failsafe;
static 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;
// 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
// 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
// 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 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
static byte command_index; // current command memory location
static byte nav_command_index; // active nav command memory location
static byte non_nav_command_index; // active non-nav command memory location
static byte nav_command_ID = NO_COMMAND; // active nav command ID
static byte non_nav_command_ID = NO_COMMAND; // active non-nav 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 float airspeed_fbwB; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error
// Location Errors
// ---------------
static long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude
static float crosstrack_error; // meters we are off trackline
// Battery Sensors
// ---------------
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
static float current_amps;
static 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
// Barometer Sensor variables
// --------------------------
static unsigned long abs_pressure;
// Altitude Sensor variables
// ----------------------
static int sonar_alt;
// 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;
// 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
// 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
// Waypoints
// ---------
static long wp_distance; // meters - distance between plane and next waypoint
static long wp_totalDistance; // meters - distance between old and next waypoint
// repeating event control
// -----------------------
static byte event_id; // what to do - see defines
static long event_timer; // when the event was asked for in ms
static uint16_t event_delay; // how long to delay the next firing of event in millis
static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
static int event_value; // per command value, such as PWM for servos
static int event_undo_value; // the value used to cycle events (alternate value to event_value)
// delay command
// --------------
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start;
static 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 guided_WP; // guided mode waypoint
static struct Location next_nav_command; // command preloaded
static struct Location next_nonnav_command; // command preloaded
static long target_altitude; // used for altitude management between waypoints
static long offset_altitude; // used for altitude management between waypoints
static bool home_is_set; // Flag for if we have g_gps lock and have set the home location
// IMU variables
// -------------
static 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;
// 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;
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;
static byte slow_loopCounter;
static byte superslow_loopCounter;
static byte counter_one_herz;
static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static float load; // % MCU cycles used
AP_Relay relay;
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
void setup() {
memcheck_init();
init_ardupilot();
}
void loop()
{
// We want this to execute at 50Hz if possible
// -------------------------------------------
if (millis()-fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop;
G_Dt = (float)delta_ms_fast_loop / 1000.f;
fast_loopTimer = millis();
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// Execute the medium loop
// -----------------------
medium_loop();
counter_one_herz++;
if(counter_one_herz == 50){
one_second_loop();
counter_one_herz = 0;
}
if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
resetPerfData();
}
}
fast_loopTimeStamp = millis();
}
}
// Main loop 50Hz
static void fast_loop()
{
// This is the fast loop - we want it to execute at 50Hz if possible
// -----------------------------------------------------------------
if (delta_ms_fast_loop > G_Dt_max)
G_Dt_max = delta_ms_fast_loop;
// Read radio
// ----------
read_radio();
// try to send any deferred messages if the serial port now has
// some space available
gcs_send_message(MSG_RETRY_DEFERRED);
// check for loss of control signal failsafe condition
// ------------------------------------
check_short_failsafe();
// Read Airspeed
// -------------
if (g.airspeed_enabled == true) {
#if HIL_MODE != HIL_MODE_ATTITUDE
read_airspeed();
#endif
} else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
calc_airspeed_errors();
}
#if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update
gcs_update();
#endif
dcm.update_DCM();
// uses the yaw from the DCM to give more accurate turns
calc_bearing_error();
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw();
#endif
// inertial navigation
// ------------------
#if INERTIAL_NAVIGATION == ENABLED
// TODO: implement inertial nav function
inertialNavigation();
#endif
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
// apply desired roll, pitch and yaw to the plane
// ----------------------------------------------
if (control_mode > MANUAL)
stabilize();
// write out the servo PWM values
// ------------------------------
set_servos();
// XXX is it appropriate to be doing the comms below on the fast loop?
gcs_update();
gcs_data_stream_send(45,1000);
}
static void medium_loop()
{
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
// This case deals with the GPS
//-------------------------------
case 0:
medium_loopCounter++;
if(GPS_enabled) update_GPS();
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
#endif
/*{
Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
Vector3f tempaccel = imu.get_accel();
Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t"));
Serial.println(tempaccel.z, DEC);
}*/
break;
// This case performs some navigation computations
//------------------------------------------------
case 1:
medium_loopCounter++;
if(g_gps->new_data){
g_gps->new_data = false;
dTnav = millis() - nav_loopTimer;
nav_loopTimer = millis();
// calculate the plane's desired bearing
// -------------------------------------
navigate();
}
break;
// command processing
//------------------------------
case 2:
medium_loopCounter++;
// Read altitude from sensors
// ------------------
update_alt();
if(g.sonar_enabled) sonar_alt = sonar.read();
// altitude smoothing
// ------------------
if (control_mode != FLY_BY_WIRE_B)
calc_altitude_error();
// perform next command
// --------------------
update_commands();
break;
// This case deals with sending high rate telemetry
//-------------------------------------------------
case 3:
medium_loopCounter++;
#if HIL_MODE != HIL_MODE_ATTITUDE
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
#endif
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_GPS)
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
// send all requested output streams with rates requested
// between 5 and 45 Hz
gcs_data_stream_send(5,45);
break;
// This case controls the slow loop
//---------------------------------
case 4:
medium_loopCounter = 0;
delta_ms_medium_loop = millis() - medium_loopTimer;
medium_loopTimer = millis();
if (g.battery_monitoring != 0){
read_battery();
}
slow_loop();
break;
}
}
static void slow_loop()
{
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
switch (slow_loopCounter){
case 0:
slow_loopCounter++;
check_long_failsafe();
superslow_loopCounter++;
if(superslow_loopCounter >=200) { // 200 = Execute every minute
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled) {
compass.save_offsets();
}
#endif
superslow_loopCounter = 0;
}
break;
case 1:
slow_loopCounter++;
// Read 3-position switch on radio
// -------------------------------
read_control_switch();
// Read Control Surfaces/Mix switches
// ----------------------------------
update_servo_switches();
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
break;
case 2:
slow_loopCounter = 0;
update_events();
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);
break;
}
}
static void one_second_loop()
{
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
}
static void update_GPS(void)
{
g_gps->update();
update_GPS_light();
if (g_gps->new_data && g_gps->fix) {
// for performance
// ---------------
gps_fix_count++;
if(ground_start_count > 1){
ground_start_count--;
ground_start_avg += g_gps->ground_speed;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
ground_start_count = 5;
} else {
if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){
startup_ground();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
init_home();
} else if (ENABLE_AIR_START == 0) {
init_home();
}
ground_start_count = 0;
}
}
current_loc.lng = g_gps->longitude; // Lon * 10**7
current_loc.lat = g_gps->latitude; // Lat * 10**7
}
}
static void update_current_flight_mode(void)
{
if(control_mode == AUTO){
crash_checker();
switch(nav_command_ID){
case MAV_CMD_NAV_TAKEOFF:
if (hold_course > -1) {
calc_nav_roll();
} else {
nav_roll = 0;
}
if (g.airspeed_enabled == true)
{
calc_nav_pitch();
if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch;
} else {
nav_pitch = (long)((float)g_gps->ground_speed / (float)g.airspeed_cruise * (float)takeoff_pitch * 0.5);
nav_pitch = constrain(nav_pitch, 500l, (long)takeoff_pitch);
}
g.channel_throttle.servo_out = g.throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle
// What is the case for doing something else? Why wouldn't you want max throttle for TO?
// ******************************
break;
case MAV_CMD_NAV_LAND:
calc_nav_roll();
if (g.airspeed_enabled == true){
calc_nav_pitch();
calc_throttle();
}else{
calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle
calc_throttle(); // throttle based on altitude error
nav_pitch = landing_pitch; // pitch held constant
}
if (land_complete){
g.channel_throttle.servo_out = 0;
}
break;
default:
hold_course = -1;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
}
}else{
switch(control_mode){
case RTL:
case LOITER:
case GUIDED:
hold_course = -1;
crash_checker();
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case FLY_BY_WIRE_A:
// set nav_roll and nav_pitch using sticks
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
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:
// Substitute stick inputs for Navigation control output
// We use g.pitch_limit_min because its magnitude is
// normally greater than g.pitch_limit_max
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) {
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
} else {
if (g.channel_pitch.norm_input()<0)
altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ;
else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;
}
if (g.airspeed_enabled == true)
{
airspeed_fbwB = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) *
g.channel_throttle.servo_out) +
((int)g.flybywire_airspeed_min * 100);
airspeed_energy_error = (long)(((long)airspeed_fbwB *
(long)airspeed_fbwB) -
((long)airspeed * (long)airspeed))/20000;
airspeed_error = (airspeed_error - airspeed);
}
calc_throttle();
calc_nav_pitch();
break;
case STABILIZE:
nav_roll = 0;
nav_pitch = 0;
// throttle is passthrough
break;
case CIRCLE:
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS
// ----------------------------------------------------
nav_roll = g.roll_limit / 3;
nav_pitch = 0;
if (failsafe != FAILSAFE_NONE){
g.channel_throttle.servo_out = g.throttle_cruise;
}
break;
case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle();
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
}
}
}
static void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
// distance and bearing calcs only
if(control_mode == AUTO){
verify_commands();
}else{
switch(control_mode){
case LOITER:
case RTL:
case GUIDED:
update_loiter();
calc_bearing_error();
break;
}
}
}
static void update_alt()
{
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
#else
// this function is in place to potentially add a sonar sensor in the future
//altitude_sensor = BARO;
current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100)
current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
#endif
// Calculate new climb rate
//if(medium_loopCounter == 0 && slow_loopCounter == 0)
// add_altitude_data(millis() / 100, g_gps->altitude / 10);
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/Attitude.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//****************************************************************
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************
static void stabilize()
{
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 > 0)
speed_scaler = (STANDARD_SPEED * 100) / airspeed;
else
speed_scaler = 2.0;
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
// Should maybe be to the 2/7 power, but we aren't goint to implement that...
}else{
speed_scaler = 1.67;
}
speed_scaler = constrain(speed_scaler, 0.6, 1.67); // This case is constrained tighter as we don't have real speed info
}
if(crash_timer > 0){
nav_roll = 0;
}
if (inverted_flight) {
// we want to fly upside down. We need to cope with wrap of
// the roll_sensor interfering with wrap of nav_roll, which
// would really confuse the PID code. The easiest way to
// handle this is to ensure both go in the same direction from
// zero
nav_roll += 18000;
if (dcm.roll_sensor < 0) nav_roll -= 36000;
}
// For Testing Only
// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10;
// Serial.printf_P(PSTR(" roll_sensor "));
// Serial.print(roll_sensor,DEC);
// Calculate dersired servo output for the roll
// ---------------------------------------------
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler);
long tempcalc = nav_pitch +
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
// -----------------------------------------------------------
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) {
// TODO: use RC_Channel control_mix function?
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
ch1_inf = fabs(ch1_inf);
ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
ch2_inf = fabs(ch2_inf);
ch2_inf = min(ch2_inf, 400.0);
ch2_inf = ((400.0 - ch2_inf) /400.0);
// scale the sensor input based on the stick input
// -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf;
g.channel_pitch.servo_out *= ch2_inf;
// Mix in stick inputs
// -------------------
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
//Serial.printf_P(PSTR(" servo_out[CH_ROLL] "));
//Serial.println(servo_out[CH_ROLL],DEC);
}
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
// important for steering on the ground during landing
// -----------------------------------------------
if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) {
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
// Apply output to Rudder
// ----------------------
calc_nav_yaw(speed_scaler);
g.channel_rudder.servo_out *= ch4_inf;
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
// Call slew rate limiter if used
// ------------------------------
//#if(ROLL_SLEW_LIMIT != 0)
// g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out);
//#endif
}
static void crash_checker()
{
if(dcm.pitch_sensor < -4500){
crash_timer = 255;
}
if(crash_timer > 0)
crash_timer--;
}
static void calc_throttle()
{
if (g.airspeed_enabled == false) {
int throttle_target = g.throttle_cruise + throttle_nudge;
// no airspeed sensor, we use nav pitch to determine the proper throttle output
// AUTO, RTL, etc
// ---------------------------------------------------------------------------
if (nav_pitch >= 0) {
g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch / g.pitch_limit_max;
} else {
g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch / g.pitch_limit_min;
}
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
} else {
// throttle control with airspeed compensation
// -------------------------------------------
energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;
// positive energy errors make the throttle go higher
g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav);
g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out,
g.throttle_min.get(), g.throttle_max.get()); // TODO - resolve why "saved" is used here versus "current"
}
}
/*****************************************
* Calculate desired roll/pitch/yaw angles (in medium freq loop)
*****************************************/
// Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ----------------------------------------------------------------------------------------
static void calc_nav_yaw(float speed_scaler)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
Vector3f temp = imu.get_accel();
long error = -temp.y;
// Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero)
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler);
#else
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
// XXX probably need something here based on heading
#endif
}
static void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
if (g.airspeed_enabled == true) {
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
} else {
nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav);
}
nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get());
}
#define YAW_DAMPENER 0
static void calc_nav_roll()
{
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
// This does not make provisions for wind speed in excess of airframe speed
nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0);
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
// negative error = left turn
// positive error = right turn
// Calculate the required roll of the plane
// ----------------------------------------
nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
Vector3f omega;
omega = dcm.get_gyro();
// rate limiter
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -6000, 6000); // limit input
int dampener = rate * YAW_DAMPENER; // 34377 * .175 = 6000
// add in yaw dampener
nav_roll -= dampener;
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
}
/*****************************************
* Roll servo slew limit
*****************************************/
/*
float roll_slew_limit(float servo)
{
static float last;
float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f);
last = servo;
return temp;
}*/
/*****************************************
* Throttle slew limit
*****************************************/
static void throttle_slew_limit()
{
static int last = 1000;
if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit
float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000
Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last);
g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
last = g.channel_throttle.radio_out;
}
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_I(void)
{
g.pidNavRoll.reset_I();
g.pidNavPitchAirspeed.reset_I();
g.pidNavPitchAltitude.reset_I();
g.pidTeThrottle.reset_I();
// g.pidAltitudeThrottle.reset_I();
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
static void set_servos(void)
{
int flapSpeedSource = 0;
// vectorize the rc channels
RC_Channel_aux* rc_array[NUM_CHANNELS];
rc_array[CH_1] = NULL;
rc_array[CH_2] = NULL;
rc_array[CH_3] = NULL;
rc_array[CH_4] = NULL;
rc_array[CH_5] = &g.rc_5;
rc_array[CH_6] = &g.rc_6;
rc_array[CH_7] = &g.rc_7;
rc_array[CH_8] = &g.rc_8;
if(control_mode == MANUAL){
// do a direct pass through of radio values
if (g.mix_mode == 0){
g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
} else {
g.channel_roll.radio_out = APM_RC.InputCh(CH_ROLL);
g.channel_pitch.radio_out = APM_RC.InputCh(CH_PITCH);
}
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
// FIXME To me it does not make sense to control the aileron using radio_in in manual mode
// Doug could you please take a look at this ?
if (g_rc_function[RC_Channel_aux::k_aileron]) {
if (g_rc_function[RC_Channel_aux::k_aileron] != rc_array[g.flight_mode_channel-1]) {
g_rc_function[RC_Channel_aux::k_aileron]->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in;
}
}
// only use radio_in if the channel is not used as flight_mode_channel
if (g_rc_function[RC_Channel_aux::k_flap_auto]) {
if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) {
g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in;
} else {
g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim;
}
}
} else {
if (g.mix_mode == 0) {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g_rc_function[RC_Channel_aux::k_aileron]) {
g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out;
g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm();
}
}else{
/*Elevon mode*/
float ch1;
float ch2;
ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out);
ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out;
g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
}
#if THROTTLE_OUT == 0
g.channel_throttle.servo_out = 0;
#else
// convert 0 to 100% into PWM
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
// We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
/* Disable throttle if following conditions are met:
1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
AND
2 - Our reported altitude is within 10 meters of the home altitude.
3 - Our reported speed is under 5 meters per second.
4 - We are not performing a takeoff in Auto mode
OR
5 - Home location is not set
*/
if (
(control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) &&
(abs(home.alt - current_loc.alt) < 1000) &&
((g.airspeed_enabled ? airspeed : g_gps->ground_speed) < 500 ) &&
!(control_mode==AUTO && takeoff_complete == false)
) {
g.channel_throttle.servo_out = 0;
g.channel_throttle.calc_pwm();
}
#endif
g.channel_throttle.calc_pwm();
/* 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();
}
// Auto flap deployment
if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) {
if(control_mode < FLY_BY_WIRE_B) {
// only use radio_in if the channel is not used as flight_mode_channel
if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) {
g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in;
} else {
g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim;
}
} else if (control_mode >= FLY_BY_WIRE_B) {
if (control_mode == FLY_BY_WIRE_B) {
flapSpeedSource = airspeed_fbwB/100;
} else if (g.airspeed_enabled == true) {
flapSpeedSource = g.airspeed_cruise/100;
} else {
flapSpeedSource = g.throttle_cruise;
}
if ( flapSpeedSource > g.flap_1_speed) {
g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = 0;
} else if (flapSpeedSource > g.flap_2_speed) {
g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_1_percent;
} else {
g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_2_percent;
}
g_rc_function[RC_Channel_aux::k_flap_auto]->calc_pwm();
}
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
// Route configurable aux. functions to their respective servos
g.rc_5.output_ch(CH_5);
g.rc_6.output_ch(CH_6);
g.rc_7.output_ch(CH_7);
g.rc_8.output_ch(CH_8);
#endif
}
static void demo_servos(byte i) {
while(i > 0){
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
APM_RC.OutputCh(1, 1400);
mavlink_delay(400);
APM_RC.OutputCh(1, 1600);
mavlink_delay(200);
APM_RC.OutputCh(1, 1500);
#endif
mavlink_delay(400);
i--;
}
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/GCS_Mavlink.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Mavlink_compat.h"
// use this to prevent recursion during sensor init
static bool in_mavlink_delay;
// this costs us 51 bytes, but means that low priority
// messages don't block the CPU
static mavlink_statustext_t pending_status;
// true when we have received at least 1 MAVLink packet
static bool mavlink_active;
// check if a message will fit in the payload space available
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
/*
!!NOTE!!
the use of NOINLINE separate functions for each message type avoids
a compiler bug in gcc that would cause it to use far more stack
space than is needed. Without the NOINLINE we use the sum of the
stack needed for each message type. Please be careful to follow the
pattern below when adding any new messages
*/
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
#ifdef MAVLINK10
uint8_t base_mode = 0;
uint8_t system_status = MAV_STATE_ACTIVE;
// we map the custom_mode to our internal mode plus 16, to lower
// the chance that a ground station will give us 0 and we
// interpret it as manual. This is necessary as the SET_MODE
// command has no way to indicate that the custom_mode is filled in
uint32_t custom_mode = control_mode + 16;
// work out the base_mode. This value is almost completely useless
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
switch (control_mode) {
case MANUAL:
base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case FLY_BY_WIRE_C:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case AUTO:
case RTL:
case LOITER:
case GUIDED:
case CIRCLE:
base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
case INITIALISING:
system_status = MAV_STATE_CALIBRATING;
break;
}
if (control_mode != MANUAL && control_mode != INITIALISING) {
// stabiliser of some form is enabled
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
#if ENABLE_STICK_MIXING==ENABLED
if (control_mode != INITIALISING) {
// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
#endif
#if HIL_MODE != HIL_MODE_DISABLED
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
#endif
// we are armed if we are not initialising
if (control_mode != INITIALISING) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_FIXED_WING,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
#else // MAVLINK10
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
#endif // MAVLINK10
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
#ifdef MAVLINK10
uint32_t control_sensors_present = 0;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// first what sensors/controllers we have
control_sensors_present |= (1<<0); // 3D gyro present
control_sensors_present |= (1<<1); // 3D accelerometer present
if (g.compass_enabled) {
control_sensors_present |= (1<<2); // compass present
}
control_sensors_present |= (1<<3); // absolute pressure sensor present
if (g_gps->fix) {
control_sensors_present |= (1<<5); // GPS present
}
control_sensors_present |= (1<<10); // 3D angular rate control
control_sensors_present |= (1<<11); // attitude stabilisation
control_sensors_present |= (1<<12); // yaw position
control_sensors_present |= (1<<13); // altitude control
control_sensors_present |= (1<<14); // X/Y position control
control_sensors_present |= (1<<15); // motor control
// now what sensors/controllers are enabled
// first the sensors
control_sensors_enabled = control_sensors_present & 0x1FF;
// now the controllers
control_sensors_enabled = control_sensors_present & 0x1FF;
switch (control_mode) {
case MANUAL:
break;
case STABILIZE:
case FLY_BY_WIRE_A:
control_sensors_enabled |= (1<<10); // 3D angular rate control
control_sensors_enabled |= (1<<11); // attitude stabilisation
break;
case FLY_BY_WIRE_B:
control_sensors_enabled |= (1<<10); // 3D angular rate control
control_sensors_enabled |= (1<<11); // attitude stabilisation
control_sensors_enabled |= (1<<15); // motor control
break;
case FLY_BY_WIRE_C:
control_sensors_enabled |= (1<<10); // 3D angular rate control
control_sensors_enabled |= (1<<11); // attitude stabilisation
control_sensors_enabled |= (1<<13); // altitude control
control_sensors_enabled |= (1<<15); // motor control
break;
case AUTO:
case RTL:
case LOITER:
case GUIDED:
case CIRCLE:
control_sensors_enabled |= (1<<10); // 3D angular rate control
control_sensors_enabled |= (1<<11); // attitude stabilisation
control_sensors_enabled |= (1<<12); // yaw position
control_sensors_enabled |= (1<<13); // altitude control
control_sensors_enabled |= (1<<14); // X/Y position control
control_sensors_enabled |= (1<<15); // motor control
break;
case INITIALISING:
break;
}
// at the moment all sensors/controllers are assumed healthy
control_sensors_health = control_sensors_present;
uint16_t battery_current = -1;
uint8_t battery_remaining = -1;
if (current_total != 0 && g.pack_capacity != 0) {
battery_remaining = (100.0 * (g.pack_capacity - current_total) / g.pack_capacity);
}
if (current_total != 0) {
battery_current = current_amps * 100;
}
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
(uint16_t)(load * 1000),
battery_voltage * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
#else // MAVLINK10
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case STABILIZE:
mode = MAV_MODE_TEST1;
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;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
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;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
load * 1000,
battery_voltage * 1000,
battery_remaining,
packet_drops);
#endif // MAVLINK10
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
{
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
}
static void NOINLINE send_location(mavlink_channel_t chan)
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
#ifdef MAVLINK10
mavlink_msg_global_position_int_send(
chan,
millis(),
current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees
g_gps->altitude*10, // millimeters above sea level
current_loc.alt * 10, // millimeters above ground
g_gps->ground_speed * rot.a.x, // X speed cm/s
g_gps->ground_speed * rot.b.x, // Y speed cm/s
g_gps->ground_speed * rot.c.x,
g_gps->ground_course); // course in 1/100 degree
#else // MAVLINK10
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
#endif // MAVLINK10
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
airspeed_error,
crosstrack_error);
}
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
#ifdef MAVLINK10
uint8_t fix;
if (g_gps->status() == 2) {
fix = 3;
} else {
fix = 0;
}
mavlink_msg_gps_raw_int_send(
chan,
micros(),
fix,
g_gps->latitude, // in 1E7 degrees
g_gps->longitude, // in 1E7 degrees
g_gps->altitude * 10, // in mm
g_gps->hdop,
65535,
g_gps->ground_speed, // cm/s
g_gps->ground_course, // 1/100 degrees,
g_gps->num_sats);
#else // MAVLINK10
mavlink_msg_gps_raw_send(
chan,
micros(),
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
#endif // MAVLINK10
}
static void NOINLINE send_servo_out(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with
// HIL maintainers
#ifdef MAVLINK10
mavlink_msg_rc_channels_scaled_send(
chan,
millis(),
0, // port 0
10000 * g.channel_roll.norm_output(),
10000 * g.channel_pitch.norm_output(),
10000 * g.channel_throttle.norm_output(),
10000 * g.channel_rudder.norm_output(),
0,
0,
0,
0,
rssi);
#else // MAVLINK10
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.channel_roll.norm_output(),
10000 * g.channel_pitch.norm_output(),
10000 * g.channel_throttle.norm_output(),
10000 * g.channel_rudder.norm_output(),
0,
0,
0,
0,
rssi);
#endif // MAVLINK10
}
static void NOINLINE send_radio_in(mavlink_channel_t chan)
{
uint8_t rssi = 1;
#ifdef MAVLINK10
mavlink_msg_rc_channels_raw_send(
chan,
millis(),
0, // port
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
#else // MAVLINK10
mavlink_msg_rc_channels_raw_send(
chan,
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
#endif // MAVLINK10
}
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
#ifdef MAVLINK10
mavlink_msg_servo_output_raw_send(
chan,
micros(),
0, // port
g.channel_roll.radio_out,
g.channel_pitch.radio_out,
g.channel_throttle.radio_out,
g.channel_rudder.radio_out,
g.rc_5.radio_out, // XXX currently only 4 RC channels defined
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
#else // MAVLINK10
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
g.channel_pitch.radio_out,
g.channel_throttle.radio_out,
g.channel_rudder.radio_out,
g.rc_5.radio_out, // XXX currently only 4 RC channels defined
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
#endif // MAVLINK10
}
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
(int)g.channel_throttle.servo_out,
current_loc.alt / 100.0,
0);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
mavlink_msg_raw_imu_send(
chan,
micros(),
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
mavlink_msg_scaled_pressure_send(
chan,
micros(),
(float)barometer.Press/100.0,
(float)(barometer.Press-g.ground_pressure)/100.0,
(int)(barometer.Temp*10));
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_gps_status(mavlink_channel_t chan)
{
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_waypoint_current_send(
chan,
g.command_index);
}
static void NOINLINE send_statustext(mavlink_channel_t chan)
{
mavlink_msg_statustext_send(
chan,
pending_status.severity,
pending_status.text);
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
return false;
}
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
return true;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
break;
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
}
break;
case MSG_GPS_RAW:
#ifdef MAVLINK10
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
#else
CHECK_PAYLOAD_SIZE(GPS_RAW);
#endif
send_gps_raw(chan);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(chan);
break;
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
break;
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
break;
#endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_GPS_STATUS:
CHECK_PAYLOAD_SIZE(GPS_STATUS);
send_gps_status(chan);
break;
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
send_current_waypoint(chan);
break;
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_param_send();
} else {
gcs3.queued_param_send();
}
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_waypoint_send();
} else {
gcs3.queued_waypoint_send();
}
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
break;
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
}
return true;
}
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
static struct mavlink_queue {
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
{
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// don't send status MAVLink messages for 2 seconds after
// bootup, to try to prevent Xbee bricking
return;
}
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
pending_status.severity = (uint8_t)severity;
strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
} else {
// send immediately
#ifdef MAVLINK10
mavlink_msg_statustext_send(chan, severity, str);
#else
mavlink_msg_statustext_send(chan, severity, (const int8_t*) str);
#endif
}
}
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0),
// parameters
// note, all values not explicitly initialised here are zeroed
waypoint_send_timeout(1000), // 1 second
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"))
{
}
void
GCS_MAVLINK::init(FastSerial * port)
{
GCS_Class::init(port);
if (port == &Serial) {
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
}else{
mavlink_comm_1_port = port;
chan = MAVLINK_COMM_1;
}
_queued_parameter = NULL;
}
void
GCS_MAVLINK::update(void)
{
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
while (comm_get_available(chan))
{
uint8_t c = comm_receive_ch(chan);
#if CLI_ENABLED == ENABLED
/* allow CLI to be started by hitting enter 3 times, if no
heartbeat packets have been received */
if (mavlink_active == 0) {
if (c == '\n' || c == '\r') {
crlf_count++;
} else {
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli();
}
}
#endif
// Try to get a new message
if (mavlink_parse_char(chan, c, &msg, &status)) {
mavlink_active = 1;
handleMessage(&msg);
}
}
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
if (NULL != _queued_parameter) {
send_message(MSG_NEXT_PARAM);
}
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total) {
send_message(MSG_NEXT_WAYPOINT);
}
// stop waypoint sending if timeout
if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
waypoint_sending = false;
}
// stop waypoint receiving if timeout
if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
waypoint_receiving = false;
}
}
void
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_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_GPS_STATUS);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT);
}
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
// sent with GPS read
send_message(MSG_LOCATION);
}
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
send_message(MSG_SERVO_OUT);
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
}
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream
}
}
}
void
GCS_MAVLINK::send_message(enum ap_message id)
{
mavlink_send_message(chan,id, packet_drops);
}
void
GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
{
mavlink_send_text(chan,severity,str);
}
void
GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
{
mavlink_statustext_t m;
uint8_t i;
for (i=0; i<sizeof(m.text); i++) {
m.text[i] = pgm_read_byte((const prog_char *)(str++));
}
if (i < sizeof(m.text)) m.text[i] = 0;
mavlink_send_text(chan, severity, (const char *)m.text);
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
struct Location tell_command = {}; // command for telemetry
static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...)
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
// 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;
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;
switch(packet.req_stream_id){
case MAV_DATA_STREAM_ALL:
streamRateRawSensors = freq;
streamRateExtendedStatus = freq;
streamRateRCChannels = freq;
streamRateRawController = freq;
streamRatePosition = freq;
streamRateExtra1 = freq;
streamRateExtra2 = freq;
streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group.
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.
break;
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_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;
}
break;
}
#ifdef MAVLINK10
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) break;
uint8_t result;
// do command
send_text(SEVERITY_LOW,PSTR("command received: "));
switch(packet.command) {
case MAV_CMD_NAV_LOITER_UNLIM:
set_mode(LOITER);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
set_mode(RTL);
result = MAV_RESULT_ACCEPTED;
break;
#if 0
// not implemented yet, but could implement some of them
case MAV_CMD_NAV_LAND:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_NAV_ROI:
case MAV_CMD_NAV_PATHPLANNING:
break;
#endif
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (packet.param1 == 1 ||
packet.param2 == 1 ||
packet.param3 == 1) {
startup_IMU_ground();
}
if (packet.param4 == 1) {
trim_radio();
}
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
break;
}
mavlink_msg_command_ack_send(
chan,
packet.command,
result);
break;
}
#else // MAVLINK10
case MAVLINK_MSG_ID_ACTION:
{
// decode
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (mavlink_check_target(packet.target,packet.target_component)) break;
uint8_t result = 0;
// do action
send_text(SEVERITY_LOW,PSTR("action received: "));
//Serial.println(packet.action);
switch(packet.action){
case MAV_ACTION_LAUNCH:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_RETURN:
set_mode(RTL);
result=1;
break;
case MAV_ACTION_EMCY_LAND:
//set_mode(LAND);
break;
case MAV_ACTION_HALT:
do_loiter_at_location();
result=1;
break;
/* No mappable implementation in APM 2.0
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
break;
*/
case MAV_ACTION_CONTINUE:
process_next_command();
result=1;
break;
case MAV_ACTION_SET_MANUAL:
set_mode(MANUAL);
result=1;
break;
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
result=1;
break;
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
result=1;
break;
case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all();
result=1;
break;
case MAV_ACTION_CALIBRATE_RC: break;
trim_radio();
result=1;
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_IMU_ground();
result=1;
break;
/* For future implemtation
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
*/
/* Takeoff is not an implemented flight mode in APM 2.0
case MAV_ACTION_TAKEOFF:
set_mode(TAKEOFF);
break;
*/
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
result=1;
break;
/* Land is not an implemented flight mode in APM 2.0
case MAV_ACTION_LAND:
set_mode(LAND);
break;
*/
case MAV_ACTION_LOITER:
set_mode(LOITER);
result=1;
break;
default: break;
}
mavlink_msg_action_ack_send(
chan,
packet.action,
result
);
break;
}
#endif
case MAVLINK_MSG_ID_SET_MODE:
{
// decode
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg, &packet);
#ifdef MAVLINK10
// we ignore base_mode as there is no sane way to map
// from that bitmap to a APM flight mode. We rely on
// custom_mode instead.
// see comment on custom_mode above
int16_t adjusted_mode = packet.custom_mode - 16;
switch (adjusted_mode) {
case MANUAL:
case CIRCLE:
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case FLY_BY_WIRE_C:
case AUTO:
case RTL:
case LOITER:
set_mode(adjusted_mode);
break;
}
#else // MAVLINK10
switch(packet.mode){
case MAV_MODE_MANUAL:
set_mode(MANUAL);
break;
case MAV_MODE_GUIDED:
set_mode(GUIDED);
break;
case MAV_MODE_AUTO:
if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO);
if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL);
if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER);
mav_nav = 255;
break;
case MAV_MODE_TEST1:
set_mode(STABILIZE);
break;
case MAV_MODE_TEST2:
if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A);
if(mav_nav == 2) set_mode(FLY_BY_WIRE_B);
//if(mav_nav == 3) set_mode(FLY_BY_WIRE_C);
mav_nav = 255;
break;
}
#endif
break;
}
#ifndef MAVLINK10
case MAVLINK_MSG_ID_SET_NAV_MODE:
{
// decode
mavlink_set_nav_mode_t packet;
mavlink_msg_set_nav_mode_decode(msg, &packet);
// To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message
mav_nav = packet.nav_mode;
break;
}
#endif // MAVLINK10
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
// 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;
// Start sending waypoints
mavlink_msg_waypoint_count_send(
chan,msg->sysid,
msg->compid,
g.command_total + 1); // + home
waypoint_timelast_send = millis();
waypoint_sending = true;
waypoint_receiving = false;
waypoint_dest_sysid = msg->sysid;
waypoint_dest_compid = msg->compid;
break;
}
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
// Check if sending waypiont
//if (!waypoint_sending) break;
// 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW
// decode
mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
// send waypoint
tell_command = get_cmd_with_index(packet.seq);
// set frame of waypoint
uint8_t frame;
if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
} else {
frame = MAV_FRAME_GLOBAL; // reference frame
}
float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
// time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == (uint16_t)g.command_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) {
// command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude)
if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt
} else {
z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
}
}
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_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_CONDITION_CHANGE_ALT:
x=0; // Clear fields loaded above that we don't want sent for this command
y=0;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_DO_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_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
}
mavlink_msg_waypoint_send(chan,msg->sysid,
msg->compid,
packet.seq,
frame,
tell_command.id,
current,
autocontinue,
param1,
param2,
param3,
param4,
x,
y,
z);
// update last waypoint comm stamp
waypoint_timelast_send = millis();
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK:
{
// decode
mavlink_waypoint_ack_t packet;
mavlink_msg_waypoint_ack_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// turn off waypoint send
waypoint_sending = false;
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// decode
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Var::first();
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{
// 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;
// clear all commands
g.command_total.set_and_save(0);
// note that we don't send multiple acks, as otherwise a
// GCS that is doing a clear followed by a set may see
// the additional ACKs as ACKs of the set operations
mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
// decode
mavlink_waypoint_set_current_t packet;
mavlink_msg_waypoint_set_current_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// set current command
change_command(packet.seq);
mavlink_msg_waypoint_current_send(chan, g.command_index);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
// decode
mavlink_waypoint_count_t packet;
mavlink_msg_waypoint_count_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// start waypoint receiving
if (packet.count > MAX_WAYPOINTS) {
packet.count = MAX_WAYPOINTS;
}
g.command_total.set_and_save(packet.count - 1);
waypoint_timelast_receive = millis();
waypoint_receiving = true;
waypoint_sending = false;
waypoint_request_i = 0;
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:
{
// decode
mavlink_waypoint_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_waypoint_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// 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;
}
#ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // 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;
}
#endif
#ifdef MAV_FRAME_LOCAL
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;
}
#endif
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;
}
default:
result = MAV_MISSION_UNSUPPORTED_FRAME;
break;
}
if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_WAYPOINT:
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
case MAV_CMD_NAV_LAND:
break;
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_DO_SET_HOME:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.lat = packet.param1;
break;
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command.lat = packet.param1;
break;
case MAV_CMD_DO_JUMP:
tell_command.lat = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_REPEAT_SERVO:
tell_command.lng = packet.param4;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
tell_command.lat = packet.param3;
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
default:
#ifdef MAVLINK10
result = MAV_MISSION_UNSUPPORTED;
#endif
break;
}
if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
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;
// add home alt if needed
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){
guided_WP.alt += home.alt;
}
set_mode(GUIDED);
// make any new wp uploaded instant (in case we are already in Guided mode)
set_guided_WP();
// 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) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_failed;
}
set_cmd_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.command_total){
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
result);
send_text(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
}
}
break;
mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
result);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
AP_Var *vp;
AP_Meta_class::Type_id var_type;
// decode
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
// set parameter
char key[ONBOARD_PARAM_NAME_LENGTH+1];
strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH);
key[ONBOARD_PARAM_NAME_LENGTH] = 0;
// find the requested parameter
vp = AP_Var::find(key);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
// 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;
// fetch the variable type ID
var_type = vp->meta_type_id();
// handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) {
((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_float16) {
((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
break;
}
// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
#ifdef MAVLINK10
mavlink_msg_param_value_send(
chan,
key,
vp->cast_to_float(),
mav_var_type(vp->meta_type_id()),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
#else // MAVLINK10
mavlink_msg_param_value_send(
chan,
(int8_t *)key,
vp->cast_to_float(),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
#endif // MAVLINK10
}
break;
} // end case
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
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;
v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw;
v[2] = packet.chan3_raw;
v[3] = packet.chan4_raw;
v[4] = packet.chan5_raw;
v[5] = packet.chan6_raw;
v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw;
rc_override_active = APM_RC.setHIL(v);
rc_override_fs_timer = millis();
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if(msg->sysid != g.sysid_my_gcs) break;
rc_override_fs_timer = millis();
pmTest1++;
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode.
#ifdef MAVLINK10
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
// decode
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(msg, &packet);
// set gps hil sensor
g_gps->setHIL(packet.time_usec/1000.0,
packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,
packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 0);
break;
}
#else // MAVLINK10
case MAVLINK_MSG_ID_GPS_RAW:
{
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
// set gps hil sensor
g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
packet.v,packet.hdg,0,0);
break;
}
#endif // MAVLINK10
// Is this resolved? - MAVLink protocol change.....
case MAVLINK_MSG_ID_VFR_HUD:
{
// decode
mavlink_vfr_hud_t packet;
mavlink_msg_vfr_hud_decode(msg, &packet);
// set airspeed
airspeed = 100*packet.airspeed;
break;
}
#endif
#if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE:
{
// decode
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set dcm hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
break;
}
#endif
#if HIL_MODE == HIL_MODE_SENSORS
case MAVLINK_MSG_ID_RAW_IMU:
{
// decode
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
// set imu hil sensors
// TODO: check scaling for temp/absPress
float temp = 70;
float absPress = 1;
//Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
//Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
// rad/sec
Vector3f gyros;
gyros.x = (float)packet.xgyro / 1000.0;
gyros.y = (float)packet.ygyro / 1000.0;
gyros.z = (float)packet.zgyro / 1000.0;
// m/s/s
Vector3f accels;
accels.x = (float)packet.xacc / 1000.0;
accels.y = (float)packet.yacc / 1000.0;
accels.z = (float)packet.zacc / 1000.0;
imu.set_gyro(gyros);
imu.set_accel(accels);
compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
break;
}
case MAVLINK_MSG_ID_RAW_PRESSURE:
{
// decode
mavlink_raw_pressure_t packet;
mavlink_msg_raw_pressure_decode(msg, &packet);
// set pressure hil sensor
// TODO: check scaling
float temp = 70;
barometer.setHIL(temp,packet.press_diff1 + 101325);
break;
}
#endif // HIL_MODE
} // end switch
} // end handle mavlink
uint16_t
GCS_MAVLINK::_count_parameters()
{
// if we haven't cached the parameter count yet...
if (0 == _parameter_count) {
AP_Var *vp;
vp = AP_Var::first();
do {
// if a parameter responds to cast_to_float then we are going to be able to report it
if (!isnan(vp->cast_to_float())) {
_parameter_count++;
}
} while (NULL != (vp = vp->next()));
}
return _parameter_count;
}
AP_Var *
GCS_MAVLINK::_find_parameter(uint16_t index)
{
AP_Var *vp;
vp = AP_Var::first();
while (NULL != vp) {
// if the parameter is reportable
if (!(isnan(vp->cast_to_float()))) {
// if we have counted down to the index we want
if (0 == index) {
// return the parameter
return vp;
}
// count off this parameter, as it is reportable but not
// the one we want
index--;
}
// and move to the next parameter
vp = vp->next();
}
return NULL;
}
/**
* @brief Send the next pending parameter, called from deferred message
* handling code
*/
void
GCS_MAVLINK::queued_param_send()
{
// Check to see if we are sending parameters
if (NULL == _queued_parameter) return;
AP_Var *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
_queued_parameter = _queued_parameter->next();
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
#ifdef MAVLINK10
mavlink_msg_param_value_send(
chan,
param_name,
value,
mav_var_type(vp->meta_type_id()),
_queued_parameter_count,
_queued_parameter_index);
#else // MAVLINK10
mavlink_msg_param_value_send(
chan,
(int8_t*)param_name,
value,
_queued_parameter_count,
_queued_parameter_index);
#endif // MAVLINK10
_queued_parameter_index++;
}
}
/**
* @brief Send the next pending waypoint, called from deferred message
* handling code
*/
void
GCS_MAVLINK::queued_waypoint_send()
{
if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.command_total) {
mavlink_msg_waypoint_request_send(
chan,
waypoint_dest_sysid,
waypoint_dest_compid,
waypoint_request_i);
}
}
/*
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_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);
gcs_send_message(MSG_EXTENDED_STATUS1);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs_update();
}
delay(1);
} while (millis() - tstart < t);
in_mavlink_delay = false;
}
/*
send a message on both GCS links
*/
static void gcs_send_message(enum ap_message id)
{
gcs0.send_message(id);
gcs3.send_message(id);
}
/*
send data streams in the given rate range on both links
*/
static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
{
gcs0.data_stream_send(freqMin, freqMax);
gcs3.data_stream_send(freqMin, freqMax);
}
/*
look for incoming commands on the GCS links
*/
static void gcs_update(void)
{
gcs0.update();
gcs3.update();
}
static void gcs_send_text(gcs_severity severity, const char *str)
{
gcs0.send_text(severity, str);
gcs3.send_text(severity, str);
}
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
{
gcs0.send_text(severity, str);
gcs3.send_text(severity, str);
}
/*
send a low priority formatted message to the GCS
only one fits in the queue, so if you send more than one before the
last one gets into the serial buffer then the old one will be lost
*/
static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
{
char fmtstr[40];
va_list ap;
uint8_t i;
for (i=0; i<sizeof(fmtstr)-1; i++) {
fmtstr[i] = pgm_read_byte((const prog_char *)(fmt++));
if (fmtstr[i] == 0) break;
}
fmtstr[i] = 0;
pending_status.severity = (uint8_t)SEVERITY_LOW;
va_start(ap, fmt);
vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
va_end(ap);
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/Log.pde"
// -*- 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
#define HEAD_BYTE1 0xA3 // Decimal 163
#define HEAD_BYTE2 0x95 // Decimal 149
#define END_BYTE 0xBA // Decimal 186
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
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);
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads from flash memory
static int8_t help_log(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\n"
"Commands:\n"
" dump <n> dump log <n>\n"
" erase erase all logs\n"
" 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 = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
{"disable", select_logs},
{"help", help_log}
};
// 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)
{
int log_start;
int log_end;
byte last_log_num = get_num_logs();
Serial.printf_P(PSTR("logs enabled: "));
if (0 == g.log_bitmask) {
Serial.printf_P(PSTR("none"));
}else{
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED);
PLOG(GPS);
PLOG(PM);
PLOG(CTUN);
PLOG(NTUN);
PLOG(MODE);
PLOG(RAW);
PLOG(CMD);
PLOG(CUR);
#undef PLOG
}
Serial.println();
if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs available for download\n"));
}else{
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);
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"),
i, log_start, log_end);
}
Serial.println();
}
return(true);
}
static int8_t
dump_log(uint8_t argc, const Menu::arg *argv)
{
byte dump_log;
int dump_log_start;
int dump_log_end;
byte last_log_num;
// check that the requested log number can be read
dump_log = argv[1].i;
last_log_num = get_num_logs();
if ((argc != 2) || (dump_log < 1) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log number\n"));
return(-1);
}
get_log_boundaries(dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
dump_log,
dump_log_start,
dump_log_end);
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Log read complete\n"));
return 0;
}
static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv)
{
for(int i = 10 ; i > 0; i--) {
Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i);
delay(1000);
}
Serial.printf_P(PSTR("\nErasing log...\n"));
for(int j = 1; j < 4096; j++)
DataFlash.PageErase(j);
DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_INDEX_MSG);
DataFlash.WriteByte(0);
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
Serial.printf_P(PSTR("\nLog erased.\n"));
return 0;
}
static int8_t
select_logs(uint8_t argc, const Menu::arg *argv)
{
uint16_t bits;
if (argc != 2) {
Serial.printf_P(PSTR("missing log type\n"));
return(-1);
}
bits = 0;
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// that name as the argument to the command, and set the bit in
// bits accordingly.
//
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~0;
} else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST);
TARG(ATTITUDE_MED);
TARG(GPS);
TARG(PM);
TARG(CTUN);
TARG(NTUN);
TARG(MODE);
TARG(RAW);
TARG(CMD);
TARG(CUR);
#undef TARG
}
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
g.log_bitmask.set_and_save(g.log_bitmask | bits);
}else{
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
}
return(0);
}
static int8_t
process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
static byte get_num_logs(void)
{
int page = 1;
byte data;
byte log_step = 0;
DataFlash.StartRead(1);
while (page == 1) {
data = DataFlash.ReadByte();
switch(log_step){ //This is a state machine to read the packets
case 0:
if(data==HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data==HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data==LOG_INDEX_MSG){
byte num_logs = DataFlash.ReadByte();
return num_logs;
}else{
log_step=0; // Restart, we have a problem...
}
break;
}
page = DataFlash.GetPage();
}
return 0;
}
// send the number of the last log?
static void start_new_log(byte num_existing_logs)
{
int start_pages[50] = {0,0,0};
int end_pages[50] = {0,0,0};
if(num_existing_logs > 0) {
for(int i=0;i<num_existing_logs;i++) {
get_log_boundaries(i+1,start_pages[i],end_pages[i]);
}
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
}
if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
if(num_existing_logs > 0)
start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
else
start_pages[0] = 2;
num_existing_logs++;
DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_INDEX_MSG);
DataFlash.WriteByte(num_existing_logs);
for(int i=0;i<MAX_NUM_LOGS;i++) {
DataFlash.WriteInt(start_pages[i]);
DataFlash.WriteInt(end_pages[i]);
}
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
}else{
gcs_send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full - logging discontinued"));
}
}
static void get_log_boundaries(byte log_num, int & start_page, int & end_page)
{
int page = 1;
byte data;
byte log_step = 0;
DataFlash.StartRead(1);
while (page == 1) {
data = DataFlash.ReadByte();
switch(log_step) //This is a state machine to read the packets
{
case 0:
if(data==HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data==HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data==LOG_INDEX_MSG){
byte num_logs = DataFlash.ReadByte();
for(int i=0;i<log_num;i++) {
start_page = DataFlash.ReadInt();
end_page = DataFlash.ReadInt();
}
if(log_num==num_logs)
end_page = find_last_log_page(start_page);
return; // This is the normal exit point
}else{
log_step=0; // Restart, we have a problem...
}
break;
}
page = DataFlash.GetPage();
}
// Error condition if we reach here with page = 2 TO DO - report condition
}
static int find_last_log_page(int bottom_page)
{
int top_page = 4096;
int look_page;
long check;
while((top_page - bottom_page) > 1) {
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
check = DataFlash.ReadLong();
if(check == (long)0xFFFFFFFF)
top_page = look_page;
else
bottom_page = look_page;
}
return top_page;
}
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(log_roll);
DataFlash.WriteInt(log_pitch);
DataFlash.WriteInt(log_yaw);
DataFlash.WriteByte(END_BYTE);
}
// Write a performance monitoring packet. Total length : 19 bytes
static void Log_Write_Performance()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteLong(millis()- perf_mon_timer);
DataFlash.WriteInt(mainLoop_count);
DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(dcm.gyro_sat_count);
DataFlash.WriteByte(imu.adc_constraints);
DataFlash.WriteByte(dcm.renorm_sqrt_count);
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)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
}
static void Log_Write_Startup(byte type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type);
DataFlash.WriteByte(g.command_total);
DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_cmd_with_index(0);
Log_Write_Cmd(0, &cmd);
for (int i = 1; i <= g.command_total; i++){
cmd = get_cmd_with_index(i);
Log_Write_Cmd(i, &cmd);
}
}
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning()
{
Vector3f accel = imu.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(g.channel_roll.servo_out));
DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteByte(END_BYTE);
}
#endif
// Write a navigation tuning packet. Total length : 18 bytes
static void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)wp_distance);
DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing);
DataFlash.WriteInt(altitude_error);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteInt((int)(nav_gain_scaler*1000));
DataFlash.WriteByte(END_BYTE);
}
// Write a mode packet. Total length : 5 bytes
static void Log_Write_Mode(byte mode)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_MODE_MSG);
DataFlash.WriteByte(mode);
DataFlash.WriteByte(END_BYTE);
}
// 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)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(log_Time);
DataFlash.WriteByte(log_Fix);
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);
DataFlash.WriteLong(log_Ground_Course);
DataFlash.WriteByte(END_BYTE);
}
// Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Raw()
{
Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel();
gyro *= t7; // Scale up for storage as long integers
accel *= t7;
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
DataFlash.WriteLong((long)gyro.x);
DataFlash.WriteLong((long)gyro.y);
DataFlash.WriteLong((long)gyro.z);
DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z);
DataFlash.WriteByte(END_BYTE);
}
#endif
static void Log_Write_Current()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.channel_throttle.control_in);
DataFlash.WriteInt((int)(battery_voltage * 100.0));
DataFlash.WriteInt((int)(current_amps * 100.0));
DataFlash.WriteInt((int)current_total);
DataFlash.WriteByte(END_BYTE);
}
// Read a Current packet
static void Log_Read_Current()
{
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
DataFlash.ReadInt());
}
// Read an control tuning packet
static void Log_Read_Control_Tuning()
{
float logvar;
Serial.printf_P(PSTR("CTUN:"));
for (int y = 1; y < 10; y++) {
logvar = DataFlash.ReadInt();
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read a nav tuning packet
static void Log_Read_Nav_Tuning()
{
Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
(float)((uint16_t)DataFlash.ReadInt())/100.0,
DataFlash.ReadInt(),
(float)((uint16_t)DataFlash.ReadInt())/100.0,
(float)((uint16_t)DataFlash.ReadInt())/100.0,
(float)DataFlash.ReadInt()/100.0,
(float)DataFlash.ReadInt()/100.0,
(float)DataFlash.ReadInt()/1000.0);
}
// Read a performance packet
static void Log_Read_Performance()
{
long pm_time;
int logvar;
Serial.printf_P(PSTR("PM:"));
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
Serial.print(comma);
for (int y = 1; y <= 9; y++) {
if(y < 3 || y > 7){
logvar = DataFlash.ReadInt();
}else{
logvar = DataFlash.ReadByte();
}
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read a command processing packet
static void Log_Read_Cmd()
{
byte logvarb;
long logvarl;
Serial.printf_P(PSTR("CMD:"));
for(int i = 1; i < 4; i++) {
logvarb = DataFlash.ReadByte();
Serial.print(logvarb, DEC);
Serial.print(comma);
}
for(int i = 1; i < 4; i++) {
logvarl = DataFlash.ReadLong();
Serial.print(logvarl, DEC);
Serial.print(comma);
}
Serial.println(" ");
}
static void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
if (logbyte == TYPE_AIRSTART_MSG)
Serial.printf_P(PSTR("AIR START - "));
else if (logbyte == TYPE_GROUNDSTART_MSG)
Serial.printf_P(PSTR("GROUND START - "));
else
Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
}
// Read an attitude packet
static void Log_Read_Attitude()
{
Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(uint16_t)DataFlash.ReadInt());
}
// Read a mode packet
static 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()
{
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
DataFlash.ReadLong(),
(int)DataFlash.ReadByte(),
(int)DataFlash.ReadByte(),
(float)DataFlash.ReadLong() / t7,
(float)DataFlash.ReadLong() / t7,
(float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing
(float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0);
}
// Read a raw accel/gyro packet
static void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW:"));
for (int y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read the DataFlash log memory : Packet Parser
static void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step = 0;
int packet_count = 0;
int page = start_page;
#ifdef AIRFRAME_NAME
Serial.printf_P(PSTR((AIRFRAME_NAME)
#endif
Serial.printf_P(PSTR("\n" THISFIRMWARE
"\nFree RAM: %u\n"),
memcheck_available_memory());
DataFlash.StartRead(start_page);
while (page < end_page && page != -1){
data = DataFlash.ReadByte();
switch(log_step) // This is a state machine to read the packets
{
case 0:
if(data == HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data == HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data == LOG_ATTITUDE_MSG){
Log_Read_Attitude();
log_step++;
}else if(data == LOG_MODE_MSG){
Log_Read_Mode();
log_step++;
}else if(data == LOG_CONTROL_TUNING_MSG){
Log_Read_Control_Tuning();
log_step++;
}else if(data == LOG_NAV_TUNING_MSG){
Log_Read_Nav_Tuning();
log_step++;
}else if(data == LOG_PERFORMANCE_MSG){
Log_Read_Performance();
log_step++;
}else if(data == LOG_RAW_MSG){
Log_Read_Raw();
log_step++;
}else if(data == LOG_CMD_MSG){
Log_Read_Cmd();
log_step++;
}else if(data == LOG_CURRENT_MSG){
Log_Read_Current();
log_step++;
}else if(data == LOG_STARTUP_MSG){
Log_Read_Startup();
log_step++;
}else {
if(data == LOG_GPS_MSG){
Log_Read_GPS();
log_step++;
}else{
Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
log_step = 0; // Restart, we have a problem...
}
}
break;
case 3:
if(data == END_BYTE){
packet_count++;
}else{
Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
}
log_step = 0; // Restart sequence: new packet...
break;
}
page = DataFlash.GetPage();
}
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
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/climb_rate.pde"
// -*- 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 long xoffset;
unsigned char n;
// Intermediate variables for regression calculation
long xi;
long yi;
long xiyi;
unsigned long xi2;
#if 0 // currently unused
static void add_altitude_data(unsigned long xl, long y)
{
//Reset the regression if our X variable overflowed
if (xl < xoffset)
n = 0;
//To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length
if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH)
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;
} else {
if (n == 0) {
xoffset = xl;
xi = 0;
yi = 0;
xiyi = 0;
xi2 = 0;
}
n++;
}
history[hindex].x = xl - xoffset;
history[hindex].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;
if (++hindex >= ALTITUDE_HISTORY_LENGTH)
hindex = 0;
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* Functions in this file:
void init_commands()
void update_auto()
void reload_commands_airstart()
struct Location get_cmd_with_index(int i)
void set_cmd_with_index(struct Location temp, int i)
void increment_cmd_index()
void decrement_cmd_index()
long read_alt_to_hold()
void set_next_WP(struct Location *wp)
void set_guided_WP(void)
void init_home()
************************************************************
*/
static void init_commands()
{
g.command_index.set_and_save(0);
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
}
static void update_auto()
{
if (g.command_index >= g.command_total) {
handle_no_commands();
if(g.command_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
}
} else {
if(g.command_index != 0) {
g.command_index = nav_command_index;
nav_command_index--;
}
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
process_next_command();
}
}
// this is only used by an air-start
static void reload_commands_airstart()
{
init_commands();
g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all?
decrement_cmd_index();
}
// Getters
// -------
static struct Location get_cmd_with_index(int i)
{
struct Location temp;
long mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.command_total) {
temp.id = CMD_BLANK;
}else{
// read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.options = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.p1 = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
}
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
temp.alt += home.alt;
}
return temp;
}
// Setters
// -------
static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.command_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 {
temp.options = 0;
}
eeprom_write_byte((uint8_t *) mem, temp.id);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.options);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.p1);
mem++;
eeprom_write_dword((uint32_t *) mem, temp.alt);
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat);
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng);
}
static void increment_cmd_index()
{
if (g.command_index <= g.command_total) {
g.command_index.set_and_save(g.command_index + 1);
}
}
static void decrement_cmd_index()
{
if (g.command_index > 0) {
g.command_index.set_and_save(g.command_index - 1);
}
}
static long read_alt_to_hold()
{
if(g.RTL_altitude < 0)
return current_loc.alt;
else
return g.RTL_altitude + home.alt;
}
/*
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)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP = next_WP;
// Load the next_WP slot
// ---------------------
next_WP = *wp;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
offset_altitude = next_WP.alt - prev_WP.alt;
else
offset_altitude = 0;
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
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();
}
static void set_guided_WP(void)
{
// 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);
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);
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
// run this at setup on the ground
// -------------------------------
void init_home()
{
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
// block until we get a good fix
// -----------------------------
while (!g_gps->new_data || !g_gps->fix) {
g_gps->update();
}
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
home.alt = max(g_gps->altitude, 0);
home_is_set = true;
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
// Save prev loc
// -------------
next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands_logic.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static void
handle_process_nav_cmd()
{
// reset navigation integrators
// -------------------------
reset_I();
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
switch(next_nav_command.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp();
break;
case MAV_CMD_NAV_LAND: // LAND to Waypoint
do_land();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited();
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
do_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
do_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
default:
break;
}
}
static void
handle_process_condition_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_CONDITION_DELAY:
do_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
do_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt();
break;
/* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also)
gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
landing_pitch = next_nav_command.lng * 100;
g.airspeed_cruise = next_nav_command.alt * 100;
g.throttle_cruise = next_nav_command.lat;
landing_distance = next_nav_command.p1;
SendDebug_P("MSG: throttle_cruise = ");
SendDebugln(g.throttle_cruise,DEC);
break;
*/
default:
break;
}
}
static void handle_process_do_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_DO_JUMP:
do_jump();
break;
case MAV_CMD_DO_CHANGE_SPEED:
do_change_speed();
break;
case MAV_CMD_DO_SET_HOME:
do_set_home();
break;
case MAV_CMD_DO_SET_SERVO:
do_set_servo();
break;
case MAV_CMD_DO_SET_RELAY:
do_set_relay();
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo();
break;
case MAV_CMD_DO_REPEAT_RELAY:
do_repeat_relay();
break;
}
}
static void handle_no_commands()
{
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = home;
next_nav_command.alt = read_alt_to_hold();
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
handle_process_nav_cmd();
}
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
static bool verify_nav_command() // Returns true if command complete
{
switch(nav_command_ID) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
break;
case MAV_CMD_NAV_LAND:
return verify_land();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
break;
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
return false;
break;
}
}
static bool verify_condition_command() // Returns true if command complete
{
switch(non_nav_command_ID) {
case NO_COMMAND:
break;
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
case WAIT_COMMAND:
return 0;
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
break;
}
return false;
}
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
static void do_RTL(void)
{
control_mode = RTL;
crash_timer = 0;
next_WP = home;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
static void do_takeoff()
{
set_next_WP(&next_nav_command);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch = (int)next_nav_command.p1 * 100;
//Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch);
//Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt);
takeoff_altitude = next_nav_command.alt;
//Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude);
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
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
// Flag also used to override "on the ground" throttle disable
}
static void do_nav_wp()
{
set_next_WP(&next_nav_command);
}
static void do_land()
{
set_next_WP(&next_nav_command);
}
static void do_loiter_unlimited()
{
set_next_WP(&next_nav_command);
}
static void do_loiter_turns()
{
set_next_WP(&next_nav_command);
loiter_total = next_nav_command.p1 * 360;
}
static void do_loiter_time()
{
set_next_WP(&next_nav_command);
loiter_time = millis();
loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
static bool verify_takeoff()
{
if (g_gps->ground_speed > 300){
if(hold_course == -1){
// save our current course to take off
if(g.compass_enabled) {
hold_course = dcm.yaw_sensor;
} else {
hold_course = g_gps->ground_course;
}
}
}
if(hold_course > -1){
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
calc_bearing_error();
}
if (current_loc.alt > takeoff_altitude) {
hold_course = -1;
takeoff_complete = true;
return true;
} else {
return false;
}
}
static bool verify_land()
{
// we don't verify landing - we never go to a new Nav command after Land
if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
|| (current_loc.alt <= next_WP.alt + 300)){
land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude
if(hold_course == -1){
// save our current course to land
//hold_course = yaw_sensor;
// save the course line of the runway to land
hold_course = crosstrack_bearing;
}
}
if(hold_course > -1){
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
calc_bearing_error();
}
update_crosstrack();
return false;
}
static bool verify_nav_wp()
{
hold_course = -1;
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
return true;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
return true;
}
return false;
}
static bool verify_loiter_unlim()
{
update_loiter();
calc_bearing_error();
return false;
}
static 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
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
return true;
}
return false;
}
static bool verify_loiter_turns()
{
update_loiter();
calc_bearing_error();
if(loiter_sum > loiter_total) {
loiter_total = 0;
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
// clear the command queue;
return true;
}
return false;
}
static bool verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
return true;
}else{
return false;
}
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/
static void do_wait_delay()
{
condition_start = millis();
condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
}
static void do_change_alt()
{
condition_rate = next_nonnav_command.lat;
condition_value = next_nonnav_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()
{
condition_value = next_nonnav_command.lat;
}
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
static bool verify_wait_delay()
{
if ((unsigned)(millis() - condition_start) > condition_value){
condition_value = 0;
return true;
}
return false;
}
static bool verify_change_alt()
{
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
condition_value = 0;
return true;
}
target_altitude += condition_rate / 10;
return false;
}
static bool verify_within_distance()
{
if (wp_distance < condition_value){
condition_value = 0;
return true;
}
return false;
}
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
static void do_loiter_at_location()
{
next_WP = current_loc;
}
static void do_jump()
{
struct Location temp;
if(next_nonnav_command.lat > 0) {
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
temp = get_cmd_with_index(g.command_index);
temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter
set_cmd_with_index(temp, g.command_index);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
} else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
}
}
static void do_change_speed()
{
// Note: we have no implementation for commanded ground speed, only air speed and throttle
if(next_nonnav_command.alt > 0)
g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
if(next_nonnav_command.lat > 0)
g.throttle_cruise.set_and_save(next_nonnav_command.lat);
}
static void do_set_home()
{
if(next_nonnav_command.p1 == 1 && GPS_enabled) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_nonnav_command.lng; // Lon * 10**7
home.lat = next_nonnav_command.lat; // Lat * 10**7
home.alt = max(next_nonnav_command.alt, 0);
home_is_set = true;
}
}
static void do_set_servo()
{
APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
static void do_set_relay()
{
if (next_nonnav_command.p1 == 1) {
relay.on();
} else if (next_nonnav_command.p1 == 0) {
relay.off();
}else{
relay.toggle();
}
}
static void do_repeat_servo()
{
event_id = next_nonnav_command.p1 - 1;
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.lat * 2;
event_value = next_nonnav_command.alt;
switch(next_nonnav_command.p1) {
case CH_5:
event_undo_value = g.rc_5.radio_trim;
break;
case CH_6:
event_undo_value = g.rc_6.radio_trim;
break;
case CH_7:
event_undo_value = g.rc_7.radio_trim;
break;
case CH_8:
event_undo_value = g.rc_8.radio_trim;
break;
}
update_events();
}
}
static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.alt * 2;
update_events();
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands_process.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t cmd_index)
{
struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index - 1);
process_next_command();
}
}
// called by 10 Hz loop
// --------------------
static void update_commands(void)
{
if(home_is_set == false){
return; // don't do commands
}
if(control_mode == AUTO){
process_next_command();
} // Other (eg GCS_Auto) modes may be implemented here
}
static void verify_commands(void)
{
if(verify_nav_command()){
nav_command_ID = NO_COMMAND;
}
if(verify_condition_command()){
non_nav_command_ID = NO_COMMAND;
}
}
static void process_next_command()
{
// This function makes sure that we always have a current navigation command
// and loads conditional or immediate commands if applicable
struct Location temp;
byte old_index;
// these are Navigation/Must commands
// ---------------------------------
if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
old_index = nav_command_index;
temp.id = MAV_CMD_NAV_LAST;
while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
nav_command_index++;
temp = get_cmd_with_index(nav_command_index);
}
gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
if(nav_command_index > g.command_total){
// we are out of commands!
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
} else {
next_nav_command = temp;
nav_command_ID = next_nav_command.id;
non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded
non_nav_command_ID = NO_COMMAND;
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nav_command);
}
process_nav_cmd();
}
}
// these are Condition/May and Do/Now commands
// -------------------------------------------
if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command
non_nav_command_index = old_index + 1;
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
} else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
non_nav_command_index++;
}
//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
temp = get_cmd_with_index(non_nav_command_index);
if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do
g.command_index.set_and_save(nav_command_index);
non_nav_command_index = nav_command_index;
non_nav_command_ID = WAIT_COMMAND;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
} else { // The next command is a non-nav command. Prepare to execute it.
g.command_index.set_and_save(non_nav_command_index);
next_nonnav_command = temp;
non_nav_command_ID = next_nonnav_command.id;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nonnav_command);
}
process_non_nav_command();
}
}
}
/**************************************************/
// These functions implement the commands.
/**************************************************/
static void process_nav_cmd()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
// clear non-nav command ID and index
non_nav_command_index = NO_COMMAND; // Redundant - remove?
non_nav_command_ID = NO_COMMAND; // Redundant - remove?
handle_process_nav_cmd();
}
static void process_non_nav_command()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_condition_command();
} else {
handle_process_do_command();
// flag command ID so a new one is loaded
// -----------------------------------------
non_nav_command_ID = NO_COMMAND;
}
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/control_modes.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void read_control_switch()
{
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
set_mode(flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
prev_WP = current_loc;
// reset navigation integrators
// -------------------------
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){
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;
if (pulsewidth > 1490 && pulsewidth <= 1620) return 3;
if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual
if (pulsewidth >= 1750) return 5; // Hardware Manual
return 0;
}
static void reset_control_switch()
{
oldSwitchPosition = 0;
read_control_switch();
}
static void update_servo_switches()
{
if (!g.switch_enable) {
// switches are disabled in EEPROM (see SWITCH_ENABLE option)
// this means the EEPROM control of all channel reversal is enabled
return;
}
// up is reverse
// up is elevon
g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode
if (g.mix_mode == 0) {
g.channel_roll.set_reverse((PINE & 128) ? true : false);
g.channel_pitch.set_reverse((PINE & 64) ? true : false);
g.channel_rudder.set_reverse((PINL & 64) ? true : false);
} else {
g.reverse_elevons = (PINE & 128) ? true : false;
g.reverse_ch1_elevon = (PINE & 64) ? true : false;
g.reverse_ch2_elevon = (PINL & 64) ? true : false;
}
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/events.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void failsafe_short_on_event()
{
// This is how to handle a short loss of control signal failsafe.
failsafe = FAILSAFE_SHORT;
ch3_failsafe_timer = millis();
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on"));
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
set_mode(CIRCLE);
break;
case AUTO:
case LOITER:
if(g.short_fs_action == 1) {
set_mode(RTL);
}
break;
case CIRCLE:
case RTL:
default:
break;
}
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
static void failsafe_long_on_event()
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text_P(SEVERITY_LOW, PSTR("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:
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
case CIRCLE:
set_mode(RTL);
break;
case AUTO:
case LOITER:
if(g.long_fs_action == 1) {
set_mode(RTL);
}
break;
case RTL:
default:
break;
}
}
static void failsafe_short_off_event()
{
// We're back in radio contact
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
failsafe = FAILSAFE_NONE;
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
reset_control_switch();
// Reset control integrators
// ---------------------
reset_I();
}
#if BATTERY_EVENT == ENABLED
static void low_battery_event(void)
{
gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
}
#endif
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if (event_repeat > 0){
event_repeat --;
}
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
event_timer = millis();
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
APM_RC.OutputCh(event_id, event_value); // send to Servos
} else {
APM_RC.OutputCh(event_id, event_undo_value);
}
}
if (event_id == RELAY_TOGGLE) {
relay.toggle();
}
}
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/navigation.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
static void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (g_gps->fix == 0)
{
g_gps->new_data = false;
return;
}
if(next_WP.lat == 0){
return;
}
// waypoint distance from plane
// ----------------------------
wp_distance = get_distance(&current_loc, &next_WP);
if (wp_distance < 0){
gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
//Serial.println(wp_distance,DEC);
return;
}
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrac correction
// ------------------------------------------
nav_bearing = target_bearing;
// check if we have missed the WP
loiter_delta = (target_bearing - old_target_bearing)/100;
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta);
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
}
#if 0
// Disabled for now
void calc_distance_error()
{
distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
wp_distance = max(distance_estimate,10);
}
#endif
static void calc_airspeed_errors()
{
// XXX excess casting here
if(control_mode>=AUTO && airspeed_nudge > 0) {
airspeed_error = g.airspeed_cruise + airspeed_nudge - airspeed;
airspeed_energy_error = (long)(((long)(g.airspeed_cruise + airspeed_nudge) * (long)(g.airspeed_cruise + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
} else {
airspeed_error = g.airspeed_cruise - airspeed;
airspeed_energy_error = (long)(((long)g.airspeed_cruise * (long)g.airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
}
}
static void calc_bearing_error()
{
if(takeoff_complete == true || g.compass_enabled == true) {
bearing_error = nav_bearing - dcm.yaw_sensor;
} else {
// TODO: we need to use the Yaw gyro for in between GPS reads,
// maybe as an offset from a saved gryo value.
bearing_error = nav_bearing - g_gps->ground_course;
}
bearing_error = wrap_180(bearing_error);
}
static void calc_altitude_error()
{
if(control_mode == AUTO && offset_altitude != 0) {
// limit climb rates
target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30));
// stay within a certain range
if(prev_WP.alt > next_WP.alt){
target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
}else{
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
}
} else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
target_altitude = next_WP.alt;
}
/*
// Disabled for now
#if AIRSPEED_SENSOR == 1
long altitude_estimate; // for smoothing GPS output
// special thanks to Ryan Beall for this one
float pitch_angle = pitch_sensor - g.pitch_trim; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°)
pitch_angle = constrain(pitch_angle, -2000, 2000);
float scale = sin(radians(pitch_angle * .01));
altitude_estimate += (float)airspeed * .0002 * scale;
altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt);
// compute altitude error for throttle control
altitude_error = target_altitude - altitude_estimate;
#else
altitude_error = target_altitude - current_loc.alt;
#endif
*/
altitude_error = target_altitude - current_loc.alt;
}
static long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
static long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
static void update_loiter()
{
float power;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1);
nav_bearing += (int)(9000.0 * (2.0 + power));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
/*
if (wp_distance < g.loiter_radius){
nav_bearing += 9000;
}else{
nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
}
update_crosstrack();
*/
nav_bearing = wrap_360(nav_bearing);
}
static void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
}
}
static 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)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
static long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/planner.pde"
// -*- 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.
static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv);
// 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_Common for implementation details
static const struct Menu::command planner_menu_commands[] PROGMEM = {
{"gcs", planner_gcs},
};
// A Macro to create the Menu
MENU(planner_menu, "planner", planner_menu_commands);
static int8_t
planner_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
planner_menu.run();
return 0;
}
static int8_t
planner_gcs(uint8_t argc, const Menu::arg *argv)
{
gcs0.init(&Serial);
gcs3.init(&Serial3);
int loopcount = 0;
while (1) {
if (millis()-fast_loopTimer > 19) {
fast_loopTimer = millis();
gcs_update();
gcs_data_stream_send(45,1000);
if ((loopcount % 5) == 0) // 10 hz
gcs_data_stream_send(5,45);
if ((loopcount % 16) == 0) { // 3 hz
gcs_data_stream_send(1,5);
gcs_send_message(MSG_HEARTBEAT);
}
loopcount++;
}
}
return 0;
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/radio.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//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
static void init_rc_in()
{
// set rc reversing
update_servo_switches();
// 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_throttle.set_range(0, 100);
// set rc dead zones
g.channel_roll.set_dead_zone(60);
g.channel_pitch.set_dead_zone(60);
g.channel_rudder.set_dead_zone(60);
g.channel_throttle.set_dead_zone(6);
//g.channel_roll.dead_zone = 60;
//g.channel_pitch.dead_zone = 60;
//g.channel_rudder.dead_zone = 60;
//g.channel_throttle.dead_zone = 6;
//set auxiliary ranges
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
}
static void init_rc_out()
{
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
APM_RC.Init(); // APM Radio initialization
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
}
static void read_radio()
{
ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH);
if(g.mix_mode == 0){
g.channel_roll.set_pwm(ch1_temp);
g.channel_pitch.set_pwm(ch2_temp);
}else{
g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
}
g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
// TO DO - go through and patch throttle reverse for RC_Channel library compatibility
#if THROTTLE_REVERSE == 1
g.channel_throttle.radio_in = g.channel_throttle.radio_max + g.channel_throttle.radio_min - g.channel_throttle.radio_in;
#endif
control_failsafe(g.channel_throttle.radio_in);
g.channel_throttle.servo_out = g.channel_throttle.control_in;
if (g.channel_throttle.servo_out > 50) {
if(g.airspeed_enabled == true) {
airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
} else {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
}
} else {
airspeed_nudge = 0;
throttle_nudge = 0;
}
/*
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
g.rc_4.control_in);
*/
}
static void control_failsafe(uint16_t pwm)
{
if(g.throttle_fs_enabled == 0)
return;
// Check for failsafe condition based on loss of GCS control
if (rc_override_active) {
if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) {
ch3_failsafe = true;
} else {
ch3_failsafe = false;
}
//Check for failsafe and debounce funky reads
} else if (g.throttle_fs_enabled) {
if (pwm < (unsigned)g.throttle_fs_value){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9){
gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm);
}else if(failsafeCounter == 10) {
ch3_failsafe = true;
}else if (failsafeCounter > 10){
failsafeCounter = 11;
}
}else if(failsafeCounter > 0){
// we are no longer in failsafe condition
// but we need to recover quickly
failsafeCounter--;
if (failsafeCounter > 3){
failsafeCounter = 3;
}
if (failsafeCounter == 1){
gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm);
}else if(failsafeCounter == 0) {
ch3_failsafe = false;
}else if (failsafeCounter <0){
failsafeCounter = -1;
}
}
}
}
static void trim_control_surfaces()
{
read_radio();
// Store control surface trim values
// ---------------------------------
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
}else{
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
//Recompute values here using new values for elevon1_trim and elevon2_trim
//We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
}
// save to eeprom
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
G_RC_AUX(k_aileron)->save_eeprom();
}
static void trim_radio()
{
for (int y = 0; y < 30; y++) {
read_radio();
}
// Store the trim values
// ---------------------
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else {
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
}
// save to eeprom
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
G_RC_AUX(k_aileron)->save_eeprom();
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/sensors.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
void ReadSCP1000(void) {}
static void init_barometer(void)
{
int flashcount = 0;
long ground_pressure = 0;
int ground_temperature;
while(ground_pressure == 0){
barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = barometer.Press;
ground_temperature = barometer.Temp;
mavlink_delay(20);
//Serial.printf("barometer.Press %ld\n", barometer.Press);
}
for(int i = 0; i < 30; i++){ // We take some readings...
#if HIL_MODE == HIL_MODE_SENSORS
gcs_update(); // look for inbound hil packets
#endif
barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
mavlink_delay(20);
if(flashcount == 5) {
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, LOW);
}
if(flashcount >= 10) {
flashcount = 0;
digitalWrite(C_LED_PIN, HIGH);
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, HIGH);
}
flashcount++;
}
g.ground_pressure.set_and_save(ground_pressure);
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."));
}
static long read_barometer(void)
{
float x, scaling, temp;
barometer.Read(); // Get new data from absolute pressure sensor
//abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
scaling = (float)g.ground_pressure / (float)abs_pressure;
temp = ((float)g.ground_temperature) + 273.15f;
x = log(scaling) * temp * 29271.267f;
return (x / 10);
}
// in M/S * 100
static void read_airspeed(void)
{
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed
if (g.airspeed_offset == 0) {
// runtime enabling of airspeed, we need to do instant
// calibration before we can use it. This isn't as
// accurate as the 50 point average in zero_airspeed(),
// but it is better than using it uncalibrated
airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
g.airspeed_offset.set_and_save(airspeed_raw);
}
airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75);
airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0);
airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100;
#endif
calc_airspeed_errors();
}
static void zero_airspeed(void)
{
airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
for(int c = 0; c < 10; c++){
delay(20);
airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10);
}
g.airspeed_offset.set_and_save(airspeed_raw);
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void read_battery(void)
{
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
if(g.battery_monitoring == 1)
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
if(g.battery_monitoring == 2)
battery_voltage = battery_voltage4;
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
battery_voltage = battery_voltage1;
if(g.battery_monitoring == 4) {
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
}
#if BATTERY_EVENT == ENABLED
if(battery_voltage < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
#endif
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/setup.pde"
// -*- 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);
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
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 = {
// command function called
// ======= ===============
{"reset", setup_factory},
{"radio", setup_radio},
{"modes", setup_flightmodes},
{"compass", setup_compass},
{"declination", setup_declination},
{"battery", setup_batt_monitor},
{"show", setup_show},
{"erase", setup_erase},
};
// Create the setup menu object.
MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu.
static int8_t
setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
Serial.printf_P(PSTR("Setup Mode\n"
"\n"
"IMPORTANT: if you have not previously set this system up, use the\n"
"'reset' command to initialize the EEPROM to sensible default values\n"
"and then the 'radio' command to configure for your radio.\n"
"\n"));
// Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run();
return 0;
}
// Print the current configuration.
// Called by the setup menu 'show' command.
static int8_t
setup_show(uint8_t argc, const Menu::arg *argv)
{
// clear the area
print_blanks(8);
report_radio();
report_batt_monitor();
report_gains();
report_xtrack();
report_throttle();
report_flight_modes();
report_imu();
report_compass();
Serial.printf_P(PSTR("Raw Values\n"));
print_divider();
AP_Var_menu_show(argc, argv);
return(0);
}
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
// Called by the setup menu 'factoryreset' command.
static int8_t
setup_factory(uint8_t argc, const Menu::arg *argv)
{
int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
AP_Var::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
for (;;) {
}
// note, cannot actually return here
return(0);
}
// Perform radio setup.
// Called by the setup menu 'radio' command.
static int8_t
setup_radio(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\n\nRadio Setup:\n"));
uint8_t i;
for(i = 0; i < 100;i++){
delay(20);
read_radio();
}
if(g.channel_roll.radio_in < 500){
while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000);
// stop here
}
}
g.channel_roll.radio_min = g.channel_roll.radio_in;
g.channel_pitch.radio_min = g.channel_pitch.radio_in;
g.channel_throttle.radio_min = g.channel_throttle.radio_in;
g.channel_rudder.radio_min = g.channel_rudder.radio_in;
g.rc_5.radio_min = g.rc_5.radio_in;
g.rc_6.radio_min = g.rc_6.radio_in;
g.rc_7.radio_min = g.rc_7.radio_in;
g.rc_8.radio_min = g.rc_8.radio_in;
g.channel_roll.radio_max = g.channel_roll.radio_in;
g.channel_pitch.radio_max = g.channel_pitch.radio_in;
g.channel_throttle.radio_max = g.channel_throttle.radio_in;
g.channel_rudder.radio_max = g.channel_rudder.radio_in;
g.rc_5.radio_max = g.rc_5.radio_in;
g.rc_6.radio_max = g.rc_6.radio_in;
g.rc_7.radio_max = g.rc_7.radio_in;
g.rc_8.radio_max = g.rc_8.radio_in;
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;
g.rc_5.radio_trim = 1500;
g.rc_6.radio_trim = 1500;
g.rc_7.radio_trim = 1500;
g.rc_8.radio_trim = 1500;
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
g.channel_roll.update_min_max();
g.channel_pitch.update_min_max();
g.channel_throttle.update_min_max();
g.channel_rudder.update_min_max();
g.rc_5.update_min_max();
g.rc_6.update_min_max();
g.rc_7.update_min_max();
g.rc_8.update_min_max();
if(Serial.available() > 0){
Serial.flush();
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
g.rc_5.save_eeprom();
g.rc_6.save_eeprom();
g.rc_7.save_eeprom();
g.rc_8.save_eeprom();
print_done();
break;
}
}
trim_radio();
report_radio();
return(0);
}
static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
byte switchPosition, mode = 0;
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter();
trim_radio();
while(1){
delay(20);
read_radio();
switchPosition = readSwitch();
// look for control switch change
if (oldSwitchPosition != switchPosition){
// force position 5 to MANUAL
if (switchPosition > 4) {
flight_modes[switchPosition] = MANUAL;
}
// update our current mode
mode = flight_modes[switchPosition];
// update the user
print_switch(switchPosition, mode);
// Remember switch position
oldSwitchPosition = switchPosition;
}
// look for stick input
int radioInputSwitch = radio_input_switch();
if (radioInputSwitch != 0){
mode += radioInputSwitch;
while (
mode != MANUAL &&
mode != CIRCLE &&
mode != STABILIZE &&
mode != FLY_BY_WIRE_A &&
mode != FLY_BY_WIRE_B &&
mode != AUTO &&
mode != RTL &&
mode != LOITER)
{
if (mode < MANUAL)
mode = LOITER;
else if (mode >LOITER)
mode = MANUAL;
else
mode += radioInputSwitch;
}
// Override position 5
if(switchPosition > 4)
mode = MANUAL;
// save new mode
flight_modes[switchPosition] = mode;
// print new mode
print_switch(switchPosition, mode);
}
// escape hatch
if(Serial.available() > 0){
// save changes
for (mode=0; mode<6; mode++)
flight_modes[mode].save();
report_flight_modes();
print_done();
return (0);
}
}
}
static int8_t
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)
{
int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
zero_eeprom();
return 0;
}
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;
}
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.compass_enabled = false;
} else {
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
report_compass();
return 0;
}
g.compass_enabled.save();
report_compass();
return 0;
}
static int8_t
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
{
if(argv[1].i >= 0 && argv[1].i <= 4){
g.battery_monitoring.set_and_save(argv[1].i);
} else {
Serial.printf_P(PSTR("\nOptions: 0-4"));
}
report_batt_monitor();
return 0;
}
/***************************************************************************/
// CLI reports
/***************************************************************************/
static void report_batt_monitor()
{
//print_blanks(2);
Serial.printf_P(PSTR("Batt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell"));
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
print_blanks(2);
}
static void report_radio()
{
//print_blanks(2);
Serial.printf_P(PSTR("Radio\n"));
print_divider();
// radio
print_radio_values();
print_blanks(2);
}
static void report_gains()
{
//print_blanks(2);
Serial.printf_P(PSTR("Gains\n"));
print_divider();
Serial.printf_P(PSTR("servo roll:\n"));
print_PID(&g.pidServoRoll);
Serial.printf_P(PSTR("servo pitch:\n"));
print_PID(&g.pidServoPitch);
Serial.printf_P(PSTR("servo rudder:\n"));
print_PID(&g.pidServoRudder);
Serial.printf_P(PSTR("nav roll:\n"));
print_PID(&g.pidNavRoll);
Serial.printf_P(PSTR("nav pitch airspeed:\n"));
print_PID(&g.pidNavPitchAirspeed);
Serial.printf_P(PSTR("energry throttle:\n"));
print_PID(&g.pidTeThrottle);
Serial.printf_P(PSTR("nav pitch alt:\n"));
print_PID(&g.pidNavPitchAltitude);
print_blanks(2);
}
static void report_xtrack()
{
//print_blanks(2);
Serial.printf_P(PSTR("Crosstrack\n"));
print_divider();
// radio
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"),
(float)g.crosstrack_gain,
(int)g.crosstrack_entry_angle);
print_blanks(2);
}
static void report_throttle()
{
//print_blanks(2);
Serial.printf_P(PSTR("Throttle\n"));
print_divider();
Serial.printf_P(PSTR("min: %d\n"
"max: %d\n"
"cruise: %d\n"
"failsafe_enabled: %d\n"
"failsafe_value: %d\n"),
(int)g.throttle_min,
(int)g.throttle_max,
(int)g.throttle_cruise,
(int)g.throttle_fs_enabled,
(int)g.throttle_fs_value);
print_blanks(2);
}
static void report_imu()
{
//print_blanks(2);
Serial.printf_P(PSTR("IMU\n"));
print_divider();
print_gyro_offsets();
print_accel_offsets();
print_blanks(2);
}
static 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;
}
print_divider();
print_enabled(g.compass_enabled);
// mag declination
Serial.printf_P(PSTR("Mag Declination: %4.4f\n"),
degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets();
// mag offsets
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
offsets.x,
offsets.y,
offsets.z);
print_blanks(2);
}
static void report_flight_modes()
{
//print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n"));
print_divider();
for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]);
}
print_blanks(2);
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
static void
print_PID(PID * pid)
{
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
pid->kP(),
pid->kI(),
pid->kD(),
(long)pid->imax());
}
static void
print_radio_values()
{
Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
}
static void
print_switch(byte p, byte m)
{
Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]);
}
static void
print_done()
{
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
}
static void
print_blanks(int num)
{
while(num > 0){
num--;
Serial.println("");
}
}
static void
print_divider(void)
{
for (int i = 0; i < 40; i++) {
Serial.printf_P(PSTR("-"));
}
Serial.println("");
}
static int8_t
radio_input_switch(void)
{
static int8_t bouncer = 0;
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) {
bouncer = 10;
}
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) {
bouncer = -10;
}
if (bouncer >0) {
bouncer --;
}
if (bouncer <0) {
bouncer ++;
}
if (bouncer == 1 || bouncer == -1) {
return bouncer;
} else {
return 0;
}
}
static void zero_eeprom(void)
{
byte b = 0;
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b);
}
Serial.printf_P(PSTR("done\n"));
}
static void print_enabled(bool b)
{
if(b)
Serial.printf_P(PSTR("en"));
else
Serial.printf_P(PSTR("dis"));
Serial.printf_P(PSTR("abled\n"));
}
static void
print_accel_offsets(void)
{
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
(float)imu.ax(),
(float)imu.ay(),
(float)imu.az());
}
static void
print_gyro_offsets(void)
{
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
(float)imu.gx(),
(float)imu.gy(),
(float)imu.gz());
}
#endif // CLI_ENABLED
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/system.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*****************************************************************************
The init_ardupilot function processes everything we need for an in - air restart
We will determine later if we are actually on the ground and process a
ground start in that case.
*****************************************************************************/
#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
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads from flash memory
static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Commands:\n"
" logs log readback/setup mode\n"
" setup setup mode\n"
" test test mode\n"
"\n"
"Move the slide switch and reset to FLY.\n"
"\n"));
return(0);
}
// Command/function table for the top-level menu.
static const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"logs", process_logs},
{"setup", setup_mode},
{"test", test_mode},
{"help", main_menu_help},
{"planner", planner_mode}
};
// Create the top-level menu object.
MENU(main_menu, THISFIRMWARE, main_menu_commands);
// the user wants the CLI. It never exits
static void run_cli(void)
{
while (1) {
main_menu.run();
}
}
#endif // CLI_ENABLED
static void init_ardupilot()
{
// Console serial port
//
// The console port buffers are defined to be sufficiently large to support
// the console's use as a logging device, optionally as the GPS port when
// GPS_PROTOCOL_IMU is selected, and as the telemetry port.
//
// XXX This could be optimised to reduce the buffer sizes in the cases
// where they are not otherwise required.
//
Serial.begin(SERIAL0_BAUD, 128, 128);
// GPS serial port.
//
// XXX currently the EM406 (SiRF receiver) is nominally configured
// at 57600, however it's not been supported to date. We should
// probably standardise on 38400.
//
// XXX the 128 byte receive buffer may be too small for NMEA, depending
// on the message set configured.
//
// standard gps running
Serial1.begin(38400, 128, 16);
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %u\n"),
memcheck_available_memory());
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
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();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
} else if (g.format_version != Parameters::k_format_version) {
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();
// save the new format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
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);
}
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// Telemetry port.
//
// Not used if telemetry is going to the console.
//
// XXX for unidirectional protocols, we could (should) minimize
// the receive buffer, and the transmit buffer could also be
// shrunk for protocols that don't send large messages.
//
Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
mavlink_system.sysid = g.sysid_this_mav;
#if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization
barometer.Init(); // APM Abs Pressure sensor initialization
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
}
}
/*
Init is depricated - Jason
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC);
}
*/
#endif
#if LOGGING_ENABLED == ENABLED
DataFlash.Init(); // DataFlash log initialization
#endif
// Do GPS init
g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization
g_gps->callback = mavlink_delay;
// init the GCS
gcs0.init(&Serial);
gcs3.init(&Serial3);
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
mavlink_system.type = MAV_FIXED_WING;
rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode
pinMode(PUSHBUTTON_PIN, INPUT); // unused
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
// If the switch is in 'menu' mode, run the main menu.
//
// Since we can't be sure that the setup or test mode won't leave
// 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 && CLI_SLIDER_ENABLED == ENABLED
if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
Serial.printf_P(PSTR("\n"
"Entering interactive setup mode...\n"
"\n"
"If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n"
"Type 'help' to list commands, 'exit' to leave a submenu.\n"
"Visit the 'setup' menu for first-time configuration.\n"));
Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
run_cli();
}
#else
Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n"));
#endif // CLI_ENABLED
if(g.log_bitmask != 0){
// TODO - Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off
byte last_log_num = get_num_logs();
start_new_log(last_log_num);
}
// read in the flight switches
update_servo_switches();
if (ENABLE_AIR_START == 1) {
// Perform an air start and get back to flying
gcs_send_text_P(SEVERITY_LOW,PSTR("<init_ardupilot> AIR START"));
// Get necessary data from EEPROM
//----------------
//read_EEPROM_airstart_critical();
#if HIL_MODE != HIL_MODE_ATTITUDE
imu.init(IMU::WARM_START);
dcm.set_centripetal(1);
#endif
// This delay is important for the APM_RC library to work.
// We need some time for the comm between the 328 and 1280 to be established.
int old_pulse = 0;
while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(g.flight_mode_channel)) > 5 ||
APM_RC.InputCh(g.flight_mode_channel) == 1000 ||
APM_RC.InputCh(g.flight_mode_channel) == 1200)) {
old_pulse = APM_RC.InputCh(g.flight_mode_channel);
delay(25);
}
GPS_enabled = false;
g_gps->update();
if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED) GPS_enabled = true;
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_AIRSTART_MSG);
reload_commands_airstart(); // Get set to resume AUTO from where we left off
}else {
startup_ground();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
}
set_mode(MANUAL);
// set the correct flight mode
// ---------------------------
reset_control_switch();
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
static void startup_ground(void)
{
set_mode(INITIALISING);
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
#if(GROUND_START_DELAY > 0)
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> With Delay"));
delay(GROUND_START_DELAY * 1000);
#endif
// Makes the servos wiggle
// step 1 = 1 wiggle
// -----------------------
demo_servos(1);
//IMU ground start
//------------------------
//
startup_IMU_ground();
// read the radio to set trims
// ---------------------------
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)
{
// initialize airspeed sensor
// --------------------------
zero_airspeed();
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> zero airspeed calibrated"));
}
else
{
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> NO airspeed"));
}
#endif
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
// initialize commands
// -------------------
init_commands();
// Read in the GPS - see if one is connected
GPS_enabled = false;
for (byte counter = 0; ; counter++) {
g_gps->update();
if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){
GPS_enabled = true;
break;
}
if (counter >= 2) {
GPS_enabled = false;
break;
}
}
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
demo_servos(3);
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}
static void set_mode(byte mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
control_mode = mode;
crash_timer = 0;
switch(control_mode)
{
case INITIALISING:
case MANUAL:
case CIRCLE:
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
break;
case AUTO:
update_auto();
break;
case RTL:
do_RTL();
break;
case LOITER:
do_loiter_at_location();
break;
case GUIDED:
set_guided_WP();
break;
default:
do_RTL();
break;
}
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
static void check_long_failsafe()
{
// only act on changes
// -------------------
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){
if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
failsafe = FAILSAFE_LONG;
failsafe_long_on_event();
}
if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) {
failsafe = FAILSAFE_LONG;
failsafe_long_on_event();
}
if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
failsafe = FAILSAFE_GCS;
failsafe_long_on_event();
}
} else {
// We do not change state but allow for user to change mode
if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE;
}
}
static void check_short_failsafe()
{
// only act on changes
// -------------------
if(failsafe == FAILSAFE_NONE){
if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
failsafe_short_on_event();
}
}
if(failsafe == FAILSAFE_SHORT){
if(!ch3_failsafe) {
failsafe_short_off_event();
}
}
}
static void startup_IMU_ground(void)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
// -----------------------
demo_servos(2);
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane"));
mavlink_delay(1000);
imu.init(IMU::COLD_START, mavlink_delay);
dcm.set_centripetal(1);
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif // HIL_MODE_ATTITUDE
digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW);
}
static void update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
switch (g_gps->status()) {
case(2):
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
break;
case(1):
if (g_gps->valid_read == true){
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (GPS_light){
digitalWrite(C_LED_PIN, LOW);
} else {
digitalWrite(C_LED_PIN, HIGH);
}
g_gps->valid_read = false;
}
break;
default:
digitalWrite(C_LED_PIN, LOW);
break;
}
}
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
dcm.gyro_sat_count = 0;
imu.adc_constraints = 0;
dcm.renorm_sqrt_count = 0;
dcm.renorm_blowup_count = 0;
gps_fix_count = 0;
pmTest1 = 0;
perf_mon_timer = millis();
}
/*
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;
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/test.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
static int8_t test_gyro(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
// 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_Common for implementation details
static const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"failsafe", test_failsafe},
{"battery", test_battery},
{"relay", test_relay},
{"waypoints", test_wp},
{"xbee", test_xbee},
{"eedump", test_eedump},
{"modeswitch", test_modeswitch},
{"dipswitches", test_dipswitches},
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
#if HIL_MODE == HIL_MODE_DISABLED
{"adc", test_adc},
{"gps", test_gps},
{"rawgps", test_rawgps},
{"imu", test_imu},
{"gyro", test_gyro},
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
{"compass", test_mag},
{"current", test_current},
#elif HIL_MODE == HIL_MODE_SENSORS
{"adc", test_adc},
{"gps", test_gps},
{"imu", test_imu},
{"gyro", test_gyro},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_ATTITUDE
#endif
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
static int8_t
test_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run();
return 0;
}
static void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
}
static int8_t
test_eedump(uint8_t argc, const Menu::arg *argv)
{
int i, j;
// hexdump the EEPROM
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
Serial.printf_P(PSTR("%04x:"), i);
for (j = 0; j < 16; j++)
Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
Serial.println();
}
return(0);
}
static int8_t
test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
#if THROTTLE_REVERSE == 1
Serial.printf_P(PSTR("Throttle is reversed in config: \n"));
delay(1000);
#endif
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
update_servo_switches();
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_throttle.calc_pwm();
g.channel_rudder.calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.control_in,
g.channel_pitch.control_in,
g.channel_throttle.control_in,
g.channel_rudder.control_in,
g.rc_5.control_in,
g.rc_6.control_in,
g.rc_7.control_in,
g.rc_8.control_in);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv)
{
byte fail_test;
print_hit_enter();
for(int i = 0; i < 50; i++){
delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(g.channel_throttle.control_in > 0){
delay(20);
read_radio();
}
while(1){
delay(20);
read_radio();
if(g.channel_throttle.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in);
fail_test++;
}
if(oldSwitchPosition != readSwitch()){
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
if(fail_test > 0){
return (0);
}
if(Serial.available() > 0){
Serial.printf_P(PSTR("LOS caused no change in APM.\n"));
return (0);
}
}
}
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
if (g.battery_monitoring >=1 && g.battery_monitoring < 4) {
for (int i = 0; i < 80; i++){ // Need to get many samples for filter to stabilize
delay(20);
read_battery();
}
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
battery_voltage1,
battery_voltage2,
battery_voltage3,
battery_voltage4);
} else {
Serial.printf_P(PSTR("Not enabled\n"));
}
return (0);
}
static int8_t
test_current(uint8_t argc, const Menu::arg *argv)
{
if (g.battery_monitoring == 4) {
print_hit_enter();
delta_ms_medium_loop = 100;
while(1){
delay(100);
read_radio();
read_battery();
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
battery_voltage,
current_amps,
current_total);
// write out the servo PWM values
// ------------------------------
set_servos();
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Not enabled\n"));
return (0);
}
}
static int8_t
test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
Serial.printf_P(PSTR("Relay on\n"));
relay.on();
delay(3000);
if(Serial.available() > 0){
return (0);
}
Serial.printf_P(PSTR("Relay off\n"));
relay.off();
delay(3000);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
// save the alitude above home option
if(g.RTL_altitude < 0){
Serial.printf_P(PSTR("Hold current altitude\n"));
}else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100);
}
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(byte i = 0; i <= g.command_total; i++){
struct Location temp = get_cmd_with_index(i);
test_wp_print(&temp, i);
}
return (0);
}
static void
test_wp_print(struct Location *cmd, byte wp_index)
{
Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
cmd->alt,
cmd->lat,
cmd->lng);
}
static int8_t
test_xbee(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
while(1){
if (Serial3.available())
Serial3.write(Serial3.read());
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Control CH "));
Serial.println(FLIGHT_MODE_CHANNEL, DEC);
while(1){
delay(20);
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
Serial.printf_P(PSTR("Position %d\n"), switchPosition);
oldSwitchPosition = switchPosition;
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
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) {
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());
} 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);
}
if(Serial.available() > 0){
return (0);
}
}
}
//-------------------------------------------------------------------------------------------
// tests in this section are for real sensors or sensors that have been simulated
#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init();
delay(1000);
Serial.printf_P(PSTR("ADC\n"));
delay(1000);
while(1){
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%u\t"),adc.Ch(i));
Serial.println();
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(333);
// Blink GPS LED if we don't have a fix
// ------------------------------------
update_GPS_light();
g_gps->update();
if (g_gps->new_data){
Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
g_gps->latitude,
g_gps->longitude,
g_gps->altitude/100,
g_gps->num_sats);
}else{
Serial.printf_P(PSTR("."));
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_imu(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("Calibrating."));
imu.init(IMU::COLD_START);
print_hit_enter();
delay(1000);
while(1){
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
dcm.update_DCM();
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
medium_loopCounter = 0;
}
}
// We are using the IMU
// ---------------------
Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"),
(int)dcm.roll_sensor / 100,
(int)dcm.pitch_sensor / 100,
(uint16_t)dcm.yaw_sensor / 100);
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_gyro(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init();
delay(1000);
Serial.printf_P(PSTR("Gyro | Accel\n"));
delay(1000);
while(1){
imu.update(); // need this because we are not calling the DCM
Vector3f gyros = imu.get_gyro();
Vector3f accels = imu.get_accel();
Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"),
(int)gyros.x,
(int)gyros.y,
(int)gyros.z,
(int)accels.x,
(int)accels.y,
(int)accels.z);
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
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;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter();
while(1) {
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
dcm.update_DCM();
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;
}
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
Serial.println_P(PSTR("saving offsets"));
compass.save_offsets();
return (0);
}
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here
#if HIL_MODE == HIL_MODE_DISABLED
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 (g.airspeed_enabled == false){
Serial.printf_P(PSTR("airspeed: "));
print_enabled(false);
return (0);
}else{
print_hit_enter();
zero_airspeed();
Serial.printf_P(PSTR("airspeed: "));
print_enabled(true);
while(1){
delay(20);
read_airspeed();
Serial.printf_P(PSTR("%fm/s\n"), airspeed / 100.0);
if(Serial.available() > 0){
return (0);
}
}
}
}
static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
print_hit_enter();
home.alt = 0;
wp_distance = 0;
init_barometer();
while(1){
delay(100);
current_loc.alt = read_barometer() + home.alt;
Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld\n"),
current_loc.alt / 100.0,
abs_pressure);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
if (Serial3.available()){
digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
Serial1.write(Serial3.read());
digitalWrite(B_LED_PIN, LOW);
}
if (Serial1.available()){
digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS
Serial3.write(Serial1.read());
digitalWrite(C_LED_PIN, LOW);
}
if(Serial.available() > 0){
return (0);
}
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED
Reference in New Issue View Git Blame Copy Permalink