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

11569 lines
294 KiB
C++
Raw Blame History

#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/ArduCopter.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.0.50 Beta"
/*
ArduCopter Version 2.0 Beta
Authors: Jason Short
Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
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.
Special Thanks for Contributors:
Hein Hollander :Octo Support
Dani Saez :V Ocoto Support
Max Levine :Tri Support, Graphics
Jose Julio :Stabilization Control laws
Randy MacKay :Heli Support
Jani Hiriven :Testing feedback
Andrew Tridgell :Mavlink Support
James Goppert :Mavlink Support
Doug Weibel :Libraries
Mike Smith :Libraries, Coding support
HappyKillmore :Mavlink GCS
Michael Oborne :Mavlink GCS
Jack Dunkle :Alpha testing
Christof Schmid :Alpha testing
Oliver :Piezo support
Guntars :Arming safety suggestion
And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
*/
////////////////////////////////////////////////////////////////////////////////
// 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 <APM_PI.h> // PI library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library
#include <ModeFilter.h>
#include <AP_Relay.h> // APM relay
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <memcheck.h>
// Configuration
#include "defines.h"
#include "config.h"
// Local modules
#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 fifty_hz_loop() ;
static void slow_loop() ;
static void super_slow_loop() ;
static void update_GPS(void) ;
void update_yaw_mode(void) ;
void update_roll_pitch_mode(void) ;
void update_throttle_mode(void) ;
static void update_navigation() ;
static void read_AHRS(void) ;
static void update_trig(void);
static void update_altitude() ;
static void adjust_altitude() ;
static void tuning();
static void update_nav_wp() ;
static void update_auto_yaw() ;
static int get_stabilize_roll(long target_angle) ;
static int get_stabilize_pitch(long target_angle) ;
static int get_stabilize_yaw(long target_angle) ;
static int get_nav_throttle(long z_error) ;
static int get_rate_roll(long target_rate) ;
static int get_rate_pitch(long target_rate) ;
static int get_rate_yaw(long target_rate) ;
static void reset_hold_I(void) ;
static void reset_nav(void) ;
static long get_nav_yaw_offset(int yaw_input, int reset) ;
static int alt_hold_velocity() ;
static int get_angle_boost(int value) ;
static void init_camera() ;
static void camera_stabilization() ;
void write_byte(byte val) ;
void write_int(int val ) ;
void write_float(float val) ;
void write_long(long val) ;
void flush(byte id) ;
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 void clear_header() ;
static byte get_num_logs(void) ;
static void start_new_log() ;
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_GPS() ;
static void Log_Read_GPS() ;
static void Log_Write_Raw() ;
static void Log_Read_Raw() ;
static void Log_Write_Current() ;
static void Log_Read_Current() ;
static void Log_Write_Motors() ;
static void Log_Read_Motors() ;
static void Log_Write_Optflow() ;
static void Log_Read_Optflow() ;
static void Log_Write_Nav_Tuning() ;
static void Log_Read_Nav_Tuning() ;
static void Log_Write_Control_Tuning() ;
static void Log_Read_Control_Tuning() ;
static void Log_Write_Performance() ;
static void Log_Read_Performance() ;
static void Log_Write_Cmd(byte num, struct Location *wp) ;
static void Log_Read_Cmd() ;
static void Log_Write_Attitude2() ;
static void Log_Read_Attitude2() ;
static void Log_Write_Attitude() ;
static void Log_Read_Attitude() ;
static void Log_Write_Mode(byte mode) ;
static void Log_Read_Mode() ;
static void Log_Write_Startup() ;
static void Log_Read_Startup() ;
static void Log_Read(int start_page, int end_page) ;
static void Log_Write_Startup() ;
static void Log_Read_Startup() ;
static void Log_Read(int start_page, int end_page) ;
static void Log_Write_Cmd(byte num, struct Location *wp) ;
static void Log_Write_Mode(byte mode) ;
static void start_new_log() ;
static void Log_Write_Raw() ;
static void Log_Write_GPS() ;
static void Log_Write_Current() ;
static void Log_Write_Attitude() ;
static void Log_Write_Optflow() ;
static void Log_Write_Nav_Tuning() ;
static void Log_Write_Control_Tuning() ;
static void Log_Write_Motors() ;
static void Log_Write_Performance() ;
void userhook_init() ;
void userhook_50Hz() ;
static void init_commands() ;
static void clear_command_queue();
static struct Location get_command_with_index(int i) ;
static void set_command_with_index(struct Location temp, int i) ;
static void increment_WP_index() ;
static void decrement_WP_index() ;
static long read_alt_to_hold() ;
static Location get_LOITER_home_wp() ;
static void set_next_WP(struct Location *wp) ;
static void init_home() ;
static void handle_process_must() ;
static void handle_process_may() ;
static void handle_process_now() ;
static void handle_no_commands() ;
static bool verify_must() ;
static bool verify_may() ;
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 void do_yaw() ;
static bool verify_wait_delay() ;
static bool verify_change_alt() ;
static bool verify_within_distance() ;
static bool verify_yaw() ;
static void do_change_speed() ;
static void do_target_yaw() ;
static void do_loiter_at_location() ;
static void do_jump() ;
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 index) ;
static void update_commands(void) ;
static void verify_commands(void) ;
static bool process_next_command() ;
static void process_must() ;
static void process_may() ;
static void process_now() ;
static void read_control_switch() ;
static byte readSwitch(void);
static void reset_control_switch() ;
static void read_trim_switch() ;
static void auto_trim() ;
static void trim_accel() ;
static void failsafe_on_event() ;
static void failsafe_off_event() ;
static void low_battery_event(void) ;
void piezo_on() ;
void piezo_off() ;
void piezo_beep() ;
void roll_flip() ;
static void heli_init_swash() ;
static void heli_move_servos_to_mid() ;
static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) ;
static void init_motors_out() ;
static void output_motors_armed() ;
static void output_motors_disarmed() ;
static void output_motor_test() ;
static int heli_get_scaled_throttle(int throttle) ;
static int heli_get_angle_boost(int pwm_out) ;
static void update_lights() ;
static void update_GPS_light(void) ;
static void update_motor_light(void) ;
static void dancing_light() ;
static void clear_leds() ;
static void update_motor_leds(void) ;
static void arm_motors() ;
static void set_servos_4() ;
static void init_motors_out() ;
static void output_motors_armed() ;
static void output_motors_disarmed() ;
static void output_motor_test() ;
static void init_motors_out() ;
static void output_motors_armed() ;
static void output_motors_disarmed() ;
static void output_motor_test() ;
static void init_motors_out() ;
static void output_motors_armed() ;
static void output_motors_disarmed() ;
static void output_motor_test() ;
static void init_motors_out() ;
static void output_motors_armed() ;
static void output_motors_disarmed() ;
static void debug_motors() ;
static void output_motor_test() ;
static void init_motors_out() ;
static void output_motors_armed() ;
static void output_motors_disarmed() ;
static void output_motor_test() ;
static void init_motors_out() ;
static void output_motors_armed() ;
static void output_motors_disarmed() ;
static void output_motor_test() ;
static byte navigate() ;
static bool check_missed_wp() ;
static void calc_location_error(struct Location *next_loc) ;
static void calc_loiter(int x_error, int y_error) ;
static void calc_loiter2(int x_error, int y_error) ;
static void calc_loiter_pitch_roll() ;
static void calc_nav_rate(int max_speed) ;
static void calc_nav_pitch_roll() ;
static long get_altitude_error() ;
static int get_loiter_angle() ;
static long wrap_360(long error) ;
static long wrap_180(long error) ;
static long get_crosstrack_correction(void) ;
static long cross_track_test() ;
static void reset_crosstrack() ;
static long get_distance(struct Location *loc1, struct Location *loc2) ;
static long get_alt_distance(struct Location *loc1, struct Location *loc2) ;
static long get_bearing(struct Location *loc1, struct Location *loc2) ;
static void default_dead_zones() ;
static void init_rc_in() ;
static void init_rc_out() ;
void output_min() ;
static void read_radio() ;
static void throttle_failsafe(uint16_t pwm) ;
static void trim_radio() ;
static void ReadSCP1000(void) ;
static void init_barometer(void) ;
static long read_baro_filtered(void) ;
static long read_barometer(void) ;
static void read_airspeed(void) ;
static void zero_airspeed(void) ;
static void read_battery(void) ;
static void clear_offsets() ;
static void report_batt_monitor() ;
static void report_sonar() ;
static void report_frame() ;
static void report_radio() ;
static void report_imu() ;
static void report_compass() ;
static void report_flight_modes() ;
void report_optflow() ;
static void report_heli() ;
static void report_gyro() ;
static void print_radio_values() ;
static void print_switch(byte p, byte m, bool b) ;
static void print_done() ;
static void zero_eeprom(void) ;
static void print_accel_offsets(void) ;
static void print_gyro_offsets(void) ;
static RC_Channel * heli_get_servo(int servo_num);
static int read_num_from_serial() ;
static void print_blanks(int num) ;
static void print_divider(void) ;
static void print_enabled(boolean b) ;
static void init_esc() ;
static void print_wp(struct Location *cmd, byte index) ;
static void report_gps() ;
static void report_version() ;
static void report_tuning() ;
static void run_cli(void) ;
static void init_ardupilot() ;
static void startup_ground(void) ;
static void set_mode(byte mode) ;
static void set_failsafe(boolean mode) ;
static void init_compass() ;
static void init_optflow() ;
static void init_simple_bearing() ;
static void init_throttle_cruise() ;
static boolean check_startup_for_CLI() ;
static boolean check_startup_for_CLI() ;
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) ;
static void print_hit_enter() ;
static void fake_out_gps() ;
static void print_motor_out();
#line 88 "/home/jgoppert/Projects/ardupilotone/ArduCopter/ArduCopter.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
AP_ADC_ADS7844 adc;
APM_BMP085_Class barometer;
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
#ifdef OPTFLOW_ENABLED
AP_OpticalFlow_ADNS3080 optflow;
#endif
// 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
#ifdef OPTFLOW_ENABLED
AP_OpticalFlow_ADNS3080 optflow;
#endif
static int32_t gps_base_alt;
#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);
#else
#error Unrecognised SONAR_TYPE setting.
#endif
// agmatthews USERHOOKS
////////////////////////////////////////////////////////////////////////////////
// User variables
////////////////////////////////////////////////////////////////////////////////
#ifdef USERHOOK_VARIABLES
#include USERHOOK_VARIABLES
#endif
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
static const char *comma = ",";
static const char* flight_mode_strings[] = {
"STABILIZE",
"ACRO",
"ALT_HOLD",
"AUTO",
"GUIDED",
"LOITER",
"RTL",
"CIRCLE",
"POSITION"};
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
3 Throttle
4 Rudder (if we have ailerons)
5 Mode - 3 position switch
6 User assignable
7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)
8 TBD
*/
// test
#if ACCEL_ALT_HOLD == 1
Vector3f accels_rot;
static int accels_rot_count;
static float accels_rot_sum;
static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN;
#endif
// temp
static int y_actual_speed;
static int y_rate_error;
// calc the
static int x_actual_speed;
static int x_rate_error;
// Radio
// -----
static byte control_mode = STABILIZE;
static byte old_control_mode = STABILIZE;
static byte oldSwitchPosition; // for remembering the control mode switch
static int motor_out[8];
static bool do_simple = false;
// Heli
// ----
#if FRAME_CONFIG == HELI_FRAME
static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos
static int heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly
static long heli_servo_out[4]; // used for servo averaging for analog servos
static int heli_servo_out_count = 0; // use for servo averaging
#endif
// Failsafe
// --------
static boolean failsafe; // did our throttle dip below the failsafe value?
static boolean ch3_failsafe;
static boolean motor_armed;
static boolean motor_auto_armed; // if true,
// PIDs
// ----
static Vector3f omega;
float tuning_value;
// LED output
// ----------
static boolean motor_light; // status of the Motor safety
static boolean GPS_light; // status of the GPS light
static byte led_mode = NORMAL_LEDS;
// 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 = 10; // have we achieved first lock and set Home?
static bool did_ground_start = false; // have we ground started after first arming
// Location & Navigation
// ---------------------
static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static byte wp_control; // used to control - navgation or loiter
static byte command_must_index; // current command memory location
static byte command_may_index; // current command memory location
static byte command_must_ID; // current command ID
static byte command_may_ID; // current command ID
static byte wp_verify_byte; // used for tracking state of navigating waypoints
static float cos_roll_x = 1;
static float cos_pitch_x = 1;
static float cos_yaw_x = 1;
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static long initial_simple_bearing; // used for Simple mode
static float simple_sin_y, simple_cos_x;
static byte jump = -10; // used to track loops in jump command
static int waypoint_speed_gov;
// Acro
#if CH7_OPTION == CH7_FLIP
static bool do_flip = false;
#endif
static boolean trim_flag;
static int CH7_wp_index = 0;
// Airspeed
// --------
static int airspeed; // m/s * 100
// Location Errors
// ---------------
static long altitude_error; // meters * 100 we are off in altitude
static long old_altitude;
static int old_rate;
static long yaw_error; // how off are we pointed
static long long_error, lat_error; // temp for debugging
// 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;
static bool low_batt = false;
// Barometer Sensor variables
// --------------------------
static long abs_pressure;
static long ground_pressure;
static int ground_temperature;
// Altitude Sensor variables
// ----------------------
static int sonar_alt;
static int baro_alt;
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int altitude_rate;
// flight mode specific
// --------------------
static byte yaw_mode;
static byte roll_pitch_mode;
static byte throttle_mode;
static boolean takeoff_complete; // Flag for using take-off controls
static boolean land_complete;
static long old_alt; // used for managing altitude rates
static int velocity_land;
static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
static int manual_boost; // used in adjust altitude to make changing alt faster
static int angle_boost; // used in adjust altitude to make changing alt faster
// Loiter management
// -----------------
static long original_target_bearing; // deg * 100, used to check we are not passing the WP
static long old_target_bearing; // used to track difference in angle
static int loiter_total; // deg : how many times to loiter * 360
static int loiter_sum; // deg : how far we have turned around a waypoint
static unsigned long loiter_time; // millis : when we started LOITER mode
static unsigned 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 long nav_yaw; // deg * 100 : target yaw angle
static long auto_yaw; // deg * 100 : target yaw angle
static long nav_lat; // for error calcs
static long nav_lon; // for error calcs
static int nav_throttle; // 0-1000 for throttle control
static unsigned long throttle_integrator; // used to integrate throttle output to predict battery life
static bool invalid_throttle; // used to control when we calculate nav_throttle
//static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
static long command_yaw_start; // what angle were we to begin with
static unsigned long command_yaw_start_time; // when did we start turning
static unsigned int command_yaw_time; // how long we are turning
static long command_yaw_end; // what angle are we trying to be
static long command_yaw_delta; // how many degrees will we turn
static int command_yaw_speed; // how fast to turn
static byte command_yaw_dir;
static byte command_yaw_relative;
static int auto_level_counter;
// Waypoints
// ---------
static long wp_distance; // meters - distance between plane and next waypoint
static long wp_totalDistance; // meters - distance between old and next waypoint
//static byte next_wp_index; // Current active command index
// repeating event control
// -----------------------
static byte event_id; // what to do - see defines
static unsigned long event_timer; // when the event was asked for in ms
static unsigned int event_delay; // how long to delay the next firing of event in millis
static int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
static int event_value; // per command value, such as PWM for servos
static int event_undo_value; // the value used to undo commands
//static byte repeat_forever;
//static byte undo_event; // counter for timing the undo
// delay command
// --------------
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start;
//static int condition_rate;
// land command
// ------------
static long land_start; // when we intiated command in millis()
static long original_alt; // altitide reference for start of command
// 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 target_WP; // where do we want to you towards?
static struct Location next_command; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static long target_altitude; // used for
static boolean 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;
//static float imu_health; // Metric based on accel gain deweighting
static int gps_fix_count;
static byte gps_watchdog;
// System Timers
// --------------
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
static unsigned long fiftyhz_loopTimer;
static byte slow_loopCounter;
static int superslow_loopCounter;
static byte simple_timer; // for limiting the execution of flight mode thingys
static float dTnav; // Delta Time in milliseconds for navigation computations
static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
static byte counter_one_herz;
static bool GPS_enabled = false;
static bool new_radio_frame;
AP_Relay relay;
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
void setup() {
memcheck_init();
init_ardupilot();
}
void loop()
{
long timer = micros();
// We want this to execute fast
// ----------------------------
if ((timer - fast_loopTimer) >= 4000) {
//PORTK |= B00010000;
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer;
// Execute the fast loop
// ---------------------
fast_loop();
}
//PORTK &= B11101111;
if ((timer - fiftyhz_loopTimer) >= 20000) {
fiftyhz_loopTimer = timer;
//PORTK |= B01000000;
// reads all of the necessary trig functions for cameras, throttle, etc.
update_trig();
// perform 10hz tasks
medium_loop();
// Stuff to run at full 50hz, but after the loops
fifty_hz_loop();
counter_one_herz++;
if(counter_one_herz == 50){
super_slow_loop();
counter_one_herz = 0;
}
if (millis() - perf_mon_timer > 1200 /*20000*/) {
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
gps_fix_count = 0;
perf_mon_timer = millis();
}
//PORTK &= B10111111;
}
}
// PORTK |= B01000000;
// PORTK &= B10111111;
// Main loop
static void fast_loop()
{
// try to send any deferred messages if the serial port now has
// some space available
gcs_send_message(MSG_RETRY_DEFERRED);
// Read radio
// ----------
read_radio();
// IMU DCM Algorithm
read_AHRS();
// custom code/exceptions for flight modes
// ---------------------------------------
update_yaw_mode();
update_roll_pitch_mode();
// write out the servo PWM values
// ------------------------------
set_servos_4();
//if(motor_armed)
//Log_Write_Attitude();
// agmatthews - USERHOOKS
#ifdef USERHOOK_FASTLOOP
USERHOOK_FASTLOOP
#endif
}
static void medium_loop()
{
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
// This case deals with the GPS and Compass
//-----------------------------------------
case 0:
medium_loopCounter++;
#ifdef OPTFLOW_ENABLED
if(g.optflow_enabled){
optflow.read();
optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
// write to log
if (g.log_bitmask & MASK_LOG_OPTFLOW){
Log_Write_Optflow();
}
}
#endif
if(GPS_enabled){
update_GPS();
}
//readCommands();
#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
// auto_trim, uses an auto_level algorithm
auto_trim();
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
break;
// This case performs some navigation computations
//------------------------------------------------
case 1:
medium_loopCounter++;
// Auto control modes:
if(g_gps->new_data && g_gps->fix){
// invalidate GPS data
g_gps->new_data = false;
// we are not tracking I term on navigation, so this isn't needed
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0);
// calculate the copter's desired bearing and WP distance
// ------------------------------------------------------
if(navigate()){
// control mode specific updates
// -----------------------------
update_navigation();
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
}
}else{
g_gps->new_data = false;
}
break;
// command processing
//-------------------
case 2:
medium_loopCounter++;
// Read altitude from sensors
// --------------------------
update_altitude();
// invalidate the throttle hold value
// ----------------------------------
invalid_throttle = true;
break;
// This case deals with sending high rate telemetry
//-------------------------------------------------
case 3:
medium_loopCounter++;
// perform next command
// --------------------
if(control_mode == AUTO){
update_commands();
}
#if HIL_MODE != HIL_MODE_ATTITUDE
if(motor_armed){
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
}
#endif
// send all requested output streams with rates requested
// between 5 and 45 Hz
gcs_data_stream_send(5,45);
if (g.log_bitmask & MASK_LOG_MOTORS)
Log_Write_Motors();
break;
// This case controls the slow loop
//---------------------------------
case 4:
medium_loopCounter = 0;
if (g.battery_monitoring != 0){
read_battery();
}
// Accel trims = hold > 2 seconds
// Throttle cruise = switch less than 1 second
// --------------------------------------------
read_trim_switch();
// Check for engine arming
// -----------------------
arm_motors();
slow_loop();
break;
default:
// this is just a catch all
// ------------------------
medium_loopCounter = 0;
break;
}
// agmatthews - USERHOOKS
#ifdef USERHOOK_MEDIUMLOOP
USERHOOK_MEDIUMLOOP
#endif
}
// stuff that happens at 50 hz
// ---------------------------
static void fifty_hz_loop()
{
// moved to slower loop
// --------------------
update_throttle_mode();
// Read Sonar
// ----------
if(g.sonar_enabled){
sonar_alt = sonar.read();
}
// agmatthews - USERHOOKS
#ifdef USERHOOK_50HZLOOP
USERHOOK_50HZLOOP
#endif
#if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
// HIL for a copter needs very fast update of the servo values
gcs_send_message(MSG_RADIO_OUT);
#endif
camera_stabilization();
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw();
#endif
// kick the GCS to process uplink data
gcs_update();
gcs_data_stream_send(45,1000);
#if FRAME_CONFIG == TRI_FRAME
// servo Yaw
g.rc_4.calc_pwm();
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
#endif
}
static void slow_loop()
{
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
switch (slow_loopCounter){
case 0:
slow_loopCounter++;
superslow_loopCounter++;
if(superslow_loopCounter > 1200){
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled){
compass.save_offsets();
superslow_loopCounter = 0;
}
#endif
}
break;
case 1:
slow_loopCounter++;
// Read 3-position switch on radio
// -------------------------------
read_control_switch();
// Read main battery voltage if hooked up - does not read the 5v from radio
// ------------------------------------------------------------------------
//#if BATTERY_EVENT == 1
// read_battery();
//#endif
#if AUTO_RESET_LOITER == 1
if(control_mode == LOITER){
//if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){
// reset LOITER to current position
//next_WP = current_loc;
//}
}
#endif
break;
case 2:
slow_loopCounter = 0;
update_events();
// blink if we are armed
update_lights();
// send all requested output streams with rates requested
// between 1 and 5 Hz
gcs_data_stream_send(1,5);
if(g.radio_tuning > 0)
tuning();
#if MOTOR_LEDS == 1
update_motor_leds();
#endif
break;
default:
slow_loopCounter = 0;
break;
}
// agmatthews - USERHOOKS
#ifdef USERHOOK_SLOWLOOP
USERHOOK_SLOWLOOP
#endif
}
// 1Hz loop
static void super_slow_loop()
{
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
gcs_send_message(MSG_HEARTBEAT);
// agmatthews - USERHOOKS
#ifdef USERHOOK_SUPERSLOWLOOP
USERHOOK_SUPERSLOWLOOP
#endif
}
static void update_GPS(void)
{
g_gps->update();
update_GPS_light();
//current_loc.lng = 377697000; // Lon * 10 * *7
//current_loc.lat = -1224318000; // Lat * 10 * *7
//current_loc.alt = 100; // alt * 10 * *7
//return;
if(gps_watchdog < 12){
gps_watchdog++;
}else{
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
// commented temporarily
//nav_roll >>= 1;
//nav_pitch >>= 1;
}
if (g_gps->new_data && g_gps->fix) {
gps_watchdog = 0;
// for performance
// ---------------
gps_fix_count++;
if(ground_start_count > 1){
ground_start_count--;
} 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{
init_home();
ground_start_count = 0;
}
}
current_loc.lng = g_gps->longitude; // Lon * 10 * *7
current_loc.lat = g_gps->latitude; // Lat * 10 * *7
if (g.log_bitmask & MASK_LOG_GPS){
Log_Write_GPS();
}
}
}
void update_yaw_mode(void)
{
switch(yaw_mode){
case YAW_ACRO:
g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in);
return;
break;
case YAW_HOLD:
// calcualte new nav_yaw offset
if (control_mode <= STABILIZE){
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
}else{
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1);
}
break;
case YAW_LOOK_AT_HOME:
//nav_yaw updated in update_navigation()
break;
case YAW_AUTO:
nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -20, 20);
nav_yaw = wrap_360(nav_yaw);
break;
}
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
//Serial.printf("4: %d\n",g.rc_4.servo_out);
}
void update_roll_pitch_mode(void)
{
#if CH7_OPTION == CH7_FLIP
if (do_flip){
roll_flip();
return;
}
#endif
int control_roll = 0, control_pitch = 0;
//read_radio();
if(do_simple && new_radio_frame){
new_radio_frame = false;
simple_timer++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
if (simple_timer == 1){
// roll
simple_cos_x = sin(radians(90 - delta));
}else if (simple_timer > 2){
// pitch
simple_sin_y = cos(radians(90 - delta));
simple_timer = 0;
}
// Rotate input by the initial bearing
control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
}
switch(roll_pitch_mode){
case ROLL_PITCH_ACRO:
g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
break;
case ROLL_PITCH_STABLE:
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
break;
case ROLL_PITCH_AUTO:
// mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
g.rc_1.servo_out = get_stabilize_roll(control_roll);
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
break;
}
}
// 50 hz update rate, not 250
void update_throttle_mode(void)
{
switch(throttle_mode){
case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.rc_3.control_in));
#else
angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
#endif
}else{
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
g.pi_rate_roll.reset_I();
g.pi_rate_pitch.reset_I();
g.rc_3.servo_out = 0;
}
break;
case THROTTLE_HOLD:
// allow interactive changing of atitude
adjust_altitude();
// fall through
case THROTTLE_AUTO:
// 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){
// how far off are we
altitude_error = get_altitude_error();
// get the AP throttle
nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s
//Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator());
// clear the new data flag
invalid_throttle = false;
}
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle);
#else
angle_boost = get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
#endif
break;
}
}
// called after a GPS read
static void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
switch(control_mode){
case AUTO:
verify_commands();
// note: wp_control is handled by commands_logic
// calculates desired Yaw
update_auto_yaw();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case GUIDED:
wp_control = WP_MODE;
// check if we are close to point > loiter
wp_verify_byte = 0;
verify_nav_wp();
if (wp_control == WP_MODE) {
update_auto_yaw();
} else {
set_mode(LOITER);
}
update_nav_wp();
break;
case RTL:
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// lets just jump to Loiter Mode after RTL
set_mode(LOITER);
}else{
// calculates desired Yaw
// XXX this is an experiment
#if FRAME_CONFIG == HELI_FRAME
update_auto_yaw();
#endif
wp_control = WP_MODE;
}
// calculates the desired Roll and Pitch
update_nav_wp();
break;
// switch passthrough to LOITER
case LOITER:
case POSITION:
wp_control = LOITER_MODE;
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case CIRCLE:
yaw_tracking = MAV_ROI_WPNEXT;
wp_control = CIRCLE_MODE;
// calculates desired Yaw
update_auto_yaw();
update_nav_wp();
break;
}
if(yaw_mode == YAW_LOOK_AT_HOME){
if(home_is_set){
//nav_yaw = point_at_home_yaw();
nav_yaw = get_bearing(&current_loc, &home);
} else {
nav_yaw = 0;
}
}
}
static void read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update
gcs_update();
#endif
dcm.update_DCM_fast();
omega = dcm.get_gyro();
}
static void update_trig(void){
Vector2f yawvector;
Matrix3f temp = dcm.get_dcm_matrix();
yawvector.x = temp.a.x; // sin
yawvector.y = temp.b.x; // cos
yawvector.normalize();
sin_pitch_y = -temp.c.x;
cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x));
cos_roll_x = temp.c.z / cos_pitch_x;
sin_roll_y = temp.c.y / cos_pitch_x;
cos_yaw_x = yawvector.y; // 0 x = north
sin_yaw_y = yawvector.x; // 1 y
//flat:
// 0 ° = cos_yaw: 0.00, sin_yaw: 1.00,
// 90° = cos_yaw: 1.00, sin_yaw: 0.00,
// 180 = cos_yaw: 0.00, sin_yaw: -1.00,
// 270 = cos_yaw: -1.00, sin_yaw: 0.00,
}
// updated at 10hz
static void update_altitude()
{
altitude_sensor = BARO;
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude - gps_base_alt;
return;
#else
if(g.sonar_enabled){
// filter out offset
float scale;
// read barometer
baro_alt = read_barometer();
if(baro_alt < 1000){
#if SONAR_TILT_CORRECTION == 1
// correct alt for angle of the sonar
float temp = cos_pitch_x * cos_roll_x;
temp = max(temp, 0.707);
sonar_alt = (float)sonar_alt * temp;
#endif
scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1);
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
}else{
current_loc.alt = baro_alt + home.alt;
}
}else{
baro_alt = read_barometer();
// no sonar altitude
current_loc.alt = baro_alt + home.alt;
}
// calc the accel rate limit to 2m/s
altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer
// rate limiter to reduce some of the motor pulsing
if (altitude_rate > 0){
// going up
altitude_rate = min(altitude_rate, old_rate + 20);
}else{
// going down
altitude_rate = max(altitude_rate, old_rate - 20);
}
old_rate = altitude_rate;
old_altitude = current_loc.alt;
#endif
}
static void
adjust_altitude()
{
if(g.rc_3.control_in <= 200){
next_WP.alt -= 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
//manual_boost = (g.rc_3.control_in == 0) ? -20 : 0;
}else if (g.rc_3.control_in > 700){
next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location
//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
}
}
static void tuning(){
tuning_value = (float)g.rc_6.control_in / 1000.0;
switch(g.radio_tuning){
/*case CH6_STABILIZE_KP:
g.rc_6.set_range(0,2000); // 0 to 8
tuning_value = (float)g.rc_6.control_in / 100.0;
alt_hold_gain = tuning_value;
break;*/
case CH6_STABILIZE_KP:
g.rc_6.set_range(0,8000); // 0 to 8
g.pi_stabilize_roll.kP(tuning_value);
g.pi_stabilize_pitch.kP(tuning_value);
break;
case CH6_STABILIZE_KI:
g.rc_6.set_range(0,300); // 0 to .3
tuning_value = (float)g.rc_6.control_in / 1000.0;
g.pi_stabilize_roll.kI(tuning_value);
g.pi_stabilize_pitch.kI(tuning_value);
break;
case CH6_RATE_KP:
g.rc_6.set_range(0,300); // 0 to .3
g.pi_rate_roll.kP(tuning_value);
g.pi_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_KI:
g.rc_6.set_range(0,300); // 0 to .3
g.pi_rate_roll.kI(tuning_value);
g.pi_rate_pitch.kI(tuning_value);
break;
case CH6_YAW_KP:
g.rc_6.set_range(0,1000);
g.pi_stabilize_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KP:
g.rc_6.set_range(0,1000);
g.pi_rate_yaw.kP(tuning_value);
break;
case CH6_THROTTLE_KP:
g.rc_6.set_range(0,1000);
g.pi_throttle.kP(tuning_value);
break;
case CH6_TOP_BOTTOM_RATIO:
g.rc_6.set_range(800,1000); // .8 to 1
g.top_bottom_ratio = tuning_value;
break;
case CH6_RELAY:
g.rc_6.set_range(0,1000);
if (g.rc_6.control_in > 525) relay.on();
if (g.rc_6.control_in < 475) relay.off();
break;
case CH6_TRAVERSE_SPEED:
g.rc_6.set_range(0,1000);
g.waypoint_speed_max = g.rc_6.control_in;
break;
case CH6_LOITER_P:
g.rc_6.set_range(0,1000);
g.pi_loiter_lat.kP(tuning_value);
g.pi_loiter_lon.kP(tuning_value);
break;
case CH6_NAV_P:
g.rc_6.set_range(0,6000);
g.pi_nav_lat.kP(tuning_value);
g.pi_nav_lon.kP(tuning_value);
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
g.rc_6.set_range(1000,2000);
g.heli_ext_gyro_gain = tuning_value * 1000;
break;
#endif
}
}
static void update_nav_wp()
{
if(wp_control == LOITER_MODE){
// calc a pitch to the target
calc_location_error(&next_WP);
// use error as the desired rate towards the target
calc_loiter(long_error, lat_error);
// rotate pitch and roll to the copter frame of reference
calc_loiter_pitch_roll();
}else if(wp_control == CIRCLE_MODE){
// check if we have missed the WP
int 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;
// sum the angle around the WP
loiter_sum += loiter_delta;
// create a virtual waypoint that circles the next_WP
// Count the degrees we have circulated the WP
int circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100;
target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle)));
target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle)));
// calc the lat and long error to the target
calc_location_error(&target_WP);
// use error as the desired rate towards the target
// nav_lon, nav_lat is calculated
calc_loiter(long_error, lat_error);
// rotate pitch and roll to the copter frame of reference
calc_loiter_pitch_roll();
} else {
// use error as the desired rate towards the target
calc_nav_rate(g.waypoint_speed_max);
// rotate pitch and roll to the copter frame of reference
calc_nav_pitch_roll();
}
}
static void update_auto_yaw()
{
// this tracks a location so the copter is always pointing towards it.
if(yaw_tracking == MAV_ROI_LOCATION){
auto_yaw = get_bearing(&current_loc, &target_WP);
}else if(yaw_tracking == MAV_ROI_WPNEXT){
auto_yaw = target_bearing;
}
// MAV_ROI_NONE = basic Yaw hold
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/Attitude.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static int
get_stabilize_roll(long target_angle)
{
long error;
long rate;
error = wrap_180(target_angle - dcm.roll_sensor);
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// desired Rate:
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P:
error = rate - (long)(degrees(omega.x) * 100.0);
rate = g.pi_rate_roll.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control:
return (int)constrain(rate, -2500, 2500);
}
static int
get_stabilize_pitch(long target_angle)
{
long error;
long rate;
error = wrap_180(target_angle - dcm.pitch_sensor);
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// desired Rate:
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P:
error = rate - (long)(degrees(omega.y) * 100.0);
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control:
return (int)constrain(rate, -2500, 2500);
}
#define YAW_ERROR_MAX 2000
static int
get_stabilize_yaw(long target_angle)
{
long error;
long rate;
yaw_error = wrap_180(target_angle - dcm.yaw_sensor);
// limit the error we're feeding to the PID
yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
rate = g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt);
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
if( ! g.heli_ext_gyro_enabled ) {
// Rate P:
error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
}
#else
// Rate P:
error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control:
return (int)constrain(rate, -2500, 2500);
}
#define ALT_ERROR_MAX 500
static int
get_nav_throttle(long z_error)
{
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
rate_error = rate_error - altitude_rate;
// limit the rate
rate_error = constrain(rate_error, -120, 140);
return (int)g.pi_throttle.get_pi(rate_error, .1);
}
static int
get_rate_roll(long target_rate)
{
long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0);
return g.pi_acro_roll.get_pi(error, G_Dt);
}
static int
get_rate_pitch(long target_rate)
{
long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0);
return g.pi_acro_pitch.get_pi(error, G_Dt);
}
static int
get_rate_yaw(long target_rate)
{
long error;
error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0);
target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
// output control:
return (int)constrain(target_rate, -2500, 2500);
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_hold_I(void)
{
g.pi_loiter_lat.reset_I();
g.pi_loiter_lon.reset_I();
g.pi_crosstrack.reset_I();
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_nav(void)
{
nav_throttle = 0;
invalid_throttle = true;
g.pi_nav_lat.reset_I();
g.pi_nav_lon.reset_I();
long_error = 0;
lat_error = 0;
}
/*************************************************************
throttle control
****************************************************************/
static long
get_nav_yaw_offset(int yaw_input, int reset)
{
long _yaw;
if(reset == 0){
// we are on the ground
return dcm.yaw_sensor;
}else{
// re-define nav_yaw if we have stick input
if(yaw_input != 0){
// set nav_yaw + or - the current location
_yaw = (long)yaw_input + dcm.yaw_sensor;
// we need to wrap our value so we can be 0 to 360 (*100)
return wrap_360(_yaw);
}else{
// no stick input, lets not change nav_yaw
return nav_yaw;
}
}
}
static int alt_hold_velocity()
{
#if ACCEL_ALT_HOLD == 1
Vector3f accel_filt;
float error;
// subtract filtered Accel
error = abs(next_WP.alt - current_loc.alt) - 25;
error = min(error, 50.0);
error = max(error, 0.0);
error = 1 - (error/ 50.0);
accel_filt = imu.get_accel_filtered();
accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12
//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
return constrain(output, -70, 70);
// fast rise
//s: -17.6241, g:0.0000, e:1.0000, o:0
//s: -18.4990, g:0.0000, e:1.0000, o:0
//s: -19.3193, g:0.0000, e:1.0000, o:0
//s: -13.1310, g:47.8700, e:1.0000, o:-158
#else
return 0;
#endif
}
static int get_angle_boost(int value)
{
float temp = cos_pitch_x * cos_roll_x;
temp = 1.0 - constrain(temp, .5, 1.0);
return (int)(temp * value);
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/Camera.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//#if CAMERA_STABILIZER == ENABLED
static void init_camera()
{
// ch 6 high(right) is down.
g.rc_camera_pitch.set_angle(4500);
g.rc_camera_roll.set_angle(4500);
g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW);
g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW);
}
static void
camera_stabilization()
{
// PITCH
// -----
// allow control mixing
g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor);
g.rc_camera_pitch.servo_out = (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain;
// limit
//g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500);
// dont allow control mixing
/*
g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1;
*/
// ROLL
// -----
// allow control mixing
/*
g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-dcm.roll_sensor);
*/
// dont allow control mixing
g.rc_camera_roll.servo_out = (float)-dcm.roll_sensor * g.camera_roll_gain;
// limit
//g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500);
// Output
// ------
g.rc_camera_pitch.calc_pwm();
g.rc_camera_roll.calc_pwm();
APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out);
APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out);
//Serial.printf("c:%d\n", g.rc_camera_pitch.radio_out);
}
//#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/GCS.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
GCS Protocol
4 Ardupilot Header
D
5 Payload length
1 Message ID
1 Message Version
9 Payload byte 1
8 Payload byte 2
7 Payload byte 3
A Checksum byte 1
B Checksum byte 2
*/
/*
#if GCS_PORT == 3
# define SendSerw Serial3.write
# define SendSer Serial3.print
#else
# define SendSerw Serial.write
# define SendSer Serial.print
#endif
byte mess_buffer[60];
byte buff_pointer;
// Unions for getting byte values
union f_out{
byte bytes[4];
float value;
} floatOut;
union i_out {
byte bytes[2];
int value;
} intOut;
union l_out{
byte bytes[4];
long value;
} longOut;
// Add binary values to the buffer
void write_byte(byte val)
{
mess_buffer[buff_pointer++] = val;
}
void write_int(int val )
{
intOut.value = val;
mess_buffer[buff_pointer++] = intOut.bytes[0];
mess_buffer[buff_pointer++] = intOut.bytes[1];
}
void write_float(float val)
{
floatOut.value = val;
mess_buffer[buff_pointer++] = floatOut.bytes[0];
mess_buffer[buff_pointer++] = floatOut.bytes[1];
mess_buffer[buff_pointer++] = floatOut.bytes[2];
mess_buffer[buff_pointer++] = floatOut.bytes[3];
}
void write_long(long val)
{
longOut.value = val;
mess_buffer[buff_pointer++] = longOut.bytes[0];
mess_buffer[buff_pointer++] = longOut.bytes[1];
mess_buffer[buff_pointer++] = longOut.bytes[2];
mess_buffer[buff_pointer++] = longOut.bytes[3];
}
void flush(byte id)
{
byte mess_ck_a = 0;
byte mess_ck_b = 0;
byte i;
SendSer("4D"); // This is the message preamble
SendSerw(buff_pointer); // Length
SendSerw(2); // id
for (i = 0; i < buff_pointer; i++) {
SendSerw(mess_buffer[i]);
}
buff_pointer = 0;
}
*/
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/GCS_Mavlink.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// 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)
{
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
{
mavlink_msg_attitude_send(
chan,
micros(),
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
default:
mode = control_mode + 100;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
0,
battery_voltage * 1000,
battery_remaining,
packet_drops);
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
{
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
}
static void NOINLINE send_location(mavlink_channel_t chan)
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
target_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
0,
0);
}
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
mavlink_msg_gps_raw_send(
chan,
micros(),
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
}
static void NOINLINE send_servo_out(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
#if FRAME_CONFIG == HELI_FRAME
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out,
0,
0,
0,
0,
rssi);
#else
mavlink_msg_rc_channels_scaled_send(
chan,
g.rc_1.servo_out,
g.rc_2.servo_out,
g.rc_3.radio_out,
g.rc_4.servo_out,
10000 * g.rc_1.norm_output(),
10000 * g.rc_2.norm_output(),
10000 * g.rc_3.norm_output(),
10000 * g.rc_4.norm_output(),
rssi);
#endif
}
static void NOINLINE send_radio_in(mavlink_channel_t chan)
{
const uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
g.rc_4.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
}
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
mavlink_msg_servo_output_raw_send(
chan,
motor_out[0],
motor_out[1],
motor_out[2],
motor_out[3],
motor_out[4],
motor_out[5],
motor_out[6],
motor_out[7]);
}
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
g.rc_3.servo_out/10,
current_loc.alt / 100.0,
climb_rate);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
mavlink_msg_raw_imu_send(
chan,
micros(),
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
mavlink_msg_scaled_pressure_send(
chan,
micros(),
(float)barometer.Press/100.0,
(float)(barometer.Press-ground_pressure)/100.0,
(int)(barometer.Temp*10));
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_gps_status(mavlink_channel_t chan)
{
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
}
static void NOINLINE send_statustext(mavlink_channel_t chan)
{
mavlink_msg_statustext_send(
chan,
pending_status.severity,
pending_status.text);
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
return false;
}
switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
send_heartbeat(chan);
return true;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
break;
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo(chan);
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW);
send_gps_raw(chan);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(chan);
break;
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
break;
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
break;
#endif // HIL_MODE != HIL_MODE_ATTITUDE
case MSG_GPS_STATUS:
CHECK_PAYLOAD_SIZE(GPS_STATUS);
send_gps_status(chan);
break;
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
send_current_waypoint(chan);
break;
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_param_send();
} else {
gcs3.queued_param_send();
}
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST);
if (chan == MAVLINK_COMM_0) {
gcs0.queued_waypoint_send();
} else {
gcs3.queued_waypoint_send();
}
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
break;
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
}
return true;
}
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
static struct mavlink_queue {
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
{
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// don't send status MAVLink messages for 2 seconds after
// bootup, to try to prevent Xbee bricking
return;
}
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
pending_status.severity = (uint8_t)severity;
strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
} else {
// send immediately
mavlink_msg_statustext_send(
chan,
severity,
(const int8_t*) str);
}
}
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
packet_drops(0),
// 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 == false) {
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 = true;
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.waypoint_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);
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_GPS_STATUS);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT);
//Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get());
}
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
// sent with GPS read
#if HIL_MODE == HIL_MODE_ATTITUDE
send_message(MSG_LOCATION);
#endif
//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
}
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
send_message(MSG_SERVO_OUT);
//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
}
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
}
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
}
}
}
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.
streamRateExtra3 = freq; // Don't save!!
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);
streamRateExtendedStatus = freq;
break;
case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels = freq;
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController = freq;
break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion.set_and_save(freq);
// break;
case MAV_DATA_STREAM_POSITION:
streamRatePosition = freq;
break;
case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1 = freq;
break;
case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2 = freq;
break;
case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3 = freq;
break;
default:
break;
}
break;
}
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;
if (in_mavlink_delay) {
// don't execute action commands while in sensor
// initialisation
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(STABILIZE);
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_PRESSURE:
break;
case MAV_ACTION_CALIBRATE_ACC:
imu.init_accel(mavlink_delay);
result=1;
break;
//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;
}
case MAVLINK_MSG_ID_SET_MODE:
{
// decode
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg, &packet);
switch(packet.mode){
case MAV_MODE_MANUAL:
set_mode(STABILIZE);
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 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;
}
*/
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
//send_text_P(SEVERITY_LOW,PSTR("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.waypoint_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:
{
//send_text_P(SEVERITY_LOW,PSTR("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_command_with_index(packet.seq);
// set frame of waypoint
uint8_t frame;
if (tell_command.options & WP_OPTION_ALT_RELATIVE) {
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.waypoint_index)
current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false)
float x = 0, y = 0, z = 0;
if (tell_command.id < MAV_CMD_NAV_LAST) {
// command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude)
// ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM
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_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_TAKEOFF:
param1 = 0;
break;
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1; // ACM loiter time is in 1 second increments
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
param1 = tell_command.p1;
break;
case MAV_CMD_DO_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_NAV_WAYPOINT:
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:
{
//send_text_P(SEVERITY_LOW,PSTR("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:
{
//send_text_P(SEVERITY_LOW,PSTR("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:
{
//send_text_P(SEVERITY_LOW,PSTR("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 waypoints
uint8_t type = 0; // ok (0), error(1)
g.waypoint_total.set_and_save(0);
// send acknowledgement 3 times to makes sure it is received
for (int i=0;i<3;i++)
mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
//send_text_P(SEVERITY_LOW,PSTR("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.waypoint_index);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
//send_text_P(SEVERITY_LOW,PSTR("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.waypoint_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;
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;
}
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
//case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
default:
{
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;
}
}
*/
// we only are supporting Abs position, relative Alt
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2;
tell_command.options = 1; // store altitude relative!! Always!!
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_ROI:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_NAV_TAKEOFF:
tell_command.p1 = 0;
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.p1 = packet.param1 * 100;
break;
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1; // 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_NAV_WAYPOINT:
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;
}
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 & WP_OPTION_ALT_RELATIVE){
guided_WP.alt += home.alt;
}
set_mode(GUIDED);
// make any new wp uploaded instant (in case we are already in Guided mode)
set_next_WP(&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) break;
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) break;
set_command_with_index(tell_command, packet.seq);
// update waypoint receiving state machine
waypoint_timelast_receive = millis();
waypoint_request_i++;
if (waypoint_request_i > (uint16_t)g.waypoint_total){
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
type);
send_text(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
}
}
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
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...
}
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;
APM_RC.setHIL(v);
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode.
case MAVLINK_MSG_ID_GPS_RAW:
{
// 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);
if (gps_base_alt == 0) {
gps_base_alt = packet.alt*100;
}
current_loc.lng = packet.lon * T7;
current_loc.lat = packet.lat * T7;
current_loc.alt = g_gps->altitude - gps_base_alt;
if (!home_is_set) {
init_home();
}
break;
}
#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
#endif
/*
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.
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;
}
// 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:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
// Serial.printf_P(PSTR("gyro:\t%d\t%d\t%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);
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));
mavlink_msg_param_value_send(
chan,
(int8_t*)param_name,
value,
_queued_parameter_count,
_queued_parameter_index);
_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.waypoint_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/ArduCopter/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 bool print_log_menu(void);
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>"
" erase (all logs)\n"
" enable <name> | all\n"
" disable <name> | all\n"
"\n"));
return 0;
}
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
const struct Menu::command log_menu_commands[] PROGMEM = {
{"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{
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Serial.printf_P(PSTR(" ATTITUDE_FAST"));
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Serial.printf_P(PSTR(" ATTITUDE_MED"));
if (g.log_bitmask & MASK_LOG_GPS) Serial.printf_P(PSTR(" GPS"));
if (g.log_bitmask & MASK_LOG_PM) Serial.printf_P(PSTR(" PM"));
if (g.log_bitmask & MASK_LOG_CTUN) Serial.printf_P(PSTR(" CTUN"));
if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN"));
if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW"));
if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD"));
if (g.log_bitmask & MASK_LOG_CUR) Serial.printf_P(PSTR(" CURRENT"));
if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS"));
if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW"));
}
Serial.println();
if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n"));
}else{
Serial.printf_P(PSTR("\n%d logs\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("last_num %d "), last_log_num);
Serial.printf_P(PSTR("Log # %d, start %d, end %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;
// check that the requested log number can be read
dump_log = argv[1].i;
if (/*(argc != 2) || */ (dump_log < 1)) {
Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
Log_Read(0, 4095);
erase_logs(NULL, NULL);
return(-1);
}
get_log_boundaries(dump_log, dump_log_start, dump_log_end);
/*Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"),
dump_log,
dump_log_start,
dump_log_end);
*/
Log_Read(dump_log_start, dump_log_end);
//Serial.printf_P(PSTR("Done\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.\n"), i);
// delay(1000);
//}
// lay down a bunch of "log end" messages.
Serial.printf_P(PSTR("\nErasing log...\n"));
for(int j = 1; j < 4096; j++)
DataFlash.PageErase(j);
clear_header();
Serial.printf_P(PSTR("\nLog erased.\n"));
return (0);
}
static void clear_header()
{
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();
}
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);
TARG(MOTORS);
TARG(OPTFLOW);
#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;
}
// finds out how many logs are available
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();
//Serial.printf("num_logs, %d\n", num_logs);
return num_logs;
}else{
//Serial.printf("* %d\n", data);
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 = get_num_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 /*50*/)) {
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("Logs full"));
}
}
// All log data is stored in page 1?
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();
//Serial.printf("look page:%d, check:%d\n", look_page, check);
if(check == (long)0xFFFFFFFF)
top_page = look_page;
else
bottom_page = look_page;
}
return top_page;
}
// Write an GPS packet. Total length : 30 bytes
static void Log_Write_GPS()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(g_gps->time); // 1
DataFlash.WriteByte(g_gps->num_sats); // 2
DataFlash.WriteLong(current_loc.lat); // 3
DataFlash.WriteLong(current_loc.lng); // 4
DataFlash.WriteLong(current_loc.alt); // 5
DataFlash.WriteLong(g_gps->altitude); // 6
DataFlash.WriteInt(g_gps->ground_speed); // 7
DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8
DataFlash.WriteByte(END_BYTE);
}
// Read a GPS packet
static void Log_Read_GPS()
{
Serial.printf_P(PSTR("GPS, %ld, %d, "
"%4.7f, %4.7f, %4.4f, %4.4f, "
"%d, %u\n"),
DataFlash.ReadLong(), // 1 time
(int)DataFlash.ReadByte(), // 2 sats
(float)DataFlash.ReadLong() / t7, // 3 lat
(float)DataFlash.ReadLong() / t7, // 4 lon
(float)DataFlash.ReadLong() / 100.0, // 5 gps alt
(float)DataFlash.ReadLong() / 100.0, // 6 sensor alt
DataFlash.ReadInt(), // 7 ground speed
(uint16_t)DataFlash.ReadInt()); // 8 ground course
}
// 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();
//Vector3f accel_filt = imu.get_accel_filtered();
gyro *= t7; // Scale up for storage as long integers
accel *= t7;
//accel_filt *= 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)(accels_rot.x * t7));
//DataFlash.WriteLong((long)(accels_rot.y * t7));
//DataFlash.WriteLong((long)(accels_rot.z * t7));
DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z);
DataFlash.WriteByte(END_BYTE);
}
#endif
// 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(" ");
}
static void Log_Write_Current()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.rc_3.control_in);
DataFlash.WriteLong(throttle_integrator);
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, %ld, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
DataFlash.ReadLong(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
DataFlash.ReadInt());
}
static void Log_Write_Motors()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_MOTORS_MSG);
#if FRAME_CONFIG == TRI_FRAME
DataFlash.WriteInt(motor_out[CH_1]);//1
DataFlash.WriteInt(motor_out[CH_2]);//2
DataFlash.WriteInt(motor_out[CH_4]);//3
DataFlash.WriteInt(g.rc_4.radio_out);//4
#elif FRAME_CONFIG == HEXA_FRAME
DataFlash.WriteInt(motor_out[CH_1]);//1
DataFlash.WriteInt(motor_out[CH_2]);//2
DataFlash.WriteInt(motor_out[CH_3]);//3
DataFlash.WriteInt(motor_out[CH_4]);//4
DataFlash.WriteInt(motor_out[CH_7]);//5
DataFlash.WriteInt(motor_out[CH_8]);//6
#elif FRAME_CONFIG == Y6_FRAME
//left
DataFlash.WriteInt(motor_out[CH_2]);//1
DataFlash.WriteInt(motor_out[CH_3]);//2
//right
DataFlash.WriteInt(motor_out[CH_7]);//3
DataFlash.WriteInt(motor_out[CH_1]);//4
//back
DataFlash.WriteInt(motor_out[CH_8]);//5
DataFlash.WriteInt(motor_out[CH_4]);//6
#elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME
DataFlash.WriteInt(motor_out[CH_1]);//1
DataFlash.WriteInt(motor_out[CH_2]);//2
DataFlash.WriteInt(motor_out[CH_3]);//3
DataFlash.WriteInt(motor_out[CH_4]);//4
DataFlash.WriteInt(motor_out[CH_7]);//5
DataFlash.WriteInt(motor_out[CH_8]); //6
DataFlash.WriteInt(motor_out[CH_10]);//7
DataFlash.WriteInt(motor_out[CH_11]);//8
#elif FRAME_CONFIG == HELI_FRAME
DataFlash.WriteInt(heli_servo_out[0]);//1
DataFlash.WriteInt(heli_servo_out[1]);//2
DataFlash.WriteInt(heli_servo_out[2]);//3
DataFlash.WriteInt(heli_servo_out[3]);//4
DataFlash.WriteInt(g.heli_ext_gyro_gain);//5
#else // quads
DataFlash.WriteInt(motor_out[CH_1]);//1
DataFlash.WriteInt(motor_out[CH_2]);//2
DataFlash.WriteInt(motor_out[CH_3]);//3
DataFlash.WriteInt(motor_out[CH_4]);//4
#endif
DataFlash.WriteByte(END_BYTE);
}
// Read a Current packet
static void Log_Read_Motors()
{
#if FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME
// 1 2 3 4 5 6
Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), //1
DataFlash.ReadInt(), //2
DataFlash.ReadInt(), //3
DataFlash.ReadInt(), //4
DataFlash.ReadInt(), //5
DataFlash.ReadInt()); //6
#elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME
// 1 2 3 4 5 6 7 8
Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), //1
DataFlash.ReadInt(), //2
DataFlash.ReadInt(), //3
DataFlash.ReadInt(), //4
DataFlash.ReadInt(), //5
DataFlash.ReadInt(), //6
DataFlash.ReadInt(), //7
DataFlash.ReadInt()); //8
#elif FRAME_CONFIG == HELI_FRAME
// 1 2 3 4 5
Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), //1
DataFlash.ReadInt(), //2
DataFlash.ReadInt(), //3
DataFlash.ReadInt(), //4
DataFlash.ReadInt()); //5
#else // quads, TRIs
// 1 2 3 4
Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"),
DataFlash.ReadInt(), //1
DataFlash.ReadInt(), //2
DataFlash.ReadInt(), //3
DataFlash.ReadInt()); //4;
#endif
}
#ifdef OPTFLOW_ENABLED
// Write an optical flow packet. Total length : 18 bytes
static void Log_Write_Optflow()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_OPTFLOW_MSG);
DataFlash.WriteInt((int)optflow.dx);
DataFlash.WriteInt((int)optflow.dy);
DataFlash.WriteInt((int)optflow.surface_quality);
DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat);
DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng);
DataFlash.WriteByte(END_BYTE);
}
#endif
static void Log_Read_Optflow()
{
Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(float)DataFlash.ReadLong(),// / t7,
(float)DataFlash.ReadLong() // / t7
);
}
static void Log_Write_Nav_Tuning()
{
//Matrix3f tempmat = dcm.get_dcm_matrix();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((int)wp_distance); // 1
DataFlash.WriteInt((int)(target_bearing/100)); // 2
DataFlash.WriteInt((int)long_error); // 3
DataFlash.WriteInt((int)lat_error); // 4
DataFlash.WriteInt((int)nav_lon); // 5
DataFlash.WriteInt((int)nav_lat); // 6
DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7
DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8
DataFlash.WriteInt((int)g.pi_loiter_lon.get_integrator()); // 9
DataFlash.WriteInt((int)g.pi_loiter_lat.get_integrator()); // 10
DataFlash.WriteByte(END_BYTE);
}
static void Log_Read_Nav_Tuning()
{
// 1 2 3 4 5 6 7 8 9 10
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), // 1
DataFlash.ReadInt(), // 2
DataFlash.ReadInt(), // 3
DataFlash.ReadInt(), // 4
DataFlash.ReadInt(), // 5
DataFlash.ReadInt(), // 6
DataFlash.ReadInt(), // 7
DataFlash.ReadInt(), // 8
DataFlash.ReadInt(), // 9
DataFlash.ReadInt()); // 10
}
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
// yaw
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
DataFlash.WriteInt((int)(nav_yaw/100)); //2
DataFlash.WriteInt((int)yaw_error/100); //3
// Alt hold
DataFlash.WriteInt(sonar_alt); //4
DataFlash.WriteInt(baro_alt); //5
DataFlash.WriteInt((int)next_WP.alt); //6
DataFlash.WriteInt(nav_throttle); //7
DataFlash.WriteInt(angle_boost); //8
DataFlash.WriteInt(manual_boost); //9
DataFlash.WriteInt(g.rc_3.servo_out); //10
DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //11
DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //12
DataFlash.WriteByte(END_BYTE);
}
#endif
// Read an control tuning packet
static void Log_Read_Control_Tuning()
{
// 1 2 3 4 5 6 7 8 9 10 11 12
Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
// Control
//DataFlash.ReadByte(),
//DataFlash.ReadInt(),
// yaw
DataFlash.ReadInt(), //1
DataFlash.ReadInt(), //2
DataFlash.ReadInt(), //3
// Alt Hold
DataFlash.ReadInt(), //4
DataFlash.ReadInt(), //5
DataFlash.ReadInt(), //6
DataFlash.ReadInt(), //7
DataFlash.ReadInt(), //8
DataFlash.ReadInt(), //9
DataFlash.ReadInt(), //10
DataFlash.ReadInt(), //11
DataFlash.ReadInt()); //12
}
// 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.WriteByte( delta_ms_fast_loop);
//DataFlash.WriteByte( loop_step);
//*
//DataFlash.WriteLong( millis()- perf_mon_timer);
//DataFlash.WriteByte( dcm.gyro_sat_count); //2
//DataFlash.WriteByte( imu.adc_constraints); //3
//DataFlash.WriteByte( dcm.renorm_sqrt_count); //4
//DataFlash.WriteByte( dcm.renorm_blowup_count); //5
//DataFlash.WriteByte( gps_fix_count); //6
//DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7
// control_mode
DataFlash.WriteByte(control_mode); //1
DataFlash.WriteByte(yaw_mode); //2
DataFlash.WriteByte(roll_pitch_mode); //3
DataFlash.WriteByte(throttle_mode); //4
DataFlash.WriteInt(g.throttle_cruise.get()); //5
DataFlash.WriteLong(throttle_integrator); //6
DataFlash.WriteByte(END_BYTE);
}
// Read a performance packet
static void Log_Read_Performance()
{ //1 2 3 4 5 6
Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d, %ld\n"),
// Control
//DataFlash.ReadLong(),
//DataFlash.ReadInt(),
DataFlash.ReadByte(), //1
DataFlash.ReadByte(), //2
DataFlash.ReadByte(), //3
DataFlash.ReadByte(), //4
DataFlash.ReadInt(), //5
DataFlash.ReadLong()); //6
}
// Write a command processing packet.
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(g.waypoint_total);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->options);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
}
//CMD, 3, 0, 16, 8, 1, 800, 340440192, -1180692736
// Read a command processing packet
static void Log_Read_Cmd()
{
Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
// WP total
DataFlash.ReadByte(),
// num, id, p1, options
DataFlash.ReadByte(),
DataFlash.ReadByte(),
DataFlash.ReadByte(),
DataFlash.ReadByte(),
// Alt, lat long
DataFlash.ReadLong(),
DataFlash.ReadLong(),
DataFlash.ReadLong());
}
/*
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude2()
{
Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteLong((long)(degrees(omega.x) * 100.0));
DataFlash.WriteLong((long)(degrees(omega.y) * 100.0));
DataFlash.WriteLong((long)(accel.x * 100000));
DataFlash.WriteLong((long)(accel.y * 100000));
//DataFlash.WriteLong((long)(accel.z * 100000));
DataFlash.WriteByte(END_BYTE);
}*/
/*
// Read an attitude packet
static void Log_Read_Attitude2()
{
Serial.printf_P(PSTR("ATT, %d, %d, %ld, %ld, %1.4f, %1.4f\n"),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadLong(),
DataFlash.ReadLong(),
(float)DataFlash.ReadLong()/100000.0,
(float)DataFlash.ReadLong()/100000.0 );
}
*/
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)g.rc_1.servo_out);
DataFlash.WriteInt((int)g.rc_2.servo_out);
DataFlash.WriteInt((int)g.rc_4.servo_out);
DataFlash.WriteByte(END_BYTE);
}
// Read an attitude packet
static void Log_Read_Attitude()
{
Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(uint16_t)DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt());
}
// 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.WriteInt(g.throttle_cruise);
DataFlash.WriteByte(END_BYTE);
}
// Read a mode packet
static void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD:"));
Serial.print(flight_mode_strings[DataFlash.ReadByte()]);
Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt());
}
static void Log_Write_Startup()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(END_BYTE);
}
// Read a mode packet
static void Log_Read_Startup()
{
Serial.printf_P(PSTR("START UP\n"));
}
// Read the DataFlash log memory : Packet Parser
static void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step = 0;
int page = start_page;
DataFlash.StartRead(start_page);
while (page < end_page && page != -1){
data = DataFlash.ReadByte();
// This is a state machine to read the packets
switch(log_step){
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;
Serial.println(".");
}
break;
case 2:
log_step = 0;
switch(data){
case LOG_ATTITUDE_MSG:
Log_Read_Attitude();
break;
case LOG_MODE_MSG:
Log_Read_Mode();
break;
case LOG_CONTROL_TUNING_MSG:
Log_Read_Control_Tuning();
break;
case LOG_NAV_TUNING_MSG:
Log_Read_Nav_Tuning();
break;
case LOG_PERFORMANCE_MSG:
Log_Read_Performance();
break;
case LOG_RAW_MSG:
Log_Read_Raw();
break;
case LOG_CMD_MSG:
Log_Read_Cmd();
break;
case LOG_CURRENT_MSG:
Log_Read_Current();
break;
case LOG_STARTUP_MSG:
Log_Read_Startup();
break;
case LOG_MOTORS_MSG:
Log_Read_Motors();
break;
case LOG_OPTFLOW_MSG:
Log_Read_Optflow();
break;
case LOG_GPS_MSG:
Log_Read_GPS();
break;
}
break;
}
page = DataFlash.GetPage();
}
}
#else // LOGGING_ENABLED
static void Log_Write_Startup() {}
static void Log_Read_Startup() {}
static void Log_Read(int start_page, int end_page) {}
static void Log_Write_Cmd(byte num, struct Location *wp) {}
static void Log_Write_Mode(byte mode) {}
static void start_new_log() {}
static void Log_Write_Raw() {}
static void Log_Write_GPS() {}
static void Log_Write_Current() {}
static void Log_Write_Attitude() {}
#ifdef OPTFLOW_ENABLED
static void Log_Write_Optflow() {}
#endif
static void Log_Write_Nav_Tuning() {}
static void Log_Write_Control_Tuning() {}
static void Log_Write_Motors() {}
static void Log_Write_Performance() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
#endif // LOGGING_ENABLED
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/UserCode.pde"
// agmatthews USERHOOKS
void userhook_init()
{
// put your initialisation code here
}
void userhook_50Hz()
{
// put your 50Hz code here
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void init_commands()
{
// zero is home, but we always load the next command (1), in the code.
g.waypoint_index = 0;
// This are registers for the current may and must commands
// setting to zero will allow them to be written to by new commands
command_must_index = NO_COMMAND;
command_may_index = NO_COMMAND;
// clear the command queue
clear_command_queue();
}
// forces the loading of a new command
// queue is emptied after a new command is processed
static void clear_command_queue(){
next_command.id = NO_COMMAND;
}
// Getters
// -------
static struct Location get_command_with_index(int i)
{
struct Location temp;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.waypoint_total) {
// we do not have a valid command to load
// return a WP with a "Blank" id
temp.id = CMD_BLANK;
// no reason to carry on
return temp;
}else{
// we can load a command, we don't process it yet
// read WP position
long 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); // alt is stored in CM! Alt is stored relative!
mem += 4;
temp.lat = (long)eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
}
// 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 & WP_OPTION_ALT_RELATIVE){
//temp.alt += home.alt;
//}
if(temp.options & WP_OPTION_RELATIVE){
// If were relative, just offset from home
temp.lat += home.lat;
temp.lng += home.lng;
}
return temp;
}
// Setters
// -------
static void set_command_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
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); // Alt is stored in CM!
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7
}
static void increment_WP_index()
{
if (g.waypoint_index < g.waypoint_total) {
g.waypoint_index++;
}
SendDebugln(g.waypoint_index,DEC);
}
/*
static void decrement_WP_index()
{
if (g.waypoint_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1);
}
}*/
static long read_alt_to_hold()
{
if(g.RTL_altitude < 0)
return current_loc.alt;
else
return g.RTL_altitude;// + home.alt;
}
//********************************************************************************
// This function sets the waypoint and modes for Return to Launch
// It's not currently used
//********************************************************************************
static Location get_LOITER_home_wp()
{
//so we know where we are navigating from
next_WP = current_loc;
// read home position
struct Location temp = get_command_with_index(0); // 0 = home
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold();
return temp;
}
/*
This function sets the next waypoint command
It precalculates all the necessary stuff.
*/
static void set_next_WP(struct Location *wp)
{
//SendDebug("MSG <set_next_wp> wp_index: ");
//SendDebugln(g.waypoint_index, DEC);
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP = next_WP;
// Load the next_WP slot
// ---------------------
next_WP = *wp;
// used to control and limit the rate of climb - not used right now!
// -----------------------------------------------------------------
target_altitude = current_loc.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
// ---------------------------------
original_target_bearing = target_bearing;
// reset speed governer
// --------------------
waypoint_speed_gov = 0;
}
// run this at setup on the ground
// -------------------------------
static void 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); // we sometimes get negatives from GPS, not valid
home.alt = 0; // Home is always 0
home_is_set = true;
// to point yaw towards home until we set it with Mavlink
target_WP = home;
// Save Home to EEPROM
// -------------------
// no need to save this to EPROM
set_command_with_index(home, 0);
print_wp(&home, 0);
// Save prev loc this makes the calcs look better before commands are loaded
prev_WP = home;
// this is dangerous since we can get GPS lock at any time.
//next_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands_logic.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static void handle_process_must()
{
switch(next_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: // 19
do_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
default:
break;
}
}
static void handle_process_may()
{
switch(next_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_CONDITION_YAW:
do_yaw();
break;
default:
break;
}
}
static void handle_process_now()
{
switch(next_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;
case MAV_CMD_DO_SET_ROI:
do_target_yaw();
}
}
static void handle_no_commands()
{
/*
switch (control_mode){
default:
set_mode(RTL);
break;
}*/
//return;
//Serial.println("Handle No CMDs");
}
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
static bool verify_must()
{
//Serial.printf("vmust: %d\n", command_must_ID);
switch(command_must_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 false;
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_must: default> No current Must commands"));
return false;
break;
}
}
static bool verify_may()
{
switch(command_may_ID) {
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 MAV_CMD_CONDITION_YAW:
return verify_yaw();
break;
default:
//gcs_send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current May commands"));
return false;
break;
}
}
/********************************************************************************/
//
/********************************************************************************/
static void do_RTL(void)
{
// TODO: Altitude option from mission planner
Location temp = home;
temp.alt = read_alt_to_hold();
//so we know where we are navigating from
// --------------------------------------
next_WP = current_loc;
// Loads WP from Memory
// --------------------
set_next_WP(&temp);
// output control mode to the ground station
// -----------------------------------------
gcs_send_message(MSG_HEARTBEAT);
}
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
static void do_takeoff()
{
wp_control = LOITER_MODE;
// Start with current location
Location temp = current_loc;
// next_command.alt is a relative altitude!!!
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
temp.alt = next_command.alt + home.alt;
//Serial.printf("rel alt: %ld",temp.alt);
} else {
temp.alt = next_command.alt;
//Serial.printf("abs alt: %ld",temp.alt);
}
takeoff_complete = false;
// set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
// Set our waypoint
set_next_WP(&temp);
}
static void do_nav_wp()
{
wp_control = WP_MODE;
// next_command.alt is a relative altitude!!!
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
next_command.alt += home.alt;
}
set_next_WP(&next_command);
// this is our bitmask to verify we have met all conditions to move on
wp_verify_byte = 0;
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds and expanded to millis
loiter_time_max = next_command.p1 * 1000;
// if we don't require an altitude minimum, we save this flag as passed (1)
if((next_WP.options & WP_OPTION_ALT_REQUIRED) == 0){
// we don't need to worry about it
wp_verify_byte |= NAV_ALTITUDE;
}
}
static void do_land()
{
wp_control = LOITER_MODE;
//Serial.println("dlnd ");
// not really used right now, might be good for debugging
land_complete = false;
// A value that drives to 0 when the altitude doesn't change
velocity_land = 2000;
// used to limit decent rate
land_start = millis();
// used to limit decent rate
original_alt = current_loc.alt;
// hold at our current location
set_next_WP(&current_loc);
}
static void do_loiter_unlimited()
{
wp_control = LOITER_MODE;
//Serial.println("dloi ");
if(next_command.lat == 0)
set_next_WP(&current_loc);
else
set_next_WP(&next_command);
}
static void do_loiter_turns()
{
wp_control = CIRCLE_MODE;
if(next_command.lat == 0)
set_next_WP(&current_loc);
else
set_next_WP(&next_command);
loiter_total = next_command.p1 * 360;
loiter_sum = 0;
}
static void do_loiter_time()
{
if(next_command.lat == 0){
wp_control = LOITER_MODE;
loiter_time = millis();
set_next_WP(&current_loc);
}else{
wp_control = WP_MODE;
set_next_WP(&next_command);
}
loiter_time_max = next_command.p1 * 1000; // units are (seconds)
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
static bool verify_takeoff()
{
// wait until we are ready!
if(g.rc_3.control_in == 0){
return false;
}
if (current_loc.alt > next_WP.alt){
//Serial.println("Y");
takeoff_complete = true;
return true;
}else{
//Serial.println("N");
return false;
}
}
static bool verify_land()
{
// land at 1 meter per second
next_WP.alt = original_alt - ((millis() - land_start) / 20); // condition_value = our initial
velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
old_alt = current_loc.alt;
if(g.sonar_enabled){
// decide which sensor we're using
if(sonar_alt < 300){
next_WP = current_loc; // don't pitch or roll
next_WP.alt = -200; // force us down
}
if(sonar_alt < 40){
land_complete = true;
//Serial.println("Y");
//return true;
}
}
if(velocity_land <= 0){
land_complete = true;
//return true;
}
//Serial.printf("N, %d\n", velocity_land);
//Serial.printf("N_alt, %ld\n", next_WP.alt);
return false;
}
static bool verify_nav_wp()
{
// Altitude checking
if(next_WP.options & WP_OPTION_ALT_REQUIRED){
// we desire a certain minimum altitude
if (current_loc.alt > next_WP.alt){
// we have reached that altitude
wp_verify_byte |= NAV_ALTITUDE;
}
}
// Did we pass the WP? // Distance checking
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if we have a distance calc error, wp_distance may be less than 0
if(wp_distance > 0){
wp_verify_byte |= NAV_LOCATION;
if(loiter_time == 0){
loiter_time = millis();
}
}
}
// Hold at Waypoint checking, we cant move on until this is OK
if(wp_verify_byte & NAV_LOCATION){
// we have reached our goal
// loiter at the WP
wp_control = LOITER_MODE;
if ((millis() - loiter_time) > loiter_time_max) {
wp_verify_byte |= NAV_DELAY;
//gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//Serial.println("vlt done");
}
}
if(wp_verify_byte >= 7){
//if(wp_verify_byte & NAV_LOCATION){
char message[30];
sprintf(message,"Reached Command #%i",command_must_index);
gcs_send_text(SEVERITY_LOW,message);
wp_verify_byte = 0;
return true;
}else{
return false;
}
}
static bool verify_loiter_unlim()
{
return false;
}
static bool verify_loiter_time()
{
if(wp_control == LOITER_MODE){
if ((millis() - loiter_time) > loiter_time_max) {
return true;
}
}
if(wp_control == WP_MODE && wp_distance <= g.waypoint_radius){
// reset our loiter time
loiter_time = millis();
// switch to position hold
wp_control = LOITER_MODE;
}
return false;
}
static bool verify_loiter_turns()
{
// have we rotated around the center enough times?
// -----------------------------------------------
if(abs(loiter_sum) > loiter_total) {
loiter_total = 0;
loiter_sum = 0;
//gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: 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()
{
//Serial.print("dwd ");
condition_start = millis();
condition_value = next_command.lat * 1000; // convert to milliseconds
//Serial.println(condition_value,DEC);
}
static void do_change_alt()
{
Location temp = next_WP;
condition_start = current_loc.alt;
condition_value = next_command.alt;
temp.alt = next_command.alt;
set_next_WP(&temp);
}
static void do_within_distance()
{
condition_value = next_command.lat;
}
static void do_yaw()
{
//Serial.println("dyaw ");
yaw_tracking = MAV_ROI_NONE;
// target angle in degrees
command_yaw_start = nav_yaw; // current position
command_yaw_start_time = millis();
command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise
command_yaw_speed = next_command.lat * 100; // ms * 100
command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute
// if unspecified go 30° a second
if(command_yaw_speed == 0)
command_yaw_speed = 3000;
// if unspecified go counterclockwise
if(command_yaw_dir == 0)
command_yaw_dir = -1;
else
command_yaw_dir = 1;
if (command_yaw_relative == 1){
// relative
command_yaw_delta = next_command.alt * 100;
}else{
// absolute
command_yaw_end = next_command.alt * 100;
// calculate the delta travel in deg * 100
if(command_yaw_dir == 1){
if(command_yaw_start >= command_yaw_end){
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
}else{
command_yaw_delta = command_yaw_end - command_yaw_start;
}
}else{
if(command_yaw_start > command_yaw_end){
command_yaw_delta = command_yaw_start - command_yaw_end;
}else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
}
}
command_yaw_delta = wrap_360(command_yaw_delta);
}
// rate to turn deg per second - default is ten
command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000;
}
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
static bool verify_wait_delay()
{
//Serial.print("vwd");
if ((unsigned)(millis() - condition_start) > condition_value){
//Serial.println("y");
condition_value = 0;
return true;
}
//Serial.println("n");
return false;
}
static bool verify_change_alt()
{
if (condition_start < next_WP.alt){
// we are going higer
if(current_loc.alt > next_WP.alt){
condition_value = 0;
return true;
}
}else{
// we are going lower
if(current_loc.alt < next_WP.alt){
condition_value = 0;
return true;
}
}
return false;
}
static bool verify_within_distance()
{
if (wp_distance < condition_value){
condition_value = 0;
return true;
}
return false;
}
static bool verify_yaw()
{
//Serial.print("vyaw ");
if((millis() - command_yaw_start_time) > command_yaw_time){
// time out
// make sure we hold at the final desired yaw angle
nav_yaw = command_yaw_end;
auto_yaw = nav_yaw;
//Serial.println("Y");
return true;
}else{
// else we need to be at a certain place
// power is a ratio of the time : .5 = half done
float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
nav_yaw = wrap_360(nav_yaw);
auto_yaw = nav_yaw;
//Serial.printf("ny %ld\n",nav_yaw);
return false;
}
}
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
static void do_change_speed()
{
g.waypoint_speed_max = next_command.p1 * 100;
}
static void do_target_yaw()
{
yaw_tracking = next_command.p1;
if(yaw_tracking == MAV_ROI_LOCATION){
target_WP = next_command;
}
}
static void do_loiter_at_location()
{
next_WP = current_loc;
}
static void do_jump()
{
if(jump == -10){
jump = next_command.lat;
}
if(jump > 0) {
jump--;
command_must_index = 0;
command_may_index = 0;
// set pointer to desired index
g.waypoint_index = next_command.p1 - 1;
} else if (jump == 0){
// we're done, move along
jump = -10;
} else if (jump == -1) {
// repeat forever
g.waypoint_index = next_command.p1 - 1;
}
}
static void do_set_home()
{
if(next_command.p1 == 1) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_command.lng; // Lon * 10**7
home.lat = next_command.lat; // Lat * 10**7
home.alt = max(next_command.alt, 0);
home_is_set = true;
}
}
static void do_set_servo()
{
APM_RC.OutputCh(next_command.p1 - 1, next_command.alt);
}
static void do_set_relay()
{
if (next_command.p1 == 1) {
relay.on();
} else if (next_command.p1 == 0) {
relay.off();
}else{
relay.toggle();
}
}
static void do_repeat_servo()
{
event_id = next_command.p1 - 1;
if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_value = next_command.alt;
event_repeat = next_command.lat * 2;
event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
switch(next_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_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.alt * 2;
update_events();
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/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 index)
{
struct Location temp = get_command_with_index(index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
} else {
command_must_index = NO_COMMAND;
next_command.id = NO_COMMAND;
g.waypoint_index = index - 1;
update_commands();
}
}
// called by 10 Hz Medium loop
// ---------------------------
static void update_commands(void)
{
// fill command queue with a new command if available
if(next_command.id == NO_COMMAND){
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if (g.waypoint_index < g.waypoint_total) {
// only if we have a cmd stored in EEPROM
next_command = get_command_with_index(g.waypoint_index + 1);
//Serial.printf("queue CMD %d\n", next_command.id);
}
}
// Are we out of must commands and the queue is empty?
if(next_command.id == NO_COMMAND && command_must_index == NO_COMMAND){
// if no commands were available from EEPROM
// And we have no nav commands
// --------------------------------------------
if (command_must_ID == NO_COMMAND){
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
}
}
// check to see if we need to act on our command queue
if (process_next_command()){
//Serial.printf("did PNC: %d\n", next_command.id);
// We acted on the queue - let's debug that
// ----------------------------------------
print_wp(&next_command, g.waypoint_index);
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
// make sure we load the next command index
// ----------------------------------------
increment_WP_index();
}
}
// called with GPS navigation update - not constantly
static void verify_commands(void)
{
if(verify_must()){
//Serial.printf("verified must cmd %d\n" , command_must_index);
command_must_index = NO_COMMAND;
}else{
//Serial.printf("verified must false %d\n" , command_must_index);
}
if(verify_may()){
//Serial.printf("verified may cmd %d\n" , command_may_index);
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
}
}
static bool
process_next_command()
{
// these are Navigation/Must commands
// ---------------------------------
if (command_must_index == NO_COMMAND){ // no current command loaded
if (next_command.id < MAV_CMD_NAV_LAST ){
// we remember the index of our mission here
command_must_index = g.waypoint_index + 1;
// Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command);
// Act on the new command
process_must();
return true;
}
}
// these are Condition/May commands
// ----------------------
if (command_may_index == NO_COMMAND){
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
// we remember the index of our mission here
command_may_index = g.waypoint_index + 1;
//SendDebug("MSG <pnc> new may ");
//SendDebugln(next_command.id,DEC);
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
// Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command);
process_may();
return true;
}
// these are Do/Now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
//SendDebug("MSG <pnc> new now ");
//SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command);
process_now();
return true;
}
}
// we did not need any new commands
return false;
}
/**************************************************/
// These functions implement the commands.
/**************************************************/
static void process_must()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
//Serial.printf("pmst %d\n", (int)next_command.id);
// clear May indexes to force loading of more commands
// existing May commands are tossed.
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
// remember our command ID
command_must_ID = next_command.id;
// implements the Flight Logic
handle_process_must();
}
static void process_may()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
//Serial.print("pmay");
command_may_ID = next_command.id;
handle_process_may();
}
static void process_now()
{
//Serial.print("pnow");
handle_process_now();
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/control_modes.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void read_control_switch()
{
static bool switch_debouncer = false;
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
if(switch_debouncer){
// remember the prev location for GS
prev_WP = current_loc;
oldSwitchPosition = switchPosition;
switch_debouncer = false;
set_mode(flight_modes[switchPosition]);
#if CH7_OPTION != CH7_SIMPLE_MODE
// setup Simple mode
// do we enable simple mode?
do_simple = (g.simple_modes & (1 << switchPosition));
#endif
}else{
switch_debouncer = true;
}
}
}
static byte readSwitch(void){
int pulsewidth = g.rc_5.radio_in; // default for Arducopter
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 = -1;
read_control_switch();
}
// read at 10 hz
// set this to your trainer switch
static void read_trim_switch()
{
#if CH7_OPTION == CH7_FLIP
if (g.rc_7.control_in > 800 && g.rc_3.control_in != 0){
do_flip = true;
}
#elif CH7_OPTION == CH7_SIMPLE_MODE
do_simple = (g.rc_7.control_in > 800);
//Serial.println(g.rc_7.control_in, DEC);
#elif CH7_OPTION == CH7_RTL
static bool ch7_rtl_flag = false;
if (ch7_rtl_flag == false && g.rc_7.control_in > 800){
ch7_rtl_flag = true;
set_mode(RTL);
}
if (ch7_rtl_flag == true && g.rc_7.control_in < 800){
ch7_rtl_flag = false;
if (control_mode == RTL || control_mode == LOITER){
reset_control_switch();
}
}
#elif CH7_OPTION == CH7_SET_HOVER
// switch is engaged
if (g.rc_7.control_in > 800){
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
// set the throttle nominal
if(g.rc_3.control_in > 150){
g.throttle_cruise.set_and_save(g.rc_3.control_in);
//Serial.printf("tnom %d\n", g.throttle_cruise.get());
}
trim_flag = false;
}
}
#elif CH7_OPTION == CH7_SAVE_WP
if (g.rc_7.control_in > 800){
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
// set the next_WP
CH7_wp_index++;
current_loc.id = MAV_CMD_NAV_WAYPOINT;
g.waypoint_total.set_and_save(CH7_wp_index);
set_command_with_index(current_loc, CH7_wp_index);
}
}
#elif CH7_OPTION == CH7_ADC_FILTER
if (g.rc_7.control_in > 800){
adc.filter_result = true;
}else{
adc.filter_result = false;
}
#elif CH7_OPTION == CH7_AUTO_TRIM
if (g.rc_7.control_in > 800){
auto_level_counter = 10;
}
#endif
}
static void auto_trim()
{
if(auto_level_counter > 0){
//g.rc_1.dead_zone = 60; // 60 = .6 degrees
//g.rc_2.dead_zone = 60;
auto_level_counter--;
trim_accel();
led_mode = AUTO_TRIM_LEDS;
if(auto_level_counter == 1){
//g.rc_1.dead_zone = 0; // 60 = .6 degrees
//g.rc_2.dead_zone = 0;
led_mode = NORMAL_LEDS;
clear_leds();
imu.save();
//Serial.println("Done");
auto_level_counter = 0;
// set TC
init_throttle_cruise();
}
}
}
static void trim_accel()
{
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
if(g.rc_1.control_in > 0){ // Roll RIght
imu.ay(imu.ay() + 1);
}else if (g.rc_1.control_in < 0){
imu.ay(imu.ay() - 1);
}
if(g.rc_2.control_in > 0){ // Pitch Back
imu.ax(imu.ax() + 1);
}else if (g.rc_2.control_in < 0){
imu.ax(imu.ax() - 1);
}
/*
Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"),
dcm.roll_sensor,
dcm.pitch_sensor,
(float)imu.ax(),
(float)imu.ay(),
(float)imu.az());
//*/
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/events.pde"
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This event will be called when the failsafe changes
boolean failsafe reflects the current state
*/
static void failsafe_on_event()
{
// This is how to handle a failsafe.
switch(control_mode)
{
case AUTO:
if (g.throttle_fs_action == 1) {
set_mode(RTL);
}
// 2 = Stay in AUTO and ignore failsafe
default:
// not ready to enable yet w/o more testing
//set_mode(RTL);
break;
}
}
static void failsafe_off_event()
{
if (g.throttle_fs_action == 2){
// We're back in radio contact
// return to AP
// ---------------------------
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
reset_control_switch();
}else if (g.throttle_fs_action == 1){
// We're back in radio contact
// return to Home
// we should already be in RTL and throttle set to cruise
// ------------------------------------------------------
set_mode(RTL);
}
}
static void low_battery_event(void)
{
gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
low_batt = true;
// if we are in Auto mode, come home
if(control_mode >= AUTO)
set_mode(RTL);
}
static void update_events() // 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();
}
}
}
#if PIEZO == ENABLED
void piezo_on()
{
digitalWrite(PIEZO_PIN,HIGH);
//PORTF |= B00100000;
}
void piezo_off()
{
digitalWrite(PIEZO_PIN,LOW);
//PORTF &= ~B00100000;
}
void piezo_beep()
{
// Note: This command should not be used in time sensitive loops
piezo_on();
delay(100);
piezo_off();
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/flip.pde"
// 2010 Jose Julio
// 2011 Adapted for AC2 by Jason Short
//
// Automatic Acrobatic Procedure (AAP) v1 : Roll flip
// State machine aproach:
// Some states are fixed commands (for a fixed time)
// Some states are fixed commands (until some IMU condition)
// Some states include controls inside
#if CH7_OPTION == CH7_FLIP
void roll_flip()
{
#define AAP_THR_INC 180
#define AAP_THR_DEC 90
#define AAP_ROLL_OUT 200
#define AAP_ROLL_RATE 3000 // up to 1250
static int AAP_timer = 0;
static byte AAP_state = 0;
// Yaw
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
// Pitch
g.rc_2.servo_out = get_stabilize_pitch(0);
// State machine
switch (AAP_state){
case 0: // Step 1 : Initialize
AAP_timer = 0;
AAP_state++;
break;
case 1: // Step 2 : Increase throttle to start maneuver
if (AAP_timer < 95){ // .5 seconds
g.rc_1.servo_out = get_stabilize_roll(0);
g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
//g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
AAP_timer++;
}else{
AAP_state++;
AAP_timer = 0;
}
break;
case 2: // Step 3 : ROLL (until we reach a certain angle [45º])
if (dcm.roll_sensor < 4500){
// Roll control
g.rc_1.servo_out = AAP_ROLL_OUT; //get_rate_roll(AAP_ROLL_RATE);
g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC;
}else{
AAP_state++;
}
break;
case 3: // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45º])
if ((dcm.roll_sensor >= 4500) || (dcm.roll_sensor < -4500)){
g.rc_1.servo_out = 150; //get_rate_roll(AAP_ROLL_RATE);
g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC;
}else{
AAP_state++;
}
break;
case 4: // Step 5 : Increase throttle to stop the descend
if (AAP_timer < 90){ // .5 seconds
g.rc_1.servo_out = get_stabilize_roll(0);
g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
AAP_timer++;
}else{
AAP_state++;
AAP_timer = 0;
}
break;
case 5: // exit mode
//control_mode =
do_flip = false;
break;
}
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/heli.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == HELI_FRAME
#define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz
#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz
static int heli_manual_override = false;
static float heli_throttle_scaler = 0;
// heli_servo_averaging:
// 0 or 1 = no averaging, 250hz
// 2 = average two samples, 125hz
// 3 = averaging three samples = 83.3 hz
// 4 = averaging four samples = 62.5 hz
// 5 = averaging 5 samples = 50hz
// digital = 0 / 250hz, analog = 2 / 83.3
static void heli_init_swash()
{
int i;
int tilt_max[CH_3+1];
int total_tilt_max = 0;
// swash servo initialisation
g.heli_servo_1.set_range(0,1000);
g.heli_servo_2.set_range(0,1000);
g.heli_servo_3.set_range(0,1000);
g.heli_servo_4.set_angle(4500);
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos));
// roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90));
// collective min / max
total_tilt_max = 0;
for( i=CH_1; i<=CH_3; i++ ) {
tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100;
total_tilt_max = max(total_tilt_max,tilt_max[i]);
}
// servo min/max values - or should I use set_range?
g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1];
g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1];
g.heli_servo_2.radio_min = g.heli_coll_min - tilt_max[CH_2];
g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2];
g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3];
g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3];
// scaler for changing channel 3 radio input into collective range
heli_throttle_scaler = ((float)(g.heli_coll_max - g.heli_coll_min))/1000;
// reset the servo averaging
for( i=0; i<=3; i++ )
heli_servo_out[i] = 0;
// double check heli_servo_averaging is reasonable
if( g.heli_servo_averaging < 0 || g.heli_servo_averaging < 0 > 5 ) {
g.heli_servo_averaging = 0;
g.heli_servo_averaging.save();
}
}
static void heli_move_servos_to_mid()
{
heli_move_swash(0,0,1500,0);
}
//
// heli_move_swash - moves swash plate to attitude of parameters passed in
// - expected ranges:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 1000 ~ 2000
// yaw: -4500 ~ 4500
//
static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
{
// ensure values are acceptable:
roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max);
// swashplate servos
g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500);
if( g.heli_servo_1.get_reverse() )
g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out;
g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500);
if( g.heli_servo_2.get_reverse() )
g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out;
g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500);
if( g.heli_servo_3.get_reverse() )
g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out;
if( g.heli_servo_4.get_reverse() )
g.heli_servo_4.servo_out = -yaw_out; // should probably just use rc_4 directly like we do for a tricopter
else
g.heli_servo_4.servo_out = yaw_out;
// use servo_out to calculate pwm_out and radio_out
g.heli_servo_1.calc_pwm();
g.heli_servo_2.calc_pwm();
g.heli_servo_3.calc_pwm();
g.heli_servo_4.calc_pwm();
// add the servo values to the averaging
heli_servo_out[0] += g.heli_servo_1.servo_out;
heli_servo_out[1] += g.heli_servo_2.servo_out;
heli_servo_out[2] += g.heli_servo_3.servo_out;
heli_servo_out[3] += g.heli_servo_4.radio_out;
heli_servo_out_count++;
// is it time to move the servos?
if( heli_servo_out_count >= g.heli_servo_averaging ) {
// average the values if necessary
if( g.heli_servo_averaging >= 2 ) {
heli_servo_out[0] /= g.heli_servo_averaging;
heli_servo_out[1] /= g.heli_servo_averaging;
heli_servo_out[2] /= g.heli_servo_averaging;
heli_servo_out[3] /= g.heli_servo_averaging;
}
// actually move the servos
APM_RC.OutputCh(CH_1, heli_servo_out[0]);
APM_RC.OutputCh(CH_2, heli_servo_out[1]);
APM_RC.OutputCh(CH_3, heli_servo_out[2]);
APM_RC.OutputCh(CH_4, heli_servo_out[3]);
// output gyro value
if( g.heli_ext_gyro_enabled ) {
APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain);
}
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
#endif
// reset the averaging
heli_servo_out_count = 0;
heli_servo_out[0] = 0;
heli_servo_out[1] = 0;
heli_servo_out[2] = 0;
heli_servo_out[3] = 0;
}
}
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 40000; // 50 hz output CH 7, 8, 11
#endif
}
// these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better
static void output_motors_armed()
{
//static int counter = 0;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
if( heli_manual_override ) {
// straight pass through from radio inputs to swash plate
heli_move_swash( g.rc_1.control_in, g.rc_2.control_in, g.rc_3.radio_in, g.rc_4.control_in );
}else{
// source inputs from attitude controller
heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out );
}
}
// for helis - armed or disarmed we allow servos to move
static void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle, remove safety
motor_auto_armed = true;
}
output_motors_armed();
}
static void output_motor_test()
{
}
// heli_get_scaled_throttle - user's throttle scaled to collective range
// input is expected to be in the range of 0~1000 (ie. pwm)
// also does equivalent of angle_boost
static int heli_get_scaled_throttle(int throttle)
{
float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler;
return g.heli_coll_min - 1000 + (throttle * heli_throttle_scaler);
}
// heli_angle_boost - takes servo_out position
// adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function
// pwm_out value should be 0 ~ 1000
static int heli_get_angle_boost(int pwm_out)
{
float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
int throttle_above_center = max(1000 + pwm_out - g.heli_coll_mid,0);
return pwm_out + throttle_above_center*angle_boost_factor;
}
#endif // HELI_FRAME
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/leds.pde"
static void update_lights()
{
switch(led_mode){
case NORMAL_LEDS:
update_motor_light();
update_GPS_light();
break;
case AUTO_TRIM_LEDS:
dancing_light();
break;
}
}
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 update_motor_light(void)
{
if(motor_armed == false){
motor_light = !motor_light;
// blink
if(motor_light){
digitalWrite(A_LED_PIN, HIGH);
}else{
digitalWrite(A_LED_PIN, LOW);
}
}else{
if(!motor_light){
motor_light = true;
digitalWrite(A_LED_PIN, HIGH);
}
}
}
static void dancing_light()
{
static byte step;
if (step++ == 3)
step = 0;
switch(step)
{
case 0:
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
break;
case 1:
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, HIGH);
break;
case 2:
digitalWrite(B_LED_PIN, LOW);
digitalWrite(C_LED_PIN, HIGH);
break;
}
}
static void clear_leds()
{
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW);
motor_light = false;
led_mode = NORMAL_LEDS;
}
#if MOTOR_LEDS == 1
static void update_motor_leds(void)
{
if (motor_armed == true){
if (low_batt == true){
// blink rear
static bool blink = false;
if (blink){
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}else{
digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
}
blink = !blink;
}else{
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
}
}else {
digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define ARM_DELAY 10 // one second
#define DISARM_DELAY 10 // one second
#define LEVEL_DELAY 70 // twelve seconds
#define AUTO_LEVEL_DELAY 90 // twentyfive seconds
// called at 10hz
static void arm_motors()
{
static int arming_counter;
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (g.rc_3.control_in == 0){
// full right
if (g.rc_4.control_in > 4000) {
// don't allow arming in anything but manual
if (control_mode < ALT_HOLD){
if (arming_counter > AUTO_LEVEL_DELAY){
auto_level_counter = 155;
arming_counter = 0;
}else if (arming_counter == ARM_DELAY){
#if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif
motor_armed = true;
arming_counter = ARM_DELAY;
#if PIEZO_ARMING == 1
piezo_beep();
piezo_beep();
#endif
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
//dcm.ki_roll_pitch(0.000006);
#endif
// tune down compass
// -----------------
dcm.kp_yaw(0.08);
dcm.ki_yaw(0);
// Remember Orientation
// --------------------
init_simple_bearing();
// Reset home position
// ----------------------
if(home_is_set)
init_home();
if(did_ground_start == false){
did_ground_start = true;
startup_ground();
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
// temp hack
motor_light = true;
digitalWrite(A_LED_PIN, HIGH);
arming_counter++;
} else{
arming_counter++;
}
}
// full left
}else if (g.rc_4.control_in < -4000) {
//Serial.print(arming_counter, DEC);
if (arming_counter > LEVEL_DELAY){
//Serial.print("init");
imu.init_accel(mavlink_delay);
arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){
#if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif
motor_armed = false;
arming_counter = DISARM_DELAY;
compass.save_offsets();
#if PIEZO_ARMING == 1
piezo_beep();
#endif
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
//dcm.kp_roll_pitch(0.12); // higher for fast recovery
//dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif
// tune up compass
// -----------------
dcm.kp_yaw(0.8);
dcm.ki_yaw(0.00004);
// Clear throttle slew
// -------------------
//throttle_slew = 0;
arming_counter++;
}else{
arming_counter++;
}
// centered
}else{
arming_counter = 0;
}
}else{
arming_counter = 0;
}
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
static void
set_servos_4()
{
if (motor_armed == true && motor_auto_armed == true) {
// creates the radio_out and pwm_out values
output_motors_armed();
} else{
output_motors_disarmed();
}
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_hexa.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == HEXA_FRAME
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 5000; // 50 hz output CH 7, 8, 11
#endif
}
static void output_motors_armed()
{
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){
roll_out = g.rc_1.pwm_out / 2;
pitch_out = (float)g.rc_2.pwm_out * .866;
//left side
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back
//right side
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back
}else{
roll_out = (float)g.rc_1.pwm_out * .866;
pitch_out = g.rc_2.pwm_out / 2;
//Front side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
//Back side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK
motor_out[CH_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT
motor_out[CH_8] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
}
// Yaw
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
motor_out[CH_4] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
// Tridge's stability patch
for (int i = CH_1; i<=CH_8; i++) {
if(i == CH_5 || i == CH_6)
break;
if (motor_out[i] > out_max) {
// note that i^1 is the opposite motor
motor_out[i^1] -= motor_out[i] - out_max;
motor_out[i] = out_max;
}
}
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min;
}
#endif
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
APM_RC.Force_Out6_Out7();
#endif
}
static void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
}
static void output_motor_test()
{
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min;
if(g.frame_orientation == X_FRAME){
// 31
// 24
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_1] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_2] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_8] += 100;
motor_out[CH_4] += 100;
}
if(g.rc_2.control_in < -3000){ // front
motor_out[CH_7] += 100;
motor_out[CH_3] += 100;
}
}else{
// 3
// 2 1
// 4
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_4] += 100;
motor_out[CH_8] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_7] += 100;
motor_out[CH_3] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_2] += 100;
}
if(g.rc_2.control_in < -3000){ // front
motor_out[CH_1] += 100;
}
}
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
}
/*
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
}
*/
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_octa.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == OCTA_FRAME
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 5000; // 50 hz output CH 7, 8, 11
#endif
}
static void output_motors_armed()
{
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){
roll_out = (float)g.rc_1.pwm_out * 0.4;
pitch_out = (float)g.rc_2.pwm_out * 0.4;
//Front side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
//Back side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
//Left side
motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT
motor_out[CH_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK
//Right side
motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK
motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT
}else if(g.frame_orientation == PLUS_FRAME){
roll_out = (float)g.rc_1.pwm_out * 0.71;
pitch_out = (float)g.rc_2.pwm_out * 0.71;
//Front side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
motor_out[CH_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
//Left side
motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT
//Right side
motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT
//Back side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
}else if(g.frame_orientation == V_FRAME){
int roll_out2, pitch_out2;
int roll_out3, pitch_out3;
int roll_out4, pitch_out4;
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
roll_out2 = (float)g.rc_1.pwm_out * 0.833;
pitch_out2 = (float)g.rc_2.pwm_out * 0.34;
roll_out3 = (float)g.rc_1.pwm_out * 0.666;
pitch_out3 = (float)g.rc_2.pwm_out * 0.32;
roll_out4 = g.rc_1.pwm_out / 2;
pitch_out4 = (float)g.rc_2.pwm_out * 0.98;
//Front side
motor_out[CH_10] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
//Left side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT
motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK
//Right side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK
motor_out[CH_8] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT
//Back side
motor_out[CH_11] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT
}
// Yaw
motor_out[CH_3] += g.rc_4.pwm_out; // CCW
motor_out[CH_4] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
motor_out[CH_8] += g.rc_4.pwm_out; // CCW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_2] -= g.rc_4.pwm_out; // CW
motor_out[CH_10] -= g.rc_4.pwm_out; // CW
motor_out[CH_11] -= g.rc_4.pwm_out; // CW
// TODO add stability patch
motor_out[CH_1] = min(motor_out[CH_1], out_max);
motor_out[CH_2] = min(motor_out[CH_2], out_max);
motor_out[CH_3] = min(motor_out[CH_3], out_max);
motor_out[CH_4] = min(motor_out[CH_4], out_max);
motor_out[CH_7] = min(motor_out[CH_7], out_max);
motor_out[CH_8] = min(motor_out[CH_8], out_max);
motor_out[CH_10] = min(motor_out[CH_10], out_max);
motor_out[CH_11] = min(motor_out[CH_11], out_max);
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min);
motor_out[CH_10] = max(motor_out[CH_10], out_min);
motor_out[CH_11] = max(motor_out[CH_11], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min;
motor_out[CH_10] = g.rc_3.radio_min;
motor_out[CH_11] = g.rc_3.radio_min;
}
#endif
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
APM_RC.OutputCh(CH_10, motor_out[CH_10]);
APM_RC.OutputCh(CH_11, motor_out[CH_11]);
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
APM_RC.Force_Out6_Out7();
#endif
}
static void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 11; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
}
static void output_motor_test()
{
if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME )
{
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
}
if( g.frame_orientation == V_FRAME )
{
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
}
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_octa_quad.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == OCTA_QUAD_FRAME
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 5000; // 50 hz output CH 7, 8, 11
#endif
}
static void output_motors_armed()
{
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){
roll_out = (float)g.rc_1.pwm_out * .707;
pitch_out = (float)g.rc_2.pwm_out * .707;
// Front Left
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
motor_out[CH_8] = g.rc_3.radio_out + roll_out + pitch_out; // CW
// Front Right
motor_out[CH_10] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
motor_out[CH_11] = g.rc_3.radio_out - roll_out + pitch_out; // CW
// Back Left
motor_out[CH_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW
// Back Right
motor_out[CH_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP
motor_out[CH_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW
}if(g.frame_orientation == PLUS_FRAME){
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
// Left
motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP
motor_out[CH_8] = g.rc_3.radio_out - roll_out; // CW
// Right
motor_out[CH_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP
motor_out[CH_2] = g.rc_3.radio_out + roll_out; // CW
// Front
motor_out[CH_10] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP
motor_out[CH_11] = g.rc_3.radio_out + pitch_out; // CW
// Back
motor_out[CH_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out - pitch_out; // CW
}
// Yaw
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
motor_out[CH_10] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
motor_out[CH_11] -= g.rc_4.pwm_out; // CW
// TODO add stability patch
motor_out[CH_1] = min(motor_out[CH_1], out_max);
motor_out[CH_2] = min(motor_out[CH_2], out_max);
motor_out[CH_3] = min(motor_out[CH_3], out_max);
motor_out[CH_4] = min(motor_out[CH_4], out_max);
motor_out[CH_7] = min(motor_out[CH_7], out_max);
motor_out[CH_8] = min(motor_out[CH_8], out_max);
motor_out[CH_10] = min(motor_out[CH_10], out_max);
motor_out[CH_11] = min(motor_out[CH_11], out_max);
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min);
motor_out[CH_10] = max(motor_out[CH_10], out_min);
motor_out[CH_11] = max(motor_out[CH_11], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min;
motor_out[CH_10] = g.rc_3.radio_min;
motor_out[CH_11] = g.rc_3.radio_min;
}
#endif
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
APM_RC.OutputCh(CH_10, motor_out[CH_10]);
APM_RC.OutputCh(CH_11, motor_out[CH_11]);
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
APM_RC.Force_Out6_Out7();
#endif
}
static void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 11; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
}
static void output_motor_test()
{
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
delay(1000);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
delay(1000);
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_quad.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == QUAD_FRAME
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 40000; // 50 hz output CH 7, 8, 11
#endif
}
static void output_motors_armed()
{
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){
roll_out = g.rc_1.pwm_out * .707;
pitch_out = g.rc_2.pwm_out * .707;
// left
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT
motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
// right
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK
}else{
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
// left
motor_out[CH_1] = g.rc_3.radio_out - roll_out;
// right
motor_out[CH_2] = g.rc_3.radio_out + roll_out;
// front
motor_out[CH_3] = g.rc_3.radio_out + pitch_out;
// back
motor_out[CH_4] = g.rc_3.radio_out - pitch_out;
}
// Yaw input
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
/* We need to clip motor output at out_max. When cipping a motors
* output we also need to compensate for the instability by
* lowering the opposite motor by the same proportion. This
* ensures that we retain control when one or more of the motors
* is at its maximum output
*/
for (int i=CH_1; i<=CH_4; i++) {
if (motor_out[i] > out_max) {
// note that i^1 is the opposite motor
motor_out[i^1] -= motor_out[i] - out_max;
motor_out[i] = out_max;
}
}
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
}
#endif
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
#endif
}
static void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}
/*
static void debug_motors()
{
Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n",
motor_out[CH_1],
motor_out[CH_2],
motor_out[CH_3],
motor_out[CH_4]);
}
*/
static void output_motor_test()
{
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
if(g.frame_orientation == X_FRAME){
// 31
// 24
if(g.rc_1.control_in > 3000){
motor_out[CH_1] += 100;
motor_out[CH_4] += 100;
}
if(g.rc_1.control_in < -3000){
motor_out[CH_2] += 100;
motor_out[CH_3] += 100;
}
if(g.rc_2.control_in > 3000){
motor_out[CH_2] += 100;
motor_out[CH_4] += 100;
}
if(g.rc_2.control_in < -3000){
motor_out[CH_1] += 100;
motor_out[CH_3] += 100;
}
}else{
// 3
// 2 1
// 4
if(g.rc_1.control_in > 3000)
motor_out[CH_1] += 100;
if(g.rc_1.control_in < -3000)
motor_out[CH_2] += 100;
if(g.rc_2.control_in > 3000)
motor_out[CH_4] += 100;
if(g.rc_2.control_in < -3000)
motor_out[CH_3] += 100;
}
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_tri.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == TRI_FRAME
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 40000; // 50 hz output CH 7, 8, 11
#endif
}
static void output_motors_armed()
{
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left front
motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out;
//right front
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out;
// rear
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
//motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013;
// Tridge's stability patch
if (motor_out[CH_1] > out_max) {
motor_out[CH_2] -= (motor_out[CH_1] - out_max) >> 1;
motor_out[CH_4] -= (motor_out[CH_1] - out_max) >> 1;
motor_out[CH_1] = out_max;
}
if (motor_out[CH_2] > out_max) {
motor_out[CH_1] -= (motor_out[CH_2] - out_max) >> 1;
motor_out[CH_4] -= (motor_out[CH_2] - out_max) >> 1;
motor_out[CH_2] = out_max;
}
if (motor_out[CH_4] > out_max) {
motor_out[CH_1] -= (motor_out[CH_4] - out_max) >> 1;
motor_out[CH_2] -= (motor_out[CH_4] - out_max) >> 1;
motor_out[CH_4] = out_max;
}
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
}
#endif
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
#endif
}
static void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
}
static void output_motor_test()
{
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_1] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_2] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_4] += 100;
}
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_y6.pde"
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == Y6_FRAME
#define YAW_DIRECTION 1
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 5000; // 50 hz output CH 7, 8, 11
#endif
}
static void output_motors_armed()
{
int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
// Multi-Wii Mix
//left
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW
motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
//right
motor_out[CH_7] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
//back
motor_out[CH_8] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW
motor_out[CH_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW
//left
motor_out[CH_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW
motor_out[CH_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
//right
motor_out[CH_7] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW
motor_out[CH_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
//back
motor_out[CH_8] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW
motor_out[CH_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW
/*
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left
motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
//right
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
//back
motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
// Yaw
//top
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
motor_out[CH_8] += g.rc_4.pwm_out; // CCW
//bottom
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
*/
// TODO: add stability patch
motor_out[CH_1] = min(motor_out[CH_1], out_max);
motor_out[CH_2] = min(motor_out[CH_2], out_max);
motor_out[CH_3] = min(motor_out[CH_3], out_max);
motor_out[CH_4] = min(motor_out[CH_4], out_max);
motor_out[CH_7] = min(motor_out[CH_7], out_max);
motor_out[CH_8] = min(motor_out[CH_8], out_max);
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min;
}
#endif
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
APM_RC.Force_Out6_Out7();
#endif
}
static void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
}
static void output_motor_test()
{
motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min;
if(g.rc_1.control_in > 3000){ // right
motor_out[CH_1] += 100;
motor_out[CH_7] += 100;
}
if(g.rc_1.control_in < -3000){ // left
motor_out[CH_2] += 100;
motor_out[CH_3] += 100;
}
if(g.rc_2.control_in > 3000){ // back
motor_out[CH_8] += 100;
motor_out[CH_4] += 100;
}
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_4]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
}
#endif
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/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 byte navigate()
{
if(next_WP.lat == 0){
return 0;
}
// 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);
//print_current_waypoints();
return 0;
}
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
return 1;
}
static bool check_missed_wp()
{
long temp = target_bearing - original_target_bearing;
temp = wrap_180(temp);
return (abs(temp) > 10000); //we pased the waypoint by 10 °
}
// ------------------------------
// long_error, lat_error
static void calc_location_error(struct Location *next_loc)
{
/*
Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m
1000 = 11m = 36 feet
1800 = 19.80m = 60 feet
3000 = 33m
10000 = 111m
pitch_max = 22° (2200)
*/
// X ROLL
long_error = (float)(next_loc->lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST
// Y PITCH
lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH
}
#define NAV_ERR_MAX 800
static void calc_loiter(int x_error, int y_error)
{
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
// find the rates:
float temp = radians((float)g_gps->ground_course/100.0);
#ifdef OPTFLOW_ENABLED
// calc the cos of the error to tell how fast we are moving towards the target in cm
if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){
x_actual_speed = optflow.vlon * 10;
y_actual_speed = optflow.vlat * 10;
}else{
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
}
#else
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
#endif
y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum
nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav);
nav_lat = constrain(nav_lat, -3500, 3500);
x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -250, 250);
nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3500, 3500);
}
static void calc_loiter2(int x_error, int y_error)
{
static int last_x_error = 0;
static int last_y_error = 0;
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
// find the rates:
x_actual_speed = (float)(last_x_error - x_error)/dTnav;
y_actual_speed = (float)(last_y_error - y_error)/dTnav;
// save speeds
last_x_error = x_error;
last_y_error = y_error;
y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum
nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav);
nav_lat = constrain(nav_lat, -3500, 3500);
x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -250, 250);
nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3500, 3500);
}
// nav_roll, nav_pitch
static void calc_loiter_pitch_roll()
{
//float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0);
//float _cos_yaw_x = cos(temp);
//float _sin_yaw_y = sin(temp);
//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative
nav_pitch = -nav_pitch;
}
static void calc_nav_rate(int max_speed)
{
/*
0 1 2 3 4 5 6 7 8
...|...|...|...|...|...|...|...|
100 200 300 400
+|+
*/
max_speed = min(max_speed, (wp_distance * 50));
// limit the ramp up of the speed
if(waypoint_speed_gov < max_speed){
waypoint_speed_gov += (int)(150.0 * dTnav); // increase at 1.5/ms
// go at least 1m/s
max_speed = max(100, waypoint_speed_gov);
// limit with governer
max_speed = min(max_speed, waypoint_speed_gov);
}
// XXX target_angle should be the original desired target angle!
float temp = radians((original_target_bearing - g_gps->ground_course)/100.0);
x_actual_speed = -sin(temp) * (float)g_gps->ground_speed;
x_rate_error = -x_actual_speed;
x_rate_error = constrain(x_rate_error, -800, 800);
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
y_actual_speed = cos(temp) * (float)g_gps->ground_speed;
y_rate_error = max_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -800, 800); // added a rate error limit to keep pitching down to a minimum
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
/*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ",
max_speed,
x_actual_speed,
y_actual_speed,
x_rate_error,
y_rate_error,
nav_lon,
nav_lat);*/
}
// nav_roll, nav_pitch
static void calc_nav_pitch_roll()
{
float temp = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0);
float _cos_yaw_x = cos(temp);
float _sin_yaw_y = sin(temp);
// rotate the vector
nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x;
nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y;
// flip pitch because forward is negative
nav_pitch = -nav_pitch;
/*Serial.printf("_cos_yaw_x:%1.4f, _sin_yaw_y:%1.4f, nav_roll:%ld, nav_pitch:%ld\n",
_cos_yaw_x,
_sin_yaw_y,
nav_roll,
nav_pitch);*/
}
static long get_altitude_error()
{
return next_WP.alt - current_loc.alt;
}
static int get_loiter_angle()
{
float power;
int angle;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1);
angle = 90.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);
angle = power * 90;
}
return angle;
}
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 long get_crosstrack_correction(void)
{
// Crosstrack Error
// ----------------
if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following
// Meters we are off track line
float error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance;
// take meters * 100 to get adjustment to nav_bearing
long _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100;
// constrain answer to 30° to avoid overshoot
return constrain(_crosstrack_correction, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
}
return 0;
}
*/
/*
static long cross_track_test()
{
long temp = wrap_180(target_bearing - crosstrack_bearing);
return abs(temp);
}
*/
/*
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
*/
/*static long get_altitude_above_home(void)
{
// This is the altitude above the home location
// The GPS gives us altitude at Sea Level
// if you slope soar, you should see a negative number sometimes
// -------------------------------------------------------------
return current_loc.alt - home.alt;
}
*/
// distance is returned in meters
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_alt_distance(struct Location *loc1, struct Location *loc2)
{
return abs(loc1->alt - loc2->alt);
}
*/
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/ArduCopter/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
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\nNot 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);
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/ArduCopter/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 default_dead_zones()
{
g.rc_1.set_dead_zone(60);
g.rc_2.set_dead_zone(60);
g.rc_3.set_dead_zone(60);
g.rc_4.set_dead_zone(200);
}
static void init_rc_in()
{
// set rc channel ranges
g.rc_1.set_angle(4500);
g.rc_2.set_angle(4500);
g.rc_3.set_range(0,1000);
#if FRAME_CONFIG != HELI_FRAME
g.rc_3.scale_output = .9;
#endif
g.rc_4.set_angle(4500);
// reverse: CW = left
// normal: CW = left???
g.rc_1.set_type(RC_CHANNEL_ANGLE_RAW);
g.rc_2.set_type(RC_CHANNEL_ANGLE_RAW);
g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
// set rc dead zones
/*g.rc_1.dead_zone = 60;
g.rc_2.dead_zone = 60;
g.rc_3.dead_zone = 60;
g.rc_4.dead_zone = 300;
*/
//set auxiliary ranges
g.rc_5.set_range(0,1000);
g.rc_6.set_range(0,1000);
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
}
static void init_rc_out()
{
#if ARM_AT_STARTUP == 1
motor_armed = 1;
#endif
APM_RC.Init(); // APM Radio initialization
init_motors_out();
// fix for crazy output
OCR1B = 0xFFFF; // PB6, OUT3
OCR1C = 0xFFFF; // PB7, OUT4
OCR5B = 0xFFFF; // PL4, OUT1
OCR5C = 0xFFFF; // PL5, OUT2
OCR4B = 0xFFFF; // PH4, OUT6
OCR4C = 0xFFFF; // PH5, OUT5
// this is the camera pitch5 and roll6
APM_RC.OutputCh(CH_5, 1500);
APM_RC.OutputCh(CH_6, 1500);
// don't fuss if we are calibrating
if(g.esc_calibrate == 1)
return;
if(g.rc_3.radio_min <= 1200){
output_min();
}
for(byte i = 0; i < 5; i++){
delay(20);
read_radio();
}
// sanity check
if(g.rc_3.radio_min >= 1300){
g.rc_3.radio_min = g.rc_3.radio_in;
output_min();
}
}
void output_min()
{
#if FRAME_CONFIG == HELI_FRAME
heli_move_servos_to_mid();
#else
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
#endif
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
#if FRAME_CONFIG == OCTA_FRAME
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
#endif
}
static void read_radio()
{
if (APM_RC.GetState() == 1){
new_radio_frame = true;
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
g.rc_4.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));
#if FRAME_CONFIG != HELI_FRAME
// limit our input to 800 so we can still pitch and roll
g.rc_3.control_in = min(g.rc_3.control_in, 800);
#endif
//throttle_failsafe(g.rc_3.radio_in);
}
}
static void throttle_failsafe(uint16_t pwm)
{
if(g.throttle_fs_enabled == 0)
return;
//check for failsafe and debounce funky reads
// ------------------------------------------
if (pwm < (unsigned)g.throttle_fs_value){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9){
SendDebug("MSG FS ON ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 10) {
ch3_failsafe = true;
//set_failsafe(true);
//failsafeCounter = 10;
}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){
SendDebug("MSG FS OFF ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 0) {
ch3_failsafe = false;
//set_failsafe(false);
//failsafeCounter = -1;
}else if (failsafeCounter <0){
failsafeCounter = -1;
}
}
}
static void trim_radio()
{
for (byte i = 0; i < 30; i++){
read_radio();
}
g.rc_1.trim(); // roll
g.rc_2.trim(); // pitch
g.rc_4.trim(); // yaw
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
g.rc_4.save_eeprom();
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/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
static void ReadSCP1000(void) {}
static void init_barometer(void)
{
#if HIL_MODE == HIL_MODE_SENSORS
gcs_update(); // look for inbound hil packets for initialization
#endif
ground_temperature = barometer.Temp;
int i;
// We take some readings...
for(i = 0; i < 60; i++){
delay(20);
// get new data from absolute pressure sensor
barometer.Read();
//Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press);
}
for(i = 0; i < 20; i++){
delay(20);
#if HIL_MODE == HIL_MODE_SENSORS
gcs_update(); // look for inbound hil packets
#endif
// Get initial data from absolute pressure sensor
barometer.Read();
ground_pressure = barometer.Press;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
//Serial.printf("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press, ground_temperature, ground_pressure);
}
abs_pressure = ground_pressure;
//Serial.printf("init %ld\n", abs_pressure);
//SendDebugln("barometer calibration complete.");
}
/*
static long read_baro_filtered(void)
{
// get new data from absolute pressure sensor
barometer.Read();
return barometer.Press;
long pressure = 0;
// add new data into our filter
baro_filter[baro_filter_index] = barometer.Press;
baro_filter_index++;
// loop our filter
if(baro_filter_index >= BARO_FILTER_SIZE)
baro_filter_index = 0;
// zero out our accumulator
// sum our filter
for(byte i = 0; i < BARO_FILTER_SIZE; i++){
pressure += baro_filter[i];
}
// average our sampels
return pressure /= BARO_FILTER_SIZE;
//
}
*/
static long read_barometer(void)
{
float x, scaling, temp;
barometer.Read();
abs_pressure = barometer.Press;
//Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure);
scaling = (float)ground_pressure / (float)abs_pressure;
temp = ((float)ground_temperature / 10.0f) + 273.15f;
x = log(scaling) * temp * 29271.267f;
return (x / 10);
}
// in M/S * 100
static void read_airspeed(void)
{
}
static void zero_airspeed(void)
{
}
#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 * 0.0278; // called at 100ms on average
}
#if BATTERY_EVENT == 1
//if(battery_voltage < g.low_voltage)
// low_battery_event();
if((battery_voltage < g.low_voltage) || (g.battery_monitoring == 4 && current_total > g.pack_capacity)){
low_battery_event();
#if PIEZO_LOW_VOLTAGE == 1
// Only Activate if a battery is connected to avoid alarm on USB only
if (battery_voltage1 > 1){
piezo_on();
}else{
piezo_off();
}
#endif
}else{
#if PIEZO_LOW_VOLTAGE == 1
piezo_off();
#endif
}
#endif
}
//v: 10.9453, a: 17.4023, mah: 8.2
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/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_motors (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_tune (uint8_t argc, const Menu::arg *argv);
//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_esc (uint8_t argc, const Menu::arg *argv);
#ifdef OPTFLOW_ENABLED
static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv);
#endif
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
#if FRAME_CONFIG == HELI_FRAME
static int8_t setup_heli (uint8_t argc, const Menu::arg *argv);
static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv);
#endif
// Command/function table for the setup menu
const struct Menu::command setup_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"erase", setup_erase},
{"reset", setup_factory},
{"radio", setup_radio},
{"frame", setup_frame},
{"motors", setup_motors},
{"esc", setup_esc},
{"level", setup_accel},
{"modes", setup_flightmodes},
{"battery", setup_batt_monitor},
{"sonar", setup_sonar},
{"compass", setup_compass},
{"tune", setup_tune},
// {"offsets", setup_mag_offset},
{"declination", setup_declination},
#ifdef OPTFLOW_ENABLED
{"optflow", setup_optflow},
#endif
#if FRAME_CONFIG == HELI_FRAME
{"heli", setup_heli},
{"gyro", setup_gyro},
#endif
{"show", setup_show}
};
// 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\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"));
if(g.rc_1.radio_min >= 1300){
delay(1000);
Serial.printf_P(PSTR("\n!Warning, your radio is not configured!"));
delay(1000);
Serial.printf_P(PSTR("\n Type 'radio' to configure now.\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_version();
report_radio();
report_frame();
report_batt_monitor();
report_sonar();
//report_gains();
//report_xtrack();
//report_throttle();
report_flight_modes();
report_imu();
report_compass();
#ifdef OPTFLOW_ENABLED
report_optflow();
#endif
#if FRAME_CONFIG == HELI_FRAME
report_heli();
report_gyro();
#endif
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("\n'Y' + Enter to factory reset, any other key to abort:\n"));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
AP_Var::erase_all();
Serial.printf_P(PSTR("\nReboot APM"));
delay(1000);
//default_gains();
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.println("\n\nRadio Setup:");
uint8_t i;
for(i = 0; i < 100;i++){
delay(20);
read_radio();
}
if(g.rc_1.radio_in < 500){
while(1){
//Serial.printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000);
// stop here
}
}
g.rc_1.radio_min = g.rc_1.radio_in;
g.rc_2.radio_min = g.rc_2.radio_in;
g.rc_3.radio_min = g.rc_3.radio_in;
g.rc_4.radio_min = g.rc_4.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.rc_1.radio_max = g.rc_1.radio_in;
g.rc_2.radio_max = g.rc_2.radio_in;
g.rc_3.radio_max = g.rc_3.radio_in;
g.rc_4.radio_max = g.rc_4.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.rc_1.radio_trim = g.rc_1.radio_in;
g.rc_2.radio_trim = g.rc_2.radio_in;
g.rc_4.radio_trim = g.rc_4.radio_in;
// 3 is not trimed
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: "));
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
g.rc_1.update_min_max();
g.rc_2.update_min_max();
g.rc_3.update_min_max();
g.rc_4.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){
delay(20);
Serial.flush();
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
g.rc_3.save_eeprom();
g.rc_4.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;
}
}
report_radio();
return(0);
}
static int8_t
setup_esc(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nESC Calibration:\n"
"-1 Unplug USB and battery\n"
"-2 Move CLI/FLY switch to FLY mode\n"
"-3 Move throttle to max, connect battery\n"
"-4 After two long beeps, throttle to 0, then test\n\n"
" Press Enter to cancel.\n"));
g.esc_calibrate.set_and_save(1);
while(1){
delay(20);
if(Serial.available() > 0){
g.esc_calibrate.set_and_save(0);
return(0);
}
}
}
static int8_t
setup_motors(uint8_t argc, const Menu::arg *argv)
{
while(1){
delay(20);
read_radio();
output_motor_test();
if(Serial.available() > 0){
g.esc_calibrate.set_and_save(0);
return(0);
}
}
}
static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv)
{
imu.init_accel();
print_accel_offsets();
report_imu();
return(0);
}
static int8_t
setup_frame(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("x"))) {
g.frame_orientation.set_and_save(X_FRAME);
} else if (!strcmp_P(argv[1].str, PSTR("p"))) {
g.frame_orientation.set_and_save(PLUS_FRAME);
} else if (!strcmp_P(argv[1].str, PSTR("+"))) {
g.frame_orientation.set_and_save(PLUS_FRAME);
} else if (!strcmp_P(argv[1].str, PSTR("v"))) {
g.frame_orientation.set_and_save(V_FRAME);
}else{
Serial.printf_P(PSTR("\nOptions:[x,+,v]\n"));
report_frame();
return 0;
}
report_frame();
return 0;
}
static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
byte _switchPosition = 0;
byte _oldSwitchPosition = 0;
byte mode = 0;
Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
print_hit_enter();
while(1){
delay(20);
read_radio();
_switchPosition = readSwitch();
// look for control switch change
if (_oldSwitchPosition != _switchPosition){
mode = flight_modes[_switchPosition];
mode = constrain(mode, 0, NUM_MODES-1);
// update the user
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
// Remember switch position
_oldSwitchPosition = _switchPosition;
}
// look for stick input
if (abs(g.rc_1.control_in) > 3000){
mode++;
if(mode >= NUM_MODES)
mode = 0;
// save new mode
flight_modes[_switchPosition] = mode;
// print new mode
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// look for stick input
if (g.rc_4.control_in > 3000){
g.simple_modes |= (1<<_switchPosition);
// print new mode
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// look for stick input
if (g.rc_4.control_in < -3000){
g.simple_modes &= ~(1<<_switchPosition);
// print new mode
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
delay(500);
}
// escape hatch
if(Serial.available() > 0){
for (mode = 0; mode < 6; mode++)
flight_modes[mode].save();
g.simple_modes.save();
print_done();
report_flight_modes();
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_tune(uint8_t argc, const Menu::arg *argv)
{
g.radio_tuning.set_and_save(argv[1].i);
report_tuning();
return 0;
}
static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv)
{
zero_eeprom();
return 0;
}
static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.compass_enabled.set_and_save(true);
init_compass();
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
clear_offsets();
g.compass_enabled.set_and_save(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 (!strcmp_P(argv[1].str, PSTR("off"))) {
g.battery_monitoring.set_and_save(0);
} else if(argv[1].i > 0 && argv[1].i <= 4){
g.battery_monitoring.set_and_save(argv[1].i);
} else {
Serial.printf_P(PSTR("\nOptions: off, 1-4"));
}
report_batt_monitor();
return 0;
}
static int8_t
setup_sonar(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.sonar_enabled.set_and_save(true);
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.sonar_enabled.set_and_save(false);
}else{
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
report_sonar();
return 0;
}
report_sonar();
return 0;
}
#if FRAME_CONFIG == HELI_FRAME
// Perform heli setup.
// Called by the setup menu 'radio' command.
static int8_t
setup_heli(uint8_t argc, const Menu::arg *argv)
{
uint8_t active_servo = 0;
int value = 0;
int temp;
int state = 0; // 0 = set rev+pos, 1 = capture min/max
int max_roll, max_pitch, min_coll, max_coll, min_tail, max_tail;
// initialise swash plate
heli_init_swash();
// source swash plate movements directly from
heli_manual_override = true;
// display initial settings
report_heli();
// display help
Serial.printf_P(PSTR("Instructions:"));
print_divider();
Serial.printf_P(PSTR("\td\t\tdisplay settings\n"));
Serial.printf_P(PSTR("\t1~4\t\tselect servo\n"));
Serial.printf_P(PSTR("\ta or z\t\tmove mid up/down\n"));
Serial.printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n"));
Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n"));
Serial.printf_P(PSTR("\tp<angle>\tset pos (i.e. p0 = front, p90 = right)\n"));
Serial.printf_P(PSTR("\tr\t\treverse servo\n"));
Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n"));
Serial.printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
Serial.printf_P(PSTR("\tx\t\texit & save\n"));
// start capturing
while( value != 'x' ) {
// read radio although we don't use it yet
read_radio();
// record min/max
if( state == 1 ) {
if( abs(g.rc_1.control_in) > max_roll )
max_roll = abs(g.rc_1.control_in);
if( abs(g.rc_2.control_in) > max_pitch )
max_pitch = abs(g.rc_2.control_in);
if( g.rc_3.radio_in < min_coll )
min_coll = g.rc_3.radio_in;
if( g.rc_3.radio_in > max_coll )
max_coll = g.rc_3.radio_in;
min_tail = min(g.rc_4.radio_in, min_tail);
max_tail = max(g.rc_4.radio_in, max_tail);
//Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out);
}
if( Serial.available() ) {
value = Serial.read();
// process the user's input
switch( value ) {
case '1':
active_servo = CH_1;
break;
case '2':
active_servo = CH_2;
break;
case '3':
active_servo = CH_3;
break;
case '4':
active_servo = CH_4;
break;
case 'a':
case 'A':
heli_get_servo(active_servo)->radio_trim += 10;
break;
case 'c':
case 'C':
if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) {
g.heli_coll_mid = g.rc_3.radio_in;
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid);
}
break;
case 'd':
case 'D':
// display settings
report_heli();
break;
case 'm':
case 'M':
if( state == 0 ) {
state = 1; // switch to capture min/max mode
Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"));
// reset servo ranges
g.heli_roll_max = g.heli_pitch_max = 4500;
g.heli_coll_min = 1000;
g.heli_coll_max = 2000;
g.heli_servo_4.radio_min = 1000;
g.heli_servo_4.radio_max = 2000;
// set sensible values in temp variables
max_roll = abs(g.rc_1.control_in);
max_pitch = abs(g.rc_2.control_in);
min_coll = 2000;
max_coll = 1000;
min_tail = max_tail = abs(g.rc_4.radio_in);
}else{
state = 0; // switch back to normal mode
// double check values aren't totally terrible
if( max_roll <= 1000 || max_pitch <= 1000 || (max_coll - min_coll < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_coll,max_coll,min_tail,max_tail);
else{
g.heli_roll_max = max_roll;
g.heli_pitch_max = max_pitch;
g.heli_coll_min = min_coll;
g.heli_coll_max = max_coll;
g.heli_servo_4.radio_min = min_tail;
g.heli_servo_4.radio_max = max_tail;
// reinitialise swash
heli_init_swash();
// display settings
report_heli();
}
}
break;
case 'p':
case 'P':
temp = read_num_from_serial();
if( temp >= -360 && temp <= 360 ) {
if( active_servo == CH_1 )
g.heli_servo1_pos = temp;
if( active_servo == CH_2 )
g.heli_servo2_pos = temp;
if( active_servo == CH_3 )
g.heli_servo3_pos = temp;
heli_init_swash();
Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
}
break;
case 'r':
case 'R':
heli_get_servo(active_servo)->set_reverse(!heli_get_servo(active_servo)->get_reverse());
break;
case 't':
case 'T':
temp = read_num_from_serial();
if( temp > 1000 )
temp -= 1500;
if( temp > -500 && temp < 500 ) {
heli_get_servo(active_servo)->radio_trim = 1500 + temp;
heli_init_swash();
Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
}
break;
case 'u':
case 'U':
temp = 0;
// delay up to 2 seconds for servo type from user
while( !Serial.available() && temp < 20 ) {
temp++;
delay(100);
}
if( Serial.available() ) {
value = Serial.read();
if( value == 'a' || value == 'A' ) {
g.heli_servo_averaging = HELI_SERVO_AVERAGING_ANALOG;
Serial.printf_P(PSTR("Analog Servo %dhz\n"),250 / HELI_SERVO_AVERAGING_ANALOG);
}
if( value == 'd' || value == 'D' ) {
g.heli_servo_averaging = HELI_SERVO_AVERAGING_DIGITAL;
Serial.printf_P(PSTR("Digital Servo 250hz\n"));
}
}
break;
case 'z':
case 'Z':
heli_get_servo(active_servo)->radio_trim -= 10;
break;
}
}
// allow swash plate to move
output_motors_armed();
delay(20);
}
// display final settings
report_heli();
// save to eeprom
g.heli_servo_1.save_eeprom();
g.heli_servo_2.save_eeprom();
g.heli_servo_3.save_eeprom();
g.heli_servo_4.save_eeprom();
g.heli_servo1_pos.save();
g.heli_servo2_pos.save();
g.heli_servo3_pos.save();
g.heli_roll_max.save();
g.heli_pitch_max.save();
g.heli_coll_min.save();
g.heli_coll_max.save();
g.heli_coll_mid.save();
g.heli_servo_averaging.save();
// return swash plate movements to attitude controller
heli_manual_override = false;
return(0);
}
// setup for external tail gyro (for heli only)
static int8_t
setup_gyro(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.heli_ext_gyro_enabled.set_and_save(true);
// optionally capture the gain
if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) {
g.heli_ext_gyro_gain = argv[2].i;
g.heli_ext_gyro_gain.save();
}
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.heli_ext_gyro_enabled.set_and_save(false);
// capture gain if user simply provides a number
} else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) {
g.heli_ext_gyro_enabled.set_and_save(true);
g.heli_ext_gyro_gain = argv[1].i;
g.heli_ext_gyro_gain.save();
}else{
Serial.printf_P(PSTR("\nOptions:[on, off] gain\n"));
}
report_gyro();
return 0;
}
#endif // FRAME_CONFIG == HELI
static void clear_offsets()
{
Vector3f _offsets(0.0,0.0,0.0);
compass.set_offsets(_offsets);
compass.save_offsets();
}
/*static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
{
Vector3f _offsets;
if (!strcmp_P(argv[1].str, PSTR("c"))) {
clear_offsets();
report_compass();
return (0);
}
print_hit_enter();
init_compass();
int _min[3] = {0,0,0};
int _max[3] = {0,0,0};
compass.read();
compass.calculate(0,0); // roll = 0, pitch = 0
while(1){
delay(50);
compass.read();
compass.calculate(0,0); // roll = 0, pitch = 0
if(compass.mag_x < _min[0]) _min[0] = compass.mag_x;
if(compass.mag_y < _min[1]) _min[1] = compass.mag_y;
if(compass.mag_z < _min[2]) _min[2] = compass.mag_z;
// capture max
if(compass.mag_x > _max[0]) _max[0] = compass.mag_x;
if(compass.mag_y > _max[1]) _max[1] = compass.mag_y;
if(compass.mag_z > _max[2]) _max[2] = compass.mag_z;
// calculate offsets
_offsets.x = (float)(_max[0] + _min[0]) / -2;
_offsets.y = (float)(_max[1] + _min[1]) / -2;
_offsets.z = (float)(_max[2] + _min[2]) / -2;
// display all to user
Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"),
(uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z,
_offsets.x,
_offsets.y,
_offsets.z);
if(Serial.available() > 1){
compass.set_offsets(_offsets);
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
report_compass();
return 0;
}
}
return 0;
}
*/
#ifdef OPTFLOW_ENABLED
static int8_t
setup_optflow(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.optflow_enabled = true;
init_optflow();
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.optflow_enabled = false;
}else{
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
report_optflow();
return 0;
}
g.optflow_enabled.save();
report_optflow();
return 0;
}
#endif
/***************************************************************************/
// CLI reports
/***************************************************************************/
static void report_batt_monitor()
{
Serial.printf_P(PSTR("\nBatt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) print_enabled(false);
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3 cells"));
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4 cells"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
print_blanks(2);
}
static void report_wp(byte index = 255)
{
if(index == 255){
for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_command_with_index(i);
print_wp(&temp, i);
}
}else{
struct Location temp = get_command_with_index(index);
print_wp(&temp, index);
}
}
static void report_sonar()
{
g.sonar_enabled.load();
Serial.printf_P(PSTR("Sonar\n"));
print_divider();
print_enabled(g.sonar_enabled.get());
print_blanks(2);
}
static void report_frame()
{
Serial.printf_P(PSTR("Frame\n"));
print_divider();
#if FRAME_CONFIG == QUAD_FRAME
Serial.printf_P(PSTR("Quad frame\n"));
#elif FRAME_CONFIG == TRI_FRAME
Serial.printf_P(PSTR("TRI frame\n"));
#elif FRAME_CONFIG == HEXA_FRAME
Serial.printf_P(PSTR("Hexa frame\n"));
#elif FRAME_CONFIG == Y6_FRAME
Serial.printf_P(PSTR("Y6 frame\n"));
#elif FRAME_CONFIG == OCTA_FRAME
Serial.printf_P(PSTR("Octa frame\n"));
#elif FRAME_CONFIG == HELI_FRAME
Serial.printf_P(PSTR("Heli frame\n"));
#endif
#if FRAME_CONFIG != HELI_FRAME
if(g.frame_orientation == X_FRAME)
Serial.printf_P(PSTR("X mode\n"));
else if(g.frame_orientation == PLUS_FRAME)
Serial.printf_P(PSTR("+ mode\n"));
else if(g.frame_orientation == V_FRAME)
Serial.printf_P(PSTR("V mode\n"));
#endif
print_blanks(2);
}
static void report_radio()
{
Serial.printf_P(PSTR("Radio\n"));
print_divider();
// radio
print_radio_values();
print_blanks(2);
}
static void report_imu()
{
Serial.printf_P(PSTR("IMU\n"));
print_divider();
print_gyro_offsets();
print_accel_offsets();
print_blanks(2);
}
static void report_compass()
{
Serial.printf_P(PSTR("Compass\n"));
print_divider();
print_enabled(g.compass_enabled);
// mag declination
Serial.printf_P(PSTR("Mag Dec: %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"),
offsets.x,
offsets.y,
offsets.z);
print_blanks(2);
}
static void report_flight_modes()
{
Serial.printf_P(PSTR("Flight modes\n"));
print_divider();
for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i], (g.simple_modes & (1<<i)));
}
print_blanks(2);
}
#ifdef OPTFLOW_ENABLED
void report_optflow()
{
Serial.printf_P(PSTR("OptFlow\n"));
print_divider();
print_enabled(g.optflow_enabled);
// field of view
//Serial.printf_P(PSTR("FOV: %4.0f\n"),
// degrees(g.optflow_fov));
print_blanks(2);
}
#endif
#if FRAME_CONFIG == HELI_FRAME
static void report_heli()
{
int servo_rate;
Serial.printf_P(PSTR("Heli\n"));
print_divider();
// main servo settings
Serial.printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
Serial.printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo1_pos, (int)g.heli_servo_1.radio_min, (int)g.heli_servo_1.radio_max, (int)g.heli_servo_1.get_reverse());
Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo2_pos, (int)g.heli_servo_2.radio_min, (int)g.heli_servo_2.radio_max, (int)g.heli_servo_2.get_reverse());
Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo3_pos, (int)g.heli_servo_3.radio_min, (int)g.heli_servo_3.radio_max, (int)g.heli_servo_3.get_reverse());
Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)g.heli_servo_4.radio_min, (int)g.heli_servo_4.radio_max, (int)g.heli_servo_4.get_reverse());
Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max);
Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max);
Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max);
// calculate and print servo rate
if( g.heli_servo_averaging <= 1 ) {
servo_rate = 250;
} else {
servo_rate = 250 / g.heli_servo_averaging;
}
Serial.printf_P(PSTR("servo rate:\t%d hz\n"),servo_rate);
print_blanks(2);
}
static void report_gyro()
{
Serial.printf_P(PSTR("External Gyro:\n"));
print_divider();
print_enabled( g.heli_ext_gyro_enabled );
if( g.heli_ext_gyro_enabled )
Serial.printf_P(PSTR("gain: %d"),(int)g.heli_ext_gyro_gain);
print_blanks(2);
}
#endif // FRAME_CONFIG == HELI_FRAME
/***************************************************************************/
// CLI utilities
/***************************************************************************/
/*static void
print_PID(PI * pid)
{
Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"),
pid->kP(),
pid->kI(),
(long)pid->imax());
}
*/
static void
print_radio_values()
{
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
//Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
}
static void
print_switch(byte p, byte m, bool b)
{
Serial.printf_P(PSTR("Pos %d:\t"),p);
Serial.print(flight_mode_strings[m]);
Serial.printf_P(PSTR(",\t\tSimple: "));
if(b)
Serial.printf_P(PSTR("ON\n"));
else
Serial.printf_P(PSTR("OFF\n"));
}
static void
print_done()
{
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
}
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_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());
}
#if FRAME_CONFIG == HELI_FRAME
static RC_Channel *
heli_get_servo(int servo_num){
if( servo_num == CH_1 )
return &g.heli_servo_1;
if( servo_num == CH_2 )
return &g.heli_servo_2;
if( servo_num == CH_3 )
return &g.heli_servo_3;
if( servo_num == CH_4 )
return &g.heli_servo_4;
return NULL;
}
// Used to read integer values from the serial port
static int read_num_from_serial() {
byte index = 0;
byte timeout = 0;
char data[5] = "";
do {
if (Serial.available() == 0) {
delay(10);
timeout++;
}else{
data[index] = Serial.read();
timeout = 0;
index++;
}
}while (timeout < 5 && index < 5);
return atoi(data);
}
#endif
#endif // CLI_ENABLED
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.print("-");
}
Serial.println("");
}
static void print_enabled(boolean b)
{
if(b)
Serial.printf_P(PSTR("en"));
else
Serial.printf_P(PSTR("dis"));
Serial.printf_P(PSTR("abled\n"));
}
static void
init_esc()
{
g.esc_calibrate.set_and_save(0);
while(1){
read_radio();
delay(100);
dancing_light();
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
APM_RC.OutputCh(CH_7, g.rc_3.radio_in);
APM_RC.OutputCh(CH_8, g.rc_3.radio_in);
#if FRAME_CONFIG == OCTA_FRAME
APM_RC.OutputCh(CH_10, g.rc_3.radio_in);
APM_RC.OutputCh(CH_11, g.rc_3.radio_in);
#endif
}
}
static void print_wp(struct Location *cmd, byte index)
{
Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
cmd->alt,
cmd->lat,
cmd->lng);
}
static void report_gps()
{
Serial.printf_P(PSTR("\nGPS\n"));
print_divider();
print_enabled(GPS_enabled);
print_blanks(2);
}
static void report_version()
{
Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get());
print_divider();
print_blanks(2);
}
static void report_tuning()
{
Serial.printf_P(PSTR("\nTUNE:\n"));
print_divider();
if (g.radio_tuning == 0){
print_enabled(g.radio_tuning.get());
}else{
Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get());
}
print_blanks(2);
}
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/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\n"
" setup\n"
" test\n"
" planner\n"
"\n"
"Move the slide switch and reset to FLY.\n"
"\n"));
return(0);
}
// Command/function table for the top-level menu.
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.
//
// Not used if the IMU/X-Plane GPS is in use.
//
// 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.
//
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
Serial1.begin(38400, 128, 16);
#endif
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.
//
report_version();
// setup IO pins
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
// XXX set Analog out 14 to output
// 76543210
//DDRK |= B01010000;
#if MOTOR_LEDS == 1
pinMode(FR_LED, OUTPUT); // GPS status LED
pinMode(RE_LED, OUTPUT); // GPS status LED
pinMode(RI_LED, OUTPUT); // GPS status LED
pinMode(LE_LED, OUTPUT); // GPS status LED
#endif
#if PIEZO == 1
pinMode(PIEZO_PIN,OUTPUT);
piezo_beep();
#endif
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
//Serial.printf_P(PSTR("\n\nForcing complete parameter reset..."));
/*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);
*/
// erase all parameters
Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
delay(100); // wait for serial send
AP_Var::erase_all();
// save the new format version
g.format_version.set_and_save(Parameters::k_format_version);
// save default radio values
default_dead_zones();
Serial.printf_P(PSTR("Please Run Setup...\n"));
while (true) {
delay(1000);
if(motor_light){
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, HIGH);
digitalWrite(C_LED_PIN, HIGH);
}else{
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW);
}
motor_light = !motor_light;
}
}else{
// save default radio values
//default_dead_zones();
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
}
// 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);
#ifdef RADIO_OVERRIDE_DEFAULTS
{
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
APM_RC.setHIL(rc_override);
}
#endif
#if FRAME_CONFIG == HELI_FRAME
heli_init_swash(); // heli initialisation
#endif
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
init_camera();
#if HIL_MODE != HIL_MODE_ATTITUDE
// begin filtering the ADC Gyros
adc.filter_result = true;
adc.Init(); // APM ADC library initialization
barometer.Init(); // APM Abs Pressure sensor 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);
if(g.compass_enabled)
init_compass();
#ifdef OPTFLOW_ENABLED
// init the optical flow sensor
if(g.optflow_enabled) {
init_optflow();
}
#endif
// agmatthews USERHOOKS
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
// Logging:
// --------
// DataFlash log initialization
#if LOGGING_ENABLED == ENABLED
DataFlash.Init();
#endif
#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED
// 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 (check_startup_for_CLI()) {
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
Serial.printf_P(PSTR("\nCLI:\n\n"));
run_cli();
}
#else
Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n"));
#endif // CLI_ENABLED
if(g.esc_calibrate == 1){
init_esc();
}
// Logging:
// --------
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
start_new_log();
}
GPS_enabled = false;
// Read in the GPS
for (byte counter = 0; ; counter++) {
g_gps->update();
if (g_gps->status() != 0){
GPS_enabled = true;
break;
}
if (counter >= 2) {
GPS_enabled = false;
break;
}
}
// lengthen the idle timeout for gps Auto_detect
// ---------------------------------------------
g_gps->idleTimeout = 20000;
// print the GPS status
// --------------------
report_gps();
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif
// initialize commands
// -------------------
init_commands();
// set the correct flight mode
// ---------------------------
reset_control_switch();
startup_ground();
Log_Write_Startup();
SendDebug("\nReady to FLY ");
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
static void startup_ground(void)
{
gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro(mavlink_delay);
#if CLI_ENABLED == ENABLED
report_imu();
#endif
#endif
// reset the leds
// ---------------------------
clear_leds();
}
/*
#define YAW_HOLD 0
#define YAW_ACRO 1
#define YAW_AUTO 2
#define YAW_LOOK_AT_HOME 3
#define ROLL_PITCH_STABLE 0
#define ROLL_PITCH_ACRO 1
#define ROLL_PITCH_AUTO 2
#define THROTTLE_MANUAL 0
#define THROTTLE_HOLD 1
#define THROTTLE_AUTO 2
*/
static void set_mode(byte mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
old_control_mode = control_mode;
control_mode = mode;
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
// used to stop fly_aways
motor_auto_armed = (g.rc_3.control_in > 0);
// clearing value used in interactive alt hold
manual_boost = 0;
// clearing value used to set WP's dynamically.
CH7_wp_index = 0;
Serial.println(flight_mode_strings[control_mode]);
// report the GPS and Motor arming status
led_mode = NORMAL_LEDS;
reset_nav();
switch(control_mode)
{
case ACRO:
yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL;
reset_hold_I();
break;
case STABILIZE:
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
throttle_mode = THROTTLE_MANUAL;
reset_hold_I();
break;
case ALT_HOLD:
yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR;
reset_hold_I();
init_throttle_cruise();
next_WP = current_loc;
break;
case AUTO:
reset_hold_I();
yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP;
throttle_mode = AUTO_THR;
init_throttle_cruise();
// loads the commands from where we left off
init_commands();
break;
case CIRCLE:
yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR;
init_throttle_cruise();
next_WP = current_loc;
break;
case LOITER:
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
throttle_mode = LOITER_THR;
init_throttle_cruise();
next_WP = current_loc;
break;
case POSITION:
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_MANUAL;
next_WP = current_loc;
break;
case GUIDED:
yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
//xtrack_enabled = true;
init_throttle_cruise();
next_WP = current_loc;
set_next_WP(&guided_WP);
break;
case RTL:
yaw_mode = RTL_YAW;
roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR;
//xtrack_enabled = true;
init_throttle_cruise();
do_RTL();
break;
default:
break;
}
Log_Write_Mode(control_mode);
}
static void set_failsafe(boolean mode)
{
// only act on changes
// -------------------
if(failsafe != mode){
// store the value so we don't trip the gate twice
// -----------------------------------------------
failsafe = mode;
if (failsafe == false){
// We've regained radio contact
// ----------------------------
failsafe_off_event();
}else{
// We've lost radio contact
// ------------------------
failsafe_on_event();
}
}
}
static void
init_compass()
{
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
dcm.set_compass(&compass);
compass.init();
compass.get_offsets(); // load offsets to account for airframe magnetic interference
}
#ifdef OPTFLOW_ENABLED
static void
init_optflow()
{
if( optflow.init() == false ) {
g.optflow_enabled = false;
//SendDebug("\nFailed to Init OptFlow ");
}
optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft
optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view
}
#endif
static void
init_simple_bearing()
{
initial_simple_bearing = dcm.yaw_sensor;
}
static void
init_throttle_cruise()
{
// are we moving from manual throttle to auto_throttle?
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I();
#if FRAME_CONFIG == HELI_FRAME
g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in));
#else
g.throttle_cruise.set_and_save(g.rc_3.control_in);
#endif
}
}
#if CLI_ENABLED == ENABLED
#if BROKEN_SLIDER == 1
static boolean
check_startup_for_CLI()
{
//return true;
if((g.rc_4.radio_max) < 1600){
// CLI mode
return true;
}else if(abs(g.rc_4.control_in) > 3000){
// CLI mode
return true;
}else{
// startup to fly
return false;
}
}
#else
static boolean
check_startup_for_CLI()
{
return (digitalRead(SLIDE_SWITCH_PIN) == 0);
}
#endif // BROKEN_SLIDER
#endif // CLI_ENABLED
/*
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/ArduCopter/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_stabilize(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_tri(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_dcm(uint8_t argc, const Menu::arg *argv);
//static int8_t test_omega(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
//static int8_t test_nav(uint8_t argc, const Menu::arg *argv);
//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv);
//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv);
static int8_t test_tuning(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_baro(uint8_t argc, const Menu::arg *argv);
//static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
#ifdef OPTFLOW_ENABLED
static int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
#endif
//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_mission(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 printf that reads from flash memory
/*static int8_t help_test(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\n"
"Commands:\n"
" radio\n"
" servos\n"
" g_gps\n"
" imu\n"
" battery\n"
"\n"));
}*/
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"failsafe", test_failsafe},
// {"stabilize", test_stabilize},
{"gps", test_gps},
#if HIL_MODE != HIL_MODE_ATTITUDE
{"adc", test_adc},
#endif
{"imu", test_imu},
//{"dcm", test_dcm},
//{"omega", test_omega},
{"battery", test_battery},
{"tune", test_tuning},
{"tri", test_tri},
{"current", test_current},
{"relay", test_relay},
{"waypoints", test_wp},
//{"nav", test_nav},
#if HIL_MODE != HIL_MODE_ATTITUDE
{"altitude", test_baro},
#endif
{"sonar", test_sonar},
//{"compass", test_mag},
#ifdef OPTFLOW_ENABLED
{"optflow", test_optflow},
#endif
//{"xbee", test_xbee},
{"eedump", test_eedump},
{"rawgps", test_rawgps},
// {"mission", test_mission},
//{"reverse", test_reverse},
//{"wp", test_wp_nav},
};
// 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 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();
// servo Yaw
//APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
g.rc_4.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_tri(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();
g.rc_4.servo_out = g.rc_4.control_in;
g.rc_4.calc_pwm();
Serial.printf_P(PSTR("input: %d\toutput%d\n"),
g.rc_4.control_in,
g.rc_4.radio_out);
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
if(Serial.available() > 0){
return (0);
}
}
}
/*
static int8_t
test_nav(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(1000);
g_gps->ground_course = 19500;
calc_nav_rate2(g.waypoint_speed_max);
calc_nav_pitch_roll2();
g_gps->ground_course = 28500;
calc_nav_rate2(g.waypoint_speed_max);
calc_nav_pitch_roll2();
g_gps->ground_course = 1500;
calc_nav_rate2(g.waypoint_speed_max);
calc_nav_pitch_roll2();
g_gps->ground_course = 10500;
calc_nav_rate2(g.waypoint_speed_max);
calc_nav_pitch_roll2();
//if(Serial.available() > 0){
return (0);
//}
}
}
*/
static int8_t
test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
read_radio();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
g.rc_4.control_in,
g.rc_5.control_in,
g.rc_6.control_in,
g.rc_7.control_in);
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
/*Serial.printf_P(PSTR( "min: %d"
"\t in: %d"
"\t pwm_in: %d"
"\t sout: %d"
"\t pwm_out %d\n"),
g.rc_3.radio_min,
g.rc_3.control_in,
g.rc_3.radio_in,
g.rc_3.servo_out,
g.rc_3.pwm_out
);
*/
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv)
{
#if THROTTLE_FAILSAFE
byte fail_test;
print_hit_enter();
for(int i = 0; i < 50; i++){
delay(20);
read_radio();
}
oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(g.rc_3.control_in > 0){
delay(20);
read_radio();
}
while(1){
delay(20);
read_radio();
if(g.rc_3.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.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.rc_3.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.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 ACM.\n"));
return (0);
}
}
#else
return (0);
#endif
}
/*static int8_t
test_stabilize(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
print_hit_enter();
delay(1000);
// setup the radio
// ---------------
init_rc_in();
control_mode = STABILIZE;
Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
motor_auto_armed = false;
motor_armed = true;
while(1){
// 50 hz
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis();
G_Dt = (float)delta_ms_fast_loop / 1000.f;
if(g.compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
medium_loopCounter = 0;
}
}
// for trim features
read_trim_switch();
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
// IMU
// ---
read_AHRS();
// allow us to zero out sensors with control switches
if(g.rc_5.control_in < 600){
dcm.roll_sensor = dcm.pitch_sensor = 0;
}
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
// write out the servo PWM values
// ------------------------------
set_servos_4();
ts_num++;
if (ts_num > 10){
ts_num = 0;
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
(int)(dcm.roll_sensor/100),
(int)(dcm.pitch_sensor/100),
g.rc_1.pwm_out);
print_motor_out();
}
// R: 1417, L: 1453 F: 1453 B: 1417
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
if(Serial.available() > 0){
if(g.compass_enabled){
compass.save_offsets();
report_compass();
}
return (0);
}
}
}
}
*/
#if HIL_MODE != HIL_MODE_ATTITUDE
static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
Serial.printf_P(PSTR("ADC\n"));
delay(1000);
while(1){
for(int i = 0; i < 9; i++){
Serial.printf_P(PSTR("%u,"),adc.Ch(i));
}
Serial.println();
delay(20);
if(Serial.available() > 0){
return (0);
}
}
}
#endif
static int8_t
test_imu(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("Calibrating."));
//dcm.kp_yaw(0.02);
//dcm.ki_yaw(0);
report_imu();
imu.init_gyro();
report_imu();
print_hit_enter();
delay(1000);
//float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
fast_loopTimer = millis();
while(1){
//delay(20);
if (millis() - fast_loopTimer >= 5) {
// IMU
// ---
read_AHRS();
//Vector3f accels = imu.get_accel();
//Vector3f gyros = imu.get_gyro();
//Vector3f accel_filt = imu.get_accel_filtered();
//accels_rot = dcm.get_dcm_matrix() * accel_filt;
medium_loopCounter++;
if(medium_loopCounter == 4){
update_trig();
}
if(medium_loopCounter == 20){
//read_radio();
medium_loopCounter = 0;
//tuning();
//dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0);
/*
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld, kp:%1.4f, kp:%1.4f \n"),
dcm.roll_sensor,
dcm.pitch_sensor,
dcm.yaw_sensor,
dcm.kp_roll_pitch(),
(float)g.rc_6.control_in / 2000.0);
*/
Serial.printf_P(PSTR("%ld, %ld, %ld\n"),
dcm.roll_sensor,
dcm.pitch_sensor,
dcm.yaw_sensor);
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix());
}
}
// We are using the IMU
// ---------------------
/*
Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
"G: %4.4f, %4.4f, %4.4f\t"),
accels.x, accels.y, accels.z,
gyros.x, gyros.y, gyros.z);
*/
/*
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
cos_pitch_x,
sin_pitch_y,
cos_roll_x,
sin_roll_y,
cos_yaw_x, // x
sin_yaw_y); // y
//*/
//Log_Write_Raw();
}
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);
g_gps->new_data = false;
}else{
Serial.print(".");
}
if(Serial.available() > 0){
return (0);
}
}
}
/*
static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Gyro | Accel\n"));
Vector3f _cam_vector;
Vector3f _out_vector;
G_Dt = .02;
while(1){
for(byte i = 0; i <= 50; i++){
delay(20);
// IMU
// ---
read_AHRS();
}
Matrix3f temp = dcm.get_dcm_matrix();
Matrix3f temp_t = dcm.get_dcm_transposed();
Serial.printf_P(PSTR("dcm\n"
"%4.4f \t %4.4f \t %4.4f \n"
"%4.4f \t %4.4f \t %4.4f \n"
"%4.4f \t %4.4f \t %4.4f \n\n"),
temp.a.x, temp.a.y, temp.a.z,
temp.b.x, temp.b.y, temp.b.z,
temp.c.x, temp.c.y, temp.c.z);
int _pitch = degrees(-asin(temp.c.x));
int _roll = degrees(atan2(temp.c.y, temp.c.z));
int _yaw = degrees(atan2(temp.b.x, temp.a.x));
Serial.printf_P(PSTR( "angles\n"
"%d \t %d \t %d\n\n"),
_pitch,
_roll,
_yaw);
//_out_vector = _cam_vector * temp;
//Serial.printf_P(PSTR( "cam\n"
// "%d \t %d \t %d\n\n"),
// (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100);
if(Serial.available() > 0){
return (0);
}
}
}
*/
/*
static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Gyro | Accel\n"));
delay(1000);
while(1){
Vector3f accels = dcm.get_accel();
Serial.print("accels.z:");
Serial.print(accels.z);
Serial.print("omega.z:");
Serial.print(omega.z);
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
*/
/*static int8_t
test_omega(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
float old_yaw;
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Omega"));
delay(1000);
G_Dt = .02;
while(1){
delay(20);
// IMU
// ---
read_AHRS();
float my_oz = (dcm.yaw - old_yaw) * 50;
old_yaw = dcm.yaw;
ts_num++;
if (ts_num > 2){
ts_num = 0;
//Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz);
Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz);
}
if(Serial.available() > 0){
return (0);
}
}
return (0);
}
//*/
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
#if BATTERY_EVENT == 1
for (int i = 0; i < 20; i++){
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"));
#endif
return (0);
}
static int8_t
test_tuning(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
while(1){
delay(200);
read_radio();
tuning();
Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_current(uint8_t argc, const Menu::arg *argv)
{
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);
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
if(Serial.available() > 0){
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
Serial.printf_P(PSTR("Hold alt "));
if(g.RTL_altitude < 0){
Serial.printf_P(PSTR("\n"));
}else{
Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
}
Serial.printf_P(PSTR("%d wp\n"), (int)g.waypoint_total);
Serial.printf_P(PSTR("Hit rad: %d\n"), (int)g.waypoint_radius);
//Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
report_wp();
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);
}
}
}
/*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);
}
}
}
*/
#if HIL_MODE != HIL_MODE_ATTITUDE
static int8_t
test_baro(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
init_barometer();
while(1){
delay(100);
baro_alt = read_barometer();
Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt);
//Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2);
if(Serial.available() > 0){
return (0);
}
}
}
#endif
/*
static int8_t
test_mag(uint8_t argc, const Menu::arg *argv)
{
if(g.compass_enabled) {
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter();
while(1){
delay(100);
compass.read();
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
}
*/
/*
static int8_t
test_reverse(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
// ----------------------------------------------------------
g.rc_4.set_reverse(0);
g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
g.rc_4.servo_out = g.rc_4.control_in;
g.rc_4.calc_pwm();
Serial.printf_P(PSTR("PWM:%d input: %d\toutput%d "),
APM_RC.InputCh(CH_4),
g.rc_4.control_in,
g.rc_4.radio_out);
APM_RC.OutputCh(CH_6, g.rc_4.radio_out);
g.rc_4.set_reverse(1);
g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
g.rc_4.servo_out = g.rc_4.control_in;
g.rc_4.calc_pwm();
Serial.printf_P(PSTR("\trev input: %d\toutput%d\n"),
g.rc_4.control_in,
g.rc_4.radio_out);
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
if(Serial.available() > 0){
g.rc_4.set_reverse(0);
return (0);
}
}
}*/
/*
test the sonar
*/
static int8_t
test_sonar(uint8_t argc, const Menu::arg *argv)
{
if(g.sonar_enabled == false){
Serial.printf_P(PSTR("Sonar disabled\n"));
return (0);
}
print_hit_enter();
while(1) {
delay(100);
Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read());
//Serial.printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value);
if(Serial.available() > 0){
return (0);
}
}
return (0);
}
#ifdef OPTFLOW_ENABLED
static int8_t
test_optflow(uint8_t argc, const Menu::arg *argv)
{
///*
if(g.optflow_enabled) {
Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
print_hit_enter();
while(1){
delay(200);
optflow.read();
Log_Write_Optflow();
Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
optflow.x,
optflow.dx,
optflow.y,
optflow.dy,
optflow.surface_quality);
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("OptFlow: "));
print_enabled(false);
return (0);
}
}
#endif
/*
static int8_t
test_mission(uint8_t argc, const Menu::arg *argv)
{
//write out a basic mission to the EEPROM
//{
// uint8_t id; ///< command id
// uint8_t options; ///< options bitmask (1<<0 = relative altitude)
// uint8_t p1; ///< param 1
// int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100)
// int32_t lat; ///< param 3 - Lattitude * 10**7
// int32_t lng; ///< param 4 - Longitude * 10**7
//}
// clear home
{Location t = {0, 0, 0, 0, 0, 0};
set_command_with_index(t,0);}
// CMD opt pitch alt/cm
{Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0};
set_command_with_index(t,1);}
if (!strcmp_P(argv[1].str, PSTR("wp"))) {
// CMD opt
{Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0};
set_command_with_index(t,2);}
// CMD opt
{Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0};
set_command_with_index(t,3);}
// CMD opt
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
set_command_with_index(t,4);}
} else {
//2250 = 25 meteres
// CMD opt p1 //alt //NS //WE
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19
set_command_with_index(t,2);}
// CMD opt dir angle/deg deg/s relative
{Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
set_command_with_index(t,3);}
// CMD opt
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
set_command_with_index(t,4);}
}
g.RTL_altitude.set_and_save(300);
g.waypoint_total.set_and_save(4);
g.waypoint_radius.set_and_save(3);
test_wp(NULL, NULL);
return (0);
}
*/
static void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
}
/*
static void fake_out_gps()
{
static float rads;
g_gps->new_data = true;
g_gps->fix = true;
//int length = g.rc_6.control_in;
rads += .05;
if (rads > 6.28){
rads = 0;
}
g_gps->latitude = 377696000; // Y
g_gps->longitude = -1224319000; // X
g_gps->altitude = 9000; // meters * 100
//next_WP.lng = home.lng - length * sin(rads); // X
//next_WP.lat = home.lat + length * cos(rads); // Y
}
*/
static void print_motor_out(){
Serial.printf("out: R: %d, L: %d F: %d B: %d\n",
(motor_out[CH_1] - g.rc_3.radio_min),
(motor_out[CH_2] - g.rc_3.radio_min),
(motor_out[CH_3] - g.rc_3.radio_min),
(motor_out[CH_4] - g.rc_3.radio_min));
}
#endif // CLI_ENABLED
Reference in New Issue View Git Blame Copy Permalink