#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