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