diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp new file mode 100644 index 0000000000..f9166d0eac --- /dev/null +++ b/ArduCopter/ArduCopter.cpp @@ -0,0 +1,11568 @@ +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/ArduCopter.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define THISFIRMWARE "ArduCopter V2.0.50 Beta" +/* +ArduCopter Version 2.0 Beta +Authors: Jason Short +Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen +Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier + + +This firmware is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +Special Thanks for Contributors: + +Hein Hollander :Octo Support +Dani Saez :V Ocoto Support +Max Levine :Tri Support, Graphics +Jose Julio :Stabilization Control laws +Randy MacKay :Heli Support +Jani Hiriven :Testing feedback +Andrew Tridgell :Mavlink Support +James Goppert :Mavlink Support +Doug Weibel :Libraries +Mike Smith :Libraries, Coding support +HappyKillmore :Mavlink GCS +Michael Oborne :Mavlink GCS +Jack Dunkle :Alpha testing +Christof Schmid :Alpha testing +Oliver :Piezo support +Guntars :Arming safety suggestion + +And much more so PLEASE PM me on DIYDRONES to add your contribution to the List + +*/ + +//////////////////////////////////////////////////////////////////////////////// +// Header includes +//////////////////////////////////////////////////////////////////////////////// + +// AVR runtime +#include +#include +#include +#include + +// Libraries +#include +#include +#include // ArduPilot Mega RC Library +#include // ArduPilot GPS library +#include // Arduino I2C lib +#include // Arduino SPI lib +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega BMP085 Library +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega IMU Library +#include // ArduPilot Mega DCM Library +#include // PI library +#include // RC Channel Library +#include // Range finder library +#include // Optical Flow library +#include +#include // APM relay +#include // MAVLink GCS definitions +#include + +// Configuration +#include "defines.h" +#include "config.h" + +// Local modules +#include "Parameters.h" +#include "GCS.h" + +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +// +// Note that FastSerial port buffers are allocated at ::begin time, +// so there is not much of a penalty to defining ports that we don't +// use. +// +#line 1 "autogenerated" +#include "WProgram.h" + void setup() ; + void loop() ; + static void fast_loop() ; + static void medium_loop() ; + static void fifty_hz_loop() ; + static void slow_loop() ; + static void super_slow_loop() ; + static void update_GPS(void) ; + void update_yaw_mode(void) ; + void update_roll_pitch_mode(void) ; + void update_throttle_mode(void) ; + static void update_navigation() ; + static void read_AHRS(void) ; + static void update_trig(void); + static void update_altitude() ; + static void adjust_altitude() ; + static void tuning(); + static void update_nav_wp() ; + static void update_auto_yaw() ; + static int get_stabilize_roll(long target_angle) ; + static int get_stabilize_pitch(long target_angle) ; + static int get_stabilize_yaw(long target_angle) ; + static int get_nav_throttle(long z_error) ; + static int get_rate_roll(long target_rate) ; + static int get_rate_pitch(long target_rate) ; + static int get_rate_yaw(long target_rate) ; + static void reset_hold_I(void) ; + static void reset_nav(void) ; + static long get_nav_yaw_offset(int yaw_input, int reset) ; + static int alt_hold_velocity() ; + static int get_angle_boost(int value) ; + static void init_camera() ; + static void camera_stabilization() ; + void write_byte(byte val) ; + void write_int(int val ) ; + void write_float(float val) ; + void write_long(long val) ; + void flush(byte id) ; + static NOINLINE void send_heartbeat(mavlink_channel_t chan) ; + static NOINLINE void send_attitude(mavlink_channel_t chan) ; + static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) ; + static void NOINLINE send_meminfo(mavlink_channel_t chan) ; + static void NOINLINE send_location(mavlink_channel_t chan) ; + static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) ; + static void NOINLINE send_gps_raw(mavlink_channel_t chan) ; + static void NOINLINE send_servo_out(mavlink_channel_t chan) ; + static void NOINLINE send_radio_in(mavlink_channel_t chan) ; + static void NOINLINE send_radio_out(mavlink_channel_t chan) ; + static void NOINLINE send_vfr_hud(mavlink_channel_t chan) ; + static void NOINLINE send_raw_imu1(mavlink_channel_t chan) ; + static void NOINLINE send_raw_imu2(mavlink_channel_t chan) ; + static void NOINLINE send_raw_imu3(mavlink_channel_t chan) ; + static void NOINLINE send_gps_status(mavlink_channel_t chan) ; + static void NOINLINE send_current_waypoint(mavlink_channel_t chan) ; + static void NOINLINE send_statustext(mavlink_channel_t chan) ; + static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; + static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; + void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) ; + static void mavlink_delay(unsigned long t) ; + static void gcs_send_message(enum ap_message id) ; + static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) ; + static void gcs_update(void) ; + static void gcs_send_text(gcs_severity severity, const char *str) ; + static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) ; + static bool print_log_menu(void) ; + static void clear_header() ; + static byte get_num_logs(void) ; + static void start_new_log() ; + static void get_log_boundaries(byte log_num, int & start_page, int & end_page) ; + static int find_last_log_page(int bottom_page) ; + static void Log_Write_GPS() ; + static void Log_Read_GPS() ; + static void Log_Write_Raw() ; + static void Log_Read_Raw() ; + static void Log_Write_Current() ; + static void Log_Read_Current() ; + static void Log_Write_Motors() ; + static void Log_Read_Motors() ; + static void Log_Write_Optflow() ; + static void Log_Read_Optflow() ; + static void Log_Write_Nav_Tuning() ; + static void Log_Read_Nav_Tuning() ; + static void Log_Write_Control_Tuning() ; + static void Log_Read_Control_Tuning() ; + static void Log_Write_Performance() ; + static void Log_Read_Performance() ; + static void Log_Write_Cmd(byte num, struct Location *wp) ; + static void Log_Read_Cmd() ; + static void Log_Write_Attitude2() ; + static void Log_Read_Attitude2() ; + static void Log_Write_Attitude() ; + static void Log_Read_Attitude() ; + static void Log_Write_Mode(byte mode) ; + static void Log_Read_Mode() ; + static void Log_Write_Startup() ; + static void Log_Read_Startup() ; + static void Log_Read(int start_page, int end_page) ; + static void Log_Write_Startup() ; + static void Log_Read_Startup() ; + static void Log_Read(int start_page, int end_page) ; + static void Log_Write_Cmd(byte num, struct Location *wp) ; + static void Log_Write_Mode(byte mode) ; + static void start_new_log() ; + static void Log_Write_Raw() ; + static void Log_Write_GPS() ; + static void Log_Write_Current() ; + static void Log_Write_Attitude() ; + static void Log_Write_Optflow() ; + static void Log_Write_Nav_Tuning() ; + static void Log_Write_Control_Tuning() ; + static void Log_Write_Motors() ; + static void Log_Write_Performance() ; + void userhook_init() ; + void userhook_50Hz() ; + static void init_commands() ; + static void clear_command_queue(); + static struct Location get_command_with_index(int i) ; + static void set_command_with_index(struct Location temp, int i) ; + static void increment_WP_index() ; + static void decrement_WP_index() ; + static long read_alt_to_hold() ; + static Location get_LOITER_home_wp() ; + static void set_next_WP(struct Location *wp) ; + static void init_home() ; + static void handle_process_must() ; + static void handle_process_may() ; + static void handle_process_now() ; + static void handle_no_commands() ; + static bool verify_must() ; + static bool verify_may() ; + static void do_RTL(void) ; + static void do_takeoff() ; + static void do_nav_wp() ; + static void do_land() ; + static void do_loiter_unlimited() ; + static void do_loiter_turns() ; + static void do_loiter_time() ; + static bool verify_takeoff() ; + static bool verify_land() ; + static bool verify_nav_wp() ; + static bool verify_loiter_unlim() ; + static bool verify_loiter_time() ; + static bool verify_loiter_turns() ; + static bool verify_RTL() ; + static void do_wait_delay() ; + static void do_change_alt() ; + static void do_within_distance() ; + static void do_yaw() ; + static bool verify_wait_delay() ; + static bool verify_change_alt() ; + static bool verify_within_distance() ; + static bool verify_yaw() ; + static void do_change_speed() ; + static void do_target_yaw() ; + static void do_loiter_at_location() ; + static void do_jump() ; + static void do_set_home() ; + static void do_set_servo() ; + static void do_set_relay() ; + static void do_repeat_servo() ; + static void do_repeat_relay() ; + static void change_command(uint8_t index) ; + static void update_commands(void) ; + static void verify_commands(void) ; + static bool process_next_command() ; + static void process_must() ; + static void process_may() ; + static void process_now() ; + static void read_control_switch() ; + static byte readSwitch(void); + static void reset_control_switch() ; + static void read_trim_switch() ; + static void auto_trim() ; + static void trim_accel() ; + static void failsafe_on_event() ; + static void failsafe_off_event() ; + static void low_battery_event(void) ; + void piezo_on() ; + void piezo_off() ; + void piezo_beep() ; + void roll_flip() ; + static void heli_init_swash() ; + static void heli_move_servos_to_mid() ; + static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) ; + static void init_motors_out() ; + static void output_motors_armed() ; + static void output_motors_disarmed() ; + static void output_motor_test() ; + static int heli_get_scaled_throttle(int throttle) ; + static int heli_get_angle_boost(int pwm_out) ; +static void update_lights() ; + static void update_GPS_light(void) ; + static void update_motor_light(void) ; + static void dancing_light() ; + static void clear_leds() ; + static void update_motor_leds(void) ; + static void arm_motors() ; + static void set_servos_4() ; + static void init_motors_out() ; + static void output_motors_armed() ; + static void output_motors_disarmed() ; + static void output_motor_test() ; + static void init_motors_out() ; + static void output_motors_armed() ; + static void output_motors_disarmed() ; + static void output_motor_test() ; + static void init_motors_out() ; + static void output_motors_armed() ; + static void output_motors_disarmed() ; + static void output_motor_test() ; + static void init_motors_out() ; + static void output_motors_armed() ; + static void output_motors_disarmed() ; + static void debug_motors() ; + static void output_motor_test() ; + static void init_motors_out() ; + static void output_motors_armed() ; + static void output_motors_disarmed() ; + static void output_motor_test() ; + static void init_motors_out() ; + static void output_motors_armed() ; + static void output_motors_disarmed() ; + static void output_motor_test() ; + static byte navigate() ; + static bool check_missed_wp() ; + static void calc_location_error(struct Location *next_loc) ; + static void calc_loiter(int x_error, int y_error) ; + static void calc_loiter2(int x_error, int y_error) ; + static void calc_loiter_pitch_roll() ; + static void calc_nav_rate(int max_speed) ; + static void calc_nav_pitch_roll() ; + static long get_altitude_error() ; + static int get_loiter_angle() ; + static long wrap_360(long error) ; + static long wrap_180(long error) ; + static long get_crosstrack_correction(void) ; + static long cross_track_test() ; + static void reset_crosstrack() ; + static long get_distance(struct Location *loc1, struct Location *loc2) ; + static long get_alt_distance(struct Location *loc1, struct Location *loc2) ; + static long get_bearing(struct Location *loc1, struct Location *loc2) ; + static void default_dead_zones() ; + static void init_rc_in() ; + static void init_rc_out() ; + void output_min() ; + static void read_radio() ; + static void throttle_failsafe(uint16_t pwm) ; + static void trim_radio() ; + static void ReadSCP1000(void) ; + static void init_barometer(void) ; + static long read_baro_filtered(void) ; + static long read_barometer(void) ; + static void read_airspeed(void) ; + static void zero_airspeed(void) ; + static void read_battery(void) ; + static void clear_offsets() ; + static void report_batt_monitor() ; + static void report_sonar() ; + static void report_frame() ; + static void report_radio() ; + static void report_imu() ; + static void report_compass() ; + static void report_flight_modes() ; + void report_optflow() ; + static void report_heli() ; + static void report_gyro() ; + static void print_radio_values() ; + static void print_switch(byte p, byte m, bool b) ; + static void print_done() ; + static void zero_eeprom(void) ; + static void print_accel_offsets(void) ; + static void print_gyro_offsets(void) ; + static RC_Channel * heli_get_servo(int servo_num); + static int read_num_from_serial() ; + static void print_blanks(int num) ; + static void print_divider(void) ; + static void print_enabled(boolean b) ; + static void init_esc() ; + static void print_wp(struct Location *cmd, byte index) ; + static void report_gps() ; + static void report_version() ; + static void report_tuning() ; + static void run_cli(void) ; + static void init_ardupilot() ; + static void startup_ground(void) ; + static void set_mode(byte mode) ; + static void set_failsafe(boolean mode) ; + static void init_compass() ; + static void init_optflow() ; + static void init_simple_bearing() ; + static void init_throttle_cruise() ; + static boolean check_startup_for_CLI() ; + static boolean check_startup_for_CLI() ; + static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) ; + static void print_hit_enter() ; + static void fake_out_gps() ; + static void print_motor_out(); +#line 88 "/home/jgoppert/Projects/ardupilotone/ArduCopter/ArduCopter.pde" +FastSerialPort0(Serial); // FTDI/console +FastSerialPort1(Serial1); // GPS port +FastSerialPort3(Serial3); // Telemetry port + +//////////////////////////////////////////////////////////////////////////////// +// Parameters +//////////////////////////////////////////////////////////////////////////////// +// +// Global parameters are all contained within the 'g' class. +// +static Parameters g; + + +//////////////////////////////////////////////////////////////////////////////// +// prototypes +static void update_events(void); + + +//////////////////////////////////////////////////////////////////////////////// +// Sensors +//////////////////////////////////////////////////////////////////////////////// +// +// There are three basic options related to flight sensor selection. +// +// - Normal flight mode. Real sensors are used. +// - HIL Attitude mode. Most sensors are disabled, as the HIL +// protocol supplies attitude information directly. +// - HIL Sensors mode. Synthetic sensors are configured that +// supply data from the simulation. +// + +// All GPS access should be through this pointer. +static GPS *g_gps; + +// flight modes convenience array +static AP_Int8 *flight_modes = &g.flight_mode1; + +#if HIL_MODE == HIL_MODE_DISABLED + + // real sensors + AP_ADC_ADS7844 adc; + APM_BMP085_Class barometer; + AP_Compass_HMC5843 compass(Parameters::k_param_compass); + + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow; + #endif + + // real GPS selection + #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO + AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); + + #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA + AP_GPS_NMEA g_gps_driver(&Serial1); + + #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF + AP_GPS_SIRF g_gps_driver(&Serial1); + + #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX + AP_GPS_UBLOX g_gps_driver(&Serial1); + + #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK + AP_GPS_MTK g_gps_driver(&Serial1); + + #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 + AP_GPS_MTK16 g_gps_driver(&Serial1); + + #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE + AP_GPS_None g_gps_driver(NULL); + + #else + #error Unrecognised GPS_PROTOCOL setting. + #endif // GPS PROTOCOL + +#elif HIL_MODE == HIL_MODE_SENSORS + // sensor emulators + AP_ADC_HIL adc; + APM_BMP085_HIL_Class barometer; + AP_Compass_HIL compass; + AP_GPS_HIL g_gps_driver(NULL); + +#elif HIL_MODE == HIL_MODE_ATTITUDE + AP_ADC_HIL adc; + AP_DCM_HIL dcm; + AP_GPS_HIL g_gps_driver(NULL); + AP_Compass_HIL compass; // never used + AP_IMU_Shim imu; // never used + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow; + #endif + static int32_t gps_base_alt; +#else + #error Unrecognised HIL_MODE setting. +#endif // HIL MODE + +#if HIL_MODE != HIL_MODE_ATTITUDE + #if HIL_MODE != HIL_MODE_SENSORS + // Normal + AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); + #else + // hil imu + AP_IMU_Shim imu; + #endif + // normal dcm + AP_DCM dcm(&imu, g_gps); +#endif + +//////////////////////////////////////////////////////////////////////////////// +// GCS selection +//////////////////////////////////////////////////////////////////////////////// +GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0); +GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3); + +//////////////////////////////////////////////////////////////////////////////// +// SONAR selection +//////////////////////////////////////////////////////////////////////////////// +// +ModeFilter sonar_mode_filter; + +#if SONAR_TYPE == MAX_SONAR_XL + AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); +#else + #error Unrecognised SONAR_TYPE setting. +#endif + +// agmatthews USERHOOKS +//////////////////////////////////////////////////////////////////////////////// +// User variables +//////////////////////////////////////////////////////////////////////////////// +#ifdef USERHOOK_VARIABLES +#include USERHOOK_VARIABLES +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Global variables +//////////////////////////////////////////////////////////////////////////////// +static const char *comma = ","; + +static const char* flight_mode_strings[] = { + "STABILIZE", + "ACRO", + "ALT_HOLD", + "AUTO", + "GUIDED", + "LOITER", + "RTL", + "CIRCLE", + "POSITION"}; + +/* Radio values + Channel assignments + 1 Ailerons (rudder if no ailerons) + 2 Elevator + 3 Throttle + 4 Rudder (if we have ailerons) + 5 Mode - 3 position switch + 6 User assignable + 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second) + 8 TBD +*/ + +// test +#if ACCEL_ALT_HOLD == 1 +Vector3f accels_rot; +static int accels_rot_count; +static float accels_rot_sum; +static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN; +#endif + +// temp +static int y_actual_speed; +static int y_rate_error; + +// calc the +static int x_actual_speed; +static int x_rate_error; + +// Radio +// ----- +static byte control_mode = STABILIZE; +static byte old_control_mode = STABILIZE; +static byte oldSwitchPosition; // for remembering the control mode switch +static int motor_out[8]; +static bool do_simple = false; + +// Heli +// ---- +#if FRAME_CONFIG == HELI_FRAME +static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos +static int heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly +static long heli_servo_out[4]; // used for servo averaging for analog servos +static int heli_servo_out_count = 0; // use for servo averaging +#endif + +// Failsafe +// -------- +static boolean failsafe; // did our throttle dip below the failsafe value? +static boolean ch3_failsafe; +static boolean motor_armed; +static boolean motor_auto_armed; // if true, + +// PIDs +// ---- +static Vector3f omega; +float tuning_value; + +// LED output +// ---------- +static boolean motor_light; // status of the Motor safety +static boolean GPS_light; // status of the GPS light +static byte led_mode = NORMAL_LEDS; + +// GPS variables +// ------------- +static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage +static float scaleLongUp = 1; // used to reverse longitude scaling +static float scaleLongDown = 1; // used to reverse longitude scaling +static byte ground_start_count = 10; // have we achieved first lock and set Home? +static bool did_ground_start = false; // have we ground started after first arming + +// Location & Navigation +// --------------------- +static const float radius_of_earth = 6378100; // meters +static const float gravity = 9.81; // meters/ sec^2 +static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target + +static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate +static byte wp_control; // used to control - navgation or loiter + +static byte command_must_index; // current command memory location +static byte command_may_index; // current command memory location +static byte command_must_ID; // current command ID +static byte command_may_ID; // current command ID +static byte wp_verify_byte; // used for tracking state of navigating waypoints + +static float cos_roll_x = 1; +static float cos_pitch_x = 1; +static float cos_yaw_x = 1; +static float sin_pitch_y, sin_yaw_y, sin_roll_y; +static long initial_simple_bearing; // used for Simple mode +static float simple_sin_y, simple_cos_x; +static byte jump = -10; // used to track loops in jump command +static int waypoint_speed_gov; + +// Acro +#if CH7_OPTION == CH7_FLIP +static bool do_flip = false; +#endif + +static boolean trim_flag; +static int CH7_wp_index = 0; + +// Airspeed +// -------- +static int airspeed; // m/s * 100 + +// Location Errors +// --------------- +static long altitude_error; // meters * 100 we are off in altitude +static long old_altitude; +static int old_rate; +static long yaw_error; // how off are we pointed +static long long_error, lat_error; // temp for debugging + +// Battery Sensors +// --------------- +static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter +static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter +static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter +static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter +static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter + +static float current_amps; +static float current_total; +static bool low_batt = false; + +// Barometer Sensor variables +// -------------------------- +static long abs_pressure; +static long ground_pressure; +static int ground_temperature; + +// Altitude Sensor variables +// ---------------------- +static int sonar_alt; +static int baro_alt; +static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR +static int altitude_rate; + +// flight mode specific +// -------------------- +static byte yaw_mode; +static byte roll_pitch_mode; +static byte throttle_mode; + +static boolean takeoff_complete; // Flag for using take-off controls +static boolean land_complete; +static long old_alt; // used for managing altitude rates +static int velocity_land; +static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target +static int manual_boost; // used in adjust altitude to make changing alt faster +static int angle_boost; // used in adjust altitude to make changing alt faster + +// Loiter management +// ----------------- +static long original_target_bearing; // deg * 100, used to check we are not passing the WP +static long old_target_bearing; // used to track difference in angle + +static int loiter_total; // deg : how many times to loiter * 360 +static int loiter_sum; // deg : how far we have turned around a waypoint +static unsigned long loiter_time; // millis : when we started LOITER mode +static unsigned loiter_time_max; // millis : how long to stay in LOITER mode + + +// these are the values for navigation control functions +// ---------------------------------------------------- +static long nav_roll; // deg * 100 : target roll angle +static long nav_pitch; // deg * 100 : target pitch angle +static long nav_yaw; // deg * 100 : target yaw angle +static long auto_yaw; // deg * 100 : target yaw angle +static long nav_lat; // for error calcs +static long nav_lon; // for error calcs +static int nav_throttle; // 0-1000 for throttle control + +static unsigned long throttle_integrator; // used to integrate throttle output to predict battery life +static bool invalid_throttle; // used to control when we calculate nav_throttle +//static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value + +static long command_yaw_start; // what angle were we to begin with +static unsigned long command_yaw_start_time; // when did we start turning +static unsigned int command_yaw_time; // how long we are turning +static long command_yaw_end; // what angle are we trying to be +static long command_yaw_delta; // how many degrees will we turn +static int command_yaw_speed; // how fast to turn +static byte command_yaw_dir; +static byte command_yaw_relative; + +static int auto_level_counter; + +// Waypoints +// --------- +static long wp_distance; // meters - distance between plane and next waypoint +static long wp_totalDistance; // meters - distance between old and next waypoint +//static byte next_wp_index; // Current active command index + +// repeating event control +// ----------------------- +static byte event_id; // what to do - see defines +static unsigned long event_timer; // when the event was asked for in ms +static unsigned int event_delay; // how long to delay the next firing of event in millis +static int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice +static int event_value; // per command value, such as PWM for servos +static int event_undo_value; // the value used to undo commands +//static byte repeat_forever; +//static byte undo_event; // counter for timing the undo + +// delay command +// -------------- +static long condition_value; // used in condition commands (eg delay, change alt, etc.) +static long condition_start; +//static int condition_rate; + +// land command +// ------------ +static long land_start; // when we intiated command in millis() +static long original_alt; // altitide reference for start of command + +// 3D Location vectors +// ------------------- +static struct Location home; // home location +static struct Location prev_WP; // last waypoint +static struct Location current_loc; // current location +static struct Location next_WP; // next waypoint +static struct Location target_WP; // where do we want to you towards? +static struct Location next_command; // command preloaded +static struct Location guided_WP; // guided mode waypoint +static long target_altitude; // used for +static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location + +// IMU variables +// ------------- +static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) + +// Performance monitoring +// ---------------------- +static long perf_mon_timer; +//static float imu_health; // Metric based on accel gain deweighting +static int gps_fix_count; +static byte gps_watchdog; + +// System Timers +// -------------- +static unsigned long fast_loopTimer; // Time in miliseconds of main control loop +static byte medium_loopCounter; // Counters for branching from main control loop to slower loops + +static unsigned long fiftyhz_loopTimer; + +static byte slow_loopCounter; +static int superslow_loopCounter; +static byte simple_timer; // for limiting the execution of flight mode thingys + + +static float dTnav; // Delta Time in milliseconds for navigation computations +static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav + +static byte counter_one_herz; +static bool GPS_enabled = false; +static bool new_radio_frame; + +AP_Relay relay; + +//////////////////////////////////////////////////////////////////////////////// +// Top-level logic +//////////////////////////////////////////////////////////////////////////////// + +void setup() { + memcheck_init(); + init_ardupilot(); +} + +void loop() +{ + long timer = micros(); + // We want this to execute fast + // ---------------------------- + if ((timer - fast_loopTimer) >= 4000) { + //PORTK |= B00010000; + G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops + fast_loopTimer = timer; + + // Execute the fast loop + // --------------------- + fast_loop(); + } + //PORTK &= B11101111; + + if ((timer - fiftyhz_loopTimer) >= 20000) { + fiftyhz_loopTimer = timer; + //PORTK |= B01000000; + + // reads all of the necessary trig functions for cameras, throttle, etc. + update_trig(); + + // perform 10hz tasks + medium_loop(); + + // Stuff to run at full 50hz, but after the loops + fifty_hz_loop(); + + counter_one_herz++; + + if(counter_one_herz == 50){ + super_slow_loop(); + counter_one_herz = 0; + } + + if (millis() - perf_mon_timer > 1200 /*20000*/) { + if (g.log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); + + gps_fix_count = 0; + perf_mon_timer = millis(); + } + //PORTK &= B10111111; + } +} +// PORTK |= B01000000; +// PORTK &= B10111111; + +// Main loop +static void fast_loop() +{ + // try to send any deferred messages if the serial port now has + // some space available + gcs_send_message(MSG_RETRY_DEFERRED); + + // Read radio + // ---------- + read_radio(); + + // IMU DCM Algorithm + read_AHRS(); + + // custom code/exceptions for flight modes + // --------------------------------------- + update_yaw_mode(); + update_roll_pitch_mode(); + + // write out the servo PWM values + // ------------------------------ + set_servos_4(); + + //if(motor_armed) + //Log_Write_Attitude(); + +// agmatthews - USERHOOKS +#ifdef USERHOOK_FASTLOOP + USERHOOK_FASTLOOP +#endif + +} + +static void medium_loop() +{ + // This is the start of the medium (10 Hz) loop pieces + // ----------------------------------------- + switch(medium_loopCounter) { + + // This case deals with the GPS and Compass + //----------------------------------------- + case 0: + medium_loopCounter++; + + #ifdef OPTFLOW_ENABLED + if(g.optflow_enabled){ + optflow.read(); + optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + + // write to log + if (g.log_bitmask & MASK_LOG_OPTFLOW){ + Log_Write_Optflow(); + } + } + #endif + + if(GPS_enabled){ + update_GPS(); + } + + //readCommands(); + + #if HIL_MODE != HIL_MODE_ATTITUDE + if(g.compass_enabled){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + } + #endif + + // auto_trim, uses an auto_level algorithm + auto_trim(); + + // record throttle output + // ------------------------------ + throttle_integrator += g.rc_3.servo_out; + break; + + // This case performs some navigation computations + //------------------------------------------------ + case 1: + medium_loopCounter++; + + // Auto control modes: + if(g_gps->new_data && g_gps->fix){ + + // invalidate GPS data + g_gps->new_data = false; + + // we are not tracking I term on navigation, so this isn't needed + dTnav = (float)(millis() - nav_loopTimer)/ 1000.0; + nav_loopTimer = millis(); + + // prevent runup from bad GPS + dTnav = min(dTnav, 1.0); + + // calculate the copter's desired bearing and WP distance + // ------------------------------------------------------ + if(navigate()){ + + // control mode specific updates + // ----------------------------- + update_navigation(); + + if (g.log_bitmask & MASK_LOG_NTUN) + Log_Write_Nav_Tuning(); + } + }else{ + g_gps->new_data = false; + } + break; + + // command processing + //------------------- + case 2: + medium_loopCounter++; + + // Read altitude from sensors + // -------------------------- + update_altitude(); + + // invalidate the throttle hold value + // ---------------------------------- + invalid_throttle = true; + + break; + + // This case deals with sending high rate telemetry + //------------------------------------------------- + case 3: + medium_loopCounter++; + + // perform next command + // -------------------- + if(control_mode == AUTO){ + update_commands(); + } + + #if HIL_MODE != HIL_MODE_ATTITUDE + if(motor_armed){ + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) + Log_Write_Attitude(); + + if (g.log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); + } + #endif + + // send all requested output streams with rates requested + // between 5 and 45 Hz + gcs_data_stream_send(5,45); + + if (g.log_bitmask & MASK_LOG_MOTORS) + Log_Write_Motors(); + + break; + + // This case controls the slow loop + //--------------------------------- + case 4: + medium_loopCounter = 0; + + if (g.battery_monitoring != 0){ + read_battery(); + } + + // Accel trims = hold > 2 seconds + // Throttle cruise = switch less than 1 second + // -------------------------------------------- + read_trim_switch(); + + // Check for engine arming + // ----------------------- + arm_motors(); + + + slow_loop(); + break; + + default: + // this is just a catch all + // ------------------------ + medium_loopCounter = 0; + break; + } +// agmatthews - USERHOOKS +#ifdef USERHOOK_MEDIUMLOOP + USERHOOK_MEDIUMLOOP +#endif + +} + +// stuff that happens at 50 hz +// --------------------------- +static void fifty_hz_loop() +{ + // moved to slower loop + // -------------------- + update_throttle_mode(); + + // Read Sonar + // ---------- + if(g.sonar_enabled){ + sonar_alt = sonar.read(); + } + // agmatthews - USERHOOKS + #ifdef USERHOOK_50HZLOOP + USERHOOK_50HZLOOP + #endif + + #if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME + // HIL for a copter needs very fast update of the servo values + gcs_send_message(MSG_RADIO_OUT); + #endif + + camera_stabilization(); + + # if HIL_MODE == HIL_MODE_DISABLED + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) + Log_Write_Attitude(); + + if (g.log_bitmask & MASK_LOG_RAW) + Log_Write_Raw(); + #endif + + // kick the GCS to process uplink data + gcs_update(); + gcs_data_stream_send(45,1000); + + #if FRAME_CONFIG == TRI_FRAME + // servo Yaw + g.rc_4.calc_pwm(); + APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + #endif +} + + +static void slow_loop() +{ + // This is the slow (3 1/3 Hz) loop pieces + //---------------------------------------- + switch (slow_loopCounter){ + case 0: + slow_loopCounter++; + superslow_loopCounter++; + + if(superslow_loopCounter > 1200){ + #if HIL_MODE != HIL_MODE_ATTITUDE + if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled){ + compass.save_offsets(); + superslow_loopCounter = 0; + } + #endif + } + break; + + case 1: + slow_loopCounter++; + + // Read 3-position switch on radio + // ------------------------------- + read_control_switch(); + + // Read main battery voltage if hooked up - does not read the 5v from radio + // ------------------------------------------------------------------------ + //#if BATTERY_EVENT == 1 + // read_battery(); + //#endif + + #if AUTO_RESET_LOITER == 1 + if(control_mode == LOITER){ + //if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ + // reset LOITER to current position + //next_WP = current_loc; + //} + } + #endif + + break; + + case 2: + slow_loopCounter = 0; + update_events(); + + // blink if we are armed + update_lights(); + + // send all requested output streams with rates requested + // between 1 and 5 Hz + gcs_data_stream_send(1,5); + + if(g.radio_tuning > 0) + tuning(); + + #if MOTOR_LEDS == 1 + update_motor_leds(); + #endif + + break; + + default: + slow_loopCounter = 0; + break; + + } + // agmatthews - USERHOOKS + #ifdef USERHOOK_SLOWLOOP + USERHOOK_SLOWLOOP + #endif + +} + +// 1Hz loop +static void super_slow_loop() +{ + if (g.log_bitmask & MASK_LOG_CUR) + Log_Write_Current(); + + gcs_send_message(MSG_HEARTBEAT); + // agmatthews - USERHOOKS + #ifdef USERHOOK_SUPERSLOWLOOP + USERHOOK_SUPERSLOWLOOP + #endif +} + +static void update_GPS(void) +{ + g_gps->update(); + update_GPS_light(); + + //current_loc.lng = 377697000; // Lon * 10 * *7 + //current_loc.lat = -1224318000; // Lat * 10 * *7 + //current_loc.alt = 100; // alt * 10 * *7 + //return; + if(gps_watchdog < 12){ + gps_watchdog++; + }else{ + // we have lost GPS signal for a moment. Reduce our error to avoid flyaways + // commented temporarily + //nav_roll >>= 1; + //nav_pitch >>= 1; + } + + if (g_gps->new_data && g_gps->fix) { + gps_watchdog = 0; + + // for performance + // --------------- + gps_fix_count++; + + if(ground_start_count > 1){ + ground_start_count--; + + } else if (ground_start_count == 1) { + + // We countdown N number of good GPS fixes + // so that the altitude is more accurate + // ------------------------------------- + if (current_loc.lat == 0) { + ground_start_count = 5; + + }else{ + init_home(); + ground_start_count = 0; + } + } + + current_loc.lng = g_gps->longitude; // Lon * 10 * *7 + current_loc.lat = g_gps->latitude; // Lat * 10 * *7 + + if (g.log_bitmask & MASK_LOG_GPS){ + Log_Write_GPS(); + } + } +} + + +void update_yaw_mode(void) +{ + switch(yaw_mode){ + case YAW_ACRO: + g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in); + return; + break; + + case YAW_HOLD: + // calcualte new nav_yaw offset + if (control_mode <= STABILIZE){ + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); + }else{ + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); + } + break; + + case YAW_LOOK_AT_HOME: + //nav_yaw updated in update_navigation() + break; + + case YAW_AUTO: + nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -20, 20); + nav_yaw = wrap_360(nav_yaw); + break; + } + + // Yaw control + g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + + //Serial.printf("4: %d\n",g.rc_4.servo_out); +} + +void update_roll_pitch_mode(void) +{ + #if CH7_OPTION == CH7_FLIP + if (do_flip){ + roll_flip(); + return; + } + #endif + + int control_roll = 0, control_pitch = 0; + + //read_radio(); + if(do_simple && new_radio_frame){ + new_radio_frame = false; + simple_timer++; + + int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + + if (simple_timer == 1){ + // roll + simple_cos_x = sin(radians(90 - delta)); + + }else if (simple_timer > 2){ + // pitch + simple_sin_y = cos(radians(90 - delta)); + simple_timer = 0; + } + + // Rotate input by the initial bearing + control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; + control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); + + g.rc_1.control_in = control_roll; + g.rc_2.control_in = control_pitch; + } + + switch(roll_pitch_mode){ + case ROLL_PITCH_ACRO: + g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); + break; + + case ROLL_PITCH_STABLE: + g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); + break; + + case ROLL_PITCH_AUTO: + // mix in user control with Nav control + control_roll = g.rc_1.control_mix(nav_roll); + control_pitch = g.rc_2.control_mix(nav_pitch); + g.rc_1.servo_out = get_stabilize_roll(control_roll); + g.rc_2.servo_out = get_stabilize_pitch(control_pitch); + break; + } +} + +// 50 hz update rate, not 250 +void update_throttle_mode(void) +{ + switch(throttle_mode){ + + case THROTTLE_MANUAL: + if (g.rc_3.control_in > 0){ + #if FRAME_CONFIG == HELI_FRAME + g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.rc_3.control_in)); + #else + angle_boost = get_angle_boost(g.rc_3.control_in); + g.rc_3.servo_out = g.rc_3.control_in + angle_boost; + #endif + }else{ + g.pi_stabilize_roll.reset_I(); + g.pi_stabilize_pitch.reset_I(); + g.pi_rate_roll.reset_I(); + g.pi_rate_pitch.reset_I(); + g.rc_3.servo_out = 0; + } + break; + + case THROTTLE_HOLD: + // allow interactive changing of atitude + adjust_altitude(); + // fall through + + case THROTTLE_AUTO: + // 10hz, don't run up i term + if(invalid_throttle && motor_auto_armed == true){ + + // how far off are we + altitude_error = get_altitude_error(); + + // get the AP throttle + nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s + //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); + + // clear the new data flag + invalid_throttle = false; + } + #if FRAME_CONFIG == HELI_FRAME + g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle); + #else + angle_boost = get_angle_boost(g.throttle_cruise); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost; + #endif + break; + } +} + +// called after a GPS read +static void update_navigation() +{ + // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS + // ------------------------------------------------------------------------ + switch(control_mode){ + case AUTO: + verify_commands(); + // note: wp_control is handled by commands_logic + + // calculates desired Yaw + update_auto_yaw(); + + // calculates the desired Roll and Pitch + update_nav_wp(); + break; + + case GUIDED: + wp_control = WP_MODE; + // check if we are close to point > loiter + wp_verify_byte = 0; + verify_nav_wp(); + + if (wp_control == WP_MODE) { + update_auto_yaw(); + } else { + set_mode(LOITER); + } + update_nav_wp(); + break; + + case RTL: + if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ + // lets just jump to Loiter Mode after RTL + set_mode(LOITER); + }else{ + // calculates desired Yaw + // XXX this is an experiment + #if FRAME_CONFIG == HELI_FRAME + update_auto_yaw(); + #endif + + wp_control = WP_MODE; + } + + // calculates the desired Roll and Pitch + update_nav_wp(); + break; + + // switch passthrough to LOITER + case LOITER: + case POSITION: + wp_control = LOITER_MODE; + + // calculates the desired Roll and Pitch + update_nav_wp(); + break; + + case CIRCLE: + yaw_tracking = MAV_ROI_WPNEXT; + wp_control = CIRCLE_MODE; + + // calculates desired Yaw + update_auto_yaw(); + update_nav_wp(); + break; + + } + + if(yaw_mode == YAW_LOOK_AT_HOME){ + if(home_is_set){ + //nav_yaw = point_at_home_yaw(); + nav_yaw = get_bearing(¤t_loc, &home); + } else { + nav_yaw = 0; + } + } +} + +static void read_AHRS(void) +{ + // Perform IMU calculations and get attitude info + //----------------------------------------------- + #if HIL_MODE == HIL_MODE_SENSORS + // update hil before dcm update + gcs_update(); + #endif + + dcm.update_DCM_fast(); + omega = dcm.get_gyro(); +} + +static void update_trig(void){ + Vector2f yawvector; + Matrix3f temp = dcm.get_dcm_matrix(); + + yawvector.x = temp.a.x; // sin + yawvector.y = temp.b.x; // cos + yawvector.normalize(); + + + sin_pitch_y = -temp.c.x; + cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x)); + + cos_roll_x = temp.c.z / cos_pitch_x; + sin_roll_y = temp.c.y / cos_pitch_x; + + cos_yaw_x = yawvector.y; // 0 x = north + sin_yaw_y = yawvector.x; // 1 y + + //flat: + // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, + // 90° = cos_yaw: 1.00, sin_yaw: 0.00, + // 180 = cos_yaw: 0.00, sin_yaw: -1.00, + // 270 = cos_yaw: -1.00, sin_yaw: 0.00, +} + +// updated at 10hz +static void update_altitude() +{ + altitude_sensor = BARO; + + #if HIL_MODE == HIL_MODE_ATTITUDE + current_loc.alt = g_gps->altitude - gps_base_alt; + return; + #else + + if(g.sonar_enabled){ + // filter out offset + float scale; + + // read barometer + baro_alt = read_barometer(); + + if(baro_alt < 1000){ + + #if SONAR_TILT_CORRECTION == 1 + // correct alt for angle of the sonar + float temp = cos_pitch_x * cos_roll_x; + temp = max(temp, 0.707); + sonar_alt = (float)sonar_alt * temp; + #endif + + scale = (sonar_alt - 400) / 200; + scale = constrain(scale, 0, 1); + current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; + }else{ + current_loc.alt = baro_alt + home.alt; + } + + }else{ + baro_alt = read_barometer(); + // no sonar altitude + current_loc.alt = baro_alt + home.alt; + } + + // calc the accel rate limit to 2m/s + altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer + + // rate limiter to reduce some of the motor pulsing + if (altitude_rate > 0){ + // going up + altitude_rate = min(altitude_rate, old_rate + 20); + }else{ + // going down + altitude_rate = max(altitude_rate, old_rate - 20); + } + + old_rate = altitude_rate; + old_altitude = current_loc.alt; + #endif +} + +static void +adjust_altitude() +{ + if(g.rc_3.control_in <= 200){ + next_WP.alt -= 1; // 1 meter per second + next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location + next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter + //manual_boost = (g.rc_3.control_in == 0) ? -20 : 0; + + }else if (g.rc_3.control_in > 700){ + next_WP.alt += 1; // 1 meter per second + next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location + //manual_boost = (g.rc_3.control_in == 800) ? 20 : 0; + } +} + +static void tuning(){ + tuning_value = (float)g.rc_6.control_in / 1000.0; + + switch(g.radio_tuning){ + + /*case CH6_STABILIZE_KP: + g.rc_6.set_range(0,2000); // 0 to 8 + tuning_value = (float)g.rc_6.control_in / 100.0; + alt_hold_gain = tuning_value; + break;*/ + + case CH6_STABILIZE_KP: + g.rc_6.set_range(0,8000); // 0 to 8 + g.pi_stabilize_roll.kP(tuning_value); + g.pi_stabilize_pitch.kP(tuning_value); + break; + + case CH6_STABILIZE_KI: + g.rc_6.set_range(0,300); // 0 to .3 + tuning_value = (float)g.rc_6.control_in / 1000.0; + g.pi_stabilize_roll.kI(tuning_value); + g.pi_stabilize_pitch.kI(tuning_value); + break; + + case CH6_RATE_KP: + g.rc_6.set_range(0,300); // 0 to .3 + g.pi_rate_roll.kP(tuning_value); + g.pi_rate_pitch.kP(tuning_value); + break; + + case CH6_RATE_KI: + g.rc_6.set_range(0,300); // 0 to .3 + g.pi_rate_roll.kI(tuning_value); + g.pi_rate_pitch.kI(tuning_value); + break; + + case CH6_YAW_KP: + g.rc_6.set_range(0,1000); + g.pi_stabilize_yaw.kP(tuning_value); + break; + + case CH6_YAW_RATE_KP: + g.rc_6.set_range(0,1000); + g.pi_rate_yaw.kP(tuning_value); + break; + + case CH6_THROTTLE_KP: + g.rc_6.set_range(0,1000); + g.pi_throttle.kP(tuning_value); + break; + + case CH6_TOP_BOTTOM_RATIO: + g.rc_6.set_range(800,1000); // .8 to 1 + g.top_bottom_ratio = tuning_value; + break; + + case CH6_RELAY: + g.rc_6.set_range(0,1000); + if (g.rc_6.control_in > 525) relay.on(); + if (g.rc_6.control_in < 475) relay.off(); + break; + + case CH6_TRAVERSE_SPEED: + g.rc_6.set_range(0,1000); + g.waypoint_speed_max = g.rc_6.control_in; + break; + + case CH6_LOITER_P: + g.rc_6.set_range(0,1000); + g.pi_loiter_lat.kP(tuning_value); + g.pi_loiter_lon.kP(tuning_value); + break; + + case CH6_NAV_P: + g.rc_6.set_range(0,6000); + g.pi_nav_lat.kP(tuning_value); + g.pi_nav_lon.kP(tuning_value); + break; + + #if FRAME_CONFIG == HELI_FRAME + case CH6_HELI_EXTERNAL_GYRO: + g.rc_6.set_range(1000,2000); + g.heli_ext_gyro_gain = tuning_value * 1000; + break; + #endif + } +} + +static void update_nav_wp() +{ + if(wp_control == LOITER_MODE){ + + // calc a pitch to the target + calc_location_error(&next_WP); + + // use error as the desired rate towards the target + calc_loiter(long_error, lat_error); + + // rotate pitch and roll to the copter frame of reference + calc_loiter_pitch_roll(); + + }else if(wp_control == CIRCLE_MODE){ + + // check if we have missed the WP + int loiter_delta = (target_bearing - old_target_bearing)/100; + + // reset the old value + old_target_bearing = target_bearing; + + // wrap values + if (loiter_delta > 180) loiter_delta -= 360; + if (loiter_delta < -180) loiter_delta += 360; + + // sum the angle around the WP + loiter_sum += loiter_delta; + + // create a virtual waypoint that circles the next_WP + // Count the degrees we have circulated the WP + int circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100; + + target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle))); + target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle))); + + // calc the lat and long error to the target + calc_location_error(&target_WP); + + // use error as the desired rate towards the target + // nav_lon, nav_lat is calculated + calc_loiter(long_error, lat_error); + + // rotate pitch and roll to the copter frame of reference + calc_loiter_pitch_roll(); + + } else { + // use error as the desired rate towards the target + calc_nav_rate(g.waypoint_speed_max); + // rotate pitch and roll to the copter frame of reference + calc_nav_pitch_roll(); + } +} + +static void update_auto_yaw() +{ + // this tracks a location so the copter is always pointing towards it. + if(yaw_tracking == MAV_ROI_LOCATION){ + auto_yaw = get_bearing(¤t_loc, &target_WP); + + }else if(yaw_tracking == MAV_ROI_WPNEXT){ + auto_yaw = target_bearing; + } + // MAV_ROI_NONE = basic Yaw hold +} + + + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/Attitude.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +static int +get_stabilize_roll(long target_angle) +{ + long error; + long rate; + + error = wrap_180(target_angle - dcm.roll_sensor); + + // limit the error we're feeding to the PID + error = constrain(error, -2500, 2500); + + // desired Rate: + rate = g.pi_stabilize_roll.get_pi(error, G_Dt); + //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); + +#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters + // Rate P: + error = rate - (long)(degrees(omega.x) * 100.0); + rate = g.pi_rate_roll.get_pi(error, G_Dt); + //Serial.printf("%d\t%d\n", (int)error, (int)rate); +#endif + + // output control: + return (int)constrain(rate, -2500, 2500); +} + +static int +get_stabilize_pitch(long target_angle) +{ + long error; + long rate; + + error = wrap_180(target_angle - dcm.pitch_sensor); + + // limit the error we're feeding to the PID + error = constrain(error, -2500, 2500); + + // desired Rate: + rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); + //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); + +#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters + // Rate P: + error = rate - (long)(degrees(omega.y) * 100.0); + rate = g.pi_rate_pitch.get_pi(error, G_Dt); + //Serial.printf("%d\t%d\n", (int)error, (int)rate); +#endif + + // output control: + return (int)constrain(rate, -2500, 2500); +} + + +#define YAW_ERROR_MAX 2000 +static int +get_stabilize_yaw(long target_angle) +{ + long error; + long rate; + + yaw_error = wrap_180(target_angle - dcm.yaw_sensor); + + // limit the error we're feeding to the PID + yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX); + rate = g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt); + //Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate); + +#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters + if( ! g.heli_ext_gyro_enabled ) { + // Rate P: + error = rate - (long)(degrees(omega.z) * 100.0); + rate = g.pi_rate_yaw.get_pi(error, G_Dt); + } +#else + // Rate P: + error = rate - (long)(degrees(omega.z) * 100.0); + rate = g.pi_rate_yaw.get_pi(error, G_Dt); + //Serial.printf("%d\t%d\n", (int)error, (int)rate); +#endif + + // output control: + return (int)constrain(rate, -2500, 2500); +} + +#define ALT_ERROR_MAX 500 +static int +get_nav_throttle(long z_error) +{ + // limit error to prevent I term run up + z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); + int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 + + rate_error = rate_error - altitude_rate; + + // limit the rate + rate_error = constrain(rate_error, -120, 140); + return (int)g.pi_throttle.get_pi(rate_error, .1); +} + +static int +get_rate_roll(long target_rate) +{ + long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0); + return g.pi_acro_roll.get_pi(error, G_Dt); +} + +static int +get_rate_pitch(long target_rate) +{ + long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0); + return g.pi_acro_pitch.get_pi(error, G_Dt); +} + +static int +get_rate_yaw(long target_rate) +{ + long error; + error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0); + target_rate = g.pi_rate_yaw.get_pi(error, G_Dt); + + // output control: + return (int)constrain(target_rate, -2500, 2500); +} + + +// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. +// Keeps outdated data out of our calculations +static void reset_hold_I(void) +{ + g.pi_loiter_lat.reset_I(); + g.pi_loiter_lon.reset_I(); + g.pi_crosstrack.reset_I(); +} + +// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. +// Keeps outdated data out of our calculations +static void reset_nav(void) +{ + nav_throttle = 0; + invalid_throttle = true; + + g.pi_nav_lat.reset_I(); + g.pi_nav_lon.reset_I(); + + long_error = 0; + lat_error = 0; +} + + +/************************************************************* +throttle control +****************************************************************/ + +static long +get_nav_yaw_offset(int yaw_input, int reset) +{ + long _yaw; + + if(reset == 0){ + // we are on the ground + return dcm.yaw_sensor; + + }else{ + // re-define nav_yaw if we have stick input + if(yaw_input != 0){ + // set nav_yaw + or - the current location + _yaw = (long)yaw_input + dcm.yaw_sensor; + // we need to wrap our value so we can be 0 to 360 (*100) + return wrap_360(_yaw); + + }else{ + // no stick input, lets not change nav_yaw + return nav_yaw; + } + } +} + +static int alt_hold_velocity() +{ + #if ACCEL_ALT_HOLD == 1 + Vector3f accel_filt; + float error; + + // subtract filtered Accel + error = abs(next_WP.alt - current_loc.alt) - 25; + error = min(error, 50.0); + error = max(error, 0.0); + error = 1 - (error/ 50.0); + + accel_filt = imu.get_accel_filtered(); + accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); + int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12 + + //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); + return constrain(output, -70, 70); + +// fast rise +//s: -17.6241, g:0.0000, e:1.0000, o:0 +//s: -18.4990, g:0.0000, e:1.0000, o:0 +//s: -19.3193, g:0.0000, e:1.0000, o:0 +//s: -13.1310, g:47.8700, e:1.0000, o:-158 + + #else + return 0; + #endif +} + +static int get_angle_boost(int value) +{ + float temp = cos_pitch_x * cos_roll_x; + temp = 1.0 - constrain(temp, .5, 1.0); + return (int)(temp * value); +} + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/Camera.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +//#if CAMERA_STABILIZER == ENABLED + +static void init_camera() +{ + // ch 6 high(right) is down. + g.rc_camera_pitch.set_angle(4500); + g.rc_camera_roll.set_angle(4500); + + g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW); + g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW); +} + +static void +camera_stabilization() +{ + // PITCH + // ----- + // allow control mixing + g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. + g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor); + + g.rc_camera_pitch.servo_out = (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain; + + // limit + //g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500); + + // dont allow control mixing + /* + g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1; + */ + + + // ROLL + // ----- + // allow control mixing + /* + g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. + g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-dcm.roll_sensor); + */ + + // dont allow control mixing + g.rc_camera_roll.servo_out = (float)-dcm.roll_sensor * g.camera_roll_gain; + + // limit + //g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500); + + // Output + // ------ + g.rc_camera_pitch.calc_pwm(); + g.rc_camera_roll.calc_pwm(); + + APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out); + APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out); + //Serial.printf("c:%d\n", g.rc_camera_pitch.radio_out); +} + +//#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/GCS.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* +GCS Protocol + +4 Ardupilot Header +D +5 Payload length +1 Message ID +1 Message Version +9 Payload byte 1 +8 Payload byte 2 +7 Payload byte 3 +A Checksum byte 1 +B Checksum byte 2 + +*/ + +/* +#if GCS_PORT == 3 +# define SendSerw Serial3.write +# define SendSer Serial3.print +#else +# define SendSerw Serial.write +# define SendSer Serial.print +#endif + +byte mess_buffer[60]; +byte buff_pointer; + +// Unions for getting byte values +union f_out{ + byte bytes[4]; + float value; +} floatOut; + +union i_out { + byte bytes[2]; + int value; +} intOut; + +union l_out{ + byte bytes[4]; + long value; +} longOut; + +// Add binary values to the buffer +void write_byte(byte val) +{ + mess_buffer[buff_pointer++] = val; +} + +void write_int(int val ) +{ + intOut.value = val; + mess_buffer[buff_pointer++] = intOut.bytes[0]; + mess_buffer[buff_pointer++] = intOut.bytes[1]; +} + +void write_float(float val) +{ + floatOut.value = val; + mess_buffer[buff_pointer++] = floatOut.bytes[0]; + mess_buffer[buff_pointer++] = floatOut.bytes[1]; + mess_buffer[buff_pointer++] = floatOut.bytes[2]; + mess_buffer[buff_pointer++] = floatOut.bytes[3]; +} + +void write_long(long val) +{ + longOut.value = val; + mess_buffer[buff_pointer++] = longOut.bytes[0]; + mess_buffer[buff_pointer++] = longOut.bytes[1]; + mess_buffer[buff_pointer++] = longOut.bytes[2]; + mess_buffer[buff_pointer++] = longOut.bytes[3]; +} + +void flush(byte id) +{ + byte mess_ck_a = 0; + byte mess_ck_b = 0; + byte i; + + SendSer("4D"); // This is the message preamble + SendSerw(buff_pointer); // Length + SendSerw(2); // id + + for (i = 0; i < buff_pointer; i++) { + SendSerw(mess_buffer[i]); + } + + buff_pointer = 0; +} +*/ +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/GCS_Mavlink.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// use this to prevent recursion during sensor init +static bool in_mavlink_delay; + + +// this costs us 51 bytes, but means that low priority +// messages don't block the CPU +static mavlink_statustext_t pending_status; + +// true when we have received at least 1 MAVLink packet +static bool mavlink_active; + + +// check if a message will fit in the payload space available +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + mavlink_msg_attitude_send( + chan, + micros(), + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); +} + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_HOLD; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; + default: + mode = control_mode + 100; + } + + uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + 0, + battery_voltage * 1000, + battery_remaining, + packet_drops); +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + target_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + 0, + 0); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + + #if FRAME_CONFIG == HELI_FRAME + + mavlink_msg_rc_channels_scaled_send( + chan, + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, + g.rc_4.servo_out, + 0, + 0, + 0, + 0, + rssi); + + #else + + mavlink_msg_rc_channels_scaled_send( + chan, + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, + g.rc_4.servo_out, + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), + rssi); + + #endif +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.rc_1.radio_in, + g.rc_2.radio_in, + g.rc_3.radio_in, + g.rc_4.radio_in, + g.rc_5.radio_in, + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + motor_out[0], + motor_out[1], + motor_out[2], + motor_out[3], + motor_out[4], + motor_out[5], + motor_out[6], + motor_out[7]); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + g.rc_3.servo_out/10, + current_loc.alt / 100.0, + climb_rate); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + Vector3f mag_offsets = compass.get_offsets(); + + mavlink_msg_sensor_offsets_send(chan, + mag_offsets.x, + mag_offsets.y, + mag_offsets.z, + compass.get_declination(), + barometer.RawPress, + barometer.RawTemp, + imu.gx(), imu.gy(), imu.gz(), + imu.ax(), imu.ay(), imu.az()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); +} + +static void NOINLINE send_statustext(mavlink_channel_t chan) +{ + mavlink_msg_statustext_send( + chan, + pending_status.severity, + pending_status.text); +} + + +// try to send a message, return false if it won't fit in the serial tx buffer +static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) +{ + int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; + + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // defer any messages on the telemetry port for 1 second after + // bootup, to try to prevent bricking of Xbees + return false; + } + + switch(id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + return true; + + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; + + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; + + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; + + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; + + case MSG_NAV_CONTROLLER_OUTPUT: + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); + break; + + case MSG_GPS_RAW: + CHECK_PAYLOAD_SIZE(GPS_RAW); + send_gps_raw(chan); + break; + + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; + + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; + + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; + + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; + +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; + + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; + + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE + + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; + + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; + + case MSG_NEXT_PARAM: + CHECK_PAYLOAD_SIZE(PARAM_VALUE); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_param_send(); + } else { + gcs3.queued_param_send(); + } + break; + + case MSG_NEXT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_waypoint_send(); + } else { + gcs3.queued_waypoint_send(); + } + break; + + case MSG_STATUSTEXT: + CHECK_PAYLOAD_SIZE(STATUSTEXT); + send_statustext(chan); + break; + + case MSG_RETRY_DEFERRED: + break; // just here to prevent a warning + } + return true; +} + + +#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED +static struct mavlink_queue { + enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES]; + uint8_t next_deferred_message; + uint8_t num_deferred_messages; +} mavlink_queue[2]; + +// send a message using mavlink +static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) +{ + uint8_t i, nextid; + struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; + + // see if we can send the deferred messages, if any + while (q->num_deferred_messages != 0) { + if (!mavlink_try_send_message(chan, + q->deferred_messages[q->next_deferred_message], + packet_drops)) { + break; + } + q->next_deferred_message++; + if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { + q->next_deferred_message = 0; + } + q->num_deferred_messages--; + } + + if (id == MSG_RETRY_DEFERRED) { + return; + } + + // this message id might already be deferred + for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { + if (q->deferred_messages[nextid] == id) { + // its already deferred, discard + return; + } + nextid++; + if (nextid == MAX_DEFERRED_MESSAGES) { + nextid = 0; + } + } + + if (q->num_deferred_messages != 0 || + !mavlink_try_send_message(chan, id, packet_drops)) { + // can't send it now, so defer it + if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { + // the defer buffer is full, discard + return; + } + nextid = q->next_deferred_message + q->num_deferred_messages; + if (nextid >= MAX_DEFERRED_MESSAGES) { + nextid -= MAX_DEFERRED_MESSAGES; + } + q->deferred_messages[nextid] = id; + q->num_deferred_messages++; + } +} + +void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) +{ + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // don't send status MAVLink messages for 2 seconds after + // bootup, to try to prevent Xbee bricking + return; + } + + if (severity == SEVERITY_LOW) { + // send via the deferred queuing system + pending_status.severity = (uint8_t)severity; + strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); + mavlink_send_message(chan, MSG_STATUSTEXT, 0); + } else { + // send immediately + mavlink_msg_statustext_send( + chan, + severity, + (const int8_t*) str); +} +} + + +GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : +packet_drops(0), + + +// parameters +// note, all values not explicitly initialised here are zeroed +waypoint_send_timeout(1000), // 1 second +waypoint_receive_timeout(1000), // 1 second + +// stream rates +_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), + // AP_VAR //ref //index, default, name + streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), + streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), + streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), + streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), + streamRatePosition (&_group, 4, 0, PSTR("POSITION")), + streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), + streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), + streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) +{ + +} + +void +GCS_MAVLINK::init(FastSerial * port) +{ + GCS_Class::init(port); + if (port == &Serial) { + mavlink_comm_0_port = port; + chan = MAVLINK_COMM_0; + }else{ + mavlink_comm_1_port = port; + chan = MAVLINK_COMM_1; + } + _queued_parameter = NULL; +} + +void +GCS_MAVLINK::update(void) +{ + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + status.packet_rx_drop_count = 0; + + // process received bytes + while(comm_get_available(chan)) + { + uint8_t c = comm_receive_ch(chan); + +#if CLI_ENABLED == ENABLED + /* allow CLI to be started by hitting enter 3 times, if no + heartbeat packets have been received */ + if (mavlink_active == false) { + if (c == '\n' || c == '\r') { + crlf_count++; + } else { + crlf_count = 0; + } + if (crlf_count == 3) { + run_cli(); + } + } +#endif + + // Try to get a new message + if (mavlink_parse_char(chan, c, &msg, &status)) { + mavlink_active = true; + handleMessage(&msg); + } + } + + // Update packet drops counter + packet_drops += status.packet_rx_drop_count; + + // send out queued params/ waypoints + if (NULL != _queued_parameter) { + send_message(MSG_NEXT_PARAM); + } + + if (waypoint_receiving && + waypoint_request_i <= (unsigned)g.waypoint_total) { + send_message(MSG_NEXT_WAYPOINT); + } + + // stop waypoint sending if timeout + if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ + waypoint_sending = false; + } + + // stop waypoint receiving if timeout + if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ + waypoint_receiving = false; + } +} + +void +GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) +{ + if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { + + if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ + send_message(MSG_RAW_IMU1); + send_message(MSG_RAW_IMU2); + send_message(MSG_RAW_IMU3); + //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); + } + + if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { + send_message(MSG_EXTENDED_STATUS1); + send_message(MSG_EXTENDED_STATUS2); + send_message(MSG_GPS_STATUS); + send_message(MSG_CURRENT_WAYPOINT); + send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working + send_message(MSG_NAV_CONTROLLER_OUTPUT); + //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); + } + + if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { + // sent with GPS read +#if HIL_MODE == HIL_MODE_ATTITUDE + send_message(MSG_LOCATION); +#endif + //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); + } + + if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { + // This is used for HIL. Do not change without discussing with HIL maintainers + send_message(MSG_SERVO_OUT); + //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); + } + + if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { + send_message(MSG_RADIO_OUT); + send_message(MSG_RADIO_IN); + //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); + } + + if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info + send_message(MSG_ATTITUDE); + //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); + } + + if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info + send_message(MSG_VFR_HUD); + //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); + } + + if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ + // Available datastream + //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); + } + } +} + +void +GCS_MAVLINK::send_message(enum ap_message id) +{ + mavlink_send_message(chan,id, packet_drops); +} + +void +GCS_MAVLINK::send_text(gcs_severity severity, const char *str) +{ + mavlink_send_text(chan,severity,str); +} + +void +GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) +{ + mavlink_statustext_t m; + uint8_t i; + for (i=0; imsgid) { + + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + { + // decode + mavlink_request_data_stream_t packet; + mavlink_msg_request_data_stream_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + int freq = 0; // packet frequency + + if (packet.start_stop == 0) + freq = 0; // stop sending + else if (packet.start_stop == 1) + freq = packet.req_message_rate; // start sending + else + break; + + switch(packet.req_stream_id){ + + case MAV_DATA_STREAM_ALL: + streamRateRawSensors = freq; + streamRateExtendedStatus = freq; + streamRateRCChannels = freq; + streamRateRawController = freq; + streamRatePosition = freq; + streamRateExtra1 = freq; + streamRateExtra2 = freq; + //streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. + streamRateExtra3 = freq; // Don't save!! + break; + + case MAV_DATA_STREAM_RAW_SENSORS: + streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly + // we will not continue to broadcast raw sensor data at 50Hz. + break; + case MAV_DATA_STREAM_EXTENDED_STATUS: + //streamRateExtendedStatus.set_and_save(freq); + streamRateExtendedStatus = freq; + break; + + case MAV_DATA_STREAM_RC_CHANNELS: + streamRateRCChannels = freq; + break; + + case MAV_DATA_STREAM_RAW_CONTROLLER: + streamRateRawController = freq; + break; + + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; + + case MAV_DATA_STREAM_POSITION: + streamRatePosition = freq; + break; + + case MAV_DATA_STREAM_EXTRA1: + streamRateExtra1 = freq; + break; + + case MAV_DATA_STREAM_EXTRA2: + streamRateExtra2 = freq; + break; + + case MAV_DATA_STREAM_EXTRA3: + streamRateExtra3 = freq; + break; + + default: + break; + } + break; + } + + case MAVLINK_MSG_ID_ACTION: + { + // decode + mavlink_action_t packet; + mavlink_msg_action_decode(msg, &packet); + if (mavlink_check_target(packet.target,packet.target_component)) break; + + if (in_mavlink_delay) { + // don't execute action commands while in sensor + // initialisation + break; + } + + uint8_t result = 0; + + // do action + send_text(SEVERITY_LOW,PSTR("action received: ")); +//Serial.println(packet.action); + switch(packet.action){ + + case MAV_ACTION_LAUNCH: + //set_mode(TAKEOFF); + break; + + case MAV_ACTION_RETURN: + set_mode(RTL); + result=1; + break; + + case MAV_ACTION_EMCY_LAND: + //set_mode(LAND); + break; + + case MAV_ACTION_HALT: + do_loiter_at_location(); + result=1; + break; + + /* No mappable implementation in APM 2.0 + case MAV_ACTION_MOTORS_START: + case MAV_ACTION_CONFIRM_KILL: + case MAV_ACTION_EMCY_KILL: + case MAV_ACTION_MOTORS_STOP: + case MAV_ACTION_SHUTDOWN: + break; + */ + + case MAV_ACTION_CONTINUE: + process_next_command(); + result=1; + break; + + case MAV_ACTION_SET_MANUAL: + set_mode(STABILIZE); + result=1; + break; + + case MAV_ACTION_SET_AUTO: + set_mode(AUTO); + result=1; + break; + + case MAV_ACTION_STORAGE_READ: + AP_Var::load_all(); + result=1; + break; + + case MAV_ACTION_STORAGE_WRITE: + AP_Var::save_all(); + result=1; + break; + + case MAV_ACTION_CALIBRATE_RC: break; + trim_radio(); + result=1; + break; + + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_PRESSURE: + break; + + case MAV_ACTION_CALIBRATE_ACC: + imu.init_accel(mavlink_delay); + result=1; + break; + + + //case MAV_ACTION_REBOOT: // this is a rough interpretation + //startup_IMU_ground(); + //result=1; + // break; + + /* For future implemtation + case MAV_ACTION_REC_START: break; + case MAV_ACTION_REC_PAUSE: break; + case MAV_ACTION_REC_STOP: break; + */ + + /* Takeoff is not an implemented flight mode in APM 2.0 + case MAV_ACTION_TAKEOFF: + set_mode(TAKEOFF); + break; + */ + + case MAV_ACTION_NAVIGATE: + set_mode(AUTO); + result=1; + break; + + /* Land is not an implemented flight mode in APM 2.0 + case MAV_ACTION_LAND: + set_mode(LAND); + break; + */ + + case MAV_ACTION_LOITER: + set_mode(LOITER); + result=1; + break; + + default: break; + } + + mavlink_msg_action_ack_send( + chan, + packet.action, + result + ); + + break; + } + + case MAVLINK_MSG_ID_SET_MODE: + { + // decode + mavlink_set_mode_t packet; + mavlink_msg_set_mode_decode(msg, &packet); + + switch(packet.mode){ + + case MAV_MODE_MANUAL: + set_mode(STABILIZE); + break; + + case MAV_MODE_GUIDED: + set_mode(GUIDED); + break; + + case MAV_MODE_AUTO: + if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); + if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); + if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); + mav_nav = 255; + break; + + case MAV_MODE_TEST1: + set_mode(STABILIZE); + break; + } + } + + /*case MAVLINK_MSG_ID_SET_NAV_MODE: + { + // decode + mavlink_set_nav_mode_t packet; + mavlink_msg_set_nav_mode_decode(msg, &packet); + // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message + mav_nav = packet.nav_mode; + break; + } + */ + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); + + // decode + mavlink_waypoint_request_list_t packet; + mavlink_msg_waypoint_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // Start sending waypoints + mavlink_msg_waypoint_count_send( + chan,msg->sysid, + msg->compid, + g.waypoint_total + 1); // + home + + waypoint_timelast_send = millis(); + waypoint_sending = true; + waypoint_receiving = false; + waypoint_dest_sysid = msg->sysid; + waypoint_dest_compid = msg->compid; + break; + } + + // XXX read a WP from EEPROM and send it to the GCS + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); + + // Check if sending waypiont + //if (!waypoint_sending) break; + // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW + + // decode + mavlink_waypoint_request_t packet; + mavlink_msg_waypoint_request_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // send waypoint + tell_command = get_command_with_index(packet.seq); + + // set frame of waypoint + uint8_t frame; + + if (tell_command.options & WP_OPTION_ALT_RELATIVE) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame + } else { + frame = MAV_FRAME_GLOBAL; // reference frame + } + + float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; + + // time that the mav should loiter in milliseconds + uint8_t current = 0; // 1 (true), 0 (false) + + if (packet.seq == (uint16_t)g.waypoint_index) + current = 1; + + uint8_t autocontinue = 1; // 1 (true), 0 (false) + + float x = 0, y = 0, z = 0; + + if (tell_command.id < MAV_CMD_NAV_LAST) { + // command needs scaling + x = tell_command.lat/1.0e7; // local (x), global (latitude) + y = tell_command.lng/1.0e7; // local (y), global (longitude) + // ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM + z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) + } + + + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_CONDITION_CHANGE_ALT: + case MAV_CMD_DO_SET_HOME: + param1 = tell_command.p1; + break; + + case MAV_CMD_NAV_TAKEOFF: + param1 = 0; + break; + + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1; // ACM loiter time is in 1 second increments + break; + + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; + + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; + + case MAV_CMD_DO_REPEAT_SERVO: + param4 = tell_command.lng; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + param3 = tell_command.lat; + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + + case MAV_CMD_NAV_WAYPOINT: + param1 = tell_command.p1; + break; + + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + } + + mavlink_msg_waypoint_send(chan,msg->sysid, + msg->compid, + packet.seq, + frame, + tell_command.id, + current, + autocontinue, + param1, + param2, + param3, + param4, + x, + y, + z); + + // update last waypoint comm stamp + waypoint_timelast_send = millis(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint ack")); + + // decode + mavlink_waypoint_ack_t packet; + mavlink_msg_waypoint_ack_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // turn off waypoint send + waypoint_sending = false; + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + { + //send_text_P(SEVERITY_LOW,PSTR("param request list")); + + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // Start sending parameters - next call to ::update will kick the first one out + + _queued_parameter = AP_Var::first(); + _queued_parameter_index = 0; + _queued_parameter_count = _count_parameters(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all")); + + // decode + mavlink_waypoint_clear_all_t packet; + mavlink_msg_waypoint_clear_all_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; + + // clear all waypoints + uint8_t type = 0; // ok (0), error(1) + g.waypoint_total.set_and_save(0); + + // send acknowledgement 3 times to makes sure it is received + for (int i=0;i<3;i++) + mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type); + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint set current")); + + // decode + mavlink_waypoint_set_current_t packet; + mavlink_msg_waypoint_set_current_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // set current command + change_command(packet.seq); + + mavlink_msg_waypoint_current_send(chan, g.waypoint_index); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint count")); + + // decode + mavlink_waypoint_count_t packet; + mavlink_msg_waypoint_count_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // start waypoint receiving + if (packet.count > MAX_WAYPOINTS) { + packet.count = MAX_WAYPOINTS; + } + g.waypoint_total.set_and_save(packet.count - 1); + + waypoint_timelast_receive = millis(); + waypoint_receiving = true; + waypoint_sending = false; + waypoint_request_i = 0; + break; + } + +#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS + case MAVLINK_MSG_ID_SET_MAG_OFFSETS: + { + mavlink_set_mag_offsets_t packet; + mavlink_msg_set_mag_offsets_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); + break; + } +#endif + + // XXX receive a WP from GCS and store in EEPROM + case MAVLINK_MSG_ID_WAYPOINT: + { + // decode + mavlink_waypoint_t packet; + mavlink_msg_waypoint_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // defaults + tell_command.id = packet.command; + + /* + switch (packet.frame){ + + case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: + { + tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.options = 0; // absolute altitude + break; + } + + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } + //case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + default: + { + tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + break; + } + } + */ + + // we only are supporting Abs position, relative Alt + tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2; + tell_command.options = 1; // store altitude relative!! Always!! + + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_SET_ROI: + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_NAV_TAKEOFF: + tell_command.p1 = 0; + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + tell_command.p1 = packet.param1 * 100; + break; + + case MAV_CMD_NAV_LOITER_TIME: + tell_command.p1 = packet.param1; // APM loiter time is in ten second increments + break; + + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + tell_command.lat = packet.param1; + break; + + case MAV_CMD_DO_JUMP: + tell_command.lat = packet.param2; + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_DO_REPEAT_SERVO: + tell_command.lng = packet.param4; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + tell_command.lat = packet.param3; + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_NAV_WAYPOINT: + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; + break; + } + + if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission + guided_WP = tell_command; + + // add home alt if needed + if (guided_WP.options & WP_OPTION_ALT_RELATIVE){ + guided_WP.alt += home.alt; + } + + set_mode(GUIDED); + + // make any new wp uploaded instant (in case we are already in Guided mode) + set_next_WP(&guided_WP); + + // verify we recevied the command + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + 0); + + } else { + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) break; + + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) break; + set_command_with_index(tell_command, packet.seq); + + // update waypoint receiving state machine + waypoint_timelast_receive = millis(); + waypoint_request_i++; + + if (waypoint_request_i > (uint16_t)g.waypoint_total){ + uint8_t type = 0; // ok (0), error(1) + + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + type); + + send_text(SEVERITY_LOW,PSTR("flight plan received")); + waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: + { + AP_Var *vp; + AP_Meta_class::Type_id var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[ONBOARD_PARAM_NAME_LENGTH+1]; + strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); + key[ONBOARD_PARAM_NAME_LENGTH] = 0; + + // find the requested parameter + vp = AP_Var::find(key); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + float rounding_addition = 0.01; + + // fetch the variable type ID + var_type = vp->meta_type_id(); + + // handle variables with standard type IDs + if (var_type == AP_Var::k_typeid_float) { + ((AP_Float *)vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_float16) { + ((AP_Float16 *)vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int32) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int16) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int8) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type + mavlink_msg_param_value_send( + chan, + (int8_t *)key, + vp->cast_to_float(), + _count_parameters(), + -1); // XXX we don't actually know what its index is... + } + + break; + } // end case + + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + { + // allow override of RC channel values for HIL + // or for complete GCS control of switch position + // and RC PWM values. + if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_rc_channels_override_t packet; + int16_t v[8]; + mavlink_msg_rc_channels_override_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system,packet.target_component)) + break; + + v[0] = packet.chan1_raw; + v[1] = packet.chan2_raw; + v[2] = packet.chan3_raw; + v[3] = packet.chan4_raw; + v[4] = packet.chan5_raw; + v[5] = packet.chan6_raw; + v[6] = packet.chan7_raw; + v[7] = packet.chan8_raw; + APM_RC.setHIL(v); + break; + } + +#if HIL_MODE != HIL_MODE_DISABLED + // This is used both as a sensor and to pass the location + // in HIL_ATTITUDE mode. + case MAVLINK_MSG_ID_GPS_RAW: + { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + packet.v,packet.hdg,0,0); + if (gps_base_alt == 0) { + gps_base_alt = packet.alt*100; + } + current_loc.lng = packet.lon * T7; + current_loc.lat = packet.lat * T7; + current_loc.alt = g_gps->altitude - gps_base_alt; + if (!home_is_set) { + init_home(); + } + break; + } +#if HIL_MODE == HIL_MODE_ATTITUDE + case MAVLINK_MSG_ID_ATTITUDE: + { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + break; + } +#endif +#endif +/* + case MAVLINK_MSG_ID_HEARTBEAT: + { + // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + if(msg->sysid != g.sysid_my_gcs) break; + rc_override_fs_timer = millis(); + //pmTest1++; + break; + } + + #if HIL_MODE != HIL_MODE_DISABLED + // This is used both as a sensor and to pass the location + // in HIL_ATTITUDE mode. + case MAVLINK_MSG_ID_GPS_RAW: + { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + packet.v,packet.hdg,0,0); + break; + } + + // Is this resolved? - MAVLink protocol change..... + case MAVLINK_MSG_ID_VFR_HUD: + { + // decode + mavlink_vfr_hud_t packet; + mavlink_msg_vfr_hud_decode(msg, &packet); + + // set airspeed + airspeed = 100*packet.airspeed; + break; + } + +#endif +#if HIL_MODE == HIL_MODE_ATTITUDE + case MAVLINK_MSG_ID_ATTITUDE: + { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + break; + } +#endif +*/ +#if HIL_MODE == HIL_MODE_SENSORS + + case MAVLINK_MSG_ID_RAW_IMU: + { + // decode + mavlink_raw_imu_t packet; + mavlink_msg_raw_imu_decode(msg, &packet); + + // set imu hil sensors + // TODO: check scaling for temp/absPress + float temp = 70; + float absPress = 1; + // Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); + // Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); + + // rad/sec + Vector3f gyros; + gyros.x = (float)packet.xgyro / 1000.0; + gyros.y = (float)packet.ygyro / 1000.0; + gyros.z = (float)packet.zgyro / 1000.0; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; + + imu.set_gyro(gyros); + + imu.set_accel(accels); + + compass.setHIL(packet.xmag,packet.ymag,packet.zmag); + break; + } + + case MAVLINK_MSG_ID_RAW_PRESSURE: + { + // decode + mavlink_raw_pressure_t packet; + mavlink_msg_raw_pressure_decode(msg, &packet); + + // set pressure hil sensor + // TODO: check scaling + float temp = 70; + barometer.setHIL(temp,packet.press_diff1); + break; + } +#endif // HIL_MODE + } // end switch +} // end handle mavlink + +uint16_t +GCS_MAVLINK::_count_parameters() +{ + // if we haven't cached the parameter count yet... + if (0 == _parameter_count) { + AP_Var *vp; + + vp = AP_Var::first(); + do { + // if a parameter responds to cast_to_float then we are going to be able to report it + if (!isnan(vp->cast_to_float())) { + _parameter_count++; + } + } while (NULL != (vp = vp->next())); + } + return _parameter_count; +} + +AP_Var * +GCS_MAVLINK::_find_parameter(uint16_t index) +{ + AP_Var *vp; + + vp = AP_Var::first(); + while (NULL != vp) { + + // if the parameter is reportable + if (!(isnan(vp->cast_to_float()))) { + // if we have counted down to the index we want + if (0 == index) { + // return the parameter + return vp; + } + // count off this parameter, as it is reportable but not + // the one we want + index--; + } + // and move to the next parameter + vp = vp->next(); + } + return NULL; +} + +/** +* @brief Send the next pending parameter, called from deferred message +* handling code +*/ +void +GCS_MAVLINK::queued_param_send() +{ + // Check to see if we are sending parameters + if (NULL == _queued_parameter) return; + + AP_Var *vp; + float value; + + // copy the current parameter and prepare to move to the next + vp = _queued_parameter; + _queued_parameter = _queued_parameter->next(); + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK + vp->copy_name(param_name, sizeof(param_name)); + + mavlink_msg_param_value_send( + chan, + (int8_t*)param_name, + value, + _queued_parameter_count, + _queued_parameter_index); + + _queued_parameter_index++; + } +} + +/** +* @brief Send the next pending waypoint, called from deferred message +* handling code +*/ +void +GCS_MAVLINK::queued_waypoint_send() +{ + if (waypoint_receiving && + waypoint_request_i <= (unsigned)g.waypoint_total) { + mavlink_msg_waypoint_request_send( + chan, + waypoint_dest_sysid, + waypoint_dest_compid, + waypoint_request_i); + } +} + +/* + a delay() callback that processes MAVLink packets. We set this as the + callback in long running library initialisation routines to allow + MAVLink to process packets while waiting for the initialisation to + complete +*/ +static void mavlink_delay(unsigned long t) +{ + unsigned long tstart; + static unsigned long last_1hz, last_50hz; + + if (in_mavlink_delay) { + // this should never happen, but let's not tempt fate by + // letting the stack grow too much + delay(t); + return; + } + + in_mavlink_delay = true; + + tstart = millis(); + do { + unsigned long tnow = millis(); + if (tnow - last_1hz > 1000) { + last_1hz = tnow; + gcs_send_message(MSG_HEARTBEAT); + gcs_send_message(MSG_EXTENDED_STATUS1); + } + if (tnow - last_50hz > 20) { + last_50hz = tnow; + gcs_update(); + } + delay(1); + } while (millis() - tstart < t); + + in_mavlink_delay = false; +} + +/* + send a message on both GCS links + */ +static void gcs_send_message(enum ap_message id) +{ + gcs0.send_message(id); + gcs3.send_message(id); +} + +/* + send data streams in the given rate range on both links + */ +static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) +{ + gcs0.data_stream_send(freqMin, freqMax); + gcs3.data_stream_send(freqMin, freqMax); +} + +/* + look for incoming commands on the GCS links + */ +static void gcs_update(void) +{ + gcs0.update(); + gcs3.update(); +} + +static void gcs_send_text(gcs_severity severity, const char *str) +{ + gcs0.send_text(severity, str); + gcs3.send_text(severity, str); +} + +static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) +{ + gcs0.send_text(severity, str); + gcs3.send_text(severity, str); +} + +/* + send a low priority formatted message to the GCS + only one fits in the queue, so if you send more than one before the + last one gets into the serial buffer then the old one will be lost + */ +static void gcs_send_text_fmt(const prog_char_t *fmt, ...) +{ + char fmtstr[40]; + va_list ap; + uint8_t i; + for (i=0; i" + " erase (all logs)\n" + " enable | all\n" + " disable | all\n" + "\n")); + return 0; +} + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Coommon for implementation details +const struct Menu::command log_menu_commands[] PROGMEM = { + {"dump", dump_log}, + {"erase", erase_logs}, + {"enable", select_logs}, + {"disable", select_logs}, + {"help", help_log} +}; + +// A Macro to create the Menu +MENU2(log_menu, "Log", log_menu_commands, print_log_menu); + +static void get_log_boundaries(byte log_num, int & start_page, int & end_page); + +static bool +print_log_menu(void) +{ + int log_start; + int log_end; + byte last_log_num = get_num_logs(); + + Serial.printf_P(PSTR("logs enabled: ")); + + if (0 == g.log_bitmask) { + Serial.printf_P(PSTR("none")); + }else{ + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Serial.printf_P(PSTR(" ATTITUDE_FAST")); + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Serial.printf_P(PSTR(" ATTITUDE_MED")); + if (g.log_bitmask & MASK_LOG_GPS) Serial.printf_P(PSTR(" GPS")); + if (g.log_bitmask & MASK_LOG_PM) Serial.printf_P(PSTR(" PM")); + if (g.log_bitmask & MASK_LOG_CTUN) Serial.printf_P(PSTR(" CTUN")); + if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN")); + if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW")); + if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD")); + if (g.log_bitmask & MASK_LOG_CUR) Serial.printf_P(PSTR(" CURRENT")); + if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS")); + if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW")); + } + + Serial.println(); + + if (last_log_num == 0) { + Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n")); + }else{ + Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); + + for(int i = 1; i < last_log_num + 1; i++) { + get_log_boundaries(i, log_start, log_end); + //Serial.printf_P(PSTR("last_num %d "), last_log_num); + Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"), i, log_start, log_end); + } + Serial.println(); + } + return(true); +} + +static int8_t +dump_log(uint8_t argc, const Menu::arg *argv) +{ + byte dump_log; + int dump_log_start; + int dump_log_end; + + // check that the requested log number can be read + dump_log = argv[1].i; + + if (/*(argc != 2) || */ (dump_log < 1)) { + Serial.printf_P(PSTR("bad log # %d\n"), dump_log); + Log_Read(0, 4095); + erase_logs(NULL, NULL); + return(-1); + } + + get_log_boundaries(dump_log, dump_log_start, dump_log_end); + /*Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"), + dump_log, + dump_log_start, + dump_log_end); + */ + Log_Read(dump_log_start, dump_log_end); + //Serial.printf_P(PSTR("Done\n")); + return (0); +} + +static int8_t +erase_logs(uint8_t argc, const Menu::arg *argv) +{ + //for(int i = 10 ; i > 0; i--) { + // Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds.\n"), i); + // delay(1000); + //} + + // lay down a bunch of "log end" messages. + Serial.printf_P(PSTR("\nErasing log...\n")); + for(int j = 1; j < 4096; j++) + DataFlash.PageErase(j); + + clear_header(); + + Serial.printf_P(PSTR("\nLog erased.\n")); + return (0); +} + +static void clear_header() +{ + DataFlash.StartWrite(1); + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_INDEX_MSG); + DataFlash.WriteByte(0); + DataFlash.WriteByte(END_BYTE); + DataFlash.FinishWrite(); +} + +static int8_t +select_logs(uint8_t argc, const Menu::arg *argv) +{ + uint16_t bits; + + if (argc != 2) { + Serial.printf_P(PSTR("missing log type\n")); + return(-1); + } + + bits = 0; + + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // that name as the argument to the command, and set the bit in + // bits accordingly. + // + if (!strcasecmp_P(argv[1].str, PSTR("all"))) { + bits = ~0; + } else { + #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s + TARG(ATTITUDE_FAST); + TARG(ATTITUDE_MED); + TARG(GPS); + TARG(PM); + TARG(CTUN); + TARG(NTUN); + TARG(MODE); + TARG(RAW); + TARG(CMD); + TARG(CUR); + TARG(MOTORS); + TARG(OPTFLOW); + #undef TARG + } + + if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { + g.log_bitmask.set_and_save(g.log_bitmask | bits); + }else{ + g.log_bitmask.set_and_save(g.log_bitmask & ~bits); + } + + return(0); +} + +static int8_t +process_logs(uint8_t argc, const Menu::arg *argv) +{ + log_menu.run(); + return 0; +} + + +// finds out how many logs are available +static byte get_num_logs(void) +{ + int page = 1; + byte data; + byte log_step = 0; + + DataFlash.StartRead(1); + + while (page == 1) { + data = DataFlash.ReadByte(); + + switch(log_step){ //This is a state machine to read the packets + case 0: + if(data==HEAD_BYTE1) // Head byte 1 + log_step++; + break; + + case 1: + if(data==HEAD_BYTE2) // Head byte 2 + log_step++; + else + log_step = 0; + break; + + case 2: + if(data == LOG_INDEX_MSG){ + byte num_logs = DataFlash.ReadByte(); + //Serial.printf("num_logs, %d\n", num_logs); + + return num_logs; + }else{ + //Serial.printf("* %d\n", data); + log_step = 0; // Restart, we have a problem... + } + break; + } + page = DataFlash.GetPage(); + } + return 0; +} + +// send the number of the last log? +static void start_new_log() +{ + byte num_existing_logs = get_num_logs(); + + int start_pages[50] = {0,0,0}; + int end_pages[50] = {0,0,0}; + + if(num_existing_logs > 0){ + for(int i = 0; i < num_existing_logs; i++) { + get_log_boundaries(i + 1, start_pages[i], end_pages[i]); + } + end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]); + } + + if((end_pages[num_existing_logs - 1] < 4095) && (num_existing_logs < MAX_NUM_LOGS /*50*/)) { + + if(num_existing_logs > 0) + start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1; + else + start_pages[0] = 2; + + num_existing_logs++; + + DataFlash.StartWrite(1); + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_INDEX_MSG); + DataFlash.WriteByte(num_existing_logs); + + for(int i = 0; i < MAX_NUM_LOGS; i++) { + DataFlash.WriteInt(start_pages[i]); + DataFlash.WriteInt(end_pages[i]); + } + + DataFlash.WriteByte(END_BYTE); + DataFlash.FinishWrite(); + DataFlash.StartWrite(start_pages[num_existing_logs - 1]); + + }else{ + gcs_send_text_P(SEVERITY_LOW,PSTR("Logs full")); + } +} + +// All log data is stored in page 1? +static void get_log_boundaries(byte log_num, int & start_page, int & end_page) +{ + int page = 1; + byte data; + byte log_step = 0; + + DataFlash.StartRead(1); + while (page == 1) { + data = DataFlash.ReadByte(); + switch(log_step) //This is a state machine to read the packets + { + case 0: + if(data==HEAD_BYTE1) // Head byte 1 + log_step++; + break; + case 1: + if(data==HEAD_BYTE2) // Head byte 2 + log_step++; + else + log_step = 0; + break; + case 2: + if(data==LOG_INDEX_MSG){ + byte num_logs = DataFlash.ReadByte(); + for(int i=0;i 1) { + look_page = (top_page + bottom_page) / 2; + DataFlash.StartRead(look_page); + check = DataFlash.ReadLong(); + + //Serial.printf("look page:%d, check:%d\n", look_page, check); + + if(check == (long)0xFFFFFFFF) + top_page = look_page; + else + bottom_page = look_page; + } + return top_page; +} + +// Write an GPS packet. Total length : 30 bytes +static void Log_Write_GPS() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_GPS_MSG); + + DataFlash.WriteLong(g_gps->time); // 1 + DataFlash.WriteByte(g_gps->num_sats); // 2 + + DataFlash.WriteLong(current_loc.lat); // 3 + DataFlash.WriteLong(current_loc.lng); // 4 + DataFlash.WriteLong(current_loc.alt); // 5 + DataFlash.WriteLong(g_gps->altitude); // 6 + + DataFlash.WriteInt(g_gps->ground_speed); // 7 + DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8 + + DataFlash.WriteByte(END_BYTE); +} + +// Read a GPS packet +static void Log_Read_GPS() +{ + Serial.printf_P(PSTR("GPS, %ld, %d, " + "%4.7f, %4.7f, %4.4f, %4.4f, " + "%d, %u\n"), + + DataFlash.ReadLong(), // 1 time + (int)DataFlash.ReadByte(), // 2 sats + + (float)DataFlash.ReadLong() / t7, // 3 lat + (float)DataFlash.ReadLong() / t7, // 4 lon + (float)DataFlash.ReadLong() / 100.0, // 5 gps alt + (float)DataFlash.ReadLong() / 100.0, // 6 sensor alt + + DataFlash.ReadInt(), // 7 ground speed + (uint16_t)DataFlash.ReadInt()); // 8 ground course +} + +// Write an raw accel/gyro data packet. Total length : 28 bytes +#if HIL_MODE != HIL_MODE_ATTITUDE +static void Log_Write_Raw() +{ + Vector3f gyro = imu.get_gyro(); + Vector3f accel = imu.get_accel(); + //Vector3f accel_filt = imu.get_accel_filtered(); + + gyro *= t7; // Scale up for storage as long integers + accel *= t7; + //accel_filt *= t7; + + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_RAW_MSG); + + DataFlash.WriteLong((long)gyro.x); + DataFlash.WriteLong((long)gyro.y); + DataFlash.WriteLong((long)gyro.z); + + + //DataFlash.WriteLong((long)(accels_rot.x * t7)); + //DataFlash.WriteLong((long)(accels_rot.y * t7)); + //DataFlash.WriteLong((long)(accels_rot.z * t7)); + + DataFlash.WriteLong((long)accel.x); + DataFlash.WriteLong((long)accel.y); + DataFlash.WriteLong((long)accel.z); + + DataFlash.WriteByte(END_BYTE); +} +#endif + +// Read a raw accel/gyro packet +static void Log_Read_Raw() +{ + float logvar; + Serial.printf_P(PSTR("RAW,")); + for (int y = 0; y < 6; y++) { + logvar = (float)DataFlash.ReadLong() / t7; + Serial.print(logvar); + Serial.print(comma); + } + Serial.println(" "); +} + +static void Log_Write_Current() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_CURRENT_MSG); + + DataFlash.WriteInt(g.rc_3.control_in); + DataFlash.WriteLong(throttle_integrator); + + DataFlash.WriteInt((int)(battery_voltage * 100.0)); + DataFlash.WriteInt((int)(current_amps * 100.0)); + DataFlash.WriteInt((int)current_total); + + DataFlash.WriteByte(END_BYTE); +} + +// Read a Current packet +static void Log_Read_Current() +{ + Serial.printf_P(PSTR("CURR: %d, %ld, %4.4f, %4.4f, %d\n"), + DataFlash.ReadInt(), + DataFlash.ReadLong(), + + ((float)DataFlash.ReadInt() / 100.f), + ((float)DataFlash.ReadInt() / 100.f), + DataFlash.ReadInt()); +} + +static void Log_Write_Motors() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_MOTORS_MSG); + + #if FRAME_CONFIG == TRI_FRAME + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_4]);//3 + DataFlash.WriteInt(g.rc_4.radio_out);//4 + + #elif FRAME_CONFIG == HEXA_FRAME + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_3]);//3 + DataFlash.WriteInt(motor_out[CH_4]);//4 + DataFlash.WriteInt(motor_out[CH_7]);//5 + DataFlash.WriteInt(motor_out[CH_8]);//6 + + #elif FRAME_CONFIG == Y6_FRAME + //left + DataFlash.WriteInt(motor_out[CH_2]);//1 + DataFlash.WriteInt(motor_out[CH_3]);//2 + //right + DataFlash.WriteInt(motor_out[CH_7]);//3 + DataFlash.WriteInt(motor_out[CH_1]);//4 + //back + DataFlash.WriteInt(motor_out[CH_8]);//5 + DataFlash.WriteInt(motor_out[CH_4]);//6 + + #elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_3]);//3 + DataFlash.WriteInt(motor_out[CH_4]);//4 + DataFlash.WriteInt(motor_out[CH_7]);//5 + DataFlash.WriteInt(motor_out[CH_8]); //6 + DataFlash.WriteInt(motor_out[CH_10]);//7 + DataFlash.WriteInt(motor_out[CH_11]);//8 + + #elif FRAME_CONFIG == HELI_FRAME + DataFlash.WriteInt(heli_servo_out[0]);//1 + DataFlash.WriteInt(heli_servo_out[1]);//2 + DataFlash.WriteInt(heli_servo_out[2]);//3 + DataFlash.WriteInt(heli_servo_out[3]);//4 + DataFlash.WriteInt(g.heli_ext_gyro_gain);//5 + + #else // quads + DataFlash.WriteInt(motor_out[CH_1]);//1 + DataFlash.WriteInt(motor_out[CH_2]);//2 + DataFlash.WriteInt(motor_out[CH_3]);//3 + DataFlash.WriteInt(motor_out[CH_4]);//4 + #endif + + DataFlash.WriteByte(END_BYTE); +} + +// Read a Current packet +static void Log_Read_Motors() +{ + #if FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME + // 1 2 3 4 5 6 + Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt(), //4 + DataFlash.ReadInt(), //5 + DataFlash.ReadInt()); //6 + + #elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME + // 1 2 3 4 5 6 7 8 + Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt(), //4 + + DataFlash.ReadInt(), //5 + DataFlash.ReadInt(), //6 + DataFlash.ReadInt(), //7 + DataFlash.ReadInt()); //8 + + #elif FRAME_CONFIG == HELI_FRAME + // 1 2 3 4 5 + Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt(), //4 + DataFlash.ReadInt()); //5 + + #else // quads, TRIs + // 1 2 3 4 + Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + DataFlash.ReadInt()); //4; + #endif +} + +#ifdef OPTFLOW_ENABLED +// Write an optical flow packet. Total length : 18 bytes +static void Log_Write_Optflow() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_OPTFLOW_MSG); + DataFlash.WriteInt((int)optflow.dx); + DataFlash.WriteInt((int)optflow.dy); + DataFlash.WriteInt((int)optflow.surface_quality); + DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); + DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); + DataFlash.WriteByte(END_BYTE); +} +#endif + + +static void Log_Read_Optflow() +{ + Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + (float)DataFlash.ReadLong(),// / t7, + (float)DataFlash.ReadLong() // / t7 + ); +} + +static void Log_Write_Nav_Tuning() +{ + //Matrix3f tempmat = dcm.get_dcm_matrix(); + + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_NAV_TUNING_MSG); + + DataFlash.WriteInt((int)wp_distance); // 1 + DataFlash.WriteInt((int)(target_bearing/100)); // 2 + DataFlash.WriteInt((int)long_error); // 3 + DataFlash.WriteInt((int)lat_error); // 4 + DataFlash.WriteInt((int)nav_lon); // 5 + DataFlash.WriteInt((int)nav_lat); // 6 + DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7 + DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8 + DataFlash.WriteInt((int)g.pi_loiter_lon.get_integrator()); // 9 + DataFlash.WriteInt((int)g.pi_loiter_lat.get_integrator()); // 10 + + DataFlash.WriteByte(END_BYTE); +} + + +static void Log_Read_Nav_Tuning() +{ + // 1 2 3 4 5 6 7 8 9 10 + Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), // 1 + DataFlash.ReadInt(), // 2 + DataFlash.ReadInt(), // 3 + DataFlash.ReadInt(), // 4 + DataFlash.ReadInt(), // 5 + DataFlash.ReadInt(), // 6 + DataFlash.ReadInt(), // 7 + DataFlash.ReadInt(), // 8 + DataFlash.ReadInt(), // 9 + DataFlash.ReadInt()); // 10 +} + + +// Write a control tuning packet. Total length : 22 bytes +#if HIL_MODE != HIL_MODE_ATTITUDE +static void Log_Write_Control_Tuning() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); + + // yaw + DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1 + DataFlash.WriteInt((int)(nav_yaw/100)); //2 + DataFlash.WriteInt((int)yaw_error/100); //3 + + // Alt hold + DataFlash.WriteInt(sonar_alt); //4 + DataFlash.WriteInt(baro_alt); //5 + DataFlash.WriteInt((int)next_WP.alt); //6 + + DataFlash.WriteInt(nav_throttle); //7 + DataFlash.WriteInt(angle_boost); //8 + DataFlash.WriteInt(manual_boost); //9 + + DataFlash.WriteInt(g.rc_3.servo_out); //10 + DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //11 + DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //12 + + DataFlash.WriteByte(END_BYTE); +} +#endif + +// Read an control tuning packet +static void Log_Read_Control_Tuning() +{ + // 1 2 3 4 5 6 7 8 9 10 11 12 + Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), + + // Control + //DataFlash.ReadByte(), + //DataFlash.ReadInt(), + + // yaw + DataFlash.ReadInt(), //1 + DataFlash.ReadInt(), //2 + DataFlash.ReadInt(), //3 + + // Alt Hold + DataFlash.ReadInt(), //4 + DataFlash.ReadInt(), //5 + DataFlash.ReadInt(), //6 + + DataFlash.ReadInt(), //7 + DataFlash.ReadInt(), //8 + DataFlash.ReadInt(), //9 + + DataFlash.ReadInt(), //10 + DataFlash.ReadInt(), //11 + DataFlash.ReadInt()); //12 +} + +// Write a performance monitoring packet. Total length : 19 bytes +static void Log_Write_Performance() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_PERFORMANCE_MSG); + + //DataFlash.WriteByte( delta_ms_fast_loop); + //DataFlash.WriteByte( loop_step); + + + //* + //DataFlash.WriteLong( millis()- perf_mon_timer); + + //DataFlash.WriteByte( dcm.gyro_sat_count); //2 + //DataFlash.WriteByte( imu.adc_constraints); //3 + //DataFlash.WriteByte( dcm.renorm_sqrt_count); //4 + //DataFlash.WriteByte( dcm.renorm_blowup_count); //5 + //DataFlash.WriteByte( gps_fix_count); //6 + + + + //DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7 + + + + // control_mode + DataFlash.WriteByte(control_mode); //1 + DataFlash.WriteByte(yaw_mode); //2 + DataFlash.WriteByte(roll_pitch_mode); //3 + DataFlash.WriteByte(throttle_mode); //4 + DataFlash.WriteInt(g.throttle_cruise.get()); //5 + DataFlash.WriteLong(throttle_integrator); //6 + DataFlash.WriteByte(END_BYTE); +} + +// Read a performance packet +static void Log_Read_Performance() +{ //1 2 3 4 5 6 + Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d, %ld\n"), + + // Control + //DataFlash.ReadLong(), + //DataFlash.ReadInt(), + DataFlash.ReadByte(), //1 + DataFlash.ReadByte(), //2 + DataFlash.ReadByte(), //3 + DataFlash.ReadByte(), //4 + DataFlash.ReadInt(), //5 + DataFlash.ReadLong()); //6 +} + +// Write a command processing packet. +static void Log_Write_Cmd(byte num, struct Location *wp) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_CMD_MSG); + + DataFlash.WriteByte(g.waypoint_total); + + DataFlash.WriteByte(num); + DataFlash.WriteByte(wp->id); + DataFlash.WriteByte(wp->options); + DataFlash.WriteByte(wp->p1); + DataFlash.WriteLong(wp->alt); + DataFlash.WriteLong(wp->lat); + DataFlash.WriteLong(wp->lng); + + DataFlash.WriteByte(END_BYTE); +} +//CMD, 3, 0, 16, 8, 1, 800, 340440192, -1180692736 + + +// Read a command processing packet +static void Log_Read_Cmd() +{ + Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"), + + // WP total + DataFlash.ReadByte(), + + // num, id, p1, options + DataFlash.ReadByte(), + DataFlash.ReadByte(), + DataFlash.ReadByte(), + DataFlash.ReadByte(), + + // Alt, lat long + DataFlash.ReadLong(), + DataFlash.ReadLong(), + DataFlash.ReadLong()); +} +/* +// Write an attitude packet. Total length : 10 bytes +static void Log_Write_Attitude2() +{ + Vector3f gyro = imu.get_gyro(); + Vector3f accel = imu.get_accel(); + + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_ATTITUDE_MSG); + + DataFlash.WriteInt((int)dcm.roll_sensor); + DataFlash.WriteInt((int)dcm.pitch_sensor); + + DataFlash.WriteLong((long)(degrees(omega.x) * 100.0)); + DataFlash.WriteLong((long)(degrees(omega.y) * 100.0)); + + DataFlash.WriteLong((long)(accel.x * 100000)); + DataFlash.WriteLong((long)(accel.y * 100000)); + + //DataFlash.WriteLong((long)(accel.z * 100000)); + + DataFlash.WriteByte(END_BYTE); +}*/ +/* +// Read an attitude packet +static void Log_Read_Attitude2() +{ + Serial.printf_P(PSTR("ATT, %d, %d, %ld, %ld, %1.4f, %1.4f\n"), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + + DataFlash.ReadLong(), + DataFlash.ReadLong(), + + (float)DataFlash.ReadLong()/100000.0, + (float)DataFlash.ReadLong()/100000.0 ); +} +*/ + +// Write an attitude packet. Total length : 10 bytes +static void Log_Write_Attitude() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_ATTITUDE_MSG); + + DataFlash.WriteInt((int)dcm.roll_sensor); + DataFlash.WriteInt((int)dcm.pitch_sensor); + DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); + + DataFlash.WriteInt((int)g.rc_1.servo_out); + DataFlash.WriteInt((int)g.rc_2.servo_out); + DataFlash.WriteInt((int)g.rc_4.servo_out); + + DataFlash.WriteByte(END_BYTE); +} + +// Read an attitude packet +static void Log_Read_Attitude() +{ + Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + (uint16_t)DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt()); +} + +// Write a mode packet. Total length : 5 bytes +static void Log_Write_Mode(byte mode) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_MODE_MSG); + DataFlash.WriteByte(mode); + DataFlash.WriteInt(g.throttle_cruise); + DataFlash.WriteByte(END_BYTE); +} + +// Read a mode packet +static void Log_Read_Mode() +{ + Serial.printf_P(PSTR("MOD:")); + Serial.print(flight_mode_strings[DataFlash.ReadByte()]); + Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt()); +} + +static void Log_Write_Startup() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_STARTUP_MSG); + DataFlash.WriteByte(END_BYTE); +} + +// Read a mode packet +static void Log_Read_Startup() +{ + Serial.printf_P(PSTR("START UP\n")); +} + + +// Read the DataFlash log memory : Packet Parser +static void Log_Read(int start_page, int end_page) +{ + byte data; + byte log_step = 0; + int page = start_page; + + DataFlash.StartRead(start_page); + + while (page < end_page && page != -1){ + + data = DataFlash.ReadByte(); + + // This is a state machine to read the packets + switch(log_step){ + case 0: + if(data == HEAD_BYTE1) // Head byte 1 + log_step++; + break; + + case 1: + if(data == HEAD_BYTE2) // Head byte 2 + log_step++; + else{ + log_step = 0; + Serial.println("."); + } + break; + + case 2: + log_step = 0; + switch(data){ + case LOG_ATTITUDE_MSG: + Log_Read_Attitude(); + break; + + case LOG_MODE_MSG: + Log_Read_Mode(); + break; + + case LOG_CONTROL_TUNING_MSG: + Log_Read_Control_Tuning(); + break; + + case LOG_NAV_TUNING_MSG: + Log_Read_Nav_Tuning(); + break; + + case LOG_PERFORMANCE_MSG: + Log_Read_Performance(); + break; + + case LOG_RAW_MSG: + Log_Read_Raw(); + break; + + case LOG_CMD_MSG: + Log_Read_Cmd(); + break; + + case LOG_CURRENT_MSG: + Log_Read_Current(); + break; + + case LOG_STARTUP_MSG: + Log_Read_Startup(); + break; + + case LOG_MOTORS_MSG: + Log_Read_Motors(); + break; + + case LOG_OPTFLOW_MSG: + Log_Read_Optflow(); + break; + + case LOG_GPS_MSG: + Log_Read_GPS(); + break; + } + break; + } + page = DataFlash.GetPage(); + } +} + +#else // LOGGING_ENABLED + +static void Log_Write_Startup() {} +static void Log_Read_Startup() {} +static void Log_Read(int start_page, int end_page) {} +static void Log_Write_Cmd(byte num, struct Location *wp) {} +static void Log_Write_Mode(byte mode) {} +static void start_new_log() {} +static void Log_Write_Raw() {} +static void Log_Write_GPS() {} +static void Log_Write_Current() {} +static void Log_Write_Attitude() {} +#ifdef OPTFLOW_ENABLED +static void Log_Write_Optflow() {} +#endif +static void Log_Write_Nav_Tuning() {} +static void Log_Write_Control_Tuning() {} +static void Log_Write_Motors() {} +static void Log_Write_Performance() {} +static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } + +#endif // LOGGING_ENABLED +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/UserCode.pde" +// agmatthews USERHOOKS + +void userhook_init() +{ + // put your initialisation code here + + +} + +void userhook_50Hz() +{ + // put your 50Hz code here + + +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +static void init_commands() +{ + // zero is home, but we always load the next command (1), in the code. + g.waypoint_index = 0; + + // This are registers for the current may and must commands + // setting to zero will allow them to be written to by new commands + command_must_index = NO_COMMAND; + command_may_index = NO_COMMAND; + + // clear the command queue + clear_command_queue(); +} + +// forces the loading of a new command +// queue is emptied after a new command is processed +static void clear_command_queue(){ + next_command.id = NO_COMMAND; +} + +// Getters +// ------- +static struct Location get_command_with_index(int i) +{ + struct Location temp; + + // Find out proper location in memory by using the start_byte position + the index + // -------------------------------------------------------------------------------- + if (i > g.waypoint_total) { + // we do not have a valid command to load + // return a WP with a "Blank" id + temp.id = CMD_BLANK; + + // no reason to carry on + return temp; + + }else{ + // we can load a command, we don't process it yet + // read WP position + long mem = (WP_START_BYTE) + (i * WP_SIZE); + + temp.id = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.options = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.p1 = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative! + + mem += 4; + temp.lat = (long)eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000 + + mem += 4; + temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000 + } + + // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative + //if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ + //temp.alt += home.alt; + //} + + if(temp.options & WP_OPTION_RELATIVE){ + // If were relative, just offset from home + temp.lat += home.lat; + temp.lng += home.lng; + } + + return temp; +} + +// Setters +// ------- +static void set_command_with_index(struct Location temp, int i) +{ + i = constrain(i, 0, g.waypoint_total.get()); + uint32_t mem = WP_START_BYTE + (i * WP_SIZE); + + eeprom_write_byte((uint8_t *) mem, temp.id); + + mem++; + eeprom_write_byte((uint8_t *) mem, temp.options); + + mem++; + eeprom_write_byte((uint8_t *) mem, temp.p1); + + mem++; + eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM! + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7 + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7 +} + +static void increment_WP_index() +{ + if (g.waypoint_index < g.waypoint_total) { + g.waypoint_index++; + } + + SendDebugln(g.waypoint_index,DEC); +} + +/* +static void decrement_WP_index() +{ + if (g.waypoint_index > 0) { + g.waypoint_index.set_and_save(g.waypoint_index - 1); + } +}*/ + +static long read_alt_to_hold() +{ + if(g.RTL_altitude < 0) + return current_loc.alt; + else + return g.RTL_altitude;// + home.alt; +} + + +//******************************************************************************** +// This function sets the waypoint and modes for Return to Launch +// It's not currently used +//******************************************************************************** + +static Location get_LOITER_home_wp() +{ + //so we know where we are navigating from + next_WP = current_loc; + + // read home position + struct Location temp = get_command_with_index(0); // 0 = home + temp.id = MAV_CMD_NAV_LOITER_UNLIM; + temp.alt = read_alt_to_hold(); + return temp; +} + +/* +This function sets the next waypoint command +It precalculates all the necessary stuff. +*/ + +static void set_next_WP(struct Location *wp) +{ + //SendDebug("MSG wp_index: "); + //SendDebugln(g.waypoint_index, DEC); + + // copy the current WP into the OldWP slot + // --------------------------------------- + prev_WP = next_WP; + + // Load the next_WP slot + // --------------------- + next_WP = *wp; + + // used to control and limit the rate of climb - not used right now! + // ----------------------------------------------------------------- + target_altitude = current_loc.alt; + + // this is used to offset the shrinking longitude as we go towards the poles + float rads = (abs(next_WP.lat)/t7) * 0.0174532925; + scaleLongDown = cos(rads); + scaleLongUp = 1.0f/cos(rads); + + // this is handy for the groundstation + // ----------------------------------- + wp_totalDistance = get_distance(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + target_bearing = get_bearing(¤t_loc, &next_WP); + + // to check if we have missed the WP + // --------------------------------- + original_target_bearing = target_bearing; + + // reset speed governer + // -------------------- + waypoint_speed_gov = 0; +} + + +// run this at setup on the ground +// ------------------------------- +static void init_home() +{ + // block until we get a good fix + // ----------------------------- + while (!g_gps->new_data || !g_gps->fix) { + g_gps->update(); + } + + home.id = MAV_CMD_NAV_WAYPOINT; + home.lng = g_gps->longitude; // Lon * 10**7 + home.lat = g_gps->latitude; // Lat * 10**7 + //home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid + home.alt = 0; // Home is always 0 + home_is_set = true; + + // to point yaw towards home until we set it with Mavlink + target_WP = home; + + // Save Home to EEPROM + // ------------------- + // no need to save this to EPROM + set_command_with_index(home, 0); + print_wp(&home, 0); + + // Save prev loc this makes the calcs look better before commands are loaded + prev_WP = home; + + // this is dangerous since we can get GPS lock at any time. + //next_WP = home; + + // Load home for a default guided_WP + // ------------- + guided_WP = home; + guided_WP.alt += g.RTL_altitude; +} + + + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands_logic.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/********************************************************************************/ +// Command Event Handlers +/********************************************************************************/ +static void handle_process_must() +{ + switch(next_command.id){ + + case MAV_CMD_NAV_TAKEOFF: + do_takeoff(); + break; + + case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint + do_nav_wp(); + break; + + case MAV_CMD_NAV_LAND: // LAND to Waypoint + do_land(); + break; + + case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely + do_loiter_unlimited(); + break; + + case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times + do_loiter_turns(); + break; + + case MAV_CMD_NAV_LOITER_TIME: // 19 + do_loiter_time(); + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + do_RTL(); + break; + + default: + break; + } + +} + +static void handle_process_may() +{ + switch(next_command.id){ + + case MAV_CMD_CONDITION_DELAY: + do_wait_delay(); + break; + + case MAV_CMD_CONDITION_DISTANCE: + do_within_distance(); + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + do_change_alt(); + break; + + case MAV_CMD_CONDITION_YAW: + do_yaw(); + break; + + default: + break; + } +} + +static void handle_process_now() +{ + switch(next_command.id){ + + case MAV_CMD_DO_JUMP: + do_jump(); + break; + + case MAV_CMD_DO_CHANGE_SPEED: + do_change_speed(); + break; + + case MAV_CMD_DO_SET_HOME: + do_set_home(); + break; + + case MAV_CMD_DO_SET_SERVO: + do_set_servo(); + break; + + case MAV_CMD_DO_SET_RELAY: + do_set_relay(); + break; + + case MAV_CMD_DO_REPEAT_SERVO: + do_repeat_servo(); + break; + + case MAV_CMD_DO_REPEAT_RELAY: + do_repeat_relay(); + break; + + case MAV_CMD_DO_SET_ROI: + do_target_yaw(); + } +} + +static void handle_no_commands() +{ + /* + switch (control_mode){ + default: + set_mode(RTL); + break; + }*/ + //return; + //Serial.println("Handle No CMDs"); +} + +/********************************************************************************/ +// Verify command Handlers +/********************************************************************************/ + +static bool verify_must() +{ + //Serial.printf("vmust: %d\n", command_must_ID); + + switch(command_must_ID) { + + case MAV_CMD_NAV_TAKEOFF: + return verify_takeoff(); + break; + + case MAV_CMD_NAV_LAND: + return verify_land(); + break; + + case MAV_CMD_NAV_WAYPOINT: + return verify_nav_wp(); + break; + + case MAV_CMD_NAV_LOITER_UNLIM: + return false; + break; + + case MAV_CMD_NAV_LOITER_TURNS: + return verify_loiter_turns(); + break; + + case MAV_CMD_NAV_LOITER_TIME: + return verify_loiter_time(); + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + return verify_RTL(); + break; + + default: + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current Must commands")); + return false; + break; + } +} + +static bool verify_may() +{ + switch(command_may_ID) { + + case MAV_CMD_CONDITION_DELAY: + return verify_wait_delay(); + break; + + case MAV_CMD_CONDITION_DISTANCE: + return verify_within_distance(); + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + return verify_change_alt(); + break; + + case MAV_CMD_CONDITION_YAW: + return verify_yaw(); + break; + + default: + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current May commands")); + return false; + break; + } +} + +/********************************************************************************/ +// +/********************************************************************************/ + +static void do_RTL(void) +{ + // TODO: Altitude option from mission planner + Location temp = home; + temp.alt = read_alt_to_hold(); + + //so we know where we are navigating from + // -------------------------------------- + next_WP = current_loc; + + // Loads WP from Memory + // -------------------- + set_next_WP(&temp); + + // output control mode to the ground station + // ----------------------------------------- + gcs_send_message(MSG_HEARTBEAT); +} + +/********************************************************************************/ +// Nav (Must) commands +/********************************************************************************/ + +static void do_takeoff() +{ + wp_control = LOITER_MODE; + + // Start with current location + Location temp = current_loc; + + // next_command.alt is a relative altitude!!! + if (next_command.options & WP_OPTION_ALT_RELATIVE) { + temp.alt = next_command.alt + home.alt; + //Serial.printf("rel alt: %ld",temp.alt); + } else { + temp.alt = next_command.alt; + //Serial.printf("abs alt: %ld",temp.alt); + } + + takeoff_complete = false; + // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction + + // Set our waypoint + set_next_WP(&temp); +} + +static void do_nav_wp() +{ + wp_control = WP_MODE; + + // next_command.alt is a relative altitude!!! + if (next_command.options & WP_OPTION_ALT_RELATIVE) { + next_command.alt += home.alt; + } + set_next_WP(&next_command); + + + // this is our bitmask to verify we have met all conditions to move on + wp_verify_byte = 0; + + // this will be used to remember the time in millis after we reach or pass the WP. + loiter_time = 0; + + // this is the delay, stored in seconds and expanded to millis + loiter_time_max = next_command.p1 * 1000; + + // if we don't require an altitude minimum, we save this flag as passed (1) + if((next_WP.options & WP_OPTION_ALT_REQUIRED) == 0){ + // we don't need to worry about it + wp_verify_byte |= NAV_ALTITUDE; + } +} + +static void do_land() +{ + wp_control = LOITER_MODE; + + //Serial.println("dlnd "); + + // not really used right now, might be good for debugging + land_complete = false; + + // A value that drives to 0 when the altitude doesn't change + velocity_land = 2000; + + // used to limit decent rate + land_start = millis(); + + // used to limit decent rate + original_alt = current_loc.alt; + + // hold at our current location + set_next_WP(¤t_loc); +} + +static void do_loiter_unlimited() +{ + wp_control = LOITER_MODE; + + //Serial.println("dloi "); + if(next_command.lat == 0) + set_next_WP(¤t_loc); + else + set_next_WP(&next_command); +} + +static void do_loiter_turns() +{ + wp_control = CIRCLE_MODE; + + if(next_command.lat == 0) + set_next_WP(¤t_loc); + else + set_next_WP(&next_command); + + loiter_total = next_command.p1 * 360; + loiter_sum = 0; +} + +static void do_loiter_time() +{ + if(next_command.lat == 0){ + wp_control = LOITER_MODE; + loiter_time = millis(); + set_next_WP(¤t_loc); + }else{ + wp_control = WP_MODE; + set_next_WP(&next_command); + } + + loiter_time_max = next_command.p1 * 1000; // units are (seconds) +} + +/********************************************************************************/ +// Verify Nav (Must) commands +/********************************************************************************/ + +static bool verify_takeoff() +{ + + // wait until we are ready! + if(g.rc_3.control_in == 0){ + return false; + } + + if (current_loc.alt > next_WP.alt){ + //Serial.println("Y"); + takeoff_complete = true; + return true; + + }else{ + + //Serial.println("N"); + return false; + } +} + +static bool verify_land() +{ + // land at 1 meter per second + next_WP.alt = original_alt - ((millis() - land_start) / 20); // condition_value = our initial + + velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8); + old_alt = current_loc.alt; + + if(g.sonar_enabled){ + // decide which sensor we're using + if(sonar_alt < 300){ + next_WP = current_loc; // don't pitch or roll + next_WP.alt = -200; // force us down + } + if(sonar_alt < 40){ + land_complete = true; + //Serial.println("Y"); + //return true; + } + } + + if(velocity_land <= 0){ + land_complete = true; + //return true; + } + //Serial.printf("N, %d\n", velocity_land); + //Serial.printf("N_alt, %ld\n", next_WP.alt); + + return false; +} + +static bool verify_nav_wp() +{ + // Altitude checking + if(next_WP.options & WP_OPTION_ALT_REQUIRED){ + // we desire a certain minimum altitude + if (current_loc.alt > next_WP.alt){ + // we have reached that altitude + wp_verify_byte |= NAV_ALTITUDE; + } + } + + // Did we pass the WP? // Distance checking + if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ + + // if we have a distance calc error, wp_distance may be less than 0 + if(wp_distance > 0){ + wp_verify_byte |= NAV_LOCATION; + + if(loiter_time == 0){ + loiter_time = millis(); + } + } + } + + // Hold at Waypoint checking, we cant move on until this is OK + if(wp_verify_byte & NAV_LOCATION){ + // we have reached our goal + + // loiter at the WP + wp_control = LOITER_MODE; + + if ((millis() - loiter_time) > loiter_time_max) { + wp_verify_byte |= NAV_DELAY; + //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); + //Serial.println("vlt done"); + } + } + + if(wp_verify_byte >= 7){ + //if(wp_verify_byte & NAV_LOCATION){ + char message[30]; + sprintf(message,"Reached Command #%i",command_must_index); + gcs_send_text(SEVERITY_LOW,message); + wp_verify_byte = 0; + return true; + }else{ + return false; + } +} + +static bool verify_loiter_unlim() +{ + return false; +} + +static bool verify_loiter_time() +{ + if(wp_control == LOITER_MODE){ + if ((millis() - loiter_time) > loiter_time_max) { + return true; + } + } + if(wp_control == WP_MODE && wp_distance <= g.waypoint_radius){ + // reset our loiter time + loiter_time = millis(); + // switch to position hold + wp_control = LOITER_MODE; + } + return false; +} + +static bool verify_loiter_turns() +{ + // have we rotated around the center enough times? + // ----------------------------------------------- + if(abs(loiter_sum) > loiter_total) { + loiter_total = 0; + loiter_sum = 0; + //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); + // clear the command queue; + return true; + } + return false; +} + +static bool verify_RTL() +{ + if (wp_distance <= g.waypoint_radius) { + //gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); + return true; + }else{ + return false; + } +} + +/********************************************************************************/ +// Condition (May) commands +/********************************************************************************/ + +static void do_wait_delay() +{ + //Serial.print("dwd "); + condition_start = millis(); + condition_value = next_command.lat * 1000; // convert to milliseconds + //Serial.println(condition_value,DEC); +} + +static void do_change_alt() +{ + Location temp = next_WP; + condition_start = current_loc.alt; + condition_value = next_command.alt; + temp.alt = next_command.alt; + set_next_WP(&temp); +} + +static void do_within_distance() +{ + condition_value = next_command.lat; +} + +static void do_yaw() +{ + //Serial.println("dyaw "); + yaw_tracking = MAV_ROI_NONE; + + // target angle in degrees + command_yaw_start = nav_yaw; // current position + command_yaw_start_time = millis(); + + command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise + command_yaw_speed = next_command.lat * 100; // ms * 100 + command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute + + + + // if unspecified go 30° a second + if(command_yaw_speed == 0) + command_yaw_speed = 3000; + + // if unspecified go counterclockwise + if(command_yaw_dir == 0) + command_yaw_dir = -1; + else + command_yaw_dir = 1; + + if (command_yaw_relative == 1){ + // relative + command_yaw_delta = next_command.alt * 100; + + }else{ + // absolute + command_yaw_end = next_command.alt * 100; + + // calculate the delta travel in deg * 100 + if(command_yaw_dir == 1){ + if(command_yaw_start >= command_yaw_end){ + command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); + }else{ + command_yaw_delta = command_yaw_end - command_yaw_start; + } + }else{ + if(command_yaw_start > command_yaw_end){ + command_yaw_delta = command_yaw_start - command_yaw_end; + }else{ + command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); + } + } + command_yaw_delta = wrap_360(command_yaw_delta); + } + + + // rate to turn deg per second - default is ten + command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000; +} + + +/********************************************************************************/ +// Verify Condition (May) commands +/********************************************************************************/ + +static bool verify_wait_delay() +{ + //Serial.print("vwd"); + if ((unsigned)(millis() - condition_start) > condition_value){ + //Serial.println("y"); + condition_value = 0; + return true; + } + //Serial.println("n"); + return false; +} + +static bool verify_change_alt() +{ + if (condition_start < next_WP.alt){ + // we are going higer + if(current_loc.alt > next_WP.alt){ + condition_value = 0; + return true; + } + }else{ + // we are going lower + if(current_loc.alt < next_WP.alt){ + condition_value = 0; + return true; + } + } + return false; +} + +static bool verify_within_distance() +{ + if (wp_distance < condition_value){ + condition_value = 0; + return true; + } + return false; +} + +static bool verify_yaw() +{ + //Serial.print("vyaw "); + + if((millis() - command_yaw_start_time) > command_yaw_time){ + // time out + // make sure we hold at the final desired yaw angle + nav_yaw = command_yaw_end; + auto_yaw = nav_yaw; + + //Serial.println("Y"); + return true; + + }else{ + // else we need to be at a certain place + // power is a ratio of the time : .5 = half done + float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time; + + nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir); + nav_yaw = wrap_360(nav_yaw); + auto_yaw = nav_yaw; + //Serial.printf("ny %ld\n",nav_yaw); + return false; + } +} + +/********************************************************************************/ +// Do (Now) commands +/********************************************************************************/ + +static void do_change_speed() +{ + g.waypoint_speed_max = next_command.p1 * 100; +} + +static void do_target_yaw() +{ + yaw_tracking = next_command.p1; + + if(yaw_tracking == MAV_ROI_LOCATION){ + target_WP = next_command; + } +} + +static void do_loiter_at_location() +{ + next_WP = current_loc; +} + +static void do_jump() +{ + if(jump == -10){ + jump = next_command.lat; + } + + if(jump > 0) { + jump--; + command_must_index = 0; + command_may_index = 0; + + // set pointer to desired index + g.waypoint_index = next_command.p1 - 1; + + } else if (jump == 0){ + // we're done, move along + jump = -10; + + } else if (jump == -1) { + // repeat forever + g.waypoint_index = next_command.p1 - 1; + } +} + +static void do_set_home() +{ + if(next_command.p1 == 1) { + init_home(); + } else { + home.id = MAV_CMD_NAV_WAYPOINT; + home.lng = next_command.lng; // Lon * 10**7 + home.lat = next_command.lat; // Lat * 10**7 + home.alt = max(next_command.alt, 0); + home_is_set = true; + } +} + +static void do_set_servo() +{ + APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); +} + +static void do_set_relay() +{ + if (next_command.p1 == 1) { + relay.on(); + } else if (next_command.p1 == 0) { + relay.off(); + }else{ + relay.toggle(); + } +} + +static void do_repeat_servo() +{ + event_id = next_command.p1 - 1; + + if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) { + + event_timer = 0; + event_value = next_command.alt; + event_repeat = next_command.lat * 2; + event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + + switch(next_command.p1) { + case CH_5: + event_undo_value = g.rc_5.radio_trim; + break; + case CH_6: + event_undo_value = g.rc_6.radio_trim; + break; + case CH_7: + event_undo_value = g.rc_7.radio_trim; + break; + case CH_8: + event_undo_value = g.rc_8.radio_trim; + break; + } + update_events(); + } +} + +static void do_repeat_relay() +{ + event_id = RELAY_TOGGLE; + event_timer = 0; + event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_command.alt * 2; + update_events(); +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands_process.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// For changing active command mid-mission +//---------------------------------------- +static void change_command(uint8_t index) +{ + struct Location temp = get_command_with_index(index); + + if (temp.id > MAV_CMD_NAV_LAST ){ + gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd")); + } else { + command_must_index = NO_COMMAND; + next_command.id = NO_COMMAND; + g.waypoint_index = index - 1; + update_commands(); + } +} + +// called by 10 Hz Medium loop +// --------------------------- +static void update_commands(void) +{ + // fill command queue with a new command if available + if(next_command.id == NO_COMMAND){ + + // fetch next command if the next command queue is empty + // ----------------------------------------------------- + if (g.waypoint_index < g.waypoint_total) { + + // only if we have a cmd stored in EEPROM + next_command = get_command_with_index(g.waypoint_index + 1); + //Serial.printf("queue CMD %d\n", next_command.id); + } + } + + // Are we out of must commands and the queue is empty? + if(next_command.id == NO_COMMAND && command_must_index == NO_COMMAND){ + // if no commands were available from EEPROM + // And we have no nav commands + // -------------------------------------------- + if (command_must_ID == NO_COMMAND){ + gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); + handle_no_commands(); + } + } + + // check to see if we need to act on our command queue + if (process_next_command()){ + //Serial.printf("did PNC: %d\n", next_command.id); + + // We acted on the queue - let's debug that + // ---------------------------------------- + print_wp(&next_command, g.waypoint_index); + + // invalidate command queue so a new one is loaded + // ----------------------------------------------- + clear_command_queue(); + + // make sure we load the next command index + // ---------------------------------------- + increment_WP_index(); + } +} + +// called with GPS navigation update - not constantly +static void verify_commands(void) +{ + if(verify_must()){ + //Serial.printf("verified must cmd %d\n" , command_must_index); + command_must_index = NO_COMMAND; + }else{ + //Serial.printf("verified must false %d\n" , command_must_index); + } + + if(verify_may()){ + //Serial.printf("verified may cmd %d\n" , command_may_index); + command_may_index = NO_COMMAND; + command_may_ID = NO_COMMAND; + } +} + +static bool +process_next_command() +{ + // these are Navigation/Must commands + // --------------------------------- + if (command_must_index == NO_COMMAND){ // no current command loaded + if (next_command.id < MAV_CMD_NAV_LAST ){ + + // we remember the index of our mission here + command_must_index = g.waypoint_index + 1; + + // Save CMD to Log + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(g.waypoint_index + 1, &next_command); + + // Act on the new command + process_must(); + return true; + } + } + + // these are Condition/May commands + // ---------------------- + if (command_may_index == NO_COMMAND){ + if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ + + // we remember the index of our mission here + command_may_index = g.waypoint_index + 1; + + //SendDebug("MSG new may "); + //SendDebugln(next_command.id,DEC); + //Serial.print("new command_may_index "); + //Serial.println(command_may_index,DEC); + + // Save CMD to Log + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(g.waypoint_index + 1, &next_command); + + process_may(); + return true; + } + + // these are Do/Now commands + // --------------------------- + if (next_command.id > MAV_CMD_CONDITION_LAST){ + //SendDebug("MSG new now "); + //SendDebugln(next_command.id,DEC); + + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(g.waypoint_index + 1, &next_command); + process_now(); + return true; + } + } + // we did not need any new commands + return false; +} + +/**************************************************/ +// These functions implement the commands. +/**************************************************/ +static void process_must() +{ + //gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); + //Serial.printf("pmst %d\n", (int)next_command.id); + + // clear May indexes to force loading of more commands + // existing May commands are tossed. + command_may_index = NO_COMMAND; + command_may_ID = NO_COMMAND; + + // remember our command ID + command_must_ID = next_command.id; + + // implements the Flight Logic + handle_process_must(); + +} + +static void process_may() +{ + //gcs_send_text_P(SEVERITY_LOW,PSTR("")); + //Serial.print("pmay"); + + command_may_ID = next_command.id; + handle_process_may(); +} + +static void process_now() +{ + //Serial.print("pnow"); + handle_process_now(); +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/control_modes.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +static void read_control_switch() +{ + static bool switch_debouncer = false; + byte switchPosition = readSwitch(); + + if (oldSwitchPosition != switchPosition){ + if(switch_debouncer){ + // remember the prev location for GS + prev_WP = current_loc; + oldSwitchPosition = switchPosition; + switch_debouncer = false; + + set_mode(flight_modes[switchPosition]); + + #if CH7_OPTION != CH7_SIMPLE_MODE + // setup Simple mode + // do we enable simple mode? + do_simple = (g.simple_modes & (1 << switchPosition)); + #endif + }else{ + switch_debouncer = true; + } + } +} + +static byte readSwitch(void){ + int pulsewidth = g.rc_5.radio_in; // default for Arducopter + + if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; + if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; + if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; + if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual + if (pulsewidth >= 1750) return 5; // Hardware Manual + return 0; +} + +static void reset_control_switch() +{ + oldSwitchPosition = -1; + read_control_switch(); +} + +// read at 10 hz +// set this to your trainer switch +static void read_trim_switch() +{ + #if CH7_OPTION == CH7_FLIP + if (g.rc_7.control_in > 800 && g.rc_3.control_in != 0){ + do_flip = true; + } + + #elif CH7_OPTION == CH7_SIMPLE_MODE + do_simple = (g.rc_7.control_in > 800); + //Serial.println(g.rc_7.control_in, DEC); + + #elif CH7_OPTION == CH7_RTL + static bool ch7_rtl_flag = false; + + if (ch7_rtl_flag == false && g.rc_7.control_in > 800){ + ch7_rtl_flag = true; + set_mode(RTL); + } + + if (ch7_rtl_flag == true && g.rc_7.control_in < 800){ + ch7_rtl_flag = false; + if (control_mode == RTL || control_mode == LOITER){ + reset_control_switch(); + } + } + + #elif CH7_OPTION == CH7_SET_HOVER + // switch is engaged + if (g.rc_7.control_in > 800){ + trim_flag = true; + + }else{ // switch is disengaged + + if(trim_flag){ + + // set the throttle nominal + if(g.rc_3.control_in > 150){ + g.throttle_cruise.set_and_save(g.rc_3.control_in); + //Serial.printf("tnom %d\n", g.throttle_cruise.get()); + } + trim_flag = false; + } + } + + #elif CH7_OPTION == CH7_SAVE_WP + + if (g.rc_7.control_in > 800){ + trim_flag = true; + + }else{ // switch is disengaged + + if(trim_flag){ + // set the next_WP + CH7_wp_index++; + current_loc.id = MAV_CMD_NAV_WAYPOINT; + g.waypoint_total.set_and_save(CH7_wp_index); + set_command_with_index(current_loc, CH7_wp_index); + } + } + + #elif CH7_OPTION == CH7_ADC_FILTER + if (g.rc_7.control_in > 800){ + adc.filter_result = true; + }else{ + adc.filter_result = false; + } + + #elif CH7_OPTION == CH7_AUTO_TRIM + if (g.rc_7.control_in > 800){ + auto_level_counter = 10; + } + #endif + +} + +static void auto_trim() +{ + if(auto_level_counter > 0){ + //g.rc_1.dead_zone = 60; // 60 = .6 degrees + //g.rc_2.dead_zone = 60; + + auto_level_counter--; + trim_accel(); + led_mode = AUTO_TRIM_LEDS; + + if(auto_level_counter == 1){ + //g.rc_1.dead_zone = 0; // 60 = .6 degrees + //g.rc_2.dead_zone = 0; + led_mode = NORMAL_LEDS; + clear_leds(); + imu.save(); + + //Serial.println("Done"); + auto_level_counter = 0; + + // set TC + init_throttle_cruise(); + } + } +} + + + +static void trim_accel() +{ + g.pi_stabilize_roll.reset_I(); + g.pi_stabilize_pitch.reset_I(); + + if(g.rc_1.control_in > 0){ // Roll RIght + imu.ay(imu.ay() + 1); + }else if (g.rc_1.control_in < 0){ + imu.ay(imu.ay() - 1); + } + + if(g.rc_2.control_in > 0){ // Pitch Back + imu.ax(imu.ax() + 1); + }else if (g.rc_2.control_in < 0){ + imu.ax(imu.ax() - 1); + } + + /* + Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"), + dcm.roll_sensor, + dcm.pitch_sensor, + (float)imu.ax(), + (float)imu.ay(), + (float)imu.az()); + //*/ +} + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/events.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + This event will be called when the failsafe changes + boolean failsafe reflects the current state +*/ +static void failsafe_on_event() +{ + // This is how to handle a failsafe. + switch(control_mode) + { + case AUTO: + if (g.throttle_fs_action == 1) { + set_mode(RTL); + } + // 2 = Stay in AUTO and ignore failsafe + + default: + // not ready to enable yet w/o more testing + //set_mode(RTL); + break; + } +} + +static void failsafe_off_event() +{ + if (g.throttle_fs_action == 2){ + // We're back in radio contact + // return to AP + // --------------------------- + + // re-read the switch so we can return to our preferred mode + // -------------------------------------------------------- + reset_control_switch(); + + + }else if (g.throttle_fs_action == 1){ + // We're back in radio contact + // return to Home + // we should already be in RTL and throttle set to cruise + // ------------------------------------------------------ + set_mode(RTL); + } +} + +static void low_battery_event(void) +{ + gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); + low_batt = true; + + // if we are in Auto mode, come home + if(control_mode >= AUTO) + set_mode(RTL); +} + + +static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY +{ + if(event_repeat == 0 || (millis() - event_timer) < event_delay) + return; + + if (event_repeat > 0){ + event_repeat --; + } + + if(event_repeat != 0) { // event_repeat = -1 means repeat forever + event_timer = millis(); + + if (event_id >= CH_5 && event_id <= CH_8) { + if(event_repeat%2) { + APM_RC.OutputCh(event_id, event_value); // send to Servos + } else { + APM_RC.OutputCh(event_id, event_undo_value); + } + } + + if (event_id == RELAY_TOGGLE) { + relay.toggle(); + } + } +} + +#if PIEZO == ENABLED +void piezo_on() +{ + digitalWrite(PIEZO_PIN,HIGH); + //PORTF |= B00100000; +} + +void piezo_off() +{ + digitalWrite(PIEZO_PIN,LOW); + //PORTF &= ~B00100000; +} + +void piezo_beep() +{ + // Note: This command should not be used in time sensitive loops + piezo_on(); + delay(100); + piezo_off(); +} +#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/flip.pde" +// 2010 Jose Julio +// 2011 Adapted for AC2 by Jason Short +// +// Automatic Acrobatic Procedure (AAP) v1 : Roll flip +// State machine aproach: +// Some states are fixed commands (for a fixed time) +// Some states are fixed commands (until some IMU condition) +// Some states include controls inside +#if CH7_OPTION == CH7_FLIP +void roll_flip() +{ + #define AAP_THR_INC 180 + #define AAP_THR_DEC 90 + #define AAP_ROLL_OUT 200 + #define AAP_ROLL_RATE 3000 // up to 1250 + + static int AAP_timer = 0; + static byte AAP_state = 0; + + // Yaw + g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + // Pitch + g.rc_2.servo_out = get_stabilize_pitch(0); + + // State machine + switch (AAP_state){ + case 0: // Step 1 : Initialize + AAP_timer = 0; + AAP_state++; + break; + case 1: // Step 2 : Increase throttle to start maneuver + if (AAP_timer < 95){ // .5 seconds + g.rc_1.servo_out = get_stabilize_roll(0); + g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; + //g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + AAP_timer++; + }else{ + AAP_state++; + AAP_timer = 0; + } + break; + + case 2: // Step 3 : ROLL (until we reach a certain angle [45º]) + if (dcm.roll_sensor < 4500){ + // Roll control + g.rc_1.servo_out = AAP_ROLL_OUT; //get_rate_roll(AAP_ROLL_RATE); + g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; + }else{ + AAP_state++; + } + break; + + case 3: // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45º]) + if ((dcm.roll_sensor >= 4500) || (dcm.roll_sensor < -4500)){ + g.rc_1.servo_out = 150; //get_rate_roll(AAP_ROLL_RATE); + g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; + }else{ + AAP_state++; + } + break; + + case 4: // Step 5 : Increase throttle to stop the descend + if (AAP_timer < 90){ // .5 seconds + g.rc_1.servo_out = get_stabilize_roll(0); + g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; + AAP_timer++; + }else{ + AAP_state++; + AAP_timer = 0; + } + break; + + case 5: // exit mode + //control_mode = + do_flip = false; + break; + } +} +#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/heli.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if FRAME_CONFIG == HELI_FRAME + +#define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz +#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz + +static int heli_manual_override = false; +static float heli_throttle_scaler = 0; + +// heli_servo_averaging: +// 0 or 1 = no averaging, 250hz +// 2 = average two samples, 125hz +// 3 = averaging three samples = 83.3 hz +// 4 = averaging four samples = 62.5 hz +// 5 = averaging 5 samples = 50hz +// digital = 0 / 250hz, analog = 2 / 83.3 + +static void heli_init_swash() +{ + int i; + int tilt_max[CH_3+1]; + int total_tilt_max = 0; + + // swash servo initialisation + g.heli_servo_1.set_range(0,1000); + g.heli_servo_2.set_range(0,1000); + g.heli_servo_3.set_range(0,1000); + g.heli_servo_4.set_angle(4500); + + // pitch factors + heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos)); + heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos)); + heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos)); + + // roll factors + heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90)); + heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90)); + heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90)); + + // collective min / max + total_tilt_max = 0; + for( i=CH_1; i<=CH_3; i++ ) { + tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100; + total_tilt_max = max(total_tilt_max,tilt_max[i]); + } + + // servo min/max values - or should I use set_range? + g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1]; + g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1]; + g.heli_servo_2.radio_min = g.heli_coll_min - tilt_max[CH_2]; + g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2]; + g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3]; + g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3]; + + // scaler for changing channel 3 radio input into collective range + heli_throttle_scaler = ((float)(g.heli_coll_max - g.heli_coll_min))/1000; + + // reset the servo averaging + for( i=0; i<=3; i++ ) + heli_servo_out[i] = 0; + + // double check heli_servo_averaging is reasonable + if( g.heli_servo_averaging < 0 || g.heli_servo_averaging < 0 > 5 ) { + g.heli_servo_averaging = 0; + g.heli_servo_averaging.save(); + } +} + +static void heli_move_servos_to_mid() +{ + heli_move_swash(0,0,1500,0); +} + +// +// heli_move_swash - moves swash plate to attitude of parameters passed in +// - expected ranges: +// roll : -4500 ~ 4500 +// pitch: -4500 ~ 4500 +// collective: 1000 ~ 2000 +// yaw: -4500 ~ 4500 +// +static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) +{ + // ensure values are acceptable: + roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); + pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); + coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max); + + // swashplate servos + g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); + if( g.heli_servo_1.get_reverse() ) + g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out; + + g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); + if( g.heli_servo_2.get_reverse() ) + g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out; + + g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); + if( g.heli_servo_3.get_reverse() ) + g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out; + + if( g.heli_servo_4.get_reverse() ) + g.heli_servo_4.servo_out = -yaw_out; // should probably just use rc_4 directly like we do for a tricopter + else + g.heli_servo_4.servo_out = yaw_out; + + // use servo_out to calculate pwm_out and radio_out + g.heli_servo_1.calc_pwm(); + g.heli_servo_2.calc_pwm(); + g.heli_servo_3.calc_pwm(); + g.heli_servo_4.calc_pwm(); + + // add the servo values to the averaging + heli_servo_out[0] += g.heli_servo_1.servo_out; + heli_servo_out[1] += g.heli_servo_2.servo_out; + heli_servo_out[2] += g.heli_servo_3.servo_out; + heli_servo_out[3] += g.heli_servo_4.radio_out; + heli_servo_out_count++; + + // is it time to move the servos? + if( heli_servo_out_count >= g.heli_servo_averaging ) { + + // average the values if necessary + if( g.heli_servo_averaging >= 2 ) { + heli_servo_out[0] /= g.heli_servo_averaging; + heli_servo_out[1] /= g.heli_servo_averaging; + heli_servo_out[2] /= g.heli_servo_averaging; + heli_servo_out[3] /= g.heli_servo_averaging; + } + + // actually move the servos + APM_RC.OutputCh(CH_1, heli_servo_out[0]); + APM_RC.OutputCh(CH_2, heli_servo_out[1]); + APM_RC.OutputCh(CH_3, heli_servo_out[2]); + APM_RC.OutputCh(CH_4, heli_servo_out[3]); + + // output gyro value + if( g.heli_ext_gyro_enabled ) { + APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain); + } + + #if INSTANT_PWM == 1 + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + #endif + + // reset the averaging + heli_servo_out_count = 0; + heli_servo_out[0] = 0; + heli_servo_out[1] = 0; + heli_servo_out[2] = 0; + heli_servo_out[3] = 0; + } +} + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 40000; // 50 hz output CH 7, 8, 11 + #endif +} + +// these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better +static void output_motors_armed() +{ + //static int counter = 0; + g.rc_1.calc_pwm(); + g.rc_2.calc_pwm(); + g.rc_3.calc_pwm(); + g.rc_4.calc_pwm(); + + if( heli_manual_override ) { + // straight pass through from radio inputs to swash plate + heli_move_swash( g.rc_1.control_in, g.rc_2.control_in, g.rc_3.radio_in, g.rc_4.control_in ); + }else{ + // source inputs from attitude controller + heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); + } +} + +// for helis - armed or disarmed we allow servos to move +static void output_motors_disarmed() +{ + if(g.rc_3.control_in > 0){ + // we have pushed up the throttle, remove safety + motor_auto_armed = true; + } + + output_motors_armed(); +} + +static void output_motor_test() +{ +} + +// heli_get_scaled_throttle - user's throttle scaled to collective range +// input is expected to be in the range of 0~1000 (ie. pwm) +// also does equivalent of angle_boost +static int heli_get_scaled_throttle(int throttle) +{ + float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler; + return g.heli_coll_min - 1000 + (throttle * heli_throttle_scaler); +} + +// heli_angle_boost - takes servo_out position +// adds a boost depending on roll/pitch values +// equivalent of quad's angle_boost function +// pwm_out value should be 0 ~ 1000 +static int heli_get_angle_boost(int pwm_out) +{ + float angle_boost_factor = cos_pitch_x * cos_roll_x; + angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); + int throttle_above_center = max(1000 + pwm_out - g.heli_coll_mid,0); + return pwm_out + throttle_above_center*angle_boost_factor; +} + +#endif // HELI_FRAME +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/leds.pde" +static void update_lights() +{ + switch(led_mode){ + case NORMAL_LEDS: + update_motor_light(); + update_GPS_light(); + break; + + case AUTO_TRIM_LEDS: + dancing_light(); + break; + } +} + +static void update_GPS_light(void) +{ + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + switch (g_gps->status()){ + + case(2): + digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case(1): + if (g_gps->valid_read == true){ + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light){ + digitalWrite(C_LED_PIN, LOW); + }else{ + digitalWrite(C_LED_PIN, HIGH); + } + g_gps->valid_read = false; + } + break; + + default: + digitalWrite(C_LED_PIN, LOW); + break; + } +} + +static void update_motor_light(void) +{ + if(motor_armed == false){ + motor_light = !motor_light; + + // blink + if(motor_light){ + digitalWrite(A_LED_PIN, HIGH); + }else{ + digitalWrite(A_LED_PIN, LOW); + } + }else{ + if(!motor_light){ + motor_light = true; + digitalWrite(A_LED_PIN, HIGH); + } + } +} + +static void dancing_light() +{ + static byte step; + + if (step++ == 3) + step = 0; + + switch(step) + { + case 0: + digitalWrite(C_LED_PIN, LOW); + digitalWrite(A_LED_PIN, HIGH); + break; + + case 1: + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, HIGH); + break; + + case 2: + digitalWrite(B_LED_PIN, LOW); + digitalWrite(C_LED_PIN, HIGH); + break; + } +} +static void clear_leds() +{ + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, LOW); + digitalWrite(C_LED_PIN, LOW); + motor_light = false; + led_mode = NORMAL_LEDS; +} + +#if MOTOR_LEDS == 1 +static void update_motor_leds(void) +{ + if (motor_armed == true){ + if (low_batt == true){ + // blink rear + static bool blink = false; + + if (blink){ + digitalWrite(RE_LED, HIGH); + digitalWrite(FR_LED, HIGH); + digitalWrite(RI_LED, LOW); + digitalWrite(LE_LED, LOW); + }else{ + digitalWrite(RE_LED, LOW); + digitalWrite(FR_LED, LOW); + digitalWrite(RI_LED, HIGH); + digitalWrite(LE_LED, HIGH); + } + blink = !blink; + }else{ + digitalWrite(RE_LED, HIGH); + digitalWrite(FR_LED, HIGH); + digitalWrite(RI_LED, HIGH); + digitalWrite(LE_LED, HIGH); + } + }else { + digitalWrite(RE_LED, LOW); + digitalWrite(FR_LED, LOW); + digitalWrite(RI_LED, LOW); + digitalWrite(LE_LED, LOW); + } +} +#endif + + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define ARM_DELAY 10 // one second +#define DISARM_DELAY 10 // one second +#define LEVEL_DELAY 70 // twelve seconds +#define AUTO_LEVEL_DELAY 90 // twentyfive seconds + + +// called at 10hz +static void arm_motors() +{ + static int arming_counter; + + // Arm motor output : Throttle down and full yaw right for more than 2 seconds + if (g.rc_3.control_in == 0){ + + // full right + if (g.rc_4.control_in > 4000) { + + // don't allow arming in anything but manual + if (control_mode < ALT_HOLD){ + + if (arming_counter > AUTO_LEVEL_DELAY){ + auto_level_counter = 155; + arming_counter = 0; + + }else if (arming_counter == ARM_DELAY){ + #if HIL_MODE != HIL_MODE_DISABLED + gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); + #endif + motor_armed = true; + arming_counter = ARM_DELAY; + + #if PIEZO_ARMING == 1 + piezo_beep(); + piezo_beep(); + #endif + + // Tune down DCM + // ------------------- + #if HIL_MODE != HIL_MODE_ATTITUDE + dcm.kp_roll_pitch(0.030000); + dcm.ki_roll_pitch(0.00001278), // 50 hz I term + //dcm.ki_roll_pitch(0.000006); + #endif + + // tune down compass + // ----------------- + dcm.kp_yaw(0.08); + dcm.ki_yaw(0); + + // Remember Orientation + // -------------------- + init_simple_bearing(); + + // Reset home position + // ---------------------- + if(home_is_set) + init_home(); + + if(did_ground_start == false){ + did_ground_start = true; + startup_ground(); + } + + #if HIL_MODE != HIL_MODE_ATTITUDE + // read Baro pressure at ground - + // this resets Baro for more accuracy + //----------------------------------- + init_barometer(); + #endif + + // temp hack + motor_light = true; + digitalWrite(A_LED_PIN, HIGH); + + arming_counter++; + } else{ + arming_counter++; + } + } + + // full left + }else if (g.rc_4.control_in < -4000) { + //Serial.print(arming_counter, DEC); + if (arming_counter > LEVEL_DELAY){ + //Serial.print("init"); + imu.init_accel(mavlink_delay); + arming_counter = 0; + + }else if (arming_counter == DISARM_DELAY){ + #if HIL_MODE != HIL_MODE_DISABLED + gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); + #endif + + motor_armed = false; + arming_counter = DISARM_DELAY; + compass.save_offsets(); + + #if PIEZO_ARMING == 1 + piezo_beep(); + #endif + + // Tune down DCM + // ------------------- + #if HIL_MODE != HIL_MODE_ATTITUDE + //dcm.kp_roll_pitch(0.12); // higher for fast recovery + //dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop + #endif + + // tune up compass + // ----------------- + dcm.kp_yaw(0.8); + dcm.ki_yaw(0.00004); + + // Clear throttle slew + // ------------------- + //throttle_slew = 0; + + arming_counter++; + + }else{ + arming_counter++; + } + // centered + }else{ + arming_counter = 0; + } + }else{ + arming_counter = 0; + } +} + + +/***************************************** + * Set the flight control servos based on the current calculated values + *****************************************/ +static void +set_servos_4() +{ + if (motor_armed == true && motor_auto_armed == true) { + // creates the radio_out and pwm_out values + output_motors_armed(); + } else{ + output_motors_disarmed(); + } +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_hexa.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if FRAME_CONFIG == HEXA_FRAME + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + +static void output_motors_armed() +{ + int roll_out, pitch_out; + int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; + + // Throttle is 0 to 1000 only + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); + + if(g.rc_3.servo_out > 0) + out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; + + g.rc_1.calc_pwm(); + g.rc_2.calc_pwm(); + g.rc_3.calc_pwm(); + g.rc_4.calc_pwm(); + + if(g.frame_orientation == X_FRAME){ + roll_out = g.rc_1.pwm_out / 2; + pitch_out = (float)g.rc_2.pwm_out * .866; + + //left side + motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle + motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front + motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back + + //right side + motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle + motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front + motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back + + }else{ + roll_out = (float)g.rc_1.pwm_out * .866; + pitch_out = g.rc_2.pwm_out / 2; + + //Front side + motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT + motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT + motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT + + //Back side + motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK + motor_out[CH_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT + motor_out[CH_8] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT + } + + // Yaw + motor_out[CH_2] += g.rc_4.pwm_out; // CCW + motor_out[CH_7] += g.rc_4.pwm_out; // CCW + motor_out[CH_4] += g.rc_4.pwm_out; // CCW + + motor_out[CH_3] -= g.rc_4.pwm_out; // CW + motor_out[CH_1] -= g.rc_4.pwm_out; // CW + motor_out[CH_8] -= g.rc_4.pwm_out; // CW + + + // Tridge's stability patch + for (int i = CH_1; i<=CH_8; i++) { + if(i == CH_5 || i == CH_6) + break; + if (motor_out[i] > out_max) { + // note that i^1 is the opposite motor + motor_out[i^1] -= motor_out[i] - out_max; + motor_out[i] = out_max; + } + } + + // limit output so motors don't stop + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_3] = max(motor_out[CH_3], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_7] = max(motor_out[CH_7], out_min); + motor_out[CH_8] = max(motor_out[CH_8], out_min); + + #if CUT_MOTORS == ENABLED + // if we are not sending a throttle output, we cut the motors + if(g.rc_3.servo_out == 0){ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; + } + #endif + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); + + #if INSTANT_PWM == 1 + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif + +} + +static void output_motors_disarmed() +{ + if(g.rc_3.control_in > 0){ + // we have pushed up the throttle + // remove safety + motor_auto_armed = true; + } + + // fill the motor_out[] array for HIL use + for (unsigned char i = 0; i < 8; i++) { + motor_out[i] = g.rc_3.radio_min; + } + + // Send commands to motors + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); +} + +static void output_motor_test() +{ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; + + + if(g.frame_orientation == X_FRAME){ +// 31 +// 24 + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_1] += 100; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_2] += 100; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_8] += 100; + motor_out[CH_4] += 100; + } + + if(g.rc_2.control_in < -3000){ // front + motor_out[CH_7] += 100; + motor_out[CH_3] += 100; + } + + }else{ +// 3 +// 2 1 +// 4 + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_4] += 100; + motor_out[CH_8] += 100; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_7] += 100; + motor_out[CH_3] += 100; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_2] += 100; + } + + if(g.rc_2.control_in < -3000){ // front + motor_out[CH_1] += 100; + } + + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); +} + +/* + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + delay(1000); +} +*/ + +#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_octa.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if FRAME_CONFIG == OCTA_FRAME + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + +static void output_motors_armed() +{ + int roll_out, pitch_out; + int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; + + // Throttle is 0 to 1000 only + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); + + if(g.rc_3.servo_out > 0) + out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; + + g.rc_1.calc_pwm(); + g.rc_2.calc_pwm(); + g.rc_3.calc_pwm(); + g.rc_4.calc_pwm(); + + if(g.frame_orientation == X_FRAME){ + roll_out = (float)g.rc_1.pwm_out * 0.4; + pitch_out = (float)g.rc_2.pwm_out * 0.4; + + //Front side + motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT + + //Back side + motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT + motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT + + //Left side + motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT + motor_out[CH_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK + + //Right side + motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK + motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT + + }else if(g.frame_orientation == PLUS_FRAME){ + roll_out = (float)g.rc_1.pwm_out * 0.71; + pitch_out = (float)g.rc_2.pwm_out * 0.71; + + //Front side + motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT + motor_out[CH_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT + motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT + + //Left side + motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT + + //Right side + motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT + + //Back side + motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK + motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT + motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT + + }else if(g.frame_orientation == V_FRAME){ + + int roll_out2, pitch_out2; + int roll_out3, pitch_out3; + int roll_out4, pitch_out4; + + roll_out = g.rc_1.pwm_out; + pitch_out = g.rc_2.pwm_out; + roll_out2 = (float)g.rc_1.pwm_out * 0.833; + pitch_out2 = (float)g.rc_2.pwm_out * 0.34; + roll_out3 = (float)g.rc_1.pwm_out * 0.666; + pitch_out3 = (float)g.rc_2.pwm_out * 0.32; + roll_out4 = g.rc_1.pwm_out / 2; + pitch_out4 = (float)g.rc_2.pwm_out * 0.98; + + //Front side + motor_out[CH_10] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT + + //Left side + motor_out[CH_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT + motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK + + //Right side + motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK + motor_out[CH_8] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT + + //Back side + motor_out[CH_11] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT + motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT + + } + + // Yaw + motor_out[CH_3] += g.rc_4.pwm_out; // CCW + motor_out[CH_4] += g.rc_4.pwm_out; // CCW + motor_out[CH_7] += g.rc_4.pwm_out; // CCW + motor_out[CH_8] += g.rc_4.pwm_out; // CCW + + motor_out[CH_1] -= g.rc_4.pwm_out; // CW + motor_out[CH_2] -= g.rc_4.pwm_out; // CW + motor_out[CH_10] -= g.rc_4.pwm_out; // CW + motor_out[CH_11] -= g.rc_4.pwm_out; // CW + + + // TODO add stability patch + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + motor_out[CH_7] = min(motor_out[CH_7], out_max); + motor_out[CH_8] = min(motor_out[CH_8], out_max); + motor_out[CH_10] = min(motor_out[CH_10], out_max); + motor_out[CH_11] = min(motor_out[CH_11], out_max); + + + // limit output so motors don't stop + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_3] = max(motor_out[CH_3], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_7] = max(motor_out[CH_7], out_min); + motor_out[CH_8] = max(motor_out[CH_8], out_min); + motor_out[CH_10] = max(motor_out[CH_10], out_min); + motor_out[CH_11] = max(motor_out[CH_11], out_min); + + + #if CUT_MOTORS == ENABLED + // if we are not sending a throttle output, we cut the motors + if(g.rc_3.servo_out == 0){ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; + motor_out[CH_10] = g.rc_3.radio_min; + motor_out[CH_11] = g.rc_3.radio_min; + } + #endif + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); + APM_RC.OutputCh(CH_10, motor_out[CH_10]); + APM_RC.OutputCh(CH_11, motor_out[CH_11]); + + #if INSTANT_PWM == 1 + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif +} + +static void output_motors_disarmed() +{ + if(g.rc_3.control_in > 0){ + // we have pushed up the throttle + // remove safety + motor_auto_armed = true; + } + + // fill the motor_out[] array for HIL use + for (unsigned char i = 0; i < 11; i++) { + motor_out[i] = g.rc_3.radio_min; + } + + // Send commands to motors + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); +} + +static void output_motor_test() +{ + if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME ) + { + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + delay(1000); + } + + if( g.frame_orientation == V_FRAME ) + { + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + delay(1000); + } +} + +#endif + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_octa_quad.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if FRAME_CONFIG == OCTA_QUAD_FRAME + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + +static void output_motors_armed() +{ + int roll_out, pitch_out; + int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; + + // Throttle is 0 to 1000 only + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); + + if(g.rc_3.servo_out > 0) + out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; + + g.rc_1.calc_pwm(); + g.rc_2.calc_pwm(); + g.rc_3.calc_pwm(); + g.rc_4.calc_pwm(); + + if(g.frame_orientation == X_FRAME){ + roll_out = (float)g.rc_1.pwm_out * .707; + pitch_out = (float)g.rc_2.pwm_out * .707; + + // Front Left + motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP + motor_out[CH_8] = g.rc_3.radio_out + roll_out + pitch_out; // CW + + // Front Right + motor_out[CH_10] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP + motor_out[CH_11] = g.rc_3.radio_out - roll_out + pitch_out; // CW + + // Back Left + motor_out[CH_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP + motor_out[CH_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW + + // Back Right + motor_out[CH_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP + motor_out[CH_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW + + + + }if(g.frame_orientation == PLUS_FRAME){ + roll_out = g.rc_1.pwm_out; + pitch_out = g.rc_2.pwm_out; + + // Left + motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP + motor_out[CH_8] = g.rc_3.radio_out - roll_out; // CW + + // Right + motor_out[CH_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP + motor_out[CH_2] = g.rc_3.radio_out + roll_out; // CW + + // Front + motor_out[CH_10] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP + motor_out[CH_11] = g.rc_3.radio_out + pitch_out; // CW + + // Back + motor_out[CH_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP + motor_out[CH_4] = g.rc_3.radio_out - pitch_out; // CW + + } + + // Yaw + motor_out[CH_1] += g.rc_4.pwm_out; // CCW + motor_out[CH_3] += g.rc_4.pwm_out; // CCW + motor_out[CH_7] += g.rc_4.pwm_out; // CCW + motor_out[CH_10] += g.rc_4.pwm_out; // CCW + + motor_out[CH_2] -= g.rc_4.pwm_out; // CW + motor_out[CH_4] -= g.rc_4.pwm_out; // CW + motor_out[CH_8] -= g.rc_4.pwm_out; // CW + motor_out[CH_11] -= g.rc_4.pwm_out; // CW + + // TODO add stability patch + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + motor_out[CH_7] = min(motor_out[CH_7], out_max); + motor_out[CH_8] = min(motor_out[CH_8], out_max); + motor_out[CH_10] = min(motor_out[CH_10], out_max); + motor_out[CH_11] = min(motor_out[CH_11], out_max); + + // limit output so motors don't stop + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_3] = max(motor_out[CH_3], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_7] = max(motor_out[CH_7], out_min); + motor_out[CH_8] = max(motor_out[CH_8], out_min); + motor_out[CH_10] = max(motor_out[CH_10], out_min); + motor_out[CH_11] = max(motor_out[CH_11], out_min); + + #if CUT_MOTORS == ENABLED + // if we are not sending a throttle output, we cut the motors + if(g.rc_3.servo_out == 0){ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; + motor_out[CH_10] = g.rc_3.radio_min; + motor_out[CH_11] = g.rc_3.radio_min; + } + #endif + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); + APM_RC.OutputCh(CH_10, motor_out[CH_10]); + APM_RC.OutputCh(CH_11, motor_out[CH_11]); + + #if INSTANT_PWM == 1 + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif +} + +static void output_motors_disarmed() +{ + if(g.rc_3.control_in > 0){ + // we have pushed up the throttle + // remove safety + motor_auto_armed = true; + } + + // fill the motor_out[] array for HIL use + for (unsigned char i = 0; i < 11; i++) { + motor_out[i] = g.rc_3.radio_min; + } + + // Send commands to motors + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); +} + +static void output_motor_test() +{ + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + delay(1000); + + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + delay(1000); +} + +#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_quad.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if FRAME_CONFIG == QUAD_FRAME + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 40000; // 50 hz output CH 7, 8, 11 + #endif +} + +static void output_motors_armed() +{ + int roll_out, pitch_out; + int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; + + // Throttle is 0 to 1000 only + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); + + if(g.rc_3.servo_out > 0) + out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; + + g.rc_1.calc_pwm(); + g.rc_2.calc_pwm(); + g.rc_3.calc_pwm(); + g.rc_4.calc_pwm(); + + if(g.frame_orientation == X_FRAME){ + roll_out = g.rc_1.pwm_out * .707; + pitch_out = g.rc_2.pwm_out * .707; + + // left + motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT + motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK + + // right + motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT + motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK + + }else{ + + roll_out = g.rc_1.pwm_out; + pitch_out = g.rc_2.pwm_out; + + // left + motor_out[CH_1] = g.rc_3.radio_out - roll_out; + // right + motor_out[CH_2] = g.rc_3.radio_out + roll_out; + // front + motor_out[CH_3] = g.rc_3.radio_out + pitch_out; + // back + motor_out[CH_4] = g.rc_3.radio_out - pitch_out; + } + + // Yaw input + motor_out[CH_1] += g.rc_4.pwm_out; // CCW + motor_out[CH_2] += g.rc_4.pwm_out; // CCW + motor_out[CH_3] -= g.rc_4.pwm_out; // CW + motor_out[CH_4] -= g.rc_4.pwm_out; // CW + + /* We need to clip motor output at out_max. When cipping a motors + * output we also need to compensate for the instability by + * lowering the opposite motor by the same proportion. This + * ensures that we retain control when one or more of the motors + * is at its maximum output + */ + for (int i=CH_1; i<=CH_4; i++) { + if (motor_out[i] > out_max) { + // note that i^1 is the opposite motor + motor_out[i^1] -= motor_out[i] - out_max; + motor_out[i] = out_max; + } + } + + // limit output so motors don't stop + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_3] = max(motor_out[CH_3], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + + #if CUT_MOTORS == ENABLED + // if we are not sending a throttle output, we cut the motors + if(g.rc_3.servo_out == 0){ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + } + #endif + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + + #if INSTANT_PWM == 1 + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + #endif +} + +static void output_motors_disarmed() +{ + if(g.rc_3.control_in > 0){ + // we have pushed up the throttle + // remove safety + motor_auto_armed = true; + } + + // fill the motor_out[] array for HIL use + for (unsigned char i = 0; i < 8; i++) { + motor_out[i] = g.rc_3.radio_min; + } + + // Send commands to motors + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); +} + +/* +static void debug_motors() +{ + Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n", + motor_out[CH_1], + motor_out[CH_2], + motor_out[CH_3], + motor_out[CH_4]); +} +*/ + +static void output_motor_test() +{ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + + + if(g.frame_orientation == X_FRAME){ +// 31 +// 24 + if(g.rc_1.control_in > 3000){ + motor_out[CH_1] += 100; + motor_out[CH_4] += 100; + } + + if(g.rc_1.control_in < -3000){ + motor_out[CH_2] += 100; + motor_out[CH_3] += 100; + } + + if(g.rc_2.control_in > 3000){ + motor_out[CH_2] += 100; + motor_out[CH_4] += 100; + } + + if(g.rc_2.control_in < -3000){ + motor_out[CH_1] += 100; + motor_out[CH_3] += 100; + } + + }else{ +// 3 +// 2 1 +// 4 + if(g.rc_1.control_in > 3000) + motor_out[CH_1] += 100; + + if(g.rc_1.control_in < -3000) + motor_out[CH_2] += 100; + + if(g.rc_2.control_in > 3000) + motor_out[CH_4] += 100; + + if(g.rc_2.control_in < -3000) + motor_out[CH_3] += 100; + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); +} + +#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_tri.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#if FRAME_CONFIG == TRI_FRAME + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 40000; // 50 hz output CH 7, 8, 11 + #endif +} + + +static void output_motors_armed() +{ + int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; + + // Throttle is 0 to 1000 only + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); + + if(g.rc_3.servo_out > 0) + out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; + + g.rc_1.calc_pwm(); + g.rc_2.calc_pwm(); + g.rc_3.calc_pwm(); + + int roll_out = (float)g.rc_1.pwm_out * .866; + int pitch_out = g.rc_2.pwm_out / 2; + + //left front + motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out; + //right front + motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; + // rear + motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; + + //motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013; + + // Tridge's stability patch + if (motor_out[CH_1] > out_max) { + motor_out[CH_2] -= (motor_out[CH_1] - out_max) >> 1; + motor_out[CH_4] -= (motor_out[CH_1] - out_max) >> 1; + motor_out[CH_1] = out_max; + } + + if (motor_out[CH_2] > out_max) { + motor_out[CH_1] -= (motor_out[CH_2] - out_max) >> 1; + motor_out[CH_4] -= (motor_out[CH_2] - out_max) >> 1; + motor_out[CH_2] = out_max; + } + + if (motor_out[CH_4] > out_max) { + motor_out[CH_1] -= (motor_out[CH_4] - out_max) >> 1; + motor_out[CH_2] -= (motor_out[CH_4] - out_max) >> 1; + motor_out[CH_4] = out_max; + } + + // limit output so motors don't stop + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + + #if CUT_MOTORS == ENABLED + // if we are not sending a throttle output, we cut the motors + if(g.rc_3.servo_out == 0){ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + } + #endif + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + + #if INSTANT_PWM == 1 + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + #endif +} + +static void output_motors_disarmed() +{ + if(g.rc_3.control_in > 0){ + // we have pushed up the throttle + // remove safety + motor_auto_armed = true; + } + + // fill the motor_out[] array for HIL use + for (unsigned char i = 0; i < 8; i++) { + motor_out[i] = g.rc_3.radio_min; + } + + // Send commands to motors + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); +} + +static void output_motor_test() +{ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + + + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_1] += 100; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_2] += 100; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_4] += 100; + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); +} + +#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_y6.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if FRAME_CONFIG == Y6_FRAME + +#define YAW_DIRECTION 1 + + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + +static void output_motors_armed() +{ + int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; + + // Throttle is 0 to 1000 only + g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); + + if(g.rc_3.servo_out > 0) + out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; + + g.rc_1.calc_pwm(); + g.rc_2.calc_pwm(); + g.rc_3.calc_pwm(); + g.rc_4.calc_pwm(); + + // Multi-Wii Mix + //left + motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW + motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW + //right + motor_out[CH_7] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW + motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW + //back + motor_out[CH_8] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW + motor_out[CH_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW + + //left + motor_out[CH_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW + motor_out[CH_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW + //right + motor_out[CH_7] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW + motor_out[CH_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW + //back + motor_out[CH_8] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW + motor_out[CH_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW + + + /* + int roll_out = (float)g.rc_1.pwm_out * .866; + int pitch_out = g.rc_2.pwm_out / 2; + + //left + motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP + motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW + + //right + motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP + motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW + + //back + motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP + motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW + + // Yaw + //top + motor_out[CH_2] += g.rc_4.pwm_out; // CCW + motor_out[CH_7] += g.rc_4.pwm_out; // CCW + motor_out[CH_8] += g.rc_4.pwm_out; // CCW + + //bottom + motor_out[CH_3] -= g.rc_4.pwm_out; // CW + motor_out[CH_1] -= g.rc_4.pwm_out; // CW + motor_out[CH_4] -= g.rc_4.pwm_out; // CW + */ + + // TODO: add stability patch + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + motor_out[CH_7] = min(motor_out[CH_7], out_max); + motor_out[CH_8] = min(motor_out[CH_8], out_max); + + // limit output so motors don't stop + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_3] = max(motor_out[CH_3], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_7] = max(motor_out[CH_7], out_min); + motor_out[CH_8] = max(motor_out[CH_8], out_min); + + #if CUT_MOTORS == ENABLED + // if we are not sending a throttle output, we cut the motors + if(g.rc_3.servo_out == 0){ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; + } + #endif + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); + + #if INSTANT_PWM == 1 + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif +} + +static void output_motors_disarmed() +{ + if(g.rc_3.control_in > 0){ + // we have pushed up the throttle + // remove safety + motor_auto_armed = true; + } + + // fill the motor_out[] array for HIL use + for (unsigned char i = 0; i < 8; i++) { + motor_out[i] = g.rc_3.radio_min; + } + + // Send commands to motors + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); +} + +static void output_motor_test() +{ + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; + + + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_1] += 100; + motor_out[CH_7] += 100; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_2] += 100; + motor_out[CH_3] += 100; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_8] += 100; + motor_out[CH_4] += 100; + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_4]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); +} + +#endif +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/navigation.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +//**************************************************************** +// Function that will calculate the desired direction to fly and distance +//**************************************************************** +static byte navigate() +{ + if(next_WP.lat == 0){ + return 0; + } + + // waypoint distance from plane + // ---------------------------- + wp_distance = get_distance(¤t_loc, &next_WP); + + if (wp_distance < 0){ + //gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); + //Serial.println(wp_distance,DEC); + //print_current_waypoints(); + return 0; + } + + // target_bearing is where we should be heading + // -------------------------------------------- + target_bearing = get_bearing(¤t_loc, &next_WP); + return 1; +} + +static bool check_missed_wp() +{ + long temp = target_bearing - original_target_bearing; + temp = wrap_180(temp); + return (abs(temp) > 10000); //we pased the waypoint by 10 ° +} + +// ------------------------------ + +// long_error, lat_error +static void calc_location_error(struct Location *next_loc) +{ + /* + Becuase we are using lat and lon to do our distance errors here's a quick chart: + 100 = 1m + 1000 = 11m = 36 feet + 1800 = 19.80m = 60 feet + 3000 = 33m + 10000 = 111m + pitch_max = 22° (2200) + */ + + // X ROLL + long_error = (float)(next_loc->lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST + + // Y PITCH + lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH +} + +#define NAV_ERR_MAX 800 +static void calc_loiter(int x_error, int y_error) +{ + x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); + y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); + + int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); + int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); + + // find the rates: + float temp = radians((float)g_gps->ground_course/100.0); + + #ifdef OPTFLOW_ENABLED + // calc the cos of the error to tell how fast we are moving towards the target in cm + if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){ + x_actual_speed = optflow.vlon * 10; + y_actual_speed = optflow.vlat * 10; + }else{ + x_actual_speed = (float)g_gps->ground_speed * sin(temp); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); + } + #else + x_actual_speed = (float)g_gps->ground_speed * sin(temp); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); + #endif + + y_rate_error = y_target_speed - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum + nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); + nav_lat = constrain(nav_lat, -3500, 3500); + + x_rate_error = x_target_speed - x_actual_speed; + x_rate_error = constrain(x_rate_error, -250, 250); + nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav); + nav_lon = constrain(nav_lon, -3500, 3500); +} + +static void calc_loiter2(int x_error, int y_error) +{ + static int last_x_error = 0; + static int last_y_error = 0; + + x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); + y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); + + int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); + int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); + + // find the rates: + x_actual_speed = (float)(last_x_error - x_error)/dTnav; + y_actual_speed = (float)(last_y_error - y_error)/dTnav; + + // save speeds + last_x_error = x_error; + last_y_error = y_error; + + y_rate_error = y_target_speed - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum + nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); + nav_lat = constrain(nav_lat, -3500, 3500); + + x_rate_error = x_target_speed - x_actual_speed; + x_rate_error = constrain(x_rate_error, -250, 250); + nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav); + nav_lon = constrain(nav_lon, -3500, 3500); +} + +// nav_roll, nav_pitch +static void calc_loiter_pitch_roll() +{ + + //float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); + //float _cos_yaw_x = cos(temp); + //float _sin_yaw_y = sin(temp); + + //Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y); + + // rotate the vector + nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; + nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y; + + // flip pitch because forward is negative + nav_pitch = -nav_pitch; +} + +static void calc_nav_rate(int max_speed) +{ + /* + 0 1 2 3 4 5 6 7 8 + ...|...|...|...|...|...|...|...| + 100 200 300 400 + +|+ + */ + max_speed = min(max_speed, (wp_distance * 50)); + + // limit the ramp up of the speed + if(waypoint_speed_gov < max_speed){ + + waypoint_speed_gov += (int)(150.0 * dTnav); // increase at 1.5/ms + + // go at least 1m/s + max_speed = max(100, waypoint_speed_gov); + // limit with governer + max_speed = min(max_speed, waypoint_speed_gov); + } + + // XXX target_angle should be the original desired target angle! + float temp = radians((original_target_bearing - g_gps->ground_course)/100.0); + + x_actual_speed = -sin(temp) * (float)g_gps->ground_speed; + x_rate_error = -x_actual_speed; + x_rate_error = constrain(x_rate_error, -800, 800); + nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); + + y_actual_speed = cos(temp) * (float)g_gps->ground_speed; + y_rate_error = max_speed - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -800, 800); // added a rate error limit to keep pitching down to a minimum + nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); + + /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat);*/ +} + +// nav_roll, nav_pitch +static void calc_nav_pitch_roll() +{ + float temp = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0); + float _cos_yaw_x = cos(temp); + float _sin_yaw_y = sin(temp); + + // rotate the vector + nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x; + nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y; + + // flip pitch because forward is negative + nav_pitch = -nav_pitch; + + /*Serial.printf("_cos_yaw_x:%1.4f, _sin_yaw_y:%1.4f, nav_roll:%ld, nav_pitch:%ld\n", + _cos_yaw_x, + _sin_yaw_y, + nav_roll, + nav_pitch);*/ +} + +static long get_altitude_error() +{ + return next_WP.alt - current_loc.alt; +} + +static int get_loiter_angle() +{ + float power; + int angle; + + if(wp_distance <= g.loiter_radius){ + power = float(wp_distance) / float(g.loiter_radius); + power = constrain(power, 0.5, 1); + angle = 90.0 * (2.0 + power); + }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ + power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); + power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); + angle = power * 90; + } + + return angle; +} + +static long wrap_360(long error) +{ + if (error > 36000) error -= 36000; + if (error < 0) error += 36000; + return error; +} + +static long wrap_180(long error) +{ + if (error > 18000) error -= 36000; + if (error < -18000) error += 36000; + return error; +} + +/* +static long get_crosstrack_correction(void) +{ + // Crosstrack Error + // ---------------- + if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following + + // Meters we are off track line + float error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; + + // take meters * 100 to get adjustment to nav_bearing + long _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100; + + // constrain answer to 30° to avoid overshoot + return constrain(_crosstrack_correction, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); + } + return 0; +} +*/ +/* +static long cross_track_test() +{ + long temp = wrap_180(target_bearing - crosstrack_bearing); + return abs(temp); +} +*/ +/* +static void reset_crosstrack() +{ + crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following +} +*/ +/*static long get_altitude_above_home(void) +{ + // This is the altitude above the home location + // The GPS gives us altitude at Sea Level + // if you slope soar, you should see a negative number sometimes + // ------------------------------------------------------------- + return current_loc.alt - home.alt; +} +*/ +// distance is returned in meters +static long get_distance(struct Location *loc1, struct Location *loc2) +{ + //if(loc1->lat == 0 || loc1->lng == 0) + // return -1; + //if(loc2->lat == 0 || loc2->lng == 0) + // return -1; + float dlat = (float)(loc2->lat - loc1->lat); + float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; + return sqrt(sq(dlat) + sq(dlong)) * .01113195; +} +/* +static long get_alt_distance(struct Location *loc1, struct Location *loc2) +{ + return abs(loc1->alt - loc2->alt); +} +*/ +static long get_bearing(struct Location *loc1, struct Location *loc2) +{ + long off_x = loc2->lng - loc1->lng; + long off_y = (loc2->lat - loc1->lat) * scaleLongUp; + long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + if (bearing < 0) bearing += 36000; + return bearing; +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/planner.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// These are function definitions so the Menu can be constructed before the functions +// are defined below. Order matters to the compiler. +static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Common for implementation details +const struct Menu::command planner_menu_commands[] PROGMEM = { + {"gcs", planner_gcs}, +}; + +// A Macro to create the Menu +MENU(planner_menu, "planner", planner_menu_commands); + +static int8_t +planner_mode(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Planner Mode\nNot intended for manual use\n\n")); + planner_menu.run(); + return (0); +} + +static int8_t +planner_gcs(uint8_t argc, const Menu::arg *argv) +{ + gcs0.init(&Serial); + + int loopcount = 0; + + while (1) { + if (millis()-fast_loopTimer > 19) { + fast_loopTimer = millis(); + + gcs_update(); + + gcs_data_stream_send(45, 1000); + + if ((loopcount % 5) == 0) // 10 hz + gcs_data_stream_send(5, 45); + + if ((loopcount % 16) == 0) { // 3 hz + gcs_data_stream_send(1, 5); + gcs_send_message(MSG_HEARTBEAT); + } + + loopcount++; + } + } + return 0; +} + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/radio.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +//Function that will read the radio data, limit servos and trigger a failsafe +// ---------------------------------------------------------------------------- +static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling + +static void default_dead_zones() +{ + g.rc_1.set_dead_zone(60); + g.rc_2.set_dead_zone(60); + g.rc_3.set_dead_zone(60); + g.rc_4.set_dead_zone(200); +} + +static void init_rc_in() +{ + // set rc channel ranges + g.rc_1.set_angle(4500); + g.rc_2.set_angle(4500); + g.rc_3.set_range(0,1000); + #if FRAME_CONFIG != HELI_FRAME + g.rc_3.scale_output = .9; + #endif + g.rc_4.set_angle(4500); + + // reverse: CW = left + // normal: CW = left??? + + + g.rc_1.set_type(RC_CHANNEL_ANGLE_RAW); + g.rc_2.set_type(RC_CHANNEL_ANGLE_RAW); + g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); + + // set rc dead zones + /*g.rc_1.dead_zone = 60; + g.rc_2.dead_zone = 60; + g.rc_3.dead_zone = 60; + g.rc_4.dead_zone = 300; + */ + + + //set auxiliary ranges + g.rc_5.set_range(0,1000); + g.rc_6.set_range(0,1000); + g.rc_7.set_range(0,1000); + g.rc_8.set_range(0,1000); + +} + +static void init_rc_out() +{ + #if ARM_AT_STARTUP == 1 + motor_armed = 1; + #endif + + + APM_RC.Init(); // APM Radio initialization + init_motors_out(); + + // fix for crazy output + OCR1B = 0xFFFF; // PB6, OUT3 + OCR1C = 0xFFFF; // PB7, OUT4 + OCR5B = 0xFFFF; // PL4, OUT1 + OCR5C = 0xFFFF; // PL5, OUT2 + OCR4B = 0xFFFF; // PH4, OUT6 + OCR4C = 0xFFFF; // PH5, OUT5 + + // this is the camera pitch5 and roll6 + APM_RC.OutputCh(CH_5, 1500); + APM_RC.OutputCh(CH_6, 1500); + + // don't fuss if we are calibrating + if(g.esc_calibrate == 1) + return; + + if(g.rc_3.radio_min <= 1200){ + output_min(); + } + + for(byte i = 0; i < 5; i++){ + delay(20); + read_radio(); + } + + // sanity check + if(g.rc_3.radio_min >= 1300){ + g.rc_3.radio_min = g.rc_3.radio_in; + output_min(); + } +} + +void output_min() +{ + #if FRAME_CONFIG == HELI_FRAME + heli_move_servos_to_mid(); + #else + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs + APM_RC.OutputCh(CH_2, g.rc_3.radio_min); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + #endif + + APM_RC.OutputCh(CH_7, g.rc_3.radio_min); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + + #if FRAME_CONFIG == OCTA_FRAME + APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + #endif + +} +static void read_radio() +{ + if (APM_RC.GetState() == 1){ + new_radio_frame = true; + g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); + g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); + g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); + g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); + g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); + g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); + g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); + g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); + + #if FRAME_CONFIG != HELI_FRAME + // limit our input to 800 so we can still pitch and roll + g.rc_3.control_in = min(g.rc_3.control_in, 800); + #endif + + //throttle_failsafe(g.rc_3.radio_in); + } +} + +static void throttle_failsafe(uint16_t pwm) +{ + if(g.throttle_fs_enabled == 0) + return; + + //check for failsafe and debounce funky reads + // ------------------------------------------ + if (pwm < (unsigned)g.throttle_fs_value){ + // we detect a failsafe from radio + // throttle has dropped below the mark + failsafeCounter++; + if (failsafeCounter == 9){ + SendDebug("MSG FS ON "); + SendDebugln(pwm, DEC); + }else if(failsafeCounter == 10) { + ch3_failsafe = true; + //set_failsafe(true); + //failsafeCounter = 10; + }else if (failsafeCounter > 10){ + failsafeCounter = 11; + } + + }else if(failsafeCounter > 0){ + // we are no longer in failsafe condition + // but we need to recover quickly + failsafeCounter--; + if (failsafeCounter > 3){ + failsafeCounter = 3; + } + if (failsafeCounter == 1){ + SendDebug("MSG FS OFF "); + SendDebugln(pwm, DEC); + }else if(failsafeCounter == 0) { + ch3_failsafe = false; + //set_failsafe(false); + //failsafeCounter = -1; + }else if (failsafeCounter <0){ + failsafeCounter = -1; + } + } +} + +static void trim_radio() +{ + for (byte i = 0; i < 30; i++){ + read_radio(); + } + + g.rc_1.trim(); // roll + g.rc_2.trim(); // pitch + g.rc_4.trim(); // yaw + + g.rc_1.save_eeprom(); + g.rc_2.save_eeprom(); + g.rc_4.save_eeprom(); +} + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/sensors.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// Sensors are not available in HIL_MODE_ATTITUDE +#if HIL_MODE != HIL_MODE_ATTITUDE + +static void ReadSCP1000(void) {} + +static void init_barometer(void) +{ + #if HIL_MODE == HIL_MODE_SENSORS + gcs_update(); // look for inbound hil packets for initialization + #endif + + ground_temperature = barometer.Temp; + int i; + + // We take some readings... + for(i = 0; i < 60; i++){ + delay(20); + + // get new data from absolute pressure sensor + barometer.Read(); + + //Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press); + } + + for(i = 0; i < 20; i++){ + delay(20); + + #if HIL_MODE == HIL_MODE_SENSORS + gcs_update(); // look for inbound hil packets + #endif + + // Get initial data from absolute pressure sensor + barometer.Read(); + ground_pressure = barometer.Press; + ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; + //Serial.printf("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press, ground_temperature, ground_pressure); + } + + abs_pressure = ground_pressure; + + //Serial.printf("init %ld\n", abs_pressure); + //SendDebugln("barometer calibration complete."); +} + +/* +static long read_baro_filtered(void) +{ + + // get new data from absolute pressure sensor + barometer.Read(); + + return barometer.Press; + + long pressure = 0; + // add new data into our filter + baro_filter[baro_filter_index] = barometer.Press; + baro_filter_index++; + + // loop our filter + if(baro_filter_index >= BARO_FILTER_SIZE) + baro_filter_index = 0; + + // zero out our accumulator + + // sum our filter + for(byte i = 0; i < BARO_FILTER_SIZE; i++){ + pressure += baro_filter[i]; + } + + + // average our sampels + return pressure /= BARO_FILTER_SIZE; + // +} +*/ +static long read_barometer(void) +{ + float x, scaling, temp; + + barometer.Read(); + abs_pressure = barometer.Press; + + + //Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure); + + scaling = (float)ground_pressure / (float)abs_pressure; + temp = ((float)ground_temperature / 10.0f) + 273.15f; + x = log(scaling) * temp * 29271.267f; + return (x / 10); +} + +// in M/S * 100 +static void read_airspeed(void) +{ + +} + +static void zero_airspeed(void) +{ + +} + +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void read_battery(void) +{ + battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; + battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; + battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9; + battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9; + + if(g.battery_monitoring == 1) + battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream + + if(g.battery_monitoring == 2) + battery_voltage = battery_voltage4; + + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) + battery_voltage = battery_voltage1; + + if(g.battery_monitoring == 4) { + current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin + current_total += current_amps * 0.0278; // called at 100ms on average + } + + #if BATTERY_EVENT == 1 + //if(battery_voltage < g.low_voltage) + // low_battery_event(); + + if((battery_voltage < g.low_voltage) || (g.battery_monitoring == 4 && current_total > g.pack_capacity)){ + low_battery_event(); + + #if PIEZO_LOW_VOLTAGE == 1 + // Only Activate if a battery is connected to avoid alarm on USB only + if (battery_voltage1 > 1){ + piezo_on(); + }else{ + piezo_off(); + } + #endif + + }else{ + #if PIEZO_LOW_VOLTAGE == 1 + piezo_off(); + #endif + } + #endif +} + +//v: 10.9453, a: 17.4023, mah: 8.2 +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/setup.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if CLI_ENABLED == ENABLED + +// Functions called from the setup menu +static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); +static int8_t setup_motors (uint8_t argc, const Menu::arg *argv); +static int8_t setup_accel (uint8_t argc, const Menu::arg *argv); +static int8_t setup_frame (uint8_t argc, const Menu::arg *argv); +static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); +static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); +static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); +static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); +static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); +static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); +static int8_t setup_tune (uint8_t argc, const Menu::arg *argv); +//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); +static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); +static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); +#ifdef OPTFLOW_ENABLED +static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv); +#endif +static int8_t setup_show (uint8_t argc, const Menu::arg *argv); + +#if FRAME_CONFIG == HELI_FRAME + static int8_t setup_heli (uint8_t argc, const Menu::arg *argv); + static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv); +#endif + +// Command/function table for the setup menu +const struct Menu::command setup_menu_commands[] PROGMEM = { + // command function called + // ======= =============== + {"erase", setup_erase}, + {"reset", setup_factory}, + {"radio", setup_radio}, + {"frame", setup_frame}, + {"motors", setup_motors}, + {"esc", setup_esc}, + {"level", setup_accel}, + {"modes", setup_flightmodes}, + {"battery", setup_batt_monitor}, + {"sonar", setup_sonar}, + {"compass", setup_compass}, + {"tune", setup_tune}, +// {"offsets", setup_mag_offset}, + {"declination", setup_declination}, +#ifdef OPTFLOW_ENABLED + {"optflow", setup_optflow}, +#endif +#if FRAME_CONFIG == HELI_FRAME + {"heli", setup_heli}, + {"gyro", setup_gyro}, +#endif + {"show", setup_show} +}; + +// Create the setup menu object. +MENU(setup_menu, "setup", setup_menu_commands); + +// Called from the top-level menu to run the setup menu. +static int8_t +setup_mode(uint8_t argc, const Menu::arg *argv) +{ + // Give the user some guidance + Serial.printf_P(PSTR("Setup Mode\n\n\n")); + //"\n" + //"IMPORTANT: if you have not previously set this system up, use the\n" + //"'reset' command to initialize the EEPROM to sensible default values\n" + //"and then the 'radio' command to configure for your radio.\n" + //"\n")); + + if(g.rc_1.radio_min >= 1300){ + delay(1000); + Serial.printf_P(PSTR("\n!Warning, your radio is not configured!")); + delay(1000); + Serial.printf_P(PSTR("\n Type 'radio' to configure now.\n\n")); + } + + // Run the setup menu. When the menu exits, we will return to the main menu. + setup_menu.run(); + return 0; +} + +// Print the current configuration. +// Called by the setup menu 'show' command. +static int8_t +setup_show(uint8_t argc, const Menu::arg *argv) +{ + // clear the area + print_blanks(8); + + report_version(); + report_radio(); + report_frame(); + report_batt_monitor(); + report_sonar(); + //report_gains(); + //report_xtrack(); + //report_throttle(); + report_flight_modes(); + report_imu(); + report_compass(); + +#ifdef OPTFLOW_ENABLED + report_optflow(); +#endif + +#if FRAME_CONFIG == HELI_FRAME + report_heli(); + report_gyro(); +#endif + + AP_Var_menu_show(argc, argv); + return(0); +} + +// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). +// Called by the setup menu 'factoryreset' command. +static int8_t +setup_factory(uint8_t argc, const Menu::arg *argv) +{ + int c; + + Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n")); + + do { + c = Serial.read(); + } while (-1 == c); + + if (('y' != c) && ('Y' != c)) + return(-1); + + AP_Var::erase_all(); + Serial.printf_P(PSTR("\nReboot APM")); + + delay(1000); + //default_gains(); + + for (;;) { + } + // note, cannot actually return here + return(0); +} + +// Perform radio setup. +// Called by the setup menu 'radio' command. +static int8_t +setup_radio(uint8_t argc, const Menu::arg *argv) +{ + Serial.println("\n\nRadio Setup:"); + uint8_t i; + + for(i = 0; i < 100;i++){ + delay(20); + read_radio(); + } + + if(g.rc_1.radio_in < 500){ + while(1){ + //Serial.printf_P(PSTR("\nNo radio; Check connectors.")); + delay(1000); + // stop here + } + } + + g.rc_1.radio_min = g.rc_1.radio_in; + g.rc_2.radio_min = g.rc_2.radio_in; + g.rc_3.radio_min = g.rc_3.radio_in; + g.rc_4.radio_min = g.rc_4.radio_in; + g.rc_5.radio_min = g.rc_5.radio_in; + g.rc_6.radio_min = g.rc_6.radio_in; + g.rc_7.radio_min = g.rc_7.radio_in; + g.rc_8.radio_min = g.rc_8.radio_in; + + g.rc_1.radio_max = g.rc_1.radio_in; + g.rc_2.radio_max = g.rc_2.radio_in; + g.rc_3.radio_max = g.rc_3.radio_in; + g.rc_4.radio_max = g.rc_4.radio_in; + g.rc_5.radio_max = g.rc_5.radio_in; + g.rc_6.radio_max = g.rc_6.radio_in; + g.rc_7.radio_max = g.rc_7.radio_in; + g.rc_8.radio_max = g.rc_8.radio_in; + + g.rc_1.radio_trim = g.rc_1.radio_in; + g.rc_2.radio_trim = g.rc_2.radio_in; + g.rc_4.radio_trim = g.rc_4.radio_in; + // 3 is not trimed + g.rc_5.radio_trim = 1500; + g.rc_6.radio_trim = 1500; + g.rc_7.radio_trim = 1500; + g.rc_8.radio_trim = 1500; + + + Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: ")); + while(1){ + + delay(20); + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + g.rc_1.update_min_max(); + g.rc_2.update_min_max(); + g.rc_3.update_min_max(); + g.rc_4.update_min_max(); + g.rc_5.update_min_max(); + g.rc_6.update_min_max(); + g.rc_7.update_min_max(); + g.rc_8.update_min_max(); + + if(Serial.available() > 0){ + delay(20); + Serial.flush(); + + g.rc_1.save_eeprom(); + g.rc_2.save_eeprom(); + g.rc_3.save_eeprom(); + g.rc_4.save_eeprom(); + g.rc_5.save_eeprom(); + g.rc_6.save_eeprom(); + g.rc_7.save_eeprom(); + g.rc_8.save_eeprom(); + + print_done(); + break; + } + } + report_radio(); + return(0); +} + +static int8_t +setup_esc(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("\nESC Calibration:\n" + "-1 Unplug USB and battery\n" + "-2 Move CLI/FLY switch to FLY mode\n" + "-3 Move throttle to max, connect battery\n" + "-4 After two long beeps, throttle to 0, then test\n\n" + " Press Enter to cancel.\n")); + + + g.esc_calibrate.set_and_save(1); + + while(1){ + delay(20); + + if(Serial.available() > 0){ + g.esc_calibrate.set_and_save(0); + return(0); + } + } +} + +static int8_t +setup_motors(uint8_t argc, const Menu::arg *argv) +{ + while(1){ + delay(20); + read_radio(); + output_motor_test(); + if(Serial.available() > 0){ + g.esc_calibrate.set_and_save(0); + return(0); + } + } +} + +static int8_t +setup_accel(uint8_t argc, const Menu::arg *argv) +{ + imu.init_accel(); + print_accel_offsets(); + report_imu(); + return(0); +} + +static int8_t +setup_frame(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("x"))) { + g.frame_orientation.set_and_save(X_FRAME); + } else if (!strcmp_P(argv[1].str, PSTR("p"))) { + g.frame_orientation.set_and_save(PLUS_FRAME); + } else if (!strcmp_P(argv[1].str, PSTR("+"))) { + g.frame_orientation.set_and_save(PLUS_FRAME); + } else if (!strcmp_P(argv[1].str, PSTR("v"))) { + g.frame_orientation.set_and_save(V_FRAME); + }else{ + Serial.printf_P(PSTR("\nOptions:[x,+,v]\n")); + report_frame(); + return 0; + } + + report_frame(); + return 0; +} + +static int8_t +setup_flightmodes(uint8_t argc, const Menu::arg *argv) +{ + byte _switchPosition = 0; + byte _oldSwitchPosition = 0; + byte mode = 0; + + Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n")); + print_hit_enter(); + + while(1){ + delay(20); + read_radio(); + _switchPosition = readSwitch(); + + + // look for control switch change + if (_oldSwitchPosition != _switchPosition){ + + mode = flight_modes[_switchPosition]; + mode = constrain(mode, 0, NUM_MODES-1); + + // update the user + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + + // Remember switch position + _oldSwitchPosition = _switchPosition; + } + + // look for stick input + if (abs(g.rc_1.control_in) > 3000){ + mode++; + if(mode >= NUM_MODES) + mode = 0; + + // save new mode + flight_modes[_switchPosition] = mode; + + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } + + // look for stick input + if (g.rc_4.control_in > 3000){ + g.simple_modes |= (1<<_switchPosition); + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } + + // look for stick input + if (g.rc_4.control_in < -3000){ + g.simple_modes &= ~(1<<_switchPosition); + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } + + // escape hatch + if(Serial.available() > 0){ + for (mode = 0; mode < 6; mode++) + flight_modes[mode].save(); + + g.simple_modes.save(); + print_done(); + report_flight_modes(); + return (0); + } + } +} + +static int8_t +setup_declination(uint8_t argc, const Menu::arg *argv) +{ + compass.set_declination(radians(argv[1].f)); + report_compass(); + return 0; +} + +static int8_t +setup_tune(uint8_t argc, const Menu::arg *argv) +{ + g.radio_tuning.set_and_save(argv[1].i); + report_tuning(); + return 0; +} + + + +static int8_t +setup_erase(uint8_t argc, const Menu::arg *argv) +{ + zero_eeprom(); + return 0; +} + +static int8_t +setup_compass(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("on"))) { + g.compass_enabled.set_and_save(true); + init_compass(); + + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { + clear_offsets(); + g.compass_enabled.set_and_save(false); + + }else{ + Serial.printf_P(PSTR("\nOptions:[on,off]\n")); + report_compass(); + return 0; + } + + g.compass_enabled.save(); + report_compass(); + return 0; +} + +static int8_t +setup_batt_monitor(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("off"))) { + g.battery_monitoring.set_and_save(0); + + } else if(argv[1].i > 0 && argv[1].i <= 4){ + g.battery_monitoring.set_and_save(argv[1].i); + + } else { + Serial.printf_P(PSTR("\nOptions: off, 1-4")); + } + + report_batt_monitor(); + return 0; +} + +static int8_t +setup_sonar(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("on"))) { + g.sonar_enabled.set_and_save(true); + + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { + g.sonar_enabled.set_and_save(false); + + }else{ + Serial.printf_P(PSTR("\nOptions:[on, off]\n")); + report_sonar(); + return 0; + } + + report_sonar(); + return 0; +} + +#if FRAME_CONFIG == HELI_FRAME + +// Perform heli setup. +// Called by the setup menu 'radio' command. +static int8_t +setup_heli(uint8_t argc, const Menu::arg *argv) +{ + + uint8_t active_servo = 0; + int value = 0; + int temp; + int state = 0; // 0 = set rev+pos, 1 = capture min/max + int max_roll, max_pitch, min_coll, max_coll, min_tail, max_tail; + + // initialise swash plate + heli_init_swash(); + + // source swash plate movements directly from + heli_manual_override = true; + + // display initial settings + report_heli(); + + // display help + Serial.printf_P(PSTR("Instructions:")); + print_divider(); + Serial.printf_P(PSTR("\td\t\tdisplay settings\n")); + Serial.printf_P(PSTR("\t1~4\t\tselect servo\n")); + Serial.printf_P(PSTR("\ta or z\t\tmove mid up/down\n")); + Serial.printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n")); + Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n")); + Serial.printf_P(PSTR("\tp\tset pos (i.e. p0 = front, p90 = right)\n")); + Serial.printf_P(PSTR("\tr\t\treverse servo\n")); + Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n")); + Serial.printf_P(PSTR("\tt\tset trim (-500 ~ 500)\n")); + Serial.printf_P(PSTR("\tx\t\texit & save\n")); + + // start capturing + while( value != 'x' ) { + + // read radio although we don't use it yet + read_radio(); + + // record min/max + if( state == 1 ) { + if( abs(g.rc_1.control_in) > max_roll ) + max_roll = abs(g.rc_1.control_in); + if( abs(g.rc_2.control_in) > max_pitch ) + max_pitch = abs(g.rc_2.control_in); + if( g.rc_3.radio_in < min_coll ) + min_coll = g.rc_3.radio_in; + if( g.rc_3.radio_in > max_coll ) + max_coll = g.rc_3.radio_in; + min_tail = min(g.rc_4.radio_in, min_tail); + max_tail = max(g.rc_4.radio_in, max_tail); + //Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out); + } + + if( Serial.available() ) { + value = Serial.read(); + + // process the user's input + switch( value ) { + case '1': + active_servo = CH_1; + break; + case '2': + active_servo = CH_2; + break; + case '3': + active_servo = CH_3; + break; + case '4': + active_servo = CH_4; + break; + case 'a': + case 'A': + heli_get_servo(active_servo)->radio_trim += 10; + break; + case 'c': + case 'C': + if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) { + g.heli_coll_mid = g.rc_3.radio_in; + Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid); + } + break; + case 'd': + case 'D': + // display settings + report_heli(); + break; + case 'm': + case 'M': + if( state == 0 ) { + state = 1; // switch to capture min/max mode + Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n")); + + // reset servo ranges + g.heli_roll_max = g.heli_pitch_max = 4500; + g.heli_coll_min = 1000; + g.heli_coll_max = 2000; + g.heli_servo_4.radio_min = 1000; + g.heli_servo_4.radio_max = 2000; + + // set sensible values in temp variables + max_roll = abs(g.rc_1.control_in); + max_pitch = abs(g.rc_2.control_in); + min_coll = 2000; + max_coll = 1000; + min_tail = max_tail = abs(g.rc_4.radio_in); + }else{ + state = 0; // switch back to normal mode + // double check values aren't totally terrible + if( max_roll <= 1000 || max_pitch <= 1000 || (max_coll - min_coll < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 ) + Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_coll,max_coll,min_tail,max_tail); + else{ + g.heli_roll_max = max_roll; + g.heli_pitch_max = max_pitch; + g.heli_coll_min = min_coll; + g.heli_coll_max = max_coll; + g.heli_servo_4.radio_min = min_tail; + g.heli_servo_4.radio_max = max_tail; + + // reinitialise swash + heli_init_swash(); + + // display settings + report_heli(); + } + } + break; + case 'p': + case 'P': + temp = read_num_from_serial(); + if( temp >= -360 && temp <= 360 ) { + if( active_servo == CH_1 ) + g.heli_servo1_pos = temp; + if( active_servo == CH_2 ) + g.heli_servo2_pos = temp; + if( active_servo == CH_3 ) + g.heli_servo3_pos = temp; + heli_init_swash(); + Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp); + } + break; + case 'r': + case 'R': + heli_get_servo(active_servo)->set_reverse(!heli_get_servo(active_servo)->get_reverse()); + break; + case 't': + case 'T': + temp = read_num_from_serial(); + if( temp > 1000 ) + temp -= 1500; + if( temp > -500 && temp < 500 ) { + heli_get_servo(active_servo)->radio_trim = 1500 + temp; + heli_init_swash(); + Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); + } + break; + case 'u': + case 'U': + temp = 0; + // delay up to 2 seconds for servo type from user + while( !Serial.available() && temp < 20 ) { + temp++; + delay(100); + } + if( Serial.available() ) { + value = Serial.read(); + if( value == 'a' || value == 'A' ) { + g.heli_servo_averaging = HELI_SERVO_AVERAGING_ANALOG; + Serial.printf_P(PSTR("Analog Servo %dhz\n"),250 / HELI_SERVO_AVERAGING_ANALOG); + } + if( value == 'd' || value == 'D' ) { + g.heli_servo_averaging = HELI_SERVO_AVERAGING_DIGITAL; + Serial.printf_P(PSTR("Digital Servo 250hz\n")); + } + } + break; + case 'z': + case 'Z': + heli_get_servo(active_servo)->radio_trim -= 10; + break; + } + } + + // allow swash plate to move + output_motors_armed(); + + delay(20); + } + + // display final settings + report_heli(); + + // save to eeprom + g.heli_servo_1.save_eeprom(); + g.heli_servo_2.save_eeprom(); + g.heli_servo_3.save_eeprom(); + g.heli_servo_4.save_eeprom(); + g.heli_servo1_pos.save(); + g.heli_servo2_pos.save(); + g.heli_servo3_pos.save(); + g.heli_roll_max.save(); + g.heli_pitch_max.save(); + g.heli_coll_min.save(); + g.heli_coll_max.save(); + g.heli_coll_mid.save(); + g.heli_servo_averaging.save(); + + // return swash plate movements to attitude controller + heli_manual_override = false; + + return(0); +} + +// setup for external tail gyro (for heli only) +static int8_t +setup_gyro(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("on"))) { + g.heli_ext_gyro_enabled.set_and_save(true); + + // optionally capture the gain + if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) { + g.heli_ext_gyro_gain = argv[2].i; + g.heli_ext_gyro_gain.save(); + } + + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { + g.heli_ext_gyro_enabled.set_and_save(false); + + // capture gain if user simply provides a number + } else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) { + g.heli_ext_gyro_enabled.set_and_save(true); + g.heli_ext_gyro_gain = argv[1].i; + g.heli_ext_gyro_gain.save(); + + }else{ + Serial.printf_P(PSTR("\nOptions:[on, off] gain\n")); + } + + report_gyro(); + return 0; +} + +#endif // FRAME_CONFIG == HELI + +static void clear_offsets() +{ + Vector3f _offsets(0.0,0.0,0.0); + compass.set_offsets(_offsets); + compass.save_offsets(); +} + +/*static int8_t +setup_mag_offset(uint8_t argc, const Menu::arg *argv) +{ + Vector3f _offsets; + + if (!strcmp_P(argv[1].str, PSTR("c"))) { + clear_offsets(); + report_compass(); + return (0); + } + + print_hit_enter(); + init_compass(); + + int _min[3] = {0,0,0}; + int _max[3] = {0,0,0}; + + compass.read(); + compass.calculate(0,0); // roll = 0, pitch = 0 + + while(1){ + delay(50); + + compass.read(); + compass.calculate(0,0); // roll = 0, pitch = 0 + + if(compass.mag_x < _min[0]) _min[0] = compass.mag_x; + if(compass.mag_y < _min[1]) _min[1] = compass.mag_y; + if(compass.mag_z < _min[2]) _min[2] = compass.mag_z; + + // capture max + if(compass.mag_x > _max[0]) _max[0] = compass.mag_x; + if(compass.mag_y > _max[1]) _max[1] = compass.mag_y; + if(compass.mag_z > _max[2]) _max[2] = compass.mag_z; + + // calculate offsets + _offsets.x = (float)(_max[0] + _min[0]) / -2; + _offsets.y = (float)(_max[1] + _min[1]) / -2; + _offsets.z = (float)(_max[2] + _min[2]) / -2; + + // display all to user + Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"), + + (uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100, + + compass.mag_x, + compass.mag_y, + compass.mag_z, + + _offsets.x, + _offsets.y, + _offsets.z); + + if(Serial.available() > 1){ + compass.set_offsets(_offsets); + //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); + report_compass(); + return 0; + } + } + return 0; +} +*/ + +#ifdef OPTFLOW_ENABLED +static int8_t +setup_optflow(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("on"))) { + g.optflow_enabled = true; + init_optflow(); + + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { + g.optflow_enabled = false; + + }else{ + Serial.printf_P(PSTR("\nOptions:[on, off]\n")); + report_optflow(); + return 0; + } + + g.optflow_enabled.save(); + report_optflow(); + return 0; +} +#endif + + +/***************************************************************************/ +// CLI reports +/***************************************************************************/ + +static void report_batt_monitor() +{ + Serial.printf_P(PSTR("\nBatt Mointor\n")); + print_divider(); + if(g.battery_monitoring == 0) print_enabled(false); + if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3 cells")); + if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4 cells")); + if(g.battery_monitoring == 3) Serial.printf_P(PSTR("batt volts")); + if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur")); + print_blanks(2); +} + +static void report_wp(byte index = 255) +{ + if(index == 255){ + for(byte i = 0; i <= g.waypoint_total; i++){ + struct Location temp = get_command_with_index(i); + print_wp(&temp, i); + } + }else{ + struct Location temp = get_command_with_index(index); + print_wp(&temp, index); + } +} + +static void report_sonar() +{ + g.sonar_enabled.load(); + Serial.printf_P(PSTR("Sonar\n")); + print_divider(); + print_enabled(g.sonar_enabled.get()); + print_blanks(2); +} + +static void report_frame() +{ + Serial.printf_P(PSTR("Frame\n")); + print_divider(); + +#if FRAME_CONFIG == QUAD_FRAME + Serial.printf_P(PSTR("Quad frame\n")); +#elif FRAME_CONFIG == TRI_FRAME + Serial.printf_P(PSTR("TRI frame\n")); +#elif FRAME_CONFIG == HEXA_FRAME + Serial.printf_P(PSTR("Hexa frame\n")); +#elif FRAME_CONFIG == Y6_FRAME + Serial.printf_P(PSTR("Y6 frame\n")); +#elif FRAME_CONFIG == OCTA_FRAME + Serial.printf_P(PSTR("Octa frame\n")); +#elif FRAME_CONFIG == HELI_FRAME + Serial.printf_P(PSTR("Heli frame\n")); +#endif + +#if FRAME_CONFIG != HELI_FRAME + if(g.frame_orientation == X_FRAME) + Serial.printf_P(PSTR("X mode\n")); + else if(g.frame_orientation == PLUS_FRAME) + Serial.printf_P(PSTR("+ mode\n")); + else if(g.frame_orientation == V_FRAME) + Serial.printf_P(PSTR("V mode\n")); +#endif + + print_blanks(2); +} + +static void report_radio() +{ + Serial.printf_P(PSTR("Radio\n")); + print_divider(); + // radio + print_radio_values(); + print_blanks(2); +} + +static void report_imu() +{ + Serial.printf_P(PSTR("IMU\n")); + print_divider(); + + print_gyro_offsets(); + print_accel_offsets(); + print_blanks(2); +} + +static void report_compass() +{ + Serial.printf_P(PSTR("Compass\n")); + print_divider(); + + print_enabled(g.compass_enabled); + + // mag declination + Serial.printf_P(PSTR("Mag Dec: %4.4f\n"), + degrees(compass.get_declination())); + + Vector3f offsets = compass.get_offsets(); + + // mag offsets + Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"), + offsets.x, + offsets.y, + offsets.z); + print_blanks(2); +} + +static void report_flight_modes() +{ + Serial.printf_P(PSTR("Flight modes\n")); + print_divider(); + + for(int i = 0; i < 6; i++ ){ + print_switch(i, flight_modes[i], (g.simple_modes & (1<kP(), + pid->kI(), + (long)pid->imax()); +} +*/ + +static void +print_radio_values() +{ + Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); + Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max); + Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max); + Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max); + Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); + Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); + Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); + //Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); +} + +static void +print_switch(byte p, byte m, bool b) +{ + Serial.printf_P(PSTR("Pos %d:\t"),p); + Serial.print(flight_mode_strings[m]); + Serial.printf_P(PSTR(",\t\tSimple: ")); + if(b) + Serial.printf_P(PSTR("ON\n")); + else + Serial.printf_P(PSTR("OFF\n")); +} + +static void +print_done() +{ + Serial.printf_P(PSTR("\nSaved Settings\n\n")); +} + + +static void zero_eeprom(void) +{ + byte b = 0; + + Serial.printf_P(PSTR("\nErasing EEPROM\n")); + + for (int i = 0; i < EEPROM_MAX_ADDR; i++) { + eeprom_write_byte((uint8_t *) i, b); + } + + Serial.printf_P(PSTR("done\n")); +} + +static void +print_accel_offsets(void) +{ + Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), + (float)imu.ax(), + (float)imu.ay(), + (float)imu.az()); +} + +static void +print_gyro_offsets(void) +{ + Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), + (float)imu.gx(), + (float)imu.gy(), + (float)imu.gz()); +} + +#if FRAME_CONFIG == HELI_FRAME + +static RC_Channel * +heli_get_servo(int servo_num){ + if( servo_num == CH_1 ) + return &g.heli_servo_1; + if( servo_num == CH_2 ) + return &g.heli_servo_2; + if( servo_num == CH_3 ) + return &g.heli_servo_3; + if( servo_num == CH_4 ) + return &g.heli_servo_4; + return NULL; +} + +// Used to read integer values from the serial port +static int read_num_from_serial() { + byte index = 0; + byte timeout = 0; + char data[5] = ""; + + do { + if (Serial.available() == 0) { + delay(10); + timeout++; + }else{ + data[index] = Serial.read(); + timeout = 0; + index++; + } + }while (timeout < 5 && index < 5); + + return atoi(data); +} +#endif + +#endif // CLI_ENABLED + +static void +print_blanks(int num) +{ + while(num > 0){ + num--; + Serial.println(""); + } +} + +static void +print_divider(void) +{ + for (int i = 0; i < 40; i++) { + Serial.print("-"); + } + Serial.println(""); +} + +static void print_enabled(boolean b) +{ + if(b) + Serial.printf_P(PSTR("en")); + else + Serial.printf_P(PSTR("dis")); + Serial.printf_P(PSTR("abled\n")); +} + + +static void +init_esc() +{ + g.esc_calibrate.set_and_save(0); + while(1){ + read_radio(); + delay(100); + dancing_light(); + APM_RC.OutputCh(CH_1, g.rc_3.radio_in); + APM_RC.OutputCh(CH_2, g.rc_3.radio_in); + APM_RC.OutputCh(CH_3, g.rc_3.radio_in); + APM_RC.OutputCh(CH_4, g.rc_3.radio_in); + APM_RC.OutputCh(CH_7, g.rc_3.radio_in); + APM_RC.OutputCh(CH_8, g.rc_3.radio_in); + + #if FRAME_CONFIG == OCTA_FRAME + APM_RC.OutputCh(CH_10, g.rc_3.radio_in); + APM_RC.OutputCh(CH_11, g.rc_3.radio_in); + #endif + + } +} + +static void print_wp(struct Location *cmd, byte index) +{ + Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), + (int)index, + (int)cmd->id, + (int)cmd->options, + (int)cmd->p1, + cmd->alt, + cmd->lat, + cmd->lng); +} + +static void report_gps() +{ + Serial.printf_P(PSTR("\nGPS\n")); + print_divider(); + print_enabled(GPS_enabled); + print_blanks(2); +} + +static void report_version() +{ + Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get()); + print_divider(); + print_blanks(2); +} + + +static void report_tuning() +{ + Serial.printf_P(PSTR("\nTUNE:\n")); + print_divider(); + if (g.radio_tuning == 0){ + print_enabled(g.radio_tuning.get()); + }else{ + Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get()); + } + print_blanks(2); +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/system.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/***************************************************************************** +The init_ardupilot function processes everything we need for an in - air restart + We will determine later if we are actually on the ground and process a + ground start in that case. + +*****************************************************************************/ + +#if CLI_ENABLED == ENABLED +// Functions called from the top-level menu +static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde +static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp +static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde + +// This is the help function +// PSTR is an AVR macro to read strings from flash memory +// printf_P is a version of print_f that reads from flash memory +static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Commands:\n" + " logs\n" + " setup\n" + " test\n" + " planner\n" + "\n" + "Move the slide switch and reset to FLY.\n" + "\n")); + return(0); +} + +// Command/function table for the top-level menu. +const struct Menu::command main_menu_commands[] PROGMEM = { +// command function called +// ======= =============== + {"logs", process_logs}, + {"setup", setup_mode}, + {"test", test_mode}, + {"help", main_menu_help}, + {"planner", planner_mode} +}; + +// Create the top-level menu object. +MENU(main_menu, THISFIRMWARE, main_menu_commands); + +// the user wants the CLI. It never exits +static void run_cli(void) +{ + while (1) { + main_menu.run(); + } +} + +#endif // CLI_ENABLED + +static void init_ardupilot() +{ + // Console serial port + // + // The console port buffers are defined to be sufficiently large to support + // the console's use as a logging device, optionally as the GPS port when + // GPS_PROTOCOL_IMU is selected, and as the telemetry port. + // + // XXX This could be optimised to reduce the buffer sizes in the cases + // where they are not otherwise required. + // + Serial.begin(SERIAL0_BAUD, 128, 128); + + // GPS serial port. + // + // Not used if the IMU/X-Plane GPS is in use. + // + // XXX currently the EM406 (SiRF receiver) is nominally configured + // at 57600, however it's not been supported to date. We should + // probably standardise on 38400. + // + // XXX the 128 byte receive buffer may be too small for NMEA, depending + // on the message set configured. + // + #if GPS_PROTOCOL != GPS_PROTOCOL_IMU + Serial1.begin(38400, 128, 16); + #endif + + Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE + "\n\nFree RAM: %u\n"), + memcheck_available_memory()); + + + // + // Check the EEPROM format version before loading any parameters from EEPROM. + // + report_version(); + + // setup IO pins + pinMode(C_LED_PIN, OUTPUT); // GPS status LED + pinMode(A_LED_PIN, OUTPUT); // GPS status LED + pinMode(B_LED_PIN, OUTPUT); // GPS status LED + pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode + pinMode(PUSHBUTTON_PIN, INPUT); // unused + DDRL |= B00000100; // Set Port L, pin 2 to output for the relay + + // XXX set Analog out 14 to output + // 76543210 + //DDRK |= B01010000; + + #if MOTOR_LEDS == 1 + pinMode(FR_LED, OUTPUT); // GPS status LED + pinMode(RE_LED, OUTPUT); // GPS status LED + pinMode(RI_LED, OUTPUT); // GPS status LED + pinMode(LE_LED, OUTPUT); // GPS status LED + #endif + + #if PIEZO == 1 + pinMode(PIEZO_PIN,OUTPUT); + piezo_beep(); + #endif + + + if (!g.format_version.load() || + g.format_version != Parameters::k_format_version) { + //Serial.printf_P(PSTR("\n\nForcing complete parameter reset...")); + + /*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" + "\n\nForcing complete parameter reset..."), + g.format_version.get(), + Parameters::k_format_version); + */ + + // erase all parameters + Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n")); + delay(100); // wait for serial send + AP_Var::erase_all(); + + // save the new format version + g.format_version.set_and_save(Parameters::k_format_version); + + // save default radio values + default_dead_zones(); + + Serial.printf_P(PSTR("Please Run Setup...\n")); + while (true) { + delay(1000); + if(motor_light){ + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(B_LED_PIN, HIGH); + digitalWrite(C_LED_PIN, HIGH); + }else{ + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, LOW); + digitalWrite(C_LED_PIN, LOW); + } + motor_light = !motor_light; + } + + }else{ + // save default radio values + //default_dead_zones(); + + // Load all auto-loaded EEPROM variables + AP_Var::load_all(); + } + + // Telemetry port. + // + // Not used if telemetry is going to the console. + // + // XXX for unidirectional protocols, we could (should) minimize + // the receive buffer, and the transmit buffer could also be + // shrunk for protocols that don't send large messages. + // + Serial3.begin(map_baudrate(g.serial3_baud,SERIAL3_BAUD), 128, 128); + + #ifdef RADIO_OVERRIDE_DEFAULTS + { + int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; + APM_RC.setHIL(rc_override); + } + #endif + + #if FRAME_CONFIG == HELI_FRAME + heli_init_swash(); // heli initialisation + #endif + + init_rc_in(); // sets up rc channels from radio + init_rc_out(); // sets up the timer libs + init_camera(); + + #if HIL_MODE != HIL_MODE_ATTITUDE + // begin filtering the ADC Gyros + adc.filter_result = true; + + adc.Init(); // APM ADC library initialization + barometer.Init(); // APM Abs Pressure sensor initialization + #endif + + // Do GPS init + g_gps = &g_gps_driver; + g_gps->init(); // GPS Initialization + g_gps->callback = mavlink_delay; + + // init the GCS + gcs0.init(&Serial); + gcs3.init(&Serial3); + + if(g.compass_enabled) + init_compass(); + + #ifdef OPTFLOW_ENABLED + // init the optical flow sensor + if(g.optflow_enabled) { + init_optflow(); + } + #endif + +// agmatthews USERHOOKS +#ifdef USERHOOK_INIT + USERHOOK_INIT +#endif + // Logging: + // -------- + // DataFlash log initialization +#if LOGGING_ENABLED == ENABLED + DataFlash.Init(); +#endif + +#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED + // If the switch is in 'menu' mode, run the main menu. + // + // Since we can't be sure that the setup or test mode won't leave + // the system in an odd state, we don't let the user exit the top + // menu; they must reset in order to fly. + // + if (check_startup_for_CLI()) { + digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED + Serial.printf_P(PSTR("\nCLI:\n\n")); + run_cli(); + } +#else + Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n")); +#endif // CLI_ENABLED + + if(g.esc_calibrate == 1){ + init_esc(); + } + + // Logging: + // -------- + if(g.log_bitmask != 0){ + // TODO - Here we will check on the length of the last log + // We don't want to create a bunch of little logs due to powering on and off + start_new_log(); + } + + GPS_enabled = false; + + // Read in the GPS + for (byte counter = 0; ; counter++) { + g_gps->update(); + if (g_gps->status() != 0){ + GPS_enabled = true; + break; + } + + if (counter >= 2) { + GPS_enabled = false; + break; + } + } + + // lengthen the idle timeout for gps Auto_detect + // --------------------------------------------- + g_gps->idleTimeout = 20000; + + // print the GPS status + // -------------------- + report_gps(); + + #if HIL_MODE != HIL_MODE_ATTITUDE + // read Baro pressure at ground + //----------------------------- + init_barometer(); + #endif + + // initialize commands + // ------------------- + init_commands(); + + // set the correct flight mode + // --------------------------- + reset_control_switch(); + + startup_ground(); + + Log_Write_Startup(); + + SendDebug("\nReady to FLY "); +} + +//******************************************************************************** +//This function does all the calibrations, etc. that we need during a ground start +//******************************************************************************** +static void startup_ground(void) +{ + gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START")); + + #if HIL_MODE != HIL_MODE_ATTITUDE + // Warm up and read Gyro offsets + // ----------------------------- + imu.init_gyro(mavlink_delay); + #if CLI_ENABLED == ENABLED + report_imu(); + #endif + #endif + + // reset the leds + // --------------------------- + clear_leds(); +} + +/* +#define YAW_HOLD 0 +#define YAW_ACRO 1 +#define YAW_AUTO 2 +#define YAW_LOOK_AT_HOME 3 + +#define ROLL_PITCH_STABLE 0 +#define ROLL_PITCH_ACRO 1 +#define ROLL_PITCH_AUTO 2 + +#define THROTTLE_MANUAL 0 +#define THROTTLE_HOLD 1 +#define THROTTLE_AUTO 2 + +*/ + +static void set_mode(byte mode) +{ + if(control_mode == mode){ + // don't switch modes if we are already in the correct mode. + return; + } + + old_control_mode = control_mode; + + control_mode = mode; + control_mode = constrain(control_mode, 0, NUM_MODES - 1); + + // used to stop fly_aways + motor_auto_armed = (g.rc_3.control_in > 0); + + // clearing value used in interactive alt hold + manual_boost = 0; + + // clearing value used to set WP's dynamically. + CH7_wp_index = 0; + + Serial.println(flight_mode_strings[control_mode]); + + // report the GPS and Motor arming status + led_mode = NORMAL_LEDS; + + reset_nav(); + + switch(control_mode) + { + case ACRO: + yaw_mode = YAW_ACRO; + roll_pitch_mode = ROLL_PITCH_ACRO; + throttle_mode = THROTTLE_MANUAL; + reset_hold_I(); + break; + + case STABILIZE: + yaw_mode = YAW_HOLD; + roll_pitch_mode = ROLL_PITCH_STABLE; + throttle_mode = THROTTLE_MANUAL; + reset_hold_I(); + break; + + case ALT_HOLD: + yaw_mode = ALT_HOLD_YAW; + roll_pitch_mode = ALT_HOLD_RP; + throttle_mode = ALT_HOLD_THR; + reset_hold_I(); + + init_throttle_cruise(); + next_WP = current_loc; + break; + + case AUTO: + reset_hold_I(); + yaw_mode = AUTO_YAW; + roll_pitch_mode = AUTO_RP; + throttle_mode = AUTO_THR; + + init_throttle_cruise(); + + // loads the commands from where we left off + init_commands(); + break; + + case CIRCLE: + yaw_mode = CIRCLE_YAW; + roll_pitch_mode = CIRCLE_RP; + throttle_mode = CIRCLE_THR; + + init_throttle_cruise(); + next_WP = current_loc; + break; + + case LOITER: + yaw_mode = LOITER_YAW; + roll_pitch_mode = LOITER_RP; + throttle_mode = LOITER_THR; + + init_throttle_cruise(); + next_WP = current_loc; + break; + + case POSITION: + yaw_mode = YAW_HOLD; + roll_pitch_mode = ROLL_PITCH_AUTO; + throttle_mode = THROTTLE_MANUAL; + + next_WP = current_loc; + break; + + case GUIDED: + yaw_mode = YAW_AUTO; + roll_pitch_mode = ROLL_PITCH_AUTO; + throttle_mode = THROTTLE_AUTO; + + //xtrack_enabled = true; + init_throttle_cruise(); + next_WP = current_loc; + set_next_WP(&guided_WP); + break; + + case RTL: + yaw_mode = RTL_YAW; + roll_pitch_mode = RTL_RP; + throttle_mode = RTL_THR; + + //xtrack_enabled = true; + init_throttle_cruise(); + do_RTL(); + break; + + default: + break; + } + + Log_Write_Mode(control_mode); +} + +static void set_failsafe(boolean mode) +{ + // only act on changes + // ------------------- + if(failsafe != mode){ + + // store the value so we don't trip the gate twice + // ----------------------------------------------- + failsafe = mode; + + if (failsafe == false){ + // We've regained radio contact + // ---------------------------- + failsafe_off_event(); + + }else{ + // We've lost radio contact + // ------------------------ + failsafe_on_event(); + + } + } +} + + +static void +init_compass() +{ + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + dcm.set_compass(&compass); + compass.init(); + compass.get_offsets(); // load offsets to account for airframe magnetic interference +} + +#ifdef OPTFLOW_ENABLED +static void +init_optflow() +{ + if( optflow.init() == false ) { + g.optflow_enabled = false; + //SendDebug("\nFailed to Init OptFlow "); + } + optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft + optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view +} +#endif + +static void +init_simple_bearing() +{ + initial_simple_bearing = dcm.yaw_sensor; +} + +static void +init_throttle_cruise() +{ + // are we moving from manual throttle to auto_throttle? + if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ + g.pi_throttle.reset_I(); + #if FRAME_CONFIG == HELI_FRAME + g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in)); + #else + g.throttle_cruise.set_and_save(g.rc_3.control_in); + #endif + } +} + +#if CLI_ENABLED == ENABLED +#if BROKEN_SLIDER == 1 + +static boolean +check_startup_for_CLI() +{ + //return true; + if((g.rc_4.radio_max) < 1600){ + // CLI mode + return true; + + }else if(abs(g.rc_4.control_in) > 3000){ + // CLI mode + return true; + + }else{ + // startup to fly + return false; + } +} + +#else + +static boolean +check_startup_for_CLI() +{ + return (digitalRead(SLIDE_SWITCH_PIN) == 0); +} + +#endif // BROKEN_SLIDER +#endif // CLI_ENABLED + +/* + map from a 8 bit EEPROM baud rate to a real baud rate + */ +static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) +{ + switch (rate) { + case 9: return 9600; + case 19: return 19200; + case 38: return 38400; + case 57: return 57600; + case 111: return 111100; + case 115: return 115200; + } + //Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); + return default_baud; +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/test.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if CLI_ENABLED == ENABLED + +// These are function definitions so the Menu can be constructed before the functions +// are defined below. Order matters to the compiler. +static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); +static int8_t test_radio(uint8_t argc, const Menu::arg *argv); +static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); +//static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); +static int8_t test_gps(uint8_t argc, const Menu::arg *argv); +static int8_t test_tri(uint8_t argc, const Menu::arg *argv); +static int8_t test_adc(uint8_t argc, const Menu::arg *argv); +static int8_t test_imu(uint8_t argc, const Menu::arg *argv); +//static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); +//static int8_t test_omega(uint8_t argc, const Menu::arg *argv); +static int8_t test_battery(uint8_t argc, const Menu::arg *argv); +//static int8_t test_nav(uint8_t argc, const Menu::arg *argv); + +//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv); +//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv); +static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); +static int8_t test_current(uint8_t argc, const Menu::arg *argv); +static int8_t test_relay(uint8_t argc, const Menu::arg *argv); +static int8_t test_wp(uint8_t argc, const Menu::arg *argv); +static int8_t test_baro(uint8_t argc, const Menu::arg *argv); +//static int8_t test_mag(uint8_t argc, const Menu::arg *argv); +static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); +#ifdef OPTFLOW_ENABLED +static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); +#endif +//static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); +static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); +static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); +//static int8_t test_mission(uint8_t argc, const Menu::arg *argv); + +// This is the help function +// PSTR is an AVR macro to read strings from flash memory +// printf_P is a version of printf that reads from flash memory +/*static int8_t help_test(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("\n" + "Commands:\n" + " radio\n" + " servos\n" + " g_gps\n" + " imu\n" + " battery\n" + "\n")); +}*/ + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Coommon for implementation details +const struct Menu::command test_menu_commands[] PROGMEM = { + {"pwm", test_radio_pwm}, + {"radio", test_radio}, + {"failsafe", test_failsafe}, +// {"stabilize", test_stabilize}, + {"gps", test_gps}, +#if HIL_MODE != HIL_MODE_ATTITUDE + {"adc", test_adc}, +#endif + {"imu", test_imu}, + //{"dcm", test_dcm}, + //{"omega", test_omega}, + {"battery", test_battery}, + {"tune", test_tuning}, + {"tri", test_tri}, + {"current", test_current}, + {"relay", test_relay}, + {"waypoints", test_wp}, + //{"nav", test_nav}, +#if HIL_MODE != HIL_MODE_ATTITUDE + {"altitude", test_baro}, +#endif + {"sonar", test_sonar}, + //{"compass", test_mag}, +#ifdef OPTFLOW_ENABLED + {"optflow", test_optflow}, +#endif + //{"xbee", test_xbee}, + {"eedump", test_eedump}, + {"rawgps", test_rawgps}, +// {"mission", test_mission}, + //{"reverse", test_reverse}, + //{"wp", test_wp_nav}, +}; + +// A Macro to create the Menu +MENU(test_menu, "test", test_menu_commands); + +static int8_t +test_mode(uint8_t argc, const Menu::arg *argv) +{ + //Serial.printf_P(PSTR("Test Mode\n\n")); + test_menu.run(); + return 0; +} + +static int8_t +test_eedump(uint8_t argc, const Menu::arg *argv) +{ + int i, j; + + // hexdump the EEPROM + for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { + Serial.printf_P(PSTR("%04x:"), i); + for (j = 0; j < 16; j++) + Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); + Serial.println(); + } + return(0); +} + +static int8_t +test_radio_pwm(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(20); + + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + // servo Yaw + //APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + + Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + g.rc_1.radio_in, + g.rc_2.radio_in, + g.rc_3.radio_in, + g.rc_4.radio_in, + g.rc_5.radio_in, + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_tri(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(20); + + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + g.rc_4.servo_out = g.rc_4.control_in; + g.rc_4.calc_pwm(); + + Serial.printf_P(PSTR("input: %d\toutput%d\n"), + g.rc_4.control_in, + g.rc_4.radio_out); + + APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + + if(Serial.available() > 0){ + return (0); + } + } +} + +/* +static int8_t +test_nav(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(1000); + g_gps->ground_course = 19500; + calc_nav_rate2(g.waypoint_speed_max); + calc_nav_pitch_roll2(); + + g_gps->ground_course = 28500; + calc_nav_rate2(g.waypoint_speed_max); + calc_nav_pitch_roll2(); + + g_gps->ground_course = 1500; + calc_nav_rate2(g.waypoint_speed_max); + calc_nav_pitch_roll2(); + + g_gps->ground_course = 10500; + calc_nav_rate2(g.waypoint_speed_max); + calc_nav_pitch_roll2(); + + + //if(Serial.available() > 0){ + return (0); + //} + } +} +*/ + +static int8_t +test_radio(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(20); + read_radio(); + + + Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), + g.rc_1.control_in, + g.rc_2.control_in, + g.rc_3.control_in, + g.rc_4.control_in, + g.rc_5.control_in, + g.rc_6.control_in, + g.rc_7.control_in); + + //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100)); + + /*Serial.printf_P(PSTR( "min: %d" + "\t in: %d" + "\t pwm_in: %d" + "\t sout: %d" + "\t pwm_out %d\n"), + g.rc_3.radio_min, + g.rc_3.control_in, + g.rc_3.radio_in, + g.rc_3.servo_out, + g.rc_3.pwm_out + ); + */ + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_failsafe(uint8_t argc, const Menu::arg *argv) +{ + + #if THROTTLE_FAILSAFE + byte fail_test; + print_hit_enter(); + for(int i = 0; i < 50; i++){ + delay(20); + read_radio(); + } + + oldSwitchPosition = readSwitch(); + + Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); + while(g.rc_3.control_in > 0){ + delay(20); + read_radio(); + } + + while(1){ + delay(20); + read_radio(); + + if(g.rc_3.control_in > 0){ + Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in); + fail_test++; + } + + if(oldSwitchPosition != readSwitch()){ + Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); + Serial.println(flight_mode_strings[readSwitch()]); + fail_test++; + } + + if(g.throttle_fs_enabled && g.rc_3.get_failsafe()){ + Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in); + Serial.println(flight_mode_strings[readSwitch()]); + fail_test++; + } + + if(fail_test > 0){ + return (0); + } + if(Serial.available() > 0){ + Serial.printf_P(PSTR("LOS caused no change in ACM.\n")); + return (0); + } + } + #else + return (0); + #endif +} + +/*static int8_t +test_stabilize(uint8_t argc, const Menu::arg *argv) +{ + static byte ts_num; + + + print_hit_enter(); + delay(1000); + + // setup the radio + // --------------- + init_rc_in(); + + control_mode = STABILIZE; + Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP()); + Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); + + motor_auto_armed = false; + motor_armed = true; + + while(1){ + // 50 hz + if (millis() - fast_loopTimer > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer; + fast_loopTimer = millis(); + G_Dt = (float)delta_ms_fast_loop / 1000.f; + + if(g.compass_enabled){ + medium_loopCounter++; + if(medium_loopCounter == 5){ + compass.read(); // Read magnetometer + compass.calculate(dcm.roll, dcm.pitch); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + medium_loopCounter = 0; + } + } + + // for trim features + read_trim_switch(); + + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + // IMU + // --- + read_AHRS(); + + // allow us to zero out sensors with control switches + if(g.rc_5.control_in < 600){ + dcm.roll_sensor = dcm.pitch_sensor = 0; + } + + // custom code/exceptions for flight modes + // --------------------------------------- + update_current_flight_mode(); + + // write out the servo PWM values + // ------------------------------ + set_servos_4(); + + ts_num++; + if (ts_num > 10){ + ts_num = 0; + Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "), + (int)(dcm.roll_sensor/100), + (int)(dcm.pitch_sensor/100), + g.rc_1.pwm_out); + + print_motor_out(); + } + // R: 1417, L: 1453 F: 1453 B: 1417 + + //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); + //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); + + if(Serial.available() > 0){ + if(g.compass_enabled){ + compass.save_offsets(); + report_compass(); + } + return (0); + } + + } + } +} +*/ +#if HIL_MODE != HIL_MODE_ATTITUDE +static int8_t +test_adc(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + Serial.printf_P(PSTR("ADC\n")); + delay(1000); + + while(1){ + for(int i = 0; i < 9; i++){ + Serial.printf_P(PSTR("%u,"),adc.Ch(i)); + } + Serial.println(); + delay(20); + if(Serial.available() > 0){ + return (0); + } + } +} +#endif + +static int8_t +test_imu(uint8_t argc, const Menu::arg *argv) +{ + //Serial.printf_P(PSTR("Calibrating.")); + + //dcm.kp_yaw(0.02); + //dcm.ki_yaw(0); + + report_imu(); + imu.init_gyro(); + report_imu(); + + print_hit_enter(); + delay(1000); + + //float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw; + fast_loopTimer = millis(); + + while(1){ + //delay(20); + if (millis() - fast_loopTimer >= 5) { + + // IMU + // --- + read_AHRS(); + + //Vector3f accels = imu.get_accel(); + //Vector3f gyros = imu.get_gyro(); + //Vector3f accel_filt = imu.get_accel_filtered(); + //accels_rot = dcm.get_dcm_matrix() * accel_filt; + + + medium_loopCounter++; + + if(medium_loopCounter == 4){ + update_trig(); + } + + if(medium_loopCounter == 20){ + //read_radio(); + medium_loopCounter = 0; + //tuning(); + //dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0); + + /* + Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld, kp:%1.4f, kp:%1.4f \n"), + dcm.roll_sensor, + dcm.pitch_sensor, + dcm.yaw_sensor, + dcm.kp_roll_pitch(), + (float)g.rc_6.control_in / 2000.0); + */ + Serial.printf_P(PSTR("%ld, %ld, %ld\n"), + dcm.roll_sensor, + dcm.pitch_sensor, + dcm.yaw_sensor); + + if(g.compass_enabled){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); + } + } + + // We are using the IMU + // --------------------- + /* + Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t" + "G: %4.4f, %4.4f, %4.4f\t"), + accels.x, accels.y, accels.z, + gyros.x, gyros.y, gyros.z); + */ + /* + Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"), + cos_pitch_x, + sin_pitch_y, + cos_roll_x, + sin_roll_y, + cos_yaw_x, // x + sin_yaw_y); // y + //*/ + //Log_Write_Raw(); + } + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_gps(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(333); + + // Blink GPS LED if we don't have a fix + // ------------------------------------ + update_GPS_light(); + + g_gps->update(); + + if (g_gps->new_data){ + Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), + g_gps->latitude, + g_gps->longitude, + g_gps->altitude/100, + g_gps->num_sats); + g_gps->new_data = false; + }else{ + Serial.print("."); + } + if(Serial.available() > 0){ + return (0); + } + } +} + +/* +static int8_t +test_dcm(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Gyro | Accel\n")); + Vector3f _cam_vector; + Vector3f _out_vector; + + G_Dt = .02; + + while(1){ + for(byte i = 0; i <= 50; i++){ + delay(20); + // IMU + // --- + read_AHRS(); + } + + Matrix3f temp = dcm.get_dcm_matrix(); + Matrix3f temp_t = dcm.get_dcm_transposed(); + + Serial.printf_P(PSTR("dcm\n" + "%4.4f \t %4.4f \t %4.4f \n" + "%4.4f \t %4.4f \t %4.4f \n" + "%4.4f \t %4.4f \t %4.4f \n\n"), + temp.a.x, temp.a.y, temp.a.z, + temp.b.x, temp.b.y, temp.b.z, + temp.c.x, temp.c.y, temp.c.z); + + int _pitch = degrees(-asin(temp.c.x)); + int _roll = degrees(atan2(temp.c.y, temp.c.z)); + int _yaw = degrees(atan2(temp.b.x, temp.a.x)); + Serial.printf_P(PSTR( "angles\n" + "%d \t %d \t %d\n\n"), + _pitch, + _roll, + _yaw); + + //_out_vector = _cam_vector * temp; + //Serial.printf_P(PSTR( "cam\n" + // "%d \t %d \t %d\n\n"), + // (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100); + + if(Serial.available() > 0){ + return (0); + } + } +} +*/ +/* +static int8_t +test_dcm(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Gyro | Accel\n")); + delay(1000); + + while(1){ + Vector3f accels = dcm.get_accel(); + Serial.print("accels.z:"); + Serial.print(accels.z); + Serial.print("omega.z:"); + Serial.print(omega.z); + delay(100); + + if(Serial.available() > 0){ + return (0); + } + } +} +*/ + +/*static int8_t +test_omega(uint8_t argc, const Menu::arg *argv) +{ + static byte ts_num; + float old_yaw; + + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Omega")); + delay(1000); + + G_Dt = .02; + + while(1){ + delay(20); + // IMU + // --- + read_AHRS(); + + float my_oz = (dcm.yaw - old_yaw) * 50; + + old_yaw = dcm.yaw; + + ts_num++; + if (ts_num > 2){ + ts_num = 0; + //Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz); + Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz); + } + + if(Serial.available() > 0){ + return (0); + } + } + return (0); +} +//*/ + +static int8_t +test_battery(uint8_t argc, const Menu::arg *argv) +{ +#if BATTERY_EVENT == 1 + for (int i = 0; i < 20; i++){ + delay(20); + read_battery(); + } + Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"), + battery_voltage1, + battery_voltage2, + battery_voltage3, + battery_voltage4); +#else + Serial.printf_P(PSTR("Not enabled\n")); + +#endif + return (0); +} + +static int8_t +test_tuning(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + + while(1){ + delay(200); + read_radio(); + tuning(); + Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_current(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + //delta_ms_medium_loop = 100; + + while(1){ + delay(100); + read_radio(); + read_battery(); + Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), + battery_voltage, + current_amps, + current_total); + + APM_RC.OutputCh(CH_1, g.rc_3.radio_in); + APM_RC.OutputCh(CH_2, g.rc_3.radio_in); + APM_RC.OutputCh(CH_3, g.rc_3.radio_in); + APM_RC.OutputCh(CH_4, g.rc_3.radio_in); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_relay(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + Serial.printf_P(PSTR("Relay on\n")); + relay.on(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } + + Serial.printf_P(PSTR("Relay off\n")); + relay.off(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_wp(uint8_t argc, const Menu::arg *argv) +{ + delay(1000); + + // save the alitude above home option + Serial.printf_P(PSTR("Hold alt ")); + if(g.RTL_altitude < 0){ + Serial.printf_P(PSTR("\n")); + }else{ + Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100); + } + + Serial.printf_P(PSTR("%d wp\n"), (int)g.waypoint_total); + Serial.printf_P(PSTR("Hit rad: %d\n"), (int)g.waypoint_radius); + //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); + + report_wp(); + + return (0); +} + +static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) { + print_hit_enter(); + delay(1000); + while(1){ + if (Serial3.available()){ + digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS + Serial1.write(Serial3.read()); + digitalWrite(B_LED_PIN, LOW); + } + if (Serial1.available()){ + digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS + Serial3.write(Serial1.read()); + digitalWrite(C_LED_PIN, LOW); + } + if(Serial.available() > 0){ + return (0); + } + } + } + +/*static int8_t +test_xbee(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); + + while(1){ + if (Serial3.available()) + Serial3.write(Serial3.read()); + + if(Serial.available() > 0){ + return (0); + } + } +} +*/ + +#if HIL_MODE != HIL_MODE_ATTITUDE +static int8_t +test_baro(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + init_barometer(); + + while(1){ + delay(100); + baro_alt = read_barometer(); + Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt); + //Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2); + if(Serial.available() > 0){ + return (0); + } + } +} +#endif + +/* +static int8_t +test_mag(uint8_t argc, const Menu::arg *argv) +{ + if(g.compass_enabled) { + //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); + + print_hit_enter(); + + while(1){ + delay(100); + compass.read(); + compass.calculate(dcm.get_dcm_matrix()); + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z); + + if(Serial.available() > 0){ + return (0); + } + } + } else { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } +} +*/ +/* +static int8_t +test_reverse(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(20); + + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + g.rc_4.set_reverse(0); + g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); + g.rc_4.servo_out = g.rc_4.control_in; + g.rc_4.calc_pwm(); + Serial.printf_P(PSTR("PWM:%d input: %d\toutput%d "), + APM_RC.InputCh(CH_4), + g.rc_4.control_in, + g.rc_4.radio_out); + APM_RC.OutputCh(CH_6, g.rc_4.radio_out); + + + g.rc_4.set_reverse(1); + g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); + g.rc_4.servo_out = g.rc_4.control_in; + g.rc_4.calc_pwm(); + Serial.printf_P(PSTR("\trev input: %d\toutput%d\n"), + g.rc_4.control_in, + g.rc_4.radio_out); + + APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + + if(Serial.available() > 0){ + g.rc_4.set_reverse(0); + return (0); + } + } +}*/ + +/* + test the sonar + */ +static int8_t +test_sonar(uint8_t argc, const Menu::arg *argv) +{ + if(g.sonar_enabled == false){ + Serial.printf_P(PSTR("Sonar disabled\n")); + return (0); + } + + print_hit_enter(); + while(1) { + delay(100); + + Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read()); + //Serial.printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value); + + if(Serial.available() > 0){ + return (0); + } + } + + return (0); +} + +#ifdef OPTFLOW_ENABLED +static int8_t +test_optflow(uint8_t argc, const Menu::arg *argv) +{ + ///* + if(g.optflow_enabled) { + Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); + print_hit_enter(); + + while(1){ + delay(200); + optflow.read(); + Log_Write_Optflow(); + Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"), + optflow.x, + optflow.dx, + optflow.y, + optflow.dy, + optflow.surface_quality); + + if(Serial.available() > 0){ + return (0); + } + } + } else { + Serial.printf_P(PSTR("OptFlow: ")); + print_enabled(false); + return (0); + } +} +#endif + +/* +static int8_t +test_mission(uint8_t argc, const Menu::arg *argv) +{ + //write out a basic mission to the EEPROM + +//{ +// uint8_t id; ///< command id +// uint8_t options; ///< options bitmask (1<<0 = relative altitude) +// uint8_t p1; ///< param 1 +// int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100) +// int32_t lat; ///< param 3 - Lattitude * 10**7 +// int32_t lng; ///< param 4 - Longitude * 10**7 +//} + + // clear home + {Location t = {0, 0, 0, 0, 0, 0}; + set_command_with_index(t,0);} + + // CMD opt pitch alt/cm + {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; + set_command_with_index(t,1);} + + if (!strcmp_P(argv[1].str, PSTR("wp"))) { + + // CMD opt + {Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0}; + set_command_with_index(t,2);} + // CMD opt + {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; + set_command_with_index(t,3);} + + // CMD opt + {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + set_command_with_index(t,4);} + + } else { + //2250 = 25 meteres + // CMD opt p1 //alt //NS //WE + {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 + set_command_with_index(t,2);} + + // CMD opt dir angle/deg deg/s relative + {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; + set_command_with_index(t,3);} + + // CMD opt + {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + set_command_with_index(t,4);} + + } + + g.RTL_altitude.set_and_save(300); + g.waypoint_total.set_and_save(4); + g.waypoint_radius.set_and_save(3); + + test_wp(NULL, NULL); + return (0); +} +*/ + +static void print_hit_enter() +{ + Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); +} + +/* +static void fake_out_gps() +{ + static float rads; + g_gps->new_data = true; + g_gps->fix = true; + + //int length = g.rc_6.control_in; + rads += .05; + + if (rads > 6.28){ + rads = 0; + } + + g_gps->latitude = 377696000; // Y + g_gps->longitude = -1224319000; // X + g_gps->altitude = 9000; // meters * 100 + + //next_WP.lng = home.lng - length * sin(rads); // X + //next_WP.lat = home.lat + length * cos(rads); // Y +} + +*/ + +static void print_motor_out(){ + Serial.printf("out: R: %d, L: %d F: %d B: %d\n", + (motor_out[CH_1] - g.rc_3.radio_min), + (motor_out[CH_2] - g.rc_3.radio_min), + (motor_out[CH_3] - g.rc_3.radio_min), + (motor_out[CH_4] - g.rc_3.radio_min)); +} + +#endif // CLI_ENABLED diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp new file mode 100644 index 0000000000..e51050aa6c --- /dev/null +++ b/ArduPlane/ArduPlane.cpp @@ -0,0 +1,7973 @@ +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/ArduPlane.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define THISFIRMWARE "ArduPlane V2.24" +/* +Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short +Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi +Please contribute your ideas! + + +This firmware is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. +*/ + +//////////////////////////////////////////////////////////////////////////////// +// Header includes +//////////////////////////////////////////////////////////////////////////////// + +// AVR runtime +#include +#include +#include +#include + +// Libraries +#include +#include +#include // ArduPilot Mega RC Library +#include // ArduPilot GPS library +#include // Arduino I2C lib +#include // Arduino SPI lib +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega BMP085 Library +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega IMU Library +#include // ArduPilot Mega DCM Library +#include // PID library +#include // RC Channel Library +#include // Range finder library +#include +#include // APM relay +#include // MAVLink GCS definitions +#include + +// Configuration +#include "config.h" + +// Local modules +#include "defines.h" +#include "Parameters.h" +#include "GCS.h" + +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +// +// Note that FastSerial port buffers are allocated at ::begin time, +// so there is not much of a penalty to defining ports that we don't +// use. +// +#line 1 "autogenerated" +#include "WProgram.h" + void setup() ; + void loop() ; + static void fast_loop() ; + static void medium_loop() ; + static void slow_loop() ; + static void one_second_loop() ; + static void update_GPS(void) ; + static void update_current_flight_mode(void) ; + static void update_navigation() ; + static void update_alt() ; + static void stabilize() ; + static void crash_checker() ; + static void calc_throttle() ; + static void calc_nav_yaw(float speed_scaler) ; + static void calc_nav_pitch() ; + static void calc_nav_roll() ; + float roll_slew_limit(float servo) ; + static void throttle_slew_limit() ; + static void reset_I(void) ; + static void set_servos(void) ; + static void demo_servos(byte i) ; + static NOINLINE void send_heartbeat(mavlink_channel_t chan) ; + static NOINLINE void send_attitude(mavlink_channel_t chan) ; + static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) ; + static void NOINLINE send_meminfo(mavlink_channel_t chan) ; + static void NOINLINE send_location(mavlink_channel_t chan) ; + static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) ; + static void NOINLINE send_gps_raw(mavlink_channel_t chan) ; + static void NOINLINE send_servo_out(mavlink_channel_t chan) ; + static void NOINLINE send_radio_in(mavlink_channel_t chan) ; + static void NOINLINE send_radio_out(mavlink_channel_t chan) ; + static void NOINLINE send_vfr_hud(mavlink_channel_t chan) ; + static void NOINLINE send_raw_imu1(mavlink_channel_t chan) ; + static void NOINLINE send_raw_imu2(mavlink_channel_t chan) ; + static void NOINLINE send_raw_imu3(mavlink_channel_t chan) ; + static void NOINLINE send_gps_status(mavlink_channel_t chan) ; + static void NOINLINE send_current_waypoint(mavlink_channel_t chan) ; + static void NOINLINE send_statustext(mavlink_channel_t chan) ; + static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; + static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; + void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) ; + static void mavlink_delay(unsigned long t) ; + static void gcs_send_message(enum ap_message id) ; + static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) ; + static void gcs_update(void) ; + static void gcs_send_text(gcs_severity severity, const char *str) ; + static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) ; + static bool print_log_menu(void) ; + static byte get_num_logs(void) ; + static void start_new_log(byte num_existing_logs) ; + static void get_log_boundaries(byte log_num, int & start_page, int & end_page) ; + static int find_last_log_page(int bottom_page) ; + static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) ; + static void Log_Write_Performance() ; + static void Log_Write_Cmd(byte num, struct Location *wp) ; + static void Log_Write_Startup(byte type) ; + static void Log_Write_Control_Tuning() ; + static void Log_Write_Nav_Tuning() ; + static void Log_Write_Mode(byte mode) ; + static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) ; + static void Log_Write_Raw() ; + static void Log_Write_Current() ; + static void Log_Read_Current() ; + static void Log_Read_Control_Tuning() ; + static void Log_Read_Nav_Tuning() ; + static void Log_Read_Performance() ; + static void Log_Read_Cmd() ; + static void Log_Read_Startup() ; + static void Log_Read_Attitude() ; + static void Log_Read_Mode() ; + static void Log_Read_GPS() ; + static void Log_Read_Raw() ; + static void Log_Read(int start_page, int end_page) ; + static void Log_Write_Mode(byte mode) ; + static void Log_Write_Startup(byte type) ; + static void Log_Write_Cmd(byte num, struct Location *wp) ; + static void Log_Write_Current() ; + static void Log_Write_Nav_Tuning() ; + static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) ; + static void Log_Write_Performance() ; + static byte get_num_logs(void) ; + static void start_new_log(byte num_existing_logs) ; + static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) ; + static void Log_Write_Control_Tuning() ; + static void Log_Write_Raw() ; + static void add_altitude_data(unsigned long xl, long y) ; + static void init_commands() ; + static void update_auto() ; + static void reload_commands_airstart() ; + static struct Location get_cmd_with_index(int i) ; + static void set_cmd_with_index(struct Location temp, int i) ; + static void increment_cmd_index() ; + static void decrement_cmd_index() ; + static long read_alt_to_hold() ; + static void set_next_WP(struct Location *wp) ; + static void set_guided_WP(void) ; + void init_home() ; + static void handle_process_nav_cmd() ; + static void handle_process_condition_command() ; + static void handle_process_do_command() ; + static void handle_no_commands() ; + static void do_RTL(void) ; + static void do_takeoff() ; + static void do_nav_wp() ; + static void do_land() ; + static void do_loiter_unlimited() ; + static void do_loiter_turns() ; + static void do_loiter_time() ; + static bool verify_takeoff() ; + static bool verify_land() ; + static bool verify_nav_wp() ; + static bool verify_loiter_unlim() ; + static bool verify_loiter_time() ; + static bool verify_loiter_turns() ; + static bool verify_RTL() ; + static void do_wait_delay() ; + static void do_change_alt() ; + static void do_within_distance() ; + static bool verify_wait_delay() ; + static bool verify_change_alt() ; + static bool verify_within_distance() ; + static void do_loiter_at_location() ; + static void do_jump() ; + static void do_change_speed() ; + static void do_set_home() ; + static void do_set_servo() ; + static void do_set_relay() ; + static void do_repeat_servo() ; + static void do_repeat_relay() ; + static void change_command(uint8_t cmd_index) ; + static void update_commands(void) ; + static void verify_commands(void) ; + static void process_next_command() ; + static void process_nav_cmd() ; + static void process_non_nav_command() ; + static void read_control_switch() ; + static byte readSwitch(void); + static void reset_control_switch() ; + static void update_servo_switches() ; + static void failsafe_short_on_event() ; + static void failsafe_long_on_event() ; + static void failsafe_short_off_event() ; + static void low_battery_event(void) ; + static void navigate() ; + void calc_distance_error() ; + static void calc_airspeed_errors() ; + static void calc_bearing_error() ; + static void calc_altitude_error() ; + static long wrap_360(long error) ; + static long wrap_180(long error) ; + static void update_loiter() ; + static void update_crosstrack(void) ; + static void reset_crosstrack() ; + static long get_distance(struct Location *loc1, struct Location *loc2) ; + static long get_bearing(struct Location *loc1, struct Location *loc2) ; + static void init_rc_in() ; + static void init_rc_out() ; + static void read_radio() ; + static void control_failsafe(uint16_t pwm) ; + static void trim_control_surfaces() ; + static void trim_radio() ; + void ReadSCP1000(void) ; + static void init_barometer(void) ; + static long read_barometer(void) ; + static void read_airspeed(void) ; + static void zero_airspeed(void) ; + static void read_battery(void) ; + static void report_batt_monitor() ; + static void report_radio() ; + static void report_gains() ; + static void report_xtrack() ; + static void report_throttle() ; + static void report_imu() ; + static void report_compass() ; + static void report_flight_modes() ; + static void print_PID(PID * pid) ; + static void print_radio_values() ; + static void print_switch(byte p, byte m) ; + static void print_done() ; + static void print_blanks(int num) ; + static void print_divider(void) ; + static int8_t radio_input_switch(void) ; + static void zero_eeprom(void) ; + static void print_enabled(bool b) ; + static void print_accel_offsets(void) ; + static void print_gyro_offsets(void) ; + static void run_cli(void) ; + static void init_ardupilot() ; + static void startup_ground(void) ; + static void set_mode(byte mode) ; + static void check_long_failsafe() ; + static void check_short_failsafe() ; + static void startup_IMU_ground(void) ; + static void update_GPS_light(void) ; + static void resetPerfData(void) ; + static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) ; + static void print_hit_enter() ; + static void test_wp_print(struct Location *cmd, byte wp_index) ; +#line 64 "/home/jgoppert/Projects/ardupilotone/ArduPlane/ArduPlane.pde" +FastSerialPort0(Serial); // FTDI/console +FastSerialPort1(Serial1); // GPS port +FastSerialPort3(Serial3); // Telemetry port + +//////////////////////////////////////////////////////////////////////////////// +// Parameters +//////////////////////////////////////////////////////////////////////////////// +// +// Global parameters are all contained within the 'g' class. +// +static Parameters g; + + +//////////////////////////////////////////////////////////////////////////////// +// prototypes +static void update_events(void); + + +//////////////////////////////////////////////////////////////////////////////// +// Sensors +//////////////////////////////////////////////////////////////////////////////// +// +// There are three basic options related to flight sensor selection. +// +// - Normal flight mode. Real sensors are used. +// - HIL Attitude mode. Most sensors are disabled, as the HIL +// protocol supplies attitude information directly. +// - HIL Sensors mode. Synthetic sensors are configured that +// supply data from the simulation. +// + +// All GPS access should be through this pointer. +static GPS *g_gps; + +// flight modes convenience array +static AP_Int8 *flight_modes = &g.flight_mode1; + +#if HIL_MODE == HIL_MODE_DISABLED + +// real sensors +static AP_ADC_ADS7844 adc; +static APM_BMP085_Class barometer; +static AP_Compass_HMC5843 compass(Parameters::k_param_compass); + +// real GPS selection +#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO +AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA +AP_GPS_NMEA g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF +AP_GPS_SIRF g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX +AP_GPS_UBLOX g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK +AP_GPS_MTK g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 +AP_GPS_MTK16 g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE +AP_GPS_None g_gps_driver(NULL); + +#else + #error Unrecognised GPS_PROTOCOL setting. +#endif // GPS PROTOCOL + +#elif HIL_MODE == HIL_MODE_SENSORS +// sensor emulators +AP_ADC_HIL adc; +APM_BMP085_HIL_Class barometer; +AP_Compass_HIL compass; +AP_GPS_HIL g_gps_driver(NULL); + +#elif HIL_MODE == HIL_MODE_ATTITUDE +AP_ADC_HIL adc; +AP_DCM_HIL dcm; +AP_GPS_HIL g_gps_driver(NULL); +AP_Compass_HIL compass; // never used +AP_IMU_Shim imu; // never used + +#else + #error Unrecognised HIL_MODE setting. +#endif // HIL MODE + +#if HIL_MODE != HIL_MODE_ATTITUDE + #if HIL_MODE != HIL_MODE_SENSORS + // Normal + AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); + #else + // hil imu + AP_IMU_Shim imu; + #endif + // normal dcm + AP_DCM dcm(&imu, g_gps); +#endif + +//////////////////////////////////////////////////////////////////////////////// +// GCS selection +//////////////////////////////////////////////////////////////////////////////// +// +GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0); +GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3); + +//////////////////////////////////////////////////////////////////////////////// +// SONAR selection +//////////////////////////////////////////////////////////////////////////////// +// +ModeFilter sonar_mode_filter; + +#if SONAR_TYPE == MAX_SONAR_XL + AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); +#elif SONAR_TYPE == MAX_SONAR_LV + // XXX honestly I think these output the same values + // If someone knows, can they confirm it? + AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Global variables +//////////////////////////////////////////////////////////////////////////////// + +byte control_mode = INITIALISING; +byte oldSwitchPosition; // for remembering the control mode switch +bool inverted_flight = false; + +static const char *comma = ","; + +static const char* flight_mode_strings[] = { + "Manual", + "Circle", + "Stabilize", + "", + "", + "FBW_A", + "FBW_B", + "", + "", + "", + "Auto", + "RTL", + "Loiter", + "Takeoff", + "Land"}; + + +/* Radio values + Channel assignments + 1 Ailerons (rudder if no ailerons) + 2 Elevator + 3 Throttle + 4 Rudder (if we have ailerons) + 5 Aux5 + 6 Aux6 + 7 Aux7 + 8 Aux8/Mode + Each Aux channel can be configured to have any of the available auxiliary functions assigned to it. + See libraries/RC_Channel/RC_Channel_aux.h for more information +*/ + +// Failsafe +// -------- +static int failsafe; // track which type of failsafe is being processed +static bool ch3_failsafe; +static byte crash_timer; + +// Radio +// ----- +static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm +static uint16_t elevon2_trim = 1500; +static uint16_t ch1_temp = 1500; // Used for elevon mixing +static uint16_t ch2_temp = 1500; +static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; +static bool rc_override_active = false; +static uint32_t rc_override_fs_timer = 0; +static uint32_t ch3_failsafe_timer = 0; + +// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon + +// LED output +// ---------- +static bool GPS_light; // status of the GPS light + +// GPS variables +// ------------- +static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage +static float scaleLongUp = 1; // used to reverse longitude scaling +static float scaleLongDown = 1; // used to reverse longitude scaling +static byte ground_start_count = 5; // have we achieved first lock and set Home? +static int ground_start_avg; // 5 samples to avg speed for ground start +static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present + +// Location & Navigation +// --------------------- +const float radius_of_earth = 6378100; // meters +const float gravity = 9.81; // meters/ sec^2 +static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target +static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? +static long hold_course = -1; // deg * 100 dir of plane + +static byte command_index; // current command memory location +static byte nav_command_index; // active nav command memory location +static byte non_nav_command_index; // active non-nav command memory location +static byte nav_command_ID = NO_COMMAND; // active nav command ID +static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID + +// Airspeed +// -------- +static int airspeed; // m/s * 100 +static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range +static float airspeed_error; // m/s * 100 +static float airspeed_fbwB; // m/s * 100 +static long energy_error; // energy state error (kinetic + potential) for altitude hold +static long airspeed_energy_error; // kinetic portion of energy error + +// Location Errors +// --------------- +static long bearing_error; // deg * 100 : 0 to 36000 +static long altitude_error; // meters * 100 we are off in altitude +static float crosstrack_error; // meters we are off trackline + +// Battery Sensors +// --------------- +static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter +static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter +static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter +static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter +static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter + +static float current_amps; +static float current_total; + +// Airspeed Sensors +// ---------------- +static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering +static int airspeed_pressure; // airspeed as a pressure value + +// Barometer Sensor variables +// -------------------------- +static unsigned long abs_pressure; + +// Altitude Sensor variables +// ---------------------- +static int sonar_alt; + +// flight mode specific +// -------------------- +static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. +static bool land_complete; +static long takeoff_altitude; +// static int landing_distance; // meters; +static int landing_pitch; // pitch for landing set by commands +static int takeoff_pitch; + +// Loiter management +// ----------------- +static long old_target_bearing; // deg * 100 +static int loiter_total; // deg : how many times to loiter * 360 +static int loiter_delta; // deg : how far we just turned +static int loiter_sum; // deg : how far we have turned around a waypoint +static long loiter_time; // millis : when we started LOITER mode +static int loiter_time_max; // millis : how long to stay in LOITER mode + +// these are the values for navigation control functions +// ---------------------------------------------------- +static long nav_roll; // deg * 100 : target roll angle +static long nav_pitch; // deg * 100 : target pitch angle +static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel + +// Waypoints +// --------- +static long wp_distance; // meters - distance between plane and next waypoint +static long wp_totalDistance; // meters - distance between old and next waypoint + +// repeating event control +// ----------------------- +static byte event_id; // what to do - see defines +static long event_timer; // when the event was asked for in ms +static uint16_t event_delay; // how long to delay the next firing of event in millis +static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles +static int event_value; // per command value, such as PWM for servos +static int event_undo_value; // the value used to cycle events (alternate value to event_value) + +// delay command +// -------------- +static long condition_value; // used in condition commands (eg delay, change alt, etc.) +static long condition_start; +static int condition_rate; + +// 3D Location vectors +// ------------------- +static struct Location home; // home location +static struct Location prev_WP; // last waypoint +static struct Location current_loc; // current location +static struct Location next_WP; // next waypoint +static struct Location guided_WP; // guided mode waypoint +static struct Location next_nav_command; // command preloaded +static struct Location next_nonnav_command; // command preloaded +static long target_altitude; // used for altitude management between waypoints +static long offset_altitude; // used for altitude management between waypoints +static bool home_is_set; // Flag for if we have g_gps lock and have set the home location + + +// IMU variables +// ------------- +static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) + + +// Performance monitoring +// ---------------------- +static long perf_mon_timer; // Metric based on accel gain deweighting +static int G_Dt_max = 0; // Max main loop cycle time in milliseconds +static int gps_fix_count = 0; +static int pmTest1 = 0; + + +// System Timers +// -------------- +static unsigned long fast_loopTimer; // Time in miliseconds of main control loop +static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete +static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds +static int mainLoop_count; + +static unsigned long medium_loopTimer; // Time in miliseconds of medium loop +static byte medium_loopCounter; // Counters for branching from main control loop to slower loops +static uint8_t delta_ms_medium_loop; + +static byte slow_loopCounter; +static byte superslow_loopCounter; +static byte counter_one_herz; + +static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav + +static unsigned long dTnav; // Delta Time in milliseconds for navigation computations +static float load; // % MCU cycles used + +AP_Relay relay; + +//////////////////////////////////////////////////////////////////////////////// +// Top-level logic +//////////////////////////////////////////////////////////////////////////////// + +void setup() { + memcheck_init(); + init_ardupilot(); +} + +void loop() +{ + // We want this to execute at 50Hz if possible + // ------------------------------------------- + if (millis()-fast_loopTimer > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer; + load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop; + G_Dt = (float)delta_ms_fast_loop / 1000.f; + fast_loopTimer = millis(); + + mainLoop_count++; + + // Execute the fast loop + // --------------------- + fast_loop(); + + // Execute the medium loop + // ----------------------- + medium_loop(); + + counter_one_herz++; + if(counter_one_herz == 50){ + one_second_loop(); + counter_one_herz = 0; + } + + if (millis() - perf_mon_timer > 20000) { + if (mainLoop_count != 0) { + if (g.log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); + + resetPerfData(); + } + } + + fast_loopTimeStamp = millis(); + } +} + +// Main loop 50Hz +static void fast_loop() +{ + // This is the fast loop - we want it to execute at 50Hz if possible + // ----------------------------------------------------------------- + if (delta_ms_fast_loop > G_Dt_max) + G_Dt_max = delta_ms_fast_loop; + + // Read radio + // ---------- + read_radio(); + + // try to send any deferred messages if the serial port now has + // some space available + gcs_send_message(MSG_RETRY_DEFERRED); + + // check for loss of control signal failsafe condition + // ------------------------------------ + check_short_failsafe(); + + // Read Airspeed + // ------------- + if (g.airspeed_enabled == true) { +#if HIL_MODE != HIL_MODE_ATTITUDE + read_airspeed(); +#endif + } else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { + calc_airspeed_errors(); + } + + #if HIL_MODE == HIL_MODE_SENSORS + // update hil before dcm update + gcs_update(); + #endif + + dcm.update_DCM(); + + // uses the yaw from the DCM to give more accurate turns + calc_bearing_error(); + + # if HIL_MODE == HIL_MODE_DISABLED + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) + Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); + + if (g.log_bitmask & MASK_LOG_RAW) + Log_Write_Raw(); + #endif + + // inertial navigation + // ------------------ + #if INERTIAL_NAVIGATION == ENABLED + // TODO: implement inertial nav function + inertialNavigation(); + #endif + + // custom code/exceptions for flight modes + // --------------------------------------- + update_current_flight_mode(); + + // apply desired roll, pitch and yaw to the plane + // ---------------------------------------------- + if (control_mode > MANUAL) + stabilize(); + + // write out the servo PWM values + // ------------------------------ + set_servos(); + + + // XXX is it appropriate to be doing the comms below on the fast loop? + + gcs_update(); + gcs_data_stream_send(45,1000); +} + +static void medium_loop() +{ + // This is the start of the medium (10 Hz) loop pieces + // ----------------------------------------- + switch(medium_loopCounter) { + + // This case deals with the GPS + //------------------------------- + case 0: + medium_loopCounter++; + if(GPS_enabled) update_GPS(); + + #if HIL_MODE != HIL_MODE_ATTITUDE + if(g.compass_enabled){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + } + #endif +/*{ +Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(dcm.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(dcm.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); +Vector3f tempaccel = imu.get_accel(); +Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); +Serial.println(tempaccel.z, DEC); +}*/ + + break; + + // This case performs some navigation computations + //------------------------------------------------ + case 1: + medium_loopCounter++; + + + if(g_gps->new_data){ + g_gps->new_data = false; + dTnav = millis() - nav_loopTimer; + nav_loopTimer = millis(); + + // calculate the plane's desired bearing + // ------------------------------------- + navigate(); + } + + break; + + // command processing + //------------------------------ + case 2: + medium_loopCounter++; + + // Read altitude from sensors + // ------------------ + update_alt(); + if(g.sonar_enabled) sonar_alt = sonar.read(); + + // altitude smoothing + // ------------------ + if (control_mode != FLY_BY_WIRE_B) + calc_altitude_error(); + + // perform next command + // -------------------- + update_commands(); + break; + + // This case deals with sending high rate telemetry + //------------------------------------------------- + case 3: + medium_loopCounter++; + + #if HIL_MODE != HIL_MODE_ATTITUDE + if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) + Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); + + if (g.log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); + #endif + + if (g.log_bitmask & MASK_LOG_NTUN) + Log_Write_Nav_Tuning(); + + if (g.log_bitmask & MASK_LOG_GPS) + Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); + + // send all requested output streams with rates requested + // between 5 and 45 Hz + gcs_data_stream_send(5,45); + break; + + // This case controls the slow loop + //--------------------------------- + case 4: + medium_loopCounter = 0; + delta_ms_medium_loop = millis() - medium_loopTimer; + medium_loopTimer = millis(); + + if (g.battery_monitoring != 0){ + read_battery(); + } + + slow_loop(); + break; + } +} + +static void slow_loop() +{ + // This is the slow (3 1/3 Hz) loop pieces + //---------------------------------------- + switch (slow_loopCounter){ + case 0: + slow_loopCounter++; + check_long_failsafe(); + superslow_loopCounter++; + if(superslow_loopCounter >=200) { // 200 = Execute every minute + #if HIL_MODE != HIL_MODE_ATTITUDE + if(g.compass_enabled) { + compass.save_offsets(); + } + #endif + + superslow_loopCounter = 0; + } + break; + + case 1: + slow_loopCounter++; + + // Read 3-position switch on radio + // ------------------------------- + read_control_switch(); + + // Read Control Surfaces/Mix switches + // ---------------------------------- + update_servo_switches(); + + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); + + break; + + case 2: + slow_loopCounter = 0; + update_events(); + + mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter + gcs_data_stream_send(3,5); + break; + } +} + +static void one_second_loop() +{ + if (g.log_bitmask & MASK_LOG_CUR) + Log_Write_Current(); + + // send a heartbeat + gcs_send_message(MSG_HEARTBEAT); + gcs_data_stream_send(1,3); +} + +static void update_GPS(void) +{ + g_gps->update(); + update_GPS_light(); + + if (g_gps->new_data && g_gps->fix) { + // for performance + // --------------- + gps_fix_count++; + + if(ground_start_count > 1){ + ground_start_count--; + ground_start_avg += g_gps->ground_speed; + + } else if (ground_start_count == 1) { + // We countdown N number of good GPS fixes + // so that the altitude is more accurate + // ------------------------------------- + if (current_loc.lat == 0) { + ground_start_count = 5; + + } else { + if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){ + startup_ground(); + + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + + init_home(); + } else if (ENABLE_AIR_START == 0) { + init_home(); + } + + ground_start_count = 0; + } + } + + + current_loc.lng = g_gps->longitude; // Lon * 10**7 + current_loc.lat = g_gps->latitude; // Lat * 10**7 + + } +} + +static void update_current_flight_mode(void) +{ + if(control_mode == AUTO){ + crash_checker(); + + switch(nav_command_ID){ + case MAV_CMD_NAV_TAKEOFF: + if (hold_course > -1) { + calc_nav_roll(); + } else { + nav_roll = 0; + } + + if (g.airspeed_enabled == true) + { + calc_nav_pitch(); + if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; + } else { + nav_pitch = (long)((float)g_gps->ground_speed / (float)g.airspeed_cruise * (float)takeoff_pitch * 0.5); + nav_pitch = constrain(nav_pitch, 500l, (long)takeoff_pitch); + } + + g.channel_throttle.servo_out = g.throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle + // What is the case for doing something else? Why wouldn't you want max throttle for TO? + // ****************************** + + break; + + case MAV_CMD_NAV_LAND: + calc_nav_roll(); + + if (g.airspeed_enabled == true){ + calc_nav_pitch(); + calc_throttle(); + }else{ + calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle + calc_throttle(); // throttle based on altitude error + nav_pitch = landing_pitch; // pitch held constant + } + + if (land_complete){ + g.channel_throttle.servo_out = 0; + } + break; + + default: + hold_course = -1; + calc_nav_roll(); + calc_nav_pitch(); + calc_throttle(); + break; + } + }else{ + switch(control_mode){ + case RTL: + case LOITER: + case GUIDED: + hold_course = -1; + crash_checker(); + calc_nav_roll(); + calc_nav_pitch(); + calc_throttle(); + break; + + case FLY_BY_WIRE_A: + // set nav_roll and nav_pitch using sticks + nav_roll = g.channel_roll.norm_input() * g.roll_limit; + nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min; + // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. + nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority + if (inverted_flight) nav_pitch = -nav_pitch; + break; + + case FLY_BY_WIRE_B: + // Substitute stick inputs for Navigation control output + // We use g.pitch_limit_min because its magnitude is + // normally greater than g.pitch_limit_max + nav_roll = g.channel_roll.norm_input() * g.roll_limit; + altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; + + if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) { + altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; + } else { + if (g.channel_pitch.norm_input()<0) + altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ; + else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ; + } + + if (g.airspeed_enabled == true) + { + airspeed_fbwB = ((int)(g.flybywire_airspeed_max - + g.flybywire_airspeed_min) * + g.channel_throttle.servo_out) + + ((int)g.flybywire_airspeed_min * 100); + airspeed_energy_error = (long)(((long)airspeed_fbwB * + (long)airspeed_fbwB) - + ((long)airspeed * (long)airspeed))/20000; + airspeed_error = (airspeed_error - airspeed); + } + + calc_throttle(); + calc_nav_pitch(); + break; + + case STABILIZE: + nav_roll = 0; + nav_pitch = 0; + // throttle is passthrough + break; + + case CIRCLE: + // we have no GPS installed and have lost radio contact + // or we just want to fly around in a gentle circle w/o GPS + // ---------------------------------------------------- + nav_roll = g.roll_limit / 3; + nav_pitch = 0; + + if (failsafe != FAILSAFE_NONE){ + g.channel_throttle.servo_out = g.throttle_cruise; + } + break; + + case MANUAL: + // servo_out is for Sim control only + // --------------------------------- + g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); + g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); + g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); + break; + //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 + + } + } +} + +static void update_navigation() +{ + // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS + // ------------------------------------------------------------------------ + + // distance and bearing calcs only + if(control_mode == AUTO){ + verify_commands(); + }else{ + + switch(control_mode){ + case LOITER: + case RTL: + case GUIDED: + update_loiter(); + calc_bearing_error(); + break; + + } + } +} + + +static void update_alt() +{ + #if HIL_MODE == HIL_MODE_ATTITUDE + current_loc.alt = g_gps->altitude; + #else + // this function is in place to potentially add a sonar sensor in the future + //altitude_sensor = BARO; + + current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100) + current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); + #endif + + // Calculate new climb rate + //if(medium_loopCounter == 0 && slow_loopCounter == 0) + // add_altitude_data(millis() / 100, g_gps->altitude / 10); +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/Attitude.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +//**************************************************************** +// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. +//**************************************************************** + +static void stabilize() +{ + float ch1_inf = 1.0; + float ch2_inf = 1.0; + float ch4_inf = 1.0; + float speed_scaler; + + if (g.airspeed_enabled == true){ + if(airspeed > 0) + speed_scaler = (STANDARD_SPEED * 100) / airspeed; + else + speed_scaler = 2.0; + speed_scaler = constrain(speed_scaler, 0.5, 2.0); + } else { + if (g.channel_throttle.servo_out > 0){ + speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root + // Should maybe be to the 2/7 power, but we aren't goint to implement that... + }else{ + speed_scaler = 1.67; + } + speed_scaler = constrain(speed_scaler, 0.6, 1.67); // This case is constrained tighter as we don't have real speed info + } + + if(crash_timer > 0){ + nav_roll = 0; + } + + if (inverted_flight) { + // we want to fly upside down. We need to cope with wrap of + // the roll_sensor interfering with wrap of nav_roll, which + // would really confuse the PID code. The easiest way to + // handle this is to ensure both go in the same direction from + // zero + nav_roll += 18000; + if (dcm.roll_sensor < 0) nav_roll -= 36000; + } + + // For Testing Only + // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; + // Serial.printf_P(PSTR(" roll_sensor ")); + // Serial.print(roll_sensor,DEC); + + // Calculate dersired servo output for the roll + // --------------------------------------------- + g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler); + long tempcalc = nav_pitch + + fabs(dcm.roll_sensor * g.kff_pitch_compensation) + + (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - + (dcm.pitch_sensor - g.pitch_trim); + if (inverted_flight) { + // when flying upside down the elevator control is inverted + tempcalc = -tempcalc; + } + g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); + + // Mix Stick input to allow users to override control surfaces + // ----------------------------------------------------------- + if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) { + + + // TODO: use RC_Channel control_mix function? + ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; + ch1_inf = fabs(ch1_inf); + ch1_inf = min(ch1_inf, 400.0); + ch1_inf = ((400.0 - ch1_inf) /400.0); + + ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim; + ch2_inf = fabs(ch2_inf); + ch2_inf = min(ch2_inf, 400.0); + ch2_inf = ((400.0 - ch2_inf) /400.0); + + // scale the sensor input based on the stick input + // ----------------------------------------------- + g.channel_roll.servo_out *= ch1_inf; + g.channel_pitch.servo_out *= ch2_inf; + + // Mix in stick inputs + // ------------------- + g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); + g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle(); + + //Serial.printf_P(PSTR(" servo_out[CH_ROLL] ")); + //Serial.println(servo_out[CH_ROLL],DEC); + } + + // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes + // important for steering on the ground during landing + // ----------------------------------------------- + if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) { + ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; + ch4_inf = fabs(ch4_inf); + ch4_inf = min(ch4_inf, 400.0); + ch4_inf = ((400.0 - ch4_inf) /400.0); + } + + // Apply output to Rudder + // ---------------------- + calc_nav_yaw(speed_scaler); + g.channel_rudder.servo_out *= ch4_inf; + g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle(); + + // Call slew rate limiter if used + // ------------------------------ + //#if(ROLL_SLEW_LIMIT != 0) + // g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out); + //#endif +} + +static void crash_checker() +{ + if(dcm.pitch_sensor < -4500){ + crash_timer = 255; + } + if(crash_timer > 0) + crash_timer--; +} + + +static void calc_throttle() +{ + if (g.airspeed_enabled == false) { + int throttle_target = g.throttle_cruise + throttle_nudge; + + // no airspeed sensor, we use nav pitch to determine the proper throttle output + // AUTO, RTL, etc + // --------------------------------------------------------------------------- + if (nav_pitch >= 0) { + g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch / g.pitch_limit_max; + } else { + g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch / g.pitch_limit_min; + } + + g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); + } else { + // throttle control with airspeed compensation + // ------------------------------------------- + energy_error = airspeed_energy_error + (float)altitude_error * 0.098f; + + // positive energy errors make the throttle go higher + g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav); + g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle); + + g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, + g.throttle_min.get(), g.throttle_max.get()); // TODO - resolve why "saved" is used here versus "current" + } + +} + +/***************************************** + * Calculate desired roll/pitch/yaw angles (in medium freq loop) + *****************************************/ + +// Yaw is separated into a function for future implementation of heading hold on rolling take-off +// ---------------------------------------------------------------------------------------- +static void calc_nav_yaw(float speed_scaler) +{ +#if HIL_MODE != HIL_MODE_ATTITUDE + Vector3f temp = imu.get_accel(); + long error = -temp.y; + + // Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero) + g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler); +#else + g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; + // XXX probably need something here based on heading +#endif +} + + +static void calc_nav_pitch() +{ + // Calculate the Pitch of the plane + // -------------------------------- + if (g.airspeed_enabled == true) { + nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); + } else { + nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); + } + nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get()); +} + + +#define YAW_DAMPENER 0 + +static void calc_nav_roll() +{ + + // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. + // This does not make provisions for wind speed in excess of airframe speed + nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0); + nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4); + + // negative error = left turn + // positive error = right turn + // Calculate the required roll of the plane + // ---------------------------------------- + nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100 + nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); + + Vector3f omega; + omega = dcm.get_gyro(); + + // rate limiter + long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 + rate = constrain(rate, -6000, 6000); // limit input + int dampener = rate * YAW_DAMPENER; // 34377 * .175 = 6000 + + // add in yaw dampener + nav_roll -= dampener; + nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); +} + + +/***************************************** + * Roll servo slew limit + *****************************************/ +/* +float roll_slew_limit(float servo) +{ + static float last; + float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f); + last = servo; + return temp; +}*/ + +/***************************************** + * Throttle slew limit + *****************************************/ +static void throttle_slew_limit() +{ + static int last = 1000; + if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit + + float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000 +Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last); + g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp); + last = g.channel_throttle.radio_out; + } +} + + +// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. +// Keeps outdated data out of our calculations +static void reset_I(void) +{ + g.pidNavRoll.reset_I(); + g.pidNavPitchAirspeed.reset_I(); + g.pidNavPitchAltitude.reset_I(); + g.pidTeThrottle.reset_I(); +// g.pidAltitudeThrottle.reset_I(); +} + +/***************************************** +* Set the flight control servos based on the current calculated values +*****************************************/ +static void set_servos(void) +{ + int flapSpeedSource = 0; + + // vectorize the rc channels + RC_Channel_aux* rc_array[NUM_CHANNELS]; + rc_array[CH_1] = NULL; + rc_array[CH_2] = NULL; + rc_array[CH_3] = NULL; + rc_array[CH_4] = NULL; + rc_array[CH_5] = &g.rc_5; + rc_array[CH_6] = &g.rc_6; + rc_array[CH_7] = &g.rc_7; + rc_array[CH_8] = &g.rc_8; + + if(control_mode == MANUAL){ + // do a direct pass through of radio values + if (g.mix_mode == 0){ + g.channel_roll.radio_out = g.channel_roll.radio_in; + g.channel_pitch.radio_out = g.channel_pitch.radio_in; + } else { + g.channel_roll.radio_out = APM_RC.InputCh(CH_ROLL); + g.channel_pitch.radio_out = APM_RC.InputCh(CH_PITCH); + } + g.channel_throttle.radio_out = g.channel_throttle.radio_in; + g.channel_rudder.radio_out = g.channel_rudder.radio_in; + // FIXME To me it does not make sense to control the aileron using radio_in in manual mode + // Doug could you please take a look at this ? + if (g_rc_function[RC_Channel_aux::k_aileron]) { + if (g_rc_function[RC_Channel_aux::k_aileron] != rc_array[g.flight_mode_channel-1]) { + g_rc_function[RC_Channel_aux::k_aileron]->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; + } + } + // only use radio_in if the channel is not used as flight_mode_channel + if (g_rc_function[RC_Channel_aux::k_flap_auto]) { + if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) { + g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; + } else { + g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim; + } + } + } else { + if (g.mix_mode == 0) { + g.channel_roll.calc_pwm(); + g.channel_pitch.calc_pwm(); + g.channel_rudder.calc_pwm(); + if (g_rc_function[RC_Channel_aux::k_aileron]) { + g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out; + g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); + } + + }else{ + /*Elevon mode*/ + float ch1; + float ch2; + ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out); + ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out; + g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); + g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); + } + + #if THROTTLE_OUT == 0 + g.channel_throttle.servo_out = 0; + #else + // convert 0 to 100% into PWM + g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); + + // We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. + /* Disable throttle if following conditions are met: + 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher + AND + 2 - Our reported altitude is within 10 meters of the home altitude. + 3 - Our reported speed is under 5 meters per second. + 4 - We are not performing a takeoff in Auto mode + OR + 5 - Home location is not set + */ + if ( + (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) && + (abs(home.alt - current_loc.alt) < 1000) && + ((g.airspeed_enabled ? airspeed : g_gps->ground_speed) < 500 ) && + !(control_mode==AUTO && takeoff_complete == false) + ) { + g.channel_throttle.servo_out = 0; + g.channel_throttle.calc_pwm(); + } + + #endif + + g.channel_throttle.calc_pwm(); + + /* TO DO - fix this for RC_Channel library + #if THROTTLE_REVERSE == 1 + radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE]; + #endif + */ + + throttle_slew_limit(); + } + + // Auto flap deployment + if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) { + if(control_mode < FLY_BY_WIRE_B) { + // only use radio_in if the channel is not used as flight_mode_channel + if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) { + g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; + } else { + g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim; + } + } else if (control_mode >= FLY_BY_WIRE_B) { + if (control_mode == FLY_BY_WIRE_B) { + flapSpeedSource = airspeed_fbwB/100; + } else if (g.airspeed_enabled == true) { + flapSpeedSource = g.airspeed_cruise/100; + } else { + flapSpeedSource = g.throttle_cruise; + } + if ( flapSpeedSource > g.flap_1_speed) { + g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = 0; + } else if (flapSpeedSource > g.flap_2_speed) { + g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_1_percent; + } else { + g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_2_percent; + } + g_rc_function[RC_Channel_aux::k_flap_auto]->calc_pwm(); + } + } + +#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS + // send values to the PWM timers for output + // ---------------------------------------- + APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos + APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos + APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos + APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos + // Route configurable aux. functions to their respective servos + g.rc_5.output_ch(CH_5); + g.rc_6.output_ch(CH_6); + g.rc_7.output_ch(CH_7); + g.rc_8.output_ch(CH_8); +#endif +} + +static void demo_servos(byte i) { + + while(i > 0){ + gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); +#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS + APM_RC.OutputCh(1, 1400); + mavlink_delay(400); + APM_RC.OutputCh(1, 1600); + mavlink_delay(200); + APM_RC.OutputCh(1, 1500); +#endif + mavlink_delay(400); + i--; + } +} + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/GCS_Mavlink.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Mavlink_compat.h" + +// use this to prevent recursion during sensor init +static bool in_mavlink_delay; + +// this costs us 51 bytes, but means that low priority +// messages don't block the CPU +static mavlink_statustext_t pending_status; + +// true when we have received at least 1 MAVLink packet +static bool mavlink_active; + +// check if a message will fit in the payload space available +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ +#ifdef MAVLINK10 + uint8_t base_mode = 0; + uint8_t system_status = MAV_STATE_ACTIVE; + + // we map the custom_mode to our internal mode plus 16, to lower + // the chance that a ground station will give us 0 and we + // interpret it as manual. This is necessary as the SET_MODE + // command has no way to indicate that the custom_mode is filled in + uint32_t custom_mode = control_mode + 16; + + // work out the base_mode. This value is almost completely useless + // for APM, but we calculate it as best we can so a generic + // MAVLink enabled ground station can work out something about + // what the MAV is up to. The actual bit values are highly + // ambiguous for most of the APM flight modes. In practice, you + // only get useful information from the custom_mode, which maps to + // the APM flight mode and has a well defined meaning in the + // ArduPlane documentation + switch (control_mode) { + case MANUAL: + base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + break; + case STABILIZE: + case FLY_BY_WIRE_A: + case FLY_BY_WIRE_B: + case FLY_BY_WIRE_C: + base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; + break; + case AUTO: + case RTL: + case LOITER: + case GUIDED: + case CIRCLE: + base_mode = MAV_MODE_FLAG_GUIDED_ENABLED | + MAV_MODE_FLAG_STABILIZE_ENABLED; + // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what + // APM does in any mode, as that is defined as "system finds its own goal + // positions", which APM does not currently do + break; + case INITIALISING: + system_status = MAV_STATE_CALIBRATING; + break; + } + + if (control_mode != MANUAL && control_mode != INITIALISING) { + // stabiliser of some form is enabled + base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + +#if ENABLE_STICK_MIXING==ENABLED + if (control_mode != INITIALISING) { + // all modes except INITIALISING have some form of manual + // override if stick mixing is enabled + base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } +#endif + +#if HIL_MODE != HIL_MODE_DISABLED + base_mode |= MAV_MODE_FLAG_HIL_ENABLED; +#endif + + // we are armed if we are not initialising + if (control_mode != INITIALISING) { + base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + mavlink_msg_heartbeat_send( + chan, + MAV_TYPE_FIXED_WING, + MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode, + custom_mode, + system_status); +#else // MAVLINK10 + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +#endif // MAVLINK10 +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + Vector3f omega = dcm.get_gyro(); + mavlink_msg_attitude_send( + chan, + micros(), + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); +} + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ +#ifdef MAVLINK10 + uint32_t control_sensors_present = 0; + uint32_t control_sensors_enabled; + uint32_t control_sensors_health; + + // first what sensors/controllers we have + control_sensors_present |= (1<<0); // 3D gyro present + control_sensors_present |= (1<<1); // 3D accelerometer present + if (g.compass_enabled) { + control_sensors_present |= (1<<2); // compass present + } + control_sensors_present |= (1<<3); // absolute pressure sensor present + if (g_gps->fix) { + control_sensors_present |= (1<<5); // GPS present + } + control_sensors_present |= (1<<10); // 3D angular rate control + control_sensors_present |= (1<<11); // attitude stabilisation + control_sensors_present |= (1<<12); // yaw position + control_sensors_present |= (1<<13); // altitude control + control_sensors_present |= (1<<14); // X/Y position control + control_sensors_present |= (1<<15); // motor control + + // now what sensors/controllers are enabled + + // first the sensors + control_sensors_enabled = control_sensors_present & 0x1FF; + + // now the controllers + control_sensors_enabled = control_sensors_present & 0x1FF; + + switch (control_mode) { + case MANUAL: + break; + + case STABILIZE: + case FLY_BY_WIRE_A: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + break; + + case FLY_BY_WIRE_B: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + control_sensors_enabled |= (1<<15); // motor control + break; + + case FLY_BY_WIRE_C: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + control_sensors_enabled |= (1<<13); // altitude control + control_sensors_enabled |= (1<<15); // motor control + break; + + case AUTO: + case RTL: + case LOITER: + case GUIDED: + case CIRCLE: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + control_sensors_enabled |= (1<<12); // yaw position + control_sensors_enabled |= (1<<13); // altitude control + control_sensors_enabled |= (1<<14); // X/Y position control + control_sensors_enabled |= (1<<15); // motor control + break; + + case INITIALISING: + break; + } + + // at the moment all sensors/controllers are assumed healthy + control_sensors_health = control_sensors_present; + + uint16_t battery_current = -1; + uint8_t battery_remaining = -1; + + if (current_total != 0 && g.pack_capacity != 0) { + battery_remaining = (100.0 * (g.pack_capacity - current_total) / g.pack_capacity); + } + if (current_total != 0) { + battery_current = current_amps * 100; + } + + mavlink_msg_sys_status_send( + chan, + control_sensors_present, + control_sensors_enabled, + control_sensors_health, + (uint16_t)(load * 1000), + battery_voltage * 1000, // mV + battery_current, // in 10mA units + battery_remaining, // in % + 0, // comm drops %, + 0, // comm drops in pkts, + 0, 0, 0, 0); + +#else // MAVLINK10 + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case MANUAL: + mode = MAV_MODE_MANUAL; + break; + case STABILIZE: + mode = MAV_MODE_TEST1; + break; + case FLY_BY_WIRE_A: + mode = MAV_MODE_TEST2; + nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case FLY_BY_WIRE_B: + mode = MAV_MODE_TEST2; + nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_LOITER; + break; + case INITIALISING: + mode = MAV_MODE_UNINIT; + nav_mode = MAV_NAV_GROUNDED; + break; + } + + uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + load * 1000, + battery_voltage * 1000, + battery_remaining, + packet_drops); +#endif // MAVLINK10 +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now +#ifdef MAVLINK10 + mavlink_msg_global_position_int_send( + chan, + millis(), + current_loc.lat, // in 1E7 degrees + current_loc.lng, // in 1E7 degrees + g_gps->altitude*10, // millimeters above sea level + current_loc.alt * 10, // millimeters above ground + g_gps->ground_speed * rot.a.x, // X speed cm/s + g_gps->ground_speed * rot.b.x, // Y speed cm/s + g_gps->ground_speed * rot.c.x, + g_gps->ground_course); // course in 1/100 degree +#else // MAVLINK10 + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +#endif // MAVLINK10 +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + nav_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + airspeed_error, + crosstrack_error); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ +#ifdef MAVLINK10 + uint8_t fix; + if (g_gps->status() == 2) { + fix = 3; + } else { + fix = 0; + } + + mavlink_msg_gps_raw_int_send( + chan, + micros(), + fix, + g_gps->latitude, // in 1E7 degrees + g_gps->longitude, // in 1E7 degrees + g_gps->altitude * 10, // in mm + g_gps->hdop, + 65535, + g_gps->ground_speed, // cm/s + g_gps->ground_course, // 1/100 degrees, + g_gps->num_sats); + +#else // MAVLINK10 + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +#endif // MAVLINK10 +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with + // HIL maintainers +#ifdef MAVLINK10 + mavlink_msg_rc_channels_scaled_send( + chan, + millis(), + 0, // port 0 + 10000 * g.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + 0, + 0, + 0, + 0, + rssi); + +#else // MAVLINK10 + mavlink_msg_rc_channels_scaled_send( + chan, + 10000 * g.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + 0, + 0, + 0, + 0, + rssi); +#endif // MAVLINK10 +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + uint8_t rssi = 1; +#ifdef MAVLINK10 + mavlink_msg_rc_channels_raw_send( + chan, + millis(), + 0, // port + g.channel_roll.radio_in, + g.channel_pitch.radio_in, + g.channel_throttle.radio_in, + g.channel_rudder.radio_in, + g.rc_5.radio_in, // XXX currently only 4 RC channels defined + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); + +#else // MAVLINK10 + mavlink_msg_rc_channels_raw_send( + chan, + g.channel_roll.radio_in, + g.channel_pitch.radio_in, + g.channel_throttle.radio_in, + g.channel_rudder.radio_in, + g.rc_5.radio_in, // XXX currently only 4 RC channels defined + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +#endif // MAVLINK10 +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ +#ifdef MAVLINK10 + mavlink_msg_servo_output_raw_send( + chan, + micros(), + 0, // port + g.channel_roll.radio_out, + g.channel_pitch.radio_out, + g.channel_throttle.radio_out, + g.channel_rudder.radio_out, + g.rc_5.radio_out, // XXX currently only 4 RC channels defined + g.rc_6.radio_out, + g.rc_7.radio_out, + g.rc_8.radio_out); +#else // MAVLINK10 + mavlink_msg_servo_output_raw_send( + chan, + g.channel_roll.radio_out, + g.channel_pitch.radio_out, + g.channel_throttle.radio_out, + g.channel_rudder.radio_out, + g.rc_5.radio_out, // XXX currently only 4 RC channels defined + g.rc_6.radio_out, + g.rc_7.radio_out, + g.rc_8.radio_out); +#endif // MAVLINK10 +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + (int)g.channel_throttle.servo_out, + current_loc.alt / 100.0, + 0); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-g.ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + Vector3f mag_offsets = compass.get_offsets(); + + mavlink_msg_sensor_offsets_send(chan, + mag_offsets.x, + mag_offsets.y, + mag_offsets.z, + compass.get_declination(), + barometer.RawPress, + barometer.RawTemp, + imu.gx(), imu.gy(), imu.gz(), + imu.ax(), imu.ay(), imu.az()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.command_index); +} + +static void NOINLINE send_statustext(mavlink_channel_t chan) +{ + mavlink_msg_statustext_send( + chan, + pending_status.severity, + pending_status.text); +} + + +// try to send a message, return false if it won't fit in the serial tx buffer +static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) +{ + int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; + + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // defer any messages on the telemetry port for 1 second after + // bootup, to try to prevent bricking of Xbees + return false; + } + + switch (id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + return true; + + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; + + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; + + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; + + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; + + case MSG_NAV_CONTROLLER_OUTPUT: + if (control_mode != MANUAL) { + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); + } + break; + + case MSG_GPS_RAW: +#ifdef MAVLINK10 + CHECK_PAYLOAD_SIZE(GPS_RAW_INT); +#else + CHECK_PAYLOAD_SIZE(GPS_RAW); +#endif + send_gps_raw(chan); + break; + + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; + + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; + + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; + + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; + +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; + + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; + + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE + + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; + + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; + + case MSG_NEXT_PARAM: + CHECK_PAYLOAD_SIZE(PARAM_VALUE); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_param_send(); + } else { + gcs3.queued_param_send(); + } + break; + + case MSG_NEXT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_waypoint_send(); + } else { + gcs3.queued_waypoint_send(); + } + break; + + case MSG_STATUSTEXT: + CHECK_PAYLOAD_SIZE(STATUSTEXT); + send_statustext(chan); + break; + + case MSG_RETRY_DEFERRED: + break; // just here to prevent a warning + } + return true; +} + + +#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED +static struct mavlink_queue { + enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES]; + uint8_t next_deferred_message; + uint8_t num_deferred_messages; +} mavlink_queue[2]; + +// send a message using mavlink +static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) +{ + uint8_t i, nextid; + struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; + + // see if we can send the deferred messages, if any + while (q->num_deferred_messages != 0) { + if (!mavlink_try_send_message(chan, + q->deferred_messages[q->next_deferred_message], + packet_drops)) { + break; + } + q->next_deferred_message++; + if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { + q->next_deferred_message = 0; + } + q->num_deferred_messages--; + } + + if (id == MSG_RETRY_DEFERRED) { + return; + } + + // this message id might already be deferred + for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { + if (q->deferred_messages[nextid] == id) { + // its already deferred, discard + return; + } + nextid++; + if (nextid == MAX_DEFERRED_MESSAGES) { + nextid = 0; + } + } + + if (q->num_deferred_messages != 0 || + !mavlink_try_send_message(chan, id, packet_drops)) { + // can't send it now, so defer it + if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { + // the defer buffer is full, discard + return; + } + nextid = q->next_deferred_message + q->num_deferred_messages; + if (nextid >= MAX_DEFERRED_MESSAGES) { + nextid -= MAX_DEFERRED_MESSAGES; + } + q->deferred_messages[nextid] = id; + q->num_deferred_messages++; + } +} + +void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) +{ + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // don't send status MAVLink messages for 2 seconds after + // bootup, to try to prevent Xbee bricking + return; + } + + if (severity == SEVERITY_LOW) { + // send via the deferred queuing system + pending_status.severity = (uint8_t)severity; + strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); + mavlink_send_message(chan, MSG_STATUSTEXT, 0); + } else { + // send immediately +#ifdef MAVLINK10 + mavlink_msg_statustext_send(chan, severity, str); +#else + mavlink_msg_statustext_send(chan, severity, (const int8_t*) str); +#endif + } +} + + +GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : +packet_drops(0), + +// parameters +// note, all values not explicitly initialised here are zeroed +waypoint_send_timeout(1000), // 1 second +waypoint_receive_timeout(1000), // 1 second + +// stream rates +_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), + // AP_VAR //ref //index, default, name + streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), + streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), + streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), + streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), + streamRatePosition (&_group, 4, 0, PSTR("POSITION")), + streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), + streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), + streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) +{ + +} + +void +GCS_MAVLINK::init(FastSerial * port) +{ + GCS_Class::init(port); + if (port == &Serial) { + mavlink_comm_0_port = port; + chan = MAVLINK_COMM_0; + }else{ + mavlink_comm_1_port = port; + chan = MAVLINK_COMM_1; + } + _queued_parameter = NULL; +} + +void +GCS_MAVLINK::update(void) +{ + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + status.packet_rx_drop_count = 0; + + // process received bytes + while (comm_get_available(chan)) + { + uint8_t c = comm_receive_ch(chan); + +#if CLI_ENABLED == ENABLED + /* allow CLI to be started by hitting enter 3 times, if no + heartbeat packets have been received */ + if (mavlink_active == 0) { + if (c == '\n' || c == '\r') { + crlf_count++; + } else { + crlf_count = 0; + } + if (crlf_count == 3) { + run_cli(); + } + } +#endif + + // Try to get a new message + if (mavlink_parse_char(chan, c, &msg, &status)) { + mavlink_active = 1; + handleMessage(&msg); + } + } + + // Update packet drops counter + packet_drops += status.packet_rx_drop_count; + + // send out queued params/ waypoints + if (NULL != _queued_parameter) { + send_message(MSG_NEXT_PARAM); + } + + if (waypoint_receiving && + waypoint_request_i <= (unsigned)g.command_total) { + send_message(MSG_NEXT_WAYPOINT); + } + + // stop waypoint sending if timeout + if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ + waypoint_sending = false; + } + + // stop waypoint receiving if timeout + if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ + waypoint_receiving = false; + } +} + +void +GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) +{ + if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { + + if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ + send_message(MSG_RAW_IMU1); + send_message(MSG_RAW_IMU2); + send_message(MSG_RAW_IMU3); + } + + if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { + send_message(MSG_EXTENDED_STATUS1); + send_message(MSG_EXTENDED_STATUS2); + send_message(MSG_GPS_STATUS); + send_message(MSG_CURRENT_WAYPOINT); + send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working + send_message(MSG_NAV_CONTROLLER_OUTPUT); + } + + if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { + // sent with GPS read + send_message(MSG_LOCATION); + } + + if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { + // This is used for HIL. Do not change without discussing with HIL maintainers + send_message(MSG_SERVO_OUT); + } + + if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { + send_message(MSG_RADIO_OUT); + send_message(MSG_RADIO_IN); + } + + if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info + send_message(MSG_ATTITUDE); + } + + if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info + send_message(MSG_VFR_HUD); + } + + if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ + // Available datastream + } + } +} + + + +void +GCS_MAVLINK::send_message(enum ap_message id) +{ + mavlink_send_message(chan,id, packet_drops); +} + +void +GCS_MAVLINK::send_text(gcs_severity severity, const char *str) +{ + mavlink_send_text(chan,severity,str); +} + +void +GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) +{ + mavlink_statustext_t m; + uint8_t i; + for (i=0; imsgid) { + + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + { + // decode + mavlink_request_data_stream_t packet; + mavlink_msg_request_data_stream_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + int freq = 0; // packet frequency + + if (packet.start_stop == 0) + freq = 0; // stop sending + else if (packet.start_stop == 1) + freq = packet.req_message_rate; // start sending + else + break; + + switch(packet.req_stream_id){ + + case MAV_DATA_STREAM_ALL: + streamRateRawSensors = freq; + streamRateExtendedStatus = freq; + streamRateRCChannels = freq; + streamRateRawController = freq; + streamRatePosition = freq; + streamRateExtra1 = freq; + streamRateExtra2 = freq; + streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. + break; + + case MAV_DATA_STREAM_RAW_SENSORS: + streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly + // we will not continue to broadcast raw sensor data at 50Hz. + break; + case MAV_DATA_STREAM_EXTENDED_STATUS: + streamRateExtendedStatus.set_and_save(freq); + break; + + case MAV_DATA_STREAM_RC_CHANNELS: + streamRateRCChannels.set_and_save(freq); + break; + + case MAV_DATA_STREAM_RAW_CONTROLLER: + streamRateRawController.set_and_save(freq); + break; + + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; + + case MAV_DATA_STREAM_POSITION: + streamRatePosition.set_and_save(freq); + break; + + case MAV_DATA_STREAM_EXTRA1: + streamRateExtra1.set_and_save(freq); + break; + + case MAV_DATA_STREAM_EXTRA2: + streamRateExtra2.set_and_save(freq); + break; + + case MAV_DATA_STREAM_EXTRA3: + streamRateExtra3.set_and_save(freq); + break; + + default: + break; + } + break; + } + +#ifdef MAVLINK10 + case MAVLINK_MSG_ID_COMMAND_LONG: + { + // decode + mavlink_command_long_t packet; + mavlink_msg_command_long_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; + + uint8_t result; + + // do command + send_text(SEVERITY_LOW,PSTR("command received: ")); + + switch(packet.command) { + + case MAV_CMD_NAV_LOITER_UNLIM: + set_mode(LOITER); + result = MAV_RESULT_ACCEPTED; + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + set_mode(RTL); + result = MAV_RESULT_ACCEPTED; + break; + +#if 0 + // not implemented yet, but could implement some of them + case MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_ROI: + case MAV_CMD_NAV_PATHPLANNING: + break; +#endif + + + case MAV_CMD_PREFLIGHT_CALIBRATION: + if (packet.param1 == 1 || + packet.param2 == 1 || + packet.param3 == 1) { + startup_IMU_ground(); + } + if (packet.param4 == 1) { + trim_radio(); + } + result = MAV_RESULT_ACCEPTED; + break; + + + default: + result = MAV_RESULT_UNSUPPORTED; + break; + } + + mavlink_msg_command_ack_send( + chan, + packet.command, + result); + + break; + } + +#else // MAVLINK10 + case MAVLINK_MSG_ID_ACTION: + { + // decode + mavlink_action_t packet; + mavlink_msg_action_decode(msg, &packet); + if (mavlink_check_target(packet.target,packet.target_component)) break; + + uint8_t result = 0; + + // do action + send_text(SEVERITY_LOW,PSTR("action received: ")); +//Serial.println(packet.action); + switch(packet.action){ + + case MAV_ACTION_LAUNCH: + //set_mode(TAKEOFF); + break; + + case MAV_ACTION_RETURN: + set_mode(RTL); + result=1; + break; + + case MAV_ACTION_EMCY_LAND: + //set_mode(LAND); + break; + + case MAV_ACTION_HALT: + do_loiter_at_location(); + result=1; + break; + + /* No mappable implementation in APM 2.0 + case MAV_ACTION_MOTORS_START: + case MAV_ACTION_CONFIRM_KILL: + case MAV_ACTION_EMCY_KILL: + case MAV_ACTION_MOTORS_STOP: + case MAV_ACTION_SHUTDOWN: + break; + */ + + case MAV_ACTION_CONTINUE: + process_next_command(); + result=1; + break; + + case MAV_ACTION_SET_MANUAL: + set_mode(MANUAL); + result=1; + break; + + case MAV_ACTION_SET_AUTO: + set_mode(AUTO); + result=1; + break; + + case MAV_ACTION_STORAGE_READ: + AP_Var::load_all(); + result=1; + break; + + case MAV_ACTION_STORAGE_WRITE: + AP_Var::save_all(); + result=1; + break; + + case MAV_ACTION_CALIBRATE_RC: break; + trim_radio(); + result=1; + break; + + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_ACC: + case MAV_ACTION_CALIBRATE_PRESSURE: + case MAV_ACTION_REBOOT: // this is a rough interpretation + startup_IMU_ground(); + result=1; + break; + + /* For future implemtation + case MAV_ACTION_REC_START: break; + case MAV_ACTION_REC_PAUSE: break; + case MAV_ACTION_REC_STOP: break; + */ + + /* Takeoff is not an implemented flight mode in APM 2.0 + case MAV_ACTION_TAKEOFF: + set_mode(TAKEOFF); + break; + */ + + case MAV_ACTION_NAVIGATE: + set_mode(AUTO); + result=1; + break; + + /* Land is not an implemented flight mode in APM 2.0 + case MAV_ACTION_LAND: + set_mode(LAND); + break; + */ + + case MAV_ACTION_LOITER: + set_mode(LOITER); + result=1; + break; + + default: break; + } + + mavlink_msg_action_ack_send( + chan, + packet.action, + result + ); + + break; + } +#endif + + case MAVLINK_MSG_ID_SET_MODE: + { + // decode + mavlink_set_mode_t packet; + mavlink_msg_set_mode_decode(msg, &packet); + +#ifdef MAVLINK10 + // we ignore base_mode as there is no sane way to map + // from that bitmap to a APM flight mode. We rely on + // custom_mode instead. + // see comment on custom_mode above + int16_t adjusted_mode = packet.custom_mode - 16; + + switch (adjusted_mode) { + case MANUAL: + case CIRCLE: + case STABILIZE: + case FLY_BY_WIRE_A: + case FLY_BY_WIRE_B: + case FLY_BY_WIRE_C: + case AUTO: + case RTL: + case LOITER: + set_mode(adjusted_mode); + break; + } + +#else // MAVLINK10 + + switch(packet.mode){ + + case MAV_MODE_MANUAL: + set_mode(MANUAL); + break; + + case MAV_MODE_GUIDED: + set_mode(GUIDED); + break; + + case MAV_MODE_AUTO: + if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); + if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); + if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); + mav_nav = 255; + break; + + case MAV_MODE_TEST1: + set_mode(STABILIZE); + break; + + case MAV_MODE_TEST2: + if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A); + if(mav_nav == 2) set_mode(FLY_BY_WIRE_B); + //if(mav_nav == 3) set_mode(FLY_BY_WIRE_C); + mav_nav = 255; + break; + + } +#endif + break; + } + +#ifndef MAVLINK10 + case MAVLINK_MSG_ID_SET_NAV_MODE: + { + // decode + mavlink_set_nav_mode_t packet; + mavlink_msg_set_nav_mode_decode(msg, &packet); + // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message + mav_nav = packet.nav_mode; + break; + } +#endif // MAVLINK10 + + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + // decode + mavlink_waypoint_request_list_t packet; + mavlink_msg_waypoint_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // Start sending waypoints + mavlink_msg_waypoint_count_send( + chan,msg->sysid, + msg->compid, + g.command_total + 1); // + home + + waypoint_timelast_send = millis(); + waypoint_sending = true; + waypoint_receiving = false; + waypoint_dest_sysid = msg->sysid; + waypoint_dest_compid = msg->compid; + break; + } + + + // XXX read a WP from EEPROM and send it to the GCS + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + // Check if sending waypiont + //if (!waypoint_sending) break; + // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW + + // decode + mavlink_waypoint_request_t packet; + mavlink_msg_waypoint_request_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // send waypoint + tell_command = get_cmd_with_index(packet.seq); + + // set frame of waypoint + uint8_t frame; + + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame + } else { + frame = MAV_FRAME_GLOBAL; // reference frame + } + + float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; + + // time that the mav should loiter in milliseconds + uint8_t current = 0; // 1 (true), 0 (false) + + if (packet.seq == (uint16_t)g.command_index) + current = 1; + + uint8_t autocontinue = 1; // 1 (true), 0 (false) + + float x = 0, y = 0, z = 0; + + if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { + // command needs scaling + x = tell_command.lat/1.0e7; // local (x), global (latitude) + y = tell_command.lng/1.0e7; // local (y), global (longitude) + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt + } else { + z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) + } + } + + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_DO_SET_HOME: + param1 = tell_command.p1; + break; + + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1*10; // APM loiter time is in ten second increments + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + x=0; // Clear fields loaded above that we don't want sent for this command + y=0; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; + + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; + + case MAV_CMD_DO_REPEAT_SERVO: + param4 = tell_command.lng; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + param3 = tell_command.lat; + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + } + + mavlink_msg_waypoint_send(chan,msg->sysid, + msg->compid, + packet.seq, + frame, + tell_command.id, + current, + autocontinue, + param1, + param2, + param3, + param4, + x, + y, + z); + + // update last waypoint comm stamp + waypoint_timelast_send = millis(); + break; + } + + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + // decode + mavlink_waypoint_ack_t packet; + mavlink_msg_waypoint_ack_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // turn off waypoint send + waypoint_sending = false; + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + { + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // Start sending parameters - next call to ::update will kick the first one out + + _queued_parameter = AP_Var::first(); + _queued_parameter_index = 0; + _queued_parameter_count = _count_parameters(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + // decode + mavlink_waypoint_clear_all_t packet; + mavlink_msg_waypoint_clear_all_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; + + // clear all commands + g.command_total.set_and_save(0); + + // note that we don't send multiple acks, as otherwise a + // GCS that is doing a clear followed by a set may see + // the additional ACKs as ACKs of the set operations + mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + // decode + mavlink_waypoint_set_current_t packet; + mavlink_msg_waypoint_set_current_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // set current command + change_command(packet.seq); + + mavlink_msg_waypoint_current_send(chan, g.command_index); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + // decode + mavlink_waypoint_count_t packet; + mavlink_msg_waypoint_count_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // start waypoint receiving + if (packet.count > MAX_WAYPOINTS) { + packet.count = MAX_WAYPOINTS; + } + g.command_total.set_and_save(packet.count - 1); + + waypoint_timelast_receive = millis(); + waypoint_receiving = true; + waypoint_sending = false; + waypoint_request_i = 0; + break; + } + +#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS + case MAVLINK_MSG_ID_SET_MAG_OFFSETS: + { + mavlink_set_mag_offsets_t packet; + mavlink_msg_set_mag_offsets_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); + break; + } +#endif + + // XXX receive a WP from GCS and store in EEPROM + case MAVLINK_MSG_ID_WAYPOINT: + { + // decode + mavlink_waypoint_t packet; + uint8_t result = MAV_MISSION_ACCEPTED; + + mavlink_msg_waypoint_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // defaults + tell_command.id = packet.command; + + switch (packet.frame) + { + case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: + { + tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.options = 0; // absolute altitude + break; + } + +#ifdef MAV_FRAME_LOCAL_NED + case MAV_FRAME_LOCAL_NED: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = -packet.z*1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } +#endif + +#ifdef MAV_FRAME_LOCAL + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } +#endif + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + { + tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + break; + } + + default: + result = MAV_MISSION_UNSUPPORTED_FRAME; + break; + } + + + if (result != MAV_MISSION_ACCEPTED) goto mission_failed; + + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + case MAV_CMD_NAV_WAYPOINT: + case MAV_CMD_NAV_LOITER_UNLIM: + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + case MAV_CMD_NAV_LAND: + break; + + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_DO_SET_HOME: + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + tell_command.lat = packet.param1; + break; + + case MAV_CMD_NAV_LOITER_TIME: + tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments + break; + + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + tell_command.lat = packet.param1; + break; + + case MAV_CMD_DO_JUMP: + tell_command.lat = packet.param2; + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_DO_REPEAT_SERVO: + tell_command.lng = packet.param4; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + tell_command.lat = packet.param3; + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; + break; + + default: +#ifdef MAVLINK10 + result = MAV_MISSION_UNSUPPORTED; +#endif + break; + } + + if (result != MAV_MISSION_ACCEPTED) goto mission_failed; + + if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission + guided_WP = tell_command; + + // add home alt if needed + if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ + guided_WP.alt += home.alt; + } + + set_mode(GUIDED); + + // make any new wp uploaded instant (in case we are already in Guided mode) + set_guided_WP(); + + // verify we recevied the command + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + 0); + + } else { + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) { + result = MAV_MISSION_ERROR; + goto mission_failed; + } + + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) { + result = MAV_MISSION_INVALID_SEQUENCE; + goto mission_failed; + } + + set_cmd_with_index(tell_command, packet.seq); + + // update waypoint receiving state machine + waypoint_timelast_receive = millis(); + waypoint_request_i++; + + if (waypoint_request_i > (uint16_t)g.command_total){ + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + result); + + send_text(SEVERITY_LOW,PSTR("flight plan received")); + waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } + } + break; + + mission_failed: + // we are rejecting the mission/waypoint + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + result); + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: + { + AP_Var *vp; + AP_Meta_class::Type_id var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[ONBOARD_PARAM_NAME_LENGTH+1]; + strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); + key[ONBOARD_PARAM_NAME_LENGTH] = 0; + + // find the requested parameter + vp = AP_Var::find(key); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + float rounding_addition = 0.01; + + // fetch the variable type ID + var_type = vp->meta_type_id(); + + // handle variables with standard type IDs + if (var_type == AP_Var::k_typeid_float) { + ((AP_Float *)vp)->set_and_save(packet.param_value); + } else if (var_type == AP_Var::k_typeid_float16) { + ((AP_Float16 *)vp)->set_and_save(packet.param_value); + } else if (var_type == AP_Var::k_typeid_int32) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int16) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int8) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type +#ifdef MAVLINK10 + mavlink_msg_param_value_send( + chan, + key, + vp->cast_to_float(), + mav_var_type(vp->meta_type_id()), + _count_parameters(), + -1); // XXX we don't actually know what its index is... +#else // MAVLINK10 + mavlink_msg_param_value_send( + chan, + (int8_t *)key, + vp->cast_to_float(), + _count_parameters(), + -1); // XXX we don't actually know what its index is... +#endif // MAVLINK10 + } + + break; + } // end case + + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + { + // allow override of RC channel values for HIL + // or for complete GCS control of switch position + // and RC PWM values. + if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_rc_channels_override_t packet; + int16_t v[8]; + mavlink_msg_rc_channels_override_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system,packet.target_component)) + break; + + v[0] = packet.chan1_raw; + v[1] = packet.chan2_raw; + v[2] = packet.chan3_raw; + v[3] = packet.chan4_raw; + v[4] = packet.chan5_raw; + v[5] = packet.chan6_raw; + v[6] = packet.chan7_raw; + v[7] = packet.chan8_raw; + rc_override_active = APM_RC.setHIL(v); + rc_override_fs_timer = millis(); + break; + } + + case MAVLINK_MSG_ID_HEARTBEAT: + { + // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + if(msg->sysid != g.sysid_my_gcs) break; + rc_override_fs_timer = millis(); + pmTest1++; + break; + } + + #if HIL_MODE != HIL_MODE_DISABLED + // This is used both as a sensor and to pass the location + // in HIL_ATTITUDE mode. +#ifdef MAVLINK10 + case MAVLINK_MSG_ID_GPS_RAW_INT: + { + // decode + mavlink_gps_raw_int_t packet; + mavlink_msg_gps_raw_int_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.time_usec/1000.0, + packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, + packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 0); + break; + } +#else // MAVLINK10 + case MAVLINK_MSG_ID_GPS_RAW: + { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + packet.v,packet.hdg,0,0); + break; + } +#endif // MAVLINK10 + + // Is this resolved? - MAVLink protocol change..... + case MAVLINK_MSG_ID_VFR_HUD: + { + // decode + mavlink_vfr_hud_t packet; + mavlink_msg_vfr_hud_decode(msg, &packet); + + // set airspeed + airspeed = 100*packet.airspeed; + break; + } + +#endif +#if HIL_MODE == HIL_MODE_ATTITUDE + case MAVLINK_MSG_ID_ATTITUDE: + { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + break; + } +#endif +#if HIL_MODE == HIL_MODE_SENSORS + + case MAVLINK_MSG_ID_RAW_IMU: + { + // decode + mavlink_raw_imu_t packet; + mavlink_msg_raw_imu_decode(msg, &packet); + + // set imu hil sensors + // TODO: check scaling for temp/absPress + float temp = 70; + float absPress = 1; + //Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc); + //Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro); + + // rad/sec + Vector3f gyros; + gyros.x = (float)packet.xgyro / 1000.0; + gyros.y = (float)packet.ygyro / 1000.0; + gyros.z = (float)packet.zgyro / 1000.0; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; + + imu.set_gyro(gyros); + + imu.set_accel(accels); + + compass.setHIL(packet.xmag,packet.ymag,packet.zmag); + break; + } + + case MAVLINK_MSG_ID_RAW_PRESSURE: + { + // decode + mavlink_raw_pressure_t packet; + mavlink_msg_raw_pressure_decode(msg, &packet); + + // set pressure hil sensor + // TODO: check scaling + float temp = 70; + barometer.setHIL(temp,packet.press_diff1 + 101325); + break; + } +#endif // HIL_MODE + } // end switch +} // end handle mavlink + +uint16_t +GCS_MAVLINK::_count_parameters() +{ + // if we haven't cached the parameter count yet... + if (0 == _parameter_count) { + AP_Var *vp; + + vp = AP_Var::first(); + do { + // if a parameter responds to cast_to_float then we are going to be able to report it + if (!isnan(vp->cast_to_float())) { + _parameter_count++; + } + } while (NULL != (vp = vp->next())); + } + return _parameter_count; +} + +AP_Var * +GCS_MAVLINK::_find_parameter(uint16_t index) +{ + AP_Var *vp; + + vp = AP_Var::first(); + while (NULL != vp) { + + // if the parameter is reportable + if (!(isnan(vp->cast_to_float()))) { + // if we have counted down to the index we want + if (0 == index) { + // return the parameter + return vp; + } + // count off this parameter, as it is reportable but not + // the one we want + index--; + } + // and move to the next parameter + vp = vp->next(); + } + return NULL; +} + +/** +* @brief Send the next pending parameter, called from deferred message +* handling code +*/ +void +GCS_MAVLINK::queued_param_send() +{ + // Check to see if we are sending parameters + if (NULL == _queued_parameter) return; + + AP_Var *vp; + float value; + + // copy the current parameter and prepare to move to the next + vp = _queued_parameter; + _queued_parameter = _queued_parameter->next(); + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK + vp->copy_name(param_name, sizeof(param_name)); + +#ifdef MAVLINK10 + mavlink_msg_param_value_send( + chan, + param_name, + value, + mav_var_type(vp->meta_type_id()), + _queued_parameter_count, + _queued_parameter_index); +#else // MAVLINK10 + mavlink_msg_param_value_send( + chan, + (int8_t*)param_name, + value, + _queued_parameter_count, + _queued_parameter_index); +#endif // MAVLINK10 + + _queued_parameter_index++; + } +} + +/** +* @brief Send the next pending waypoint, called from deferred message +* handling code +*/ +void +GCS_MAVLINK::queued_waypoint_send() +{ + if (waypoint_receiving && + waypoint_request_i <= (unsigned)g.command_total) { + mavlink_msg_waypoint_request_send( + chan, + waypoint_dest_sysid, + waypoint_dest_compid, + waypoint_request_i); + } +} + +/* + a delay() callback that processes MAVLink packets. We set this as the + callback in long running library initialisation routines to allow + MAVLink to process packets while waiting for the initialisation to + complete +*/ +static void mavlink_delay(unsigned long t) +{ + unsigned long tstart; + static unsigned long last_1hz, last_50hz; + + if (in_mavlink_delay) { + // this should never happen, but let's not tempt fate by + // letting the stack grow too much + delay(t); + return; + } + + in_mavlink_delay = true; + + tstart = millis(); + do { + unsigned long tnow = millis(); + if (tnow - last_1hz > 1000) { + last_1hz = tnow; + gcs_send_message(MSG_HEARTBEAT); + gcs_send_message(MSG_EXTENDED_STATUS1); + } + if (tnow - last_50hz > 20) { + last_50hz = tnow; + gcs_update(); + } + delay(1); + } while (millis() - tstart < t); + + in_mavlink_delay = false; +} + +/* + send a message on both GCS links + */ +static void gcs_send_message(enum ap_message id) +{ + gcs0.send_message(id); + gcs3.send_message(id); +} + +/* + send data streams in the given rate range on both links + */ +static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) +{ + gcs0.data_stream_send(freqMin, freqMax); + gcs3.data_stream_send(freqMin, freqMax); +} + +/* + look for incoming commands on the GCS links + */ +static void gcs_update(void) +{ + gcs0.update(); + gcs3.update(); +} + +static void gcs_send_text(gcs_severity severity, const char *str) +{ + gcs0.send_text(severity, str); + gcs3.send_text(severity, str); +} + +static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) +{ + gcs0.send_text(severity, str); + gcs3.send_text(severity, str); +} + +/* + send a low priority formatted message to the GCS + only one fits in the queue, so if you send more than one before the + last one gets into the serial buffer then the old one will be lost + */ +static void gcs_send_text_fmt(const prog_char_t *fmt, ...) +{ + char fmtstr[40]; + va_list ap; + uint8_t i; + for (i=0; i dump log \n" + " erase erase all logs\n" + " enable |all enable logging or everything\n" + " disable |all disable logging or everything\n" + "\n")); + return 0; +} + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Coommon for implementation details +static const struct Menu::command log_menu_commands[] PROGMEM = { + {"dump", dump_log}, + {"erase", erase_logs}, + {"enable", select_logs}, + {"disable", select_logs}, + {"help", help_log} +}; + +// A Macro to create the Menu +MENU2(log_menu, "Log", log_menu_commands, print_log_menu); + +static void get_log_boundaries(byte log_num, int & start_page, int & end_page); + +static bool +print_log_menu(void) +{ + int log_start; + int log_end; + byte last_log_num = get_num_logs(); + + Serial.printf_P(PSTR("logs enabled: ")); + if (0 == g.log_bitmask) { + Serial.printf_P(PSTR("none")); + }else{ + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // the bit being set and print the name of the log option to suit. + #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) + PLOG(ATTITUDE_FAST); + PLOG(ATTITUDE_MED); + PLOG(GPS); + PLOG(PM); + PLOG(CTUN); + PLOG(NTUN); + PLOG(MODE); + PLOG(RAW); + PLOG(CMD); + PLOG(CUR); + #undef PLOG + } + Serial.println(); + + if (last_log_num == 0) { + Serial.printf_P(PSTR("\nNo logs available for download\n")); + }else{ + + Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); + for(int i=1;i last_log_num)) { + Serial.printf_P(PSTR("bad log number\n")); + return(-1); + } + + get_log_boundaries(dump_log, dump_log_start, dump_log_end); + Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), + dump_log, + dump_log_start, + dump_log_end); + + Log_Read(dump_log_start, dump_log_end); + Serial.printf_P(PSTR("Log read complete\n")); + return 0; +} + +static int8_t +erase_logs(uint8_t argc, const Menu::arg *argv) +{ + for(int i = 10 ; i > 0; i--) { + Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); + delay(1000); + } + Serial.printf_P(PSTR("\nErasing log...\n")); + for(int j = 1; j < 4096; j++) + DataFlash.PageErase(j); + DataFlash.StartWrite(1); + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_INDEX_MSG); + DataFlash.WriteByte(0); + DataFlash.WriteByte(END_BYTE); + DataFlash.FinishWrite(); + Serial.printf_P(PSTR("\nLog erased.\n")); + return 0; +} + +static int8_t +select_logs(uint8_t argc, const Menu::arg *argv) +{ + uint16_t bits; + + if (argc != 2) { + Serial.printf_P(PSTR("missing log type\n")); + return(-1); + } + + bits = 0; + + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // that name as the argument to the command, and set the bit in + // bits accordingly. + // + if (!strcasecmp_P(argv[1].str, PSTR("all"))) { + bits = ~0; + } else { + #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s + TARG(ATTITUDE_FAST); + TARG(ATTITUDE_MED); + TARG(GPS); + TARG(PM); + TARG(CTUN); + TARG(NTUN); + TARG(MODE); + TARG(RAW); + TARG(CMD); + TARG(CUR); + #undef TARG + } + + if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { + g.log_bitmask.set_and_save(g.log_bitmask | bits); + }else{ + g.log_bitmask.set_and_save(g.log_bitmask & ~bits); + } + return(0); +} + +static int8_t +process_logs(uint8_t argc, const Menu::arg *argv) +{ + log_menu.run(); + return 0; +} + + +static byte get_num_logs(void) +{ + int page = 1; + byte data; + byte log_step = 0; + + DataFlash.StartRead(1); + + while (page == 1) { + data = DataFlash.ReadByte(); + + switch(log_step){ //This is a state machine to read the packets + case 0: + if(data==HEAD_BYTE1) // Head byte 1 + log_step++; + break; + + case 1: + if(data==HEAD_BYTE2) // Head byte 2 + log_step++; + else + log_step = 0; + break; + + case 2: + if(data==LOG_INDEX_MSG){ + byte num_logs = DataFlash.ReadByte(); + return num_logs; + }else{ + log_step=0; // Restart, we have a problem... + } + break; + } + page = DataFlash.GetPage(); + } + return 0; +} + +// send the number of the last log? +static void start_new_log(byte num_existing_logs) +{ + int start_pages[50] = {0,0,0}; + int end_pages[50] = {0,0,0}; + + if(num_existing_logs > 0) { + for(int i=0;i 0) + start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1; + else + start_pages[0] = 2; + num_existing_logs++; + DataFlash.StartWrite(1); + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_INDEX_MSG); + DataFlash.WriteByte(num_existing_logs); + for(int i=0;i Logs full - logging discontinued")); + } +} + +static void get_log_boundaries(byte log_num, int & start_page, int & end_page) +{ + int page = 1; + byte data; + byte log_step = 0; + + DataFlash.StartRead(1); + while (page == 1) { + data = DataFlash.ReadByte(); + switch(log_step) //This is a state machine to read the packets + { + case 0: + if(data==HEAD_BYTE1) // Head byte 1 + log_step++; + break; + case 1: + if(data==HEAD_BYTE2) // Head byte 2 + log_step++; + else + log_step = 0; + break; + case 2: + if(data==LOG_INDEX_MSG){ + byte num_logs = DataFlash.ReadByte(); + for(int i=0;i 1) { + look_page = (top_page + bottom_page) / 2; + DataFlash.StartRead(look_page); + check = DataFlash.ReadLong(); + if(check == (long)0xFFFFFFFF) + top_page = look_page; + else + bottom_page = look_page; + } + return top_page; +} + + +// Write an attitude packet. Total length : 10 bytes +static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_ATTITUDE_MSG); + DataFlash.WriteInt(log_roll); + DataFlash.WriteInt(log_pitch); + DataFlash.WriteInt(log_yaw); + DataFlash.WriteByte(END_BYTE); +} + +// Write a performance monitoring packet. Total length : 19 bytes +static void Log_Write_Performance() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_PERFORMANCE_MSG); + DataFlash.WriteLong(millis()- perf_mon_timer); + DataFlash.WriteInt(mainLoop_count); + DataFlash.WriteInt(G_Dt_max); + DataFlash.WriteByte(dcm.gyro_sat_count); + DataFlash.WriteByte(imu.adc_constraints); + DataFlash.WriteByte(dcm.renorm_sqrt_count); + DataFlash.WriteByte(dcm.renorm_blowup_count); + DataFlash.WriteByte(gps_fix_count); + DataFlash.WriteInt((int)(dcm.get_health() * 1000)); + DataFlash.WriteInt(pmTest1); + DataFlash.WriteByte(END_BYTE); +} + +// Write a command processing packet. Total length : 19 bytes +//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng) +static void Log_Write_Cmd(byte num, struct Location *wp) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_CMD_MSG); + DataFlash.WriteByte(num); + DataFlash.WriteByte(wp->id); + DataFlash.WriteByte(wp->p1); + DataFlash.WriteLong(wp->alt); + DataFlash.WriteLong(wp->lat); + DataFlash.WriteLong(wp->lng); + DataFlash.WriteByte(END_BYTE); +} + +static void Log_Write_Startup(byte type) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_STARTUP_MSG); + DataFlash.WriteByte(type); + DataFlash.WriteByte(g.command_total); + DataFlash.WriteByte(END_BYTE); + + // create a location struct to hold the temp Waypoints for printing + struct Location cmd = get_cmd_with_index(0); + Log_Write_Cmd(0, &cmd); + + for (int i = 1; i <= g.command_total; i++){ + cmd = get_cmd_with_index(i); + Log_Write_Cmd(i, &cmd); + } +} + + +// Write a control tuning packet. Total length : 22 bytes +#if HIL_MODE != HIL_MODE_ATTITUDE +static void Log_Write_Control_Tuning() +{ + Vector3f accel = imu.get_accel(); + + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); + DataFlash.WriteInt((int)(g.channel_roll.servo_out)); + DataFlash.WriteInt((int)nav_roll); + DataFlash.WriteInt((int)dcm.roll_sensor); + DataFlash.WriteInt((int)(g.channel_pitch.servo_out)); + DataFlash.WriteInt((int)nav_pitch); + DataFlash.WriteInt((int)dcm.pitch_sensor); + DataFlash.WriteInt((int)(g.channel_throttle.servo_out)); + DataFlash.WriteInt((int)(g.channel_rudder.servo_out)); + DataFlash.WriteInt((int)(accel.y * 10000)); + DataFlash.WriteByte(END_BYTE); +} +#endif + +// Write a navigation tuning packet. Total length : 18 bytes +static void Log_Write_Nav_Tuning() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_NAV_TUNING_MSG); + DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); + DataFlash.WriteInt((int)wp_distance); + DataFlash.WriteInt((uint16_t)target_bearing); + DataFlash.WriteInt((uint16_t)nav_bearing); + DataFlash.WriteInt(altitude_error); + DataFlash.WriteInt((int)airspeed); + DataFlash.WriteInt((int)(nav_gain_scaler*1000)); + DataFlash.WriteByte(END_BYTE); +} + +// Write a mode packet. Total length : 5 bytes +static void Log_Write_Mode(byte mode) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_MODE_MSG); + DataFlash.WriteByte(mode); + DataFlash.WriteByte(END_BYTE); +} + +// Write an GPS packet. Total length : 30 bytes +static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, + long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_GPS_MSG); + DataFlash.WriteLong(log_Time); + DataFlash.WriteByte(log_Fix); + DataFlash.WriteByte(log_NumSats); + DataFlash.WriteLong(log_Lattitude); + DataFlash.WriteLong(log_Longitude); + DataFlash.WriteInt(sonar_alt); // This one is just temporary for testing out sonar in fixed wing + DataFlash.WriteLong(log_mix_alt); + DataFlash.WriteLong(log_gps_alt); + DataFlash.WriteLong(log_Ground_Speed); + DataFlash.WriteLong(log_Ground_Course); + DataFlash.WriteByte(END_BYTE); +} + +// Write an raw accel/gyro data packet. Total length : 28 bytes +#if HIL_MODE != HIL_MODE_ATTITUDE +static void Log_Write_Raw() +{ + Vector3f gyro = imu.get_gyro(); + Vector3f accel = imu.get_accel(); + gyro *= t7; // Scale up for storage as long integers + accel *= t7; + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_RAW_MSG); + + DataFlash.WriteLong((long)gyro.x); + DataFlash.WriteLong((long)gyro.y); + DataFlash.WriteLong((long)gyro.z); + DataFlash.WriteLong((long)accel.x); + DataFlash.WriteLong((long)accel.y); + DataFlash.WriteLong((long)accel.z); + + DataFlash.WriteByte(END_BYTE); +} +#endif + +static void Log_Write_Current() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_CURRENT_MSG); + DataFlash.WriteInt(g.channel_throttle.control_in); + DataFlash.WriteInt((int)(battery_voltage * 100.0)); + DataFlash.WriteInt((int)(current_amps * 100.0)); + DataFlash.WriteInt((int)current_total); + DataFlash.WriteByte(END_BYTE); +} + +// Read a Current packet +static void Log_Read_Current() +{ + Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"), + DataFlash.ReadInt(), + ((float)DataFlash.ReadInt() / 100.f), + ((float)DataFlash.ReadInt() / 100.f), + DataFlash.ReadInt()); +} + +// Read an control tuning packet +static void Log_Read_Control_Tuning() +{ + float logvar; + + Serial.printf_P(PSTR("CTUN:")); + for (int y = 1; y < 10; y++) { + logvar = DataFlash.ReadInt(); + if(y < 8) logvar = logvar/100.f; + if(y == 9) logvar = logvar/10000.f; + Serial.print(logvar); + Serial.print(comma); + } + Serial.println(" "); +} + +// Read a nav tuning packet +static void Log_Read_Nav_Tuning() +{ + Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n + (float)((uint16_t)DataFlash.ReadInt())/100.0, + DataFlash.ReadInt(), + (float)((uint16_t)DataFlash.ReadInt())/100.0, + (float)((uint16_t)DataFlash.ReadInt())/100.0, + (float)DataFlash.ReadInt()/100.0, + (float)DataFlash.ReadInt()/100.0, + (float)DataFlash.ReadInt()/1000.0); +} + +// Read a performance packet +static void Log_Read_Performance() +{ + long pm_time; + int logvar; + + Serial.printf_P(PSTR("PM:")); + pm_time = DataFlash.ReadLong(); + Serial.print(pm_time); + Serial.print(comma); + for (int y = 1; y <= 9; y++) { + if(y < 3 || y > 7){ + logvar = DataFlash.ReadInt(); + }else{ + logvar = DataFlash.ReadByte(); + } + Serial.print(logvar); + Serial.print(comma); + } + Serial.println(" "); +} + +// Read a command processing packet +static void Log_Read_Cmd() +{ + byte logvarb; + long logvarl; + + Serial.printf_P(PSTR("CMD:")); + for(int i = 1; i < 4; i++) { + logvarb = DataFlash.ReadByte(); + Serial.print(logvarb, DEC); + Serial.print(comma); + } + for(int i = 1; i < 4; i++) { + logvarl = DataFlash.ReadLong(); + Serial.print(logvarl, DEC); + Serial.print(comma); + } + Serial.println(" "); +} + +static void Log_Read_Startup() +{ + byte logbyte = DataFlash.ReadByte(); + + if (logbyte == TYPE_AIRSTART_MSG) + Serial.printf_P(PSTR("AIR START - ")); + else if (logbyte == TYPE_GROUNDSTART_MSG) + Serial.printf_P(PSTR("GROUND START - ")); + else + Serial.printf_P(PSTR("UNKNOWN STARTUP - ")); + + Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte()); +} + +// Read an attitude packet +static void Log_Read_Attitude() +{ + Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + (uint16_t)DataFlash.ReadInt()); +} + +// Read a mode packet +static void Log_Read_Mode() +{ + Serial.printf_P(PSTR("MOD:")); + Serial.println(flight_mode_strings[DataFlash.ReadByte()]); +} + +// Read a GPS packet +static void Log_Read_GPS() +{ + Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"), + DataFlash.ReadLong(), + (int)DataFlash.ReadByte(), + (int)DataFlash.ReadByte(), + (float)DataFlash.ReadLong() / t7, + (float)DataFlash.ReadLong() / t7, + (float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing + (float)DataFlash.ReadLong() / 100.0, + (float)DataFlash.ReadLong() / 100.0, + (float)DataFlash.ReadLong() / 100.0, + (float)DataFlash.ReadLong() / 100.0); + +} + +// Read a raw accel/gyro packet +static void Log_Read_Raw() +{ + float logvar; + Serial.printf_P(PSTR("RAW:")); + for (int y = 0; y < 6; y++) { + logvar = (float)DataFlash.ReadLong() / t7; + Serial.print(logvar); + Serial.print(comma); + } + Serial.println(" "); +} + +// Read the DataFlash log memory : Packet Parser +static void Log_Read(int start_page, int end_page) +{ + byte data; + byte log_step = 0; + int packet_count = 0; + int page = start_page; + + #ifdef AIRFRAME_NAME + Serial.printf_P(PSTR((AIRFRAME_NAME) + #endif + Serial.printf_P(PSTR("\n" THISFIRMWARE + "\nFree RAM: %u\n"), + memcheck_available_memory()); + + DataFlash.StartRead(start_page); + while (page < end_page && page != -1){ + data = DataFlash.ReadByte(); + switch(log_step) // This is a state machine to read the packets + { + case 0: + if(data == HEAD_BYTE1) // Head byte 1 + log_step++; + break; + case 1: + if(data == HEAD_BYTE2) // Head byte 2 + log_step++; + else + log_step = 0; + break; + case 2: + if(data == LOG_ATTITUDE_MSG){ + Log_Read_Attitude(); + log_step++; + + }else if(data == LOG_MODE_MSG){ + Log_Read_Mode(); + log_step++; + + }else if(data == LOG_CONTROL_TUNING_MSG){ + Log_Read_Control_Tuning(); + log_step++; + + }else if(data == LOG_NAV_TUNING_MSG){ + Log_Read_Nav_Tuning(); + log_step++; + + }else if(data == LOG_PERFORMANCE_MSG){ + Log_Read_Performance(); + log_step++; + + }else if(data == LOG_RAW_MSG){ + Log_Read_Raw(); + log_step++; + + }else if(data == LOG_CMD_MSG){ + Log_Read_Cmd(); + log_step++; + + }else if(data == LOG_CURRENT_MSG){ + Log_Read_Current(); + log_step++; + + }else if(data == LOG_STARTUP_MSG){ + Log_Read_Startup(); + log_step++; + }else { + if(data == LOG_GPS_MSG){ + Log_Read_GPS(); + log_step++; + }else{ + Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count); + log_step = 0; // Restart, we have a problem... + } + } + break; + case 3: + if(data == END_BYTE){ + packet_count++; + }else{ + Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data); + } + log_step = 0; // Restart sequence: new packet... + break; + } + page = DataFlash.GetPage(); + } + Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); +} + +#else // LOGGING_ENABLED + +// dummy functions +static void Log_Write_Mode(byte mode) {} +static void Log_Write_Startup(byte type) {} +static void Log_Write_Cmd(byte num, struct Location *wp) {} +static void Log_Write_Current() {} +static void Log_Write_Nav_Tuning() {} +static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, + long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {} +static void Log_Write_Performance() {} +static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } +static byte get_num_logs(void) { return 0; } +static void start_new_log(byte num_existing_logs) {} +static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {} +static void Log_Write_Control_Tuning() {} +static void Log_Write_Raw() {} + + +#endif // LOGGING_ENABLED +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/climb_rate.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +struct DataPoint { + unsigned long x; + long y; +}; + +DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from +unsigned char hindex; // Index in history for the current data point + +unsigned long xoffset; +unsigned char n; + +// Intermediate variables for regression calculation +long xi; +long yi; +long xiyi; +unsigned long xi2; + +#if 0 // currently unused +static void add_altitude_data(unsigned long xl, long y) +{ + //Reset the regression if our X variable overflowed + if (xl < xoffset) + n = 0; + + //To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length + if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH) + n = 0; + + if (n == ALTITUDE_HISTORY_LENGTH) { + xi -= history[hindex].x; + yi -= history[hindex].y; + xiyi -= (long)history[hindex].x * history[hindex].y; + xi2 -= history[hindex].x * history[hindex].x; + } else { + if (n == 0) { + xoffset = xl; + xi = 0; + yi = 0; + xiyi = 0; + xi2 = 0; + } + n++; + } + + history[hindex].x = xl - xoffset; + history[hindex].y = y; + + xi += history[hindex].x; + yi += history[hindex].y; + xiyi += (long)history[hindex].x * history[hindex].y; + xi2 += history[hindex].x * history[hindex].x; + + if (++hindex >= ALTITUDE_HISTORY_LENGTH) + hindex = 0; +} +#endif + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* Functions in this file: + void init_commands() + void update_auto() + void reload_commands_airstart() + struct Location get_cmd_with_index(int i) + void set_cmd_with_index(struct Location temp, int i) + void increment_cmd_index() + void decrement_cmd_index() + long read_alt_to_hold() + void set_next_WP(struct Location *wp) + void set_guided_WP(void) + void init_home() +************************************************************ +*/ + +static void init_commands() +{ + g.command_index.set_and_save(0); + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; +} + +static void update_auto() +{ + if (g.command_index >= g.command_total) { + handle_no_commands(); + if(g.command_total == 0) { + next_WP.lat = home.lat + 1000; // so we don't have bad calcs + next_WP.lng = home.lng + 1000; // so we don't have bad calcs + } + } else { + if(g.command_index != 0) { + g.command_index = nav_command_index; + nav_command_index--; + } + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; + process_next_command(); + } +} + +// this is only used by an air-start +static void reload_commands_airstart() +{ + init_commands(); + g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all? + decrement_cmd_index(); +} + +// Getters +// ------- +static struct Location get_cmd_with_index(int i) +{ + struct Location temp; + long mem; + + // Find out proper location in memory by using the start_byte position + the index + // -------------------------------------------------------------------------------- + if (i > g.command_total) { + temp.id = CMD_BLANK; + }else{ + // read WP position + mem = (WP_START_BYTE) + (i * WP_SIZE); + temp.id = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.options = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.p1 = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.alt = (long)eeprom_read_dword((uint32_t*)mem); + + mem += 4; + temp.lat = (long)eeprom_read_dword((uint32_t*)mem); + + mem += 4; + temp.lng = (long)eeprom_read_dword((uint32_t*)mem); + } + + // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative + if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){ + temp.alt += home.alt; + } + + return temp; +} + +// Setters +// ------- +static void set_cmd_with_index(struct Location temp, int i) +{ + i = constrain(i, 0, g.command_total.get()); + uint32_t mem = WP_START_BYTE + (i * WP_SIZE); + + // Set altitude options bitmask + // XXX What is this trying to do? + if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){ + temp.options = MASK_OPTIONS_RELATIVE_ALT; + } else { + temp.options = 0; + } + + eeprom_write_byte((uint8_t *) mem, temp.id); + + mem++; + eeprom_write_byte((uint8_t *) mem, temp.options); + + mem++; + eeprom_write_byte((uint8_t *) mem, temp.p1); + + mem++; + eeprom_write_dword((uint32_t *) mem, temp.alt); + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lat); + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lng); +} + +static void increment_cmd_index() +{ + if (g.command_index <= g.command_total) { + g.command_index.set_and_save(g.command_index + 1); + } +} + +static void decrement_cmd_index() +{ + if (g.command_index > 0) { + g.command_index.set_and_save(g.command_index - 1); + } +} + +static long read_alt_to_hold() +{ + if(g.RTL_altitude < 0) + return current_loc.alt; + else + return g.RTL_altitude + home.alt; +} + + +/* +This function stores waypoint commands +It looks to see what the next command type is and finds the last command. +*/ +static void set_next_WP(struct Location *wp) +{ + // copy the current WP into the OldWP slot + // --------------------------------------- + prev_WP = next_WP; + + // Load the next_WP slot + // --------------------- + next_WP = *wp; + + // used to control FBW and limit the rate of climb + // ----------------------------------------------- + target_altitude = current_loc.alt; + + if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) + offset_altitude = next_WP.alt - prev_WP.alt; + else + offset_altitude = 0; + + // zero out our loiter vals to watch for missed waypoints + loiter_delta = 0; + loiter_sum = 0; + loiter_total = 0; + + // this is used to offset the shrinking longitude as we go towards the poles + float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; + scaleLongDown = cos(rads); + scaleLongUp = 1.0f/cos(rads); + // this is handy for the groundstation + wp_totalDistance = get_distance(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + target_bearing = get_bearing(¤t_loc, &next_WP); + nav_bearing = target_bearing; + + // to check if we have missed the WP + // ---------------------------- + old_target_bearing = target_bearing; + + // set a new crosstrack bearing + // ---------------------------- + reset_crosstrack(); +} + +static void set_guided_WP(void) +{ + // copy the current location into the OldWP slot + // --------------------------------------- + prev_WP = current_loc; + + // Load the next_WP slot + // --------------------- + next_WP = guided_WP; + + // used to control FBW and limit the rate of climb + // ----------------------------------------------- + target_altitude = current_loc.alt; + offset_altitude = next_WP.alt - prev_WP.alt; + + // this is used to offset the shrinking longitude as we go towards the poles + float rads = (abs(next_WP.lat)/t7) * 0.0174532925; + scaleLongDown = cos(rads); + scaleLongUp = 1.0f/cos(rads); + + // this is handy for the groundstation + wp_totalDistance = get_distance(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + target_bearing = get_bearing(¤t_loc, &next_WP); + + // to check if we have missed the WP + // ---------------------------- + old_target_bearing = target_bearing; + + // set a new crosstrack bearing + // ---------------------------- + reset_crosstrack(); +} + +// run this at setup on the ground +// ------------------------------- +void init_home() +{ + gcs_send_text_P(SEVERITY_LOW, PSTR("init home")); + + // block until we get a good fix + // ----------------------------- + while (!g_gps->new_data || !g_gps->fix) { + g_gps->update(); + } + + home.id = MAV_CMD_NAV_WAYPOINT; + home.lng = g_gps->longitude; // Lon * 10**7 + home.lat = g_gps->latitude; // Lat * 10**7 + home.alt = max(g_gps->altitude, 0); + home_is_set = true; + + gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); + + // Save Home to EEPROM - Command 0 + // ------------------- + set_cmd_with_index(home, 0); + + // Save prev loc + // ------------- + next_WP = prev_WP = home; + + // Load home for a default guided_WP + // ------------- + guided_WP = home; + guided_WP.alt += g.RTL_altitude; + +} + + + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands_logic.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/********************************************************************************/ +// Command Event Handlers +/********************************************************************************/ +static void +handle_process_nav_cmd() +{ + // reset navigation integrators + // ------------------------- + reset_I(); + + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); + switch(next_nav_command.id){ + + case MAV_CMD_NAV_TAKEOFF: + do_takeoff(); + break; + + case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint + do_nav_wp(); + break; + + case MAV_CMD_NAV_LAND: // LAND to Waypoint + do_land(); + break; + + case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely + do_loiter_unlimited(); + break; + + case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times + do_loiter_turns(); + break; + + case MAV_CMD_NAV_LOITER_TIME: + do_loiter_time(); + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + do_RTL(); + break; + + default: + break; + } +} + +static void +handle_process_condition_command() +{ + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); + switch(next_nonnav_command.id){ + + case MAV_CMD_CONDITION_DELAY: + do_wait_delay(); + break; + + case MAV_CMD_CONDITION_DISTANCE: + do_within_distance(); + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + do_change_alt(); + break; + + /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also) + gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set")); + + // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 + landing_pitch = next_nav_command.lng * 100; + g.airspeed_cruise = next_nav_command.alt * 100; + g.throttle_cruise = next_nav_command.lat; + landing_distance = next_nav_command.p1; + + SendDebug_P("MSG: throttle_cruise = "); + SendDebugln(g.throttle_cruise,DEC); + break; + */ + + default: + break; + } +} + +static void handle_process_do_command() +{ + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); + switch(next_nonnav_command.id){ + + case MAV_CMD_DO_JUMP: + do_jump(); + break; + + case MAV_CMD_DO_CHANGE_SPEED: + do_change_speed(); + break; + + case MAV_CMD_DO_SET_HOME: + do_set_home(); + break; + + case MAV_CMD_DO_SET_SERVO: + do_set_servo(); + break; + + case MAV_CMD_DO_SET_RELAY: + do_set_relay(); + break; + + case MAV_CMD_DO_REPEAT_SERVO: + do_repeat_servo(); + break; + + case MAV_CMD_DO_REPEAT_RELAY: + do_repeat_relay(); + break; + } +} + +static void handle_no_commands() +{ + gcs_send_text_fmt(PSTR("Returning to Home")); + next_nav_command = home; + next_nav_command.alt = read_alt_to_hold(); + next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; + nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM; + non_nav_command_ID = WAIT_COMMAND; + handle_process_nav_cmd(); + +} + +/********************************************************************************/ +// Verify command Handlers +/********************************************************************************/ + +static bool verify_nav_command() // Returns true if command complete +{ + switch(nav_command_ID) { + + case MAV_CMD_NAV_TAKEOFF: + return verify_takeoff(); + break; + + case MAV_CMD_NAV_LAND: + return verify_land(); + break; + + case MAV_CMD_NAV_WAYPOINT: + return verify_nav_wp(); + break; + + case MAV_CMD_NAV_LOITER_UNLIM: + return verify_loiter_unlim(); + break; + + case MAV_CMD_NAV_LOITER_TURNS: + return verify_loiter_turns(); + break; + + case MAV_CMD_NAV_LOITER_TIME: + return verify_loiter_time(); + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + return verify_RTL(); + break; + + default: + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd")); + return false; + break; + } +} + +static bool verify_condition_command() // Returns true if command complete +{ + switch(non_nav_command_ID) { + case NO_COMMAND: + break; + + case MAV_CMD_CONDITION_DELAY: + return verify_wait_delay(); + break; + + case MAV_CMD_CONDITION_DISTANCE: + return verify_within_distance(); + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + return verify_change_alt(); + break; + + case WAIT_COMMAND: + return 0; + break; + + + default: + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); + break; + } + return false; +} + +/********************************************************************************/ +// Nav (Must) commands +/********************************************************************************/ + +static void do_RTL(void) +{ + control_mode = RTL; + crash_timer = 0; + next_WP = home; + + // Altitude to hold over home + // Set by configuration tool + // ------------------------- + next_WP.alt = read_alt_to_hold(); + + if (g.log_bitmask & MASK_LOG_MODE) + Log_Write_Mode(control_mode); +} + +static void do_takeoff() +{ + set_next_WP(&next_nav_command); + // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 + takeoff_pitch = (int)next_nav_command.p1 * 100; + //Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch); + //Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt); + takeoff_altitude = next_nav_command.alt; + //Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude); + next_WP.lat = home.lat + 1000; // so we don't have bad calcs + next_WP.lng = home.lng + 1000; // so we don't have bad calcs + takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction + // Flag also used to override "on the ground" throttle disable +} + +static void do_nav_wp() +{ + set_next_WP(&next_nav_command); +} + +static void do_land() +{ + set_next_WP(&next_nav_command); +} + +static void do_loiter_unlimited() +{ + set_next_WP(&next_nav_command); +} + +static void do_loiter_turns() +{ + set_next_WP(&next_nav_command); + loiter_total = next_nav_command.p1 * 360; +} + +static void do_loiter_time() +{ + set_next_WP(&next_nav_command); + loiter_time = millis(); + loiter_time_max = next_nav_command.p1; // units are (seconds * 10) +} + +/********************************************************************************/ +// Verify Nav (Must) commands +/********************************************************************************/ +static bool verify_takeoff() +{ + if (g_gps->ground_speed > 300){ + if(hold_course == -1){ + // save our current course to take off + if(g.compass_enabled) { + hold_course = dcm.yaw_sensor; + } else { + hold_course = g_gps->ground_course; + } + } + } + + if(hold_course > -1){ + // recalc bearing error with hold_course; + nav_bearing = hold_course; + // recalc bearing error + calc_bearing_error(); + } + + if (current_loc.alt > takeoff_altitude) { + hold_course = -1; + takeoff_complete = true; + return true; + } else { + return false; + } +} + +static bool verify_land() +{ + // we don't verify landing - we never go to a new Nav command after Land + if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) + || (current_loc.alt <= next_WP.alt + 300)){ + + land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude + + if(hold_course == -1){ + // save our current course to land + //hold_course = yaw_sensor; + // save the course line of the runway to land + hold_course = crosstrack_bearing; + } + } + + if(hold_course > -1){ + // recalc bearing error with hold_course; + nav_bearing = hold_course; + // recalc bearing error + calc_bearing_error(); + } + + update_crosstrack(); + return false; +} + +static bool verify_nav_wp() +{ + hold_course = -1; + update_crosstrack(); + if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { + gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index); + return true; + } + // add in a more complex case + // Doug to do + if(loiter_sum > 300){ + gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP")); + return true; + } + return false; +} + +static bool verify_loiter_unlim() +{ + update_loiter(); + calc_bearing_error(); + return false; +} + +static bool verify_loiter_time() +{ + update_loiter(); + calc_bearing_error(); + if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete")); + return true; + } + return false; +} + +static bool verify_loiter_turns() +{ + update_loiter(); + calc_bearing_error(); + if(loiter_sum > loiter_total) { + loiter_total = 0; + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete")); + // clear the command queue; + return true; + } + return false; +} + +static bool verify_RTL() +{ + if (wp_distance <= g.waypoint_radius) { + gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); + return true; + }else{ + return false; + } +} + +/********************************************************************************/ +// Condition (May) commands +/********************************************************************************/ + +static void do_wait_delay() +{ + condition_start = millis(); + condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds +} + +static void do_change_alt() +{ + condition_rate = next_nonnav_command.lat; + condition_value = next_nonnav_command.alt; + target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update + next_WP.alt = condition_value; // For future nav calculations + offset_altitude = 0; // For future nav calculations +} + +static void do_within_distance() +{ + condition_value = next_nonnav_command.lat; +} + +/********************************************************************************/ +// Verify Condition (May) commands +/********************************************************************************/ + +static bool verify_wait_delay() +{ + if ((unsigned)(millis() - condition_start) > condition_value){ + condition_value = 0; + return true; + } + return false; +} + +static bool verify_change_alt() +{ + if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) { + condition_value = 0; + return true; + } + target_altitude += condition_rate / 10; + return false; +} + +static bool verify_within_distance() +{ + if (wp_distance < condition_value){ + condition_value = 0; + return true; + } + return false; +} + +/********************************************************************************/ +// Do (Now) commands +/********************************************************************************/ + +static void do_loiter_at_location() +{ + next_WP = current_loc; +} + +static void do_jump() +{ + struct Location temp; + if(next_nonnav_command.lat > 0) { + + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + temp = get_cmd_with_index(g.command_index); + temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter + + set_cmd_with_index(temp, g.command_index); + g.command_index.set_and_save(next_nonnav_command.p1 - 1); + } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + g.command_index.set_and_save(next_nonnav_command.p1 - 1); + } +} + +static void do_change_speed() +{ + // Note: we have no implementation for commanded ground speed, only air speed and throttle + if(next_nonnav_command.alt > 0) + g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100); + + if(next_nonnav_command.lat > 0) + g.throttle_cruise.set_and_save(next_nonnav_command.lat); +} + +static void do_set_home() +{ + if(next_nonnav_command.p1 == 1 && GPS_enabled) { + init_home(); + } else { + home.id = MAV_CMD_NAV_WAYPOINT; + home.lng = next_nonnav_command.lng; // Lon * 10**7 + home.lat = next_nonnav_command.lat; // Lat * 10**7 + home.alt = max(next_nonnav_command.alt, 0); + home_is_set = true; + } +} + +static void do_set_servo() +{ + APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt); +} + +static void do_set_relay() +{ + if (next_nonnav_command.p1 == 1) { + relay.on(); + } else if (next_nonnav_command.p1 == 0) { + relay.off(); + }else{ + relay.toggle(); + } +} + +static void do_repeat_servo() +{ + event_id = next_nonnav_command.p1 - 1; + + if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) { + + event_timer = 0; + event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.lat * 2; + event_value = next_nonnav_command.alt; + + switch(next_nonnav_command.p1) { + case CH_5: + event_undo_value = g.rc_5.radio_trim; + break; + case CH_6: + event_undo_value = g.rc_6.radio_trim; + break; + case CH_7: + event_undo_value = g.rc_7.radio_trim; + break; + case CH_8: + event_undo_value = g.rc_8.radio_trim; + break; + } + update_events(); + } +} + +static void do_repeat_relay() +{ + event_id = RELAY_TOGGLE; + event_timer = 0; + event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.alt * 2; + update_events(); +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands_process.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// For changing active command mid-mission +//---------------------------------------- +static void change_command(uint8_t cmd_index) +{ + struct Location temp = get_cmd_with_index(cmd_index); + if (temp.id > MAV_CMD_NAV_LAST ){ + gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); + } else { + gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index); + nav_command_ID = NO_COMMAND; + next_nav_command.id = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + nav_command_index = cmd_index - 1; + g.command_index.set_and_save(cmd_index - 1); + process_next_command(); + } +} + + +// called by 10 Hz loop +// -------------------- +static void update_commands(void) +{ + if(home_is_set == false){ + return; // don't do commands + } + + if(control_mode == AUTO){ + process_next_command(); + } // Other (eg GCS_Auto) modes may be implemented here +} + +static void verify_commands(void) +{ + if(verify_nav_command()){ + nav_command_ID = NO_COMMAND; + } + + if(verify_condition_command()){ + non_nav_command_ID = NO_COMMAND; + } +} + + +static void process_next_command() +{ + // This function makes sure that we always have a current navigation command + // and loads conditional or immediate commands if applicable + + struct Location temp; + byte old_index; + + // these are Navigation/Must commands + // --------------------------------- + if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded + old_index = nav_command_index; + temp.id = MAV_CMD_NAV_LAST; + while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { + nav_command_index++; + temp = get_cmd_with_index(nav_command_index); + } + gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index); + if(nav_command_index > g.command_total){ + // we are out of commands! + gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); + handle_no_commands(); + } else { + next_nav_command = temp; + nav_command_ID = next_nav_command.id; + non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded + non_nav_command_ID = NO_COMMAND; + + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nav_command); + } + process_nav_cmd(); + } + } + + // these are Condition/May and Do/Now commands + // ------------------------------------------- + if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command + non_nav_command_index = old_index + 1; + //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); + } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command + non_nav_command_index++; + } + + //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index); + //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); + //gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID); + if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) { + temp = get_cmd_with_index(non_nav_command_index); + if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do + g.command_index.set_and_save(nav_command_index); + non_nav_command_index = nav_command_index; + non_nav_command_ID = WAIT_COMMAND; + gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); + } else { // The next command is a non-nav command. Prepare to execute it. + g.command_index.set_and_save(non_nav_command_index); + next_nonnav_command = temp; + non_nav_command_ID = next_nonnav_command.id; + gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nonnav_command); + } + + process_non_nav_command(); + } + + } +} + +/**************************************************/ +// These functions implement the commands. +/**************************************************/ +static void process_nav_cmd() +{ + //gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); + + // clear non-nav command ID and index + non_nav_command_index = NO_COMMAND; // Redundant - remove? + non_nav_command_ID = NO_COMMAND; // Redundant - remove? + + handle_process_nav_cmd(); + +} + +static void process_non_nav_command() +{ + //gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); + + if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { + handle_process_condition_command(); + } else { + handle_process_do_command(); + // flag command ID so a new one is loaded + // ----------------------------------------- + non_nav_command_ID = NO_COMMAND; + } +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/control_modes.pde" +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +static void read_control_switch() +{ + byte switchPosition = readSwitch(); + if (oldSwitchPosition != switchPosition){ + + set_mode(flight_modes[switchPosition]); + + oldSwitchPosition = switchPosition; + prev_WP = current_loc; + + // reset navigation integrators + // ------------------------- + reset_I(); + } + + if (g.inverted_flight_ch != 0) { + // if the user has configured an inverted flight channel, then + // fly upside down when that channel goes above INVERTED_FLIGHT_PWM + inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); + } +} + +static byte readSwitch(void){ + uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); + if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; + if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; + if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; + if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual + if (pulsewidth >= 1750) return 5; // Hardware Manual + return 0; +} + +static void reset_control_switch() +{ + oldSwitchPosition = 0; + read_control_switch(); +} + +static void update_servo_switches() +{ + if (!g.switch_enable) { + // switches are disabled in EEPROM (see SWITCH_ENABLE option) + // this means the EEPROM control of all channel reversal is enabled + return; + } + // up is reverse + // up is elevon + g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode + if (g.mix_mode == 0) { + g.channel_roll.set_reverse((PINE & 128) ? true : false); + g.channel_pitch.set_reverse((PINE & 64) ? true : false); + g.channel_rudder.set_reverse((PINL & 64) ? true : false); + } else { + g.reverse_elevons = (PINE & 128) ? true : false; + g.reverse_ch1_elevon = (PINE & 64) ? true : false; + g.reverse_ch2_elevon = (PINL & 64) ? true : false; + } +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/events.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + +static void failsafe_short_on_event() +{ + // This is how to handle a short loss of control signal failsafe. + failsafe = FAILSAFE_SHORT; + ch3_failsafe_timer = millis(); + gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on")); + switch(control_mode) + { + case MANUAL: + case STABILIZE: + case FLY_BY_WIRE_A: // middle position + case FLY_BY_WIRE_B: // middle position + set_mode(CIRCLE); + break; + + case AUTO: + case LOITER: + if(g.short_fs_action == 1) { + set_mode(RTL); + } + break; + + case CIRCLE: + case RTL: + default: + break; + } + gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode); +} + +static void failsafe_long_on_event() +{ + // This is how to handle a long loss of control signal failsafe. + gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on")); + APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC + switch(control_mode) + { + case MANUAL: + case STABILIZE: + case FLY_BY_WIRE_A: // middle position + case FLY_BY_WIRE_B: // middle position + case CIRCLE: + set_mode(RTL); + break; + + case AUTO: + case LOITER: + if(g.long_fs_action == 1) { + set_mode(RTL); + } + break; + + case RTL: + default: + break; + } +} + +static void failsafe_short_off_event() +{ + // We're back in radio contact + gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off")); + failsafe = FAILSAFE_NONE; + + // re-read the switch so we can return to our preferred mode + // -------------------------------------------------------- + reset_control_switch(); + + // Reset control integrators + // --------------------- + reset_I(); +} + +#if BATTERY_EVENT == ENABLED +static void low_battery_event(void) +{ + gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); + set_mode(RTL); + g.throttle_cruise = THROTTLE_CRUISE; +} +#endif + +static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY +{ + if(event_repeat == 0 || (millis() - event_timer) < event_delay) + return; + + if (event_repeat > 0){ + event_repeat --; + } + + if(event_repeat != 0) { // event_repeat = -1 means repeat forever + event_timer = millis(); + + if (event_id >= CH_5 && event_id <= CH_8) { + if(event_repeat%2) { + APM_RC.OutputCh(event_id, event_value); // send to Servos + } else { + APM_RC.OutputCh(event_id, event_undo_value); + } + } + + if (event_id == RELAY_TOGGLE) { + relay.toggle(); + } + } +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/navigation.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +//**************************************************************** +// Function that will calculate the desired direction to fly and distance +//**************************************************************** +static void navigate() +{ + // do not navigate with corrupt data + // --------------------------------- + if (g_gps->fix == 0) + { + g_gps->new_data = false; + return; + } + + if(next_WP.lat == 0){ + return; + } + + // waypoint distance from plane + // ---------------------------- + wp_distance = get_distance(¤t_loc, &next_WP); + + if (wp_distance < 0){ + gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); + //Serial.println(wp_distance,DEC); + return; + } + + // target_bearing is where we should be heading + // -------------------------------------------- + target_bearing = get_bearing(¤t_loc, &next_WP); + + // nav_bearing will includes xtrac correction + // ------------------------------------------ + nav_bearing = target_bearing; + + // check if we have missed the WP + loiter_delta = (target_bearing - old_target_bearing)/100; + + // reset the old value + old_target_bearing = target_bearing; + + // wrap values + if (loiter_delta > 180) loiter_delta -= 360; + if (loiter_delta < -180) loiter_delta += 360; + loiter_sum += abs(loiter_delta); + + // control mode specific updates to nav_bearing + // -------------------------------------------- + update_navigation(); +} + + +#if 0 +// Disabled for now +void calc_distance_error() +{ + distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01)); + distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); + wp_distance = max(distance_estimate,10); +} +#endif + +static void calc_airspeed_errors() +{ + // XXX excess casting here + if(control_mode>=AUTO && airspeed_nudge > 0) { + airspeed_error = g.airspeed_cruise + airspeed_nudge - airspeed; + airspeed_energy_error = (long)(((long)(g.airspeed_cruise + airspeed_nudge) * (long)(g.airspeed_cruise + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation + } else { + airspeed_error = g.airspeed_cruise - airspeed; + airspeed_energy_error = (long)(((long)g.airspeed_cruise * (long)g.airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation + } +} + +static void calc_bearing_error() +{ + if(takeoff_complete == true || g.compass_enabled == true) { + bearing_error = nav_bearing - dcm.yaw_sensor; + } else { + + // TODO: we need to use the Yaw gyro for in between GPS reads, + // maybe as an offset from a saved gryo value. + bearing_error = nav_bearing - g_gps->ground_course; + } + + bearing_error = wrap_180(bearing_error); +} + +static void calc_altitude_error() +{ + if(control_mode == AUTO && offset_altitude != 0) { + // limit climb rates + target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30)); + + // stay within a certain range + if(prev_WP.alt > next_WP.alt){ + target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt); + }else{ + target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); + } + } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { + target_altitude = next_WP.alt; + } + + /* + // Disabled for now + #if AIRSPEED_SENSOR == 1 + long altitude_estimate; // for smoothing GPS output + + // special thanks to Ryan Beall for this one + float pitch_angle = pitch_sensor - g.pitch_trim; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°) + pitch_angle = constrain(pitch_angle, -2000, 2000); + float scale = sin(radians(pitch_angle * .01)); + altitude_estimate += (float)airspeed * .0002 * scale; + altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt); + + // compute altitude error for throttle control + altitude_error = target_altitude - altitude_estimate; + #else + altitude_error = target_altitude - current_loc.alt; + #endif + */ + + altitude_error = target_altitude - current_loc.alt; +} + +static long wrap_360(long error) +{ + if (error > 36000) error -= 36000; + if (error < 0) error += 36000; + return error; +} + +static long wrap_180(long error) +{ + if (error > 18000) error -= 36000; + if (error < -18000) error += 36000; + return error; +} + +static void update_loiter() +{ + float power; + + if(wp_distance <= g.loiter_radius){ + power = float(wp_distance) / float(g.loiter_radius); + power = constrain(power, 0.5, 1); + nav_bearing += (int)(9000.0 * (2.0 + power)); + }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ + power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); + power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); + nav_bearing -= power * 9000; + + }else{ + update_crosstrack(); + loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit + + } +/* + if (wp_distance < g.loiter_radius){ + nav_bearing += 9000; + }else{ + nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance); + } + + update_crosstrack(); +*/ + nav_bearing = wrap_360(nav_bearing); +} + +static void update_crosstrack(void) +{ + // Crosstrack Error + // ---------------- + if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following + crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line + nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); + nav_bearing = wrap_360(nav_bearing); + } +} + +static void reset_crosstrack() +{ + crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following +} + +static long get_distance(struct Location *loc1, struct Location *loc2) +{ + if(loc1->lat == 0 || loc1->lng == 0) + return -1; + if(loc2->lat == 0 || loc2->lng == 0) + return -1; + float dlat = (float)(loc2->lat - loc1->lat); + float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; + return sqrt(sq(dlat) + sq(dlong)) * .01113195; +} + +static long get_bearing(struct Location *loc1, struct Location *loc2) +{ + long off_x = loc2->lng - loc1->lng; + long off_y = (loc2->lat - loc1->lat) * scaleLongUp; + long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + if (bearing < 0) bearing += 36000; + return bearing; +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/planner.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// These are function definitions so the Menu can be constructed before the functions +// are defined below. Order matters to the compiler. +static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Common for implementation details +static const struct Menu::command planner_menu_commands[] PROGMEM = { + {"gcs", planner_gcs}, +}; + +// A Macro to create the Menu +MENU(planner_menu, "planner", planner_menu_commands); + +static int8_t +planner_mode(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); + planner_menu.run(); + return 0; +} +static int8_t +planner_gcs(uint8_t argc, const Menu::arg *argv) +{ + gcs0.init(&Serial); + gcs3.init(&Serial3); + + int loopcount = 0; + while (1) { + if (millis()-fast_loopTimer > 19) { + fast_loopTimer = millis(); + + gcs_update(); + gcs_data_stream_send(45,1000); + if ((loopcount % 5) == 0) // 10 hz + gcs_data_stream_send(5,45); + if ((loopcount % 16) == 0) { // 3 hz + gcs_data_stream_send(1,5); + gcs_send_message(MSG_HEARTBEAT); + } + loopcount++; + } + } + return 0; +} + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/radio.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +//Function that will read the radio data, limit servos and trigger a failsafe +// ---------------------------------------------------------------------------- +static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling + + +static void init_rc_in() +{ + // set rc reversing + update_servo_switches(); + + // set rc channel ranges + g.channel_roll.set_angle(SERVO_MAX); + g.channel_pitch.set_angle(SERVO_MAX); + g.channel_rudder.set_angle(SERVO_MAX); + g.channel_throttle.set_range(0, 100); + + // set rc dead zones + g.channel_roll.set_dead_zone(60); + g.channel_pitch.set_dead_zone(60); + g.channel_rudder.set_dead_zone(60); + g.channel_throttle.set_dead_zone(6); + + //g.channel_roll.dead_zone = 60; + //g.channel_pitch.dead_zone = 60; + //g.channel_rudder.dead_zone = 60; + //g.channel_throttle.dead_zone = 6; + + //set auxiliary ranges + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); +} + +static void init_rc_out() +{ + APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs + APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); + APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); + APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); + + APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); + APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); + APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); + APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + + APM_RC.Init(); // APM Radio initialization + + APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs + APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); + APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); + APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); + + APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); + APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); + APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); + APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); +} + +static void read_radio() +{ + ch1_temp = APM_RC.InputCh(CH_ROLL); + ch2_temp = APM_RC.InputCh(CH_PITCH); + + if(g.mix_mode == 0){ + g.channel_roll.set_pwm(ch1_temp); + g.channel_pitch.set_pwm(ch2_temp); + }else{ + g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); + g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); + } + + g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); + g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4)); + g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); + g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); + g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); + g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); + + // TO DO - go through and patch throttle reverse for RC_Channel library compatibility + #if THROTTLE_REVERSE == 1 + g.channel_throttle.radio_in = g.channel_throttle.radio_max + g.channel_throttle.radio_min - g.channel_throttle.radio_in; + #endif + + control_failsafe(g.channel_throttle.radio_in); + + g.channel_throttle.servo_out = g.channel_throttle.control_in; + + if (g.channel_throttle.servo_out > 50) { + if(g.airspeed_enabled == true) { + airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); + } else { + throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); + } + } else { + airspeed_nudge = 0; + throttle_nudge = 0; + } + + /* + Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), + g.rc_1.control_in, + g.rc_2.control_in, + g.rc_3.control_in, + g.rc_4.control_in); + */ +} + +static void control_failsafe(uint16_t pwm) +{ + if(g.throttle_fs_enabled == 0) + return; + + // Check for failsafe condition based on loss of GCS control + if (rc_override_active) { + if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) { + ch3_failsafe = true; + } else { + ch3_failsafe = false; + } + + //Check for failsafe and debounce funky reads + } else if (g.throttle_fs_enabled) { + if (pwm < (unsigned)g.throttle_fs_value){ + // we detect a failsafe from radio + // throttle has dropped below the mark + failsafeCounter++; + if (failsafeCounter == 9){ + gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm); + }else if(failsafeCounter == 10) { + ch3_failsafe = true; + }else if (failsafeCounter > 10){ + failsafeCounter = 11; + } + + }else if(failsafeCounter > 0){ + // we are no longer in failsafe condition + // but we need to recover quickly + failsafeCounter--; + if (failsafeCounter > 3){ + failsafeCounter = 3; + } + if (failsafeCounter == 1){ + gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm); + }else if(failsafeCounter == 0) { + ch3_failsafe = false; + }else if (failsafeCounter <0){ + failsafeCounter = -1; + } + } + } +} + +static void trim_control_surfaces() +{ + read_radio(); + // Store control surface trim values + // --------------------------------- + if(g.mix_mode == 0){ + g.channel_roll.radio_trim = g.channel_roll.radio_in; + g.channel_pitch.radio_trim = g.channel_pitch.radio_in; + g.channel_rudder.radio_trim = g.channel_rudder.radio_in; + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + + }else{ + elevon1_trim = ch1_temp; + elevon2_trim = ch2_temp; + //Recompute values here using new values for elevon1_trim and elevon2_trim + //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed + uint16_t center = 1500; + g.channel_roll.radio_trim = center; + g.channel_pitch.radio_trim = center; + } + + // save to eeprom + g.channel_roll.save_eeprom(); + g.channel_pitch.save_eeprom(); + g.channel_throttle.save_eeprom(); + g.channel_rudder.save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); +} + +static void trim_radio() +{ + for (int y = 0; y < 30; y++) { + read_radio(); + } + + // Store the trim values + // --------------------- + if(g.mix_mode == 0){ + g.channel_roll.radio_trim = g.channel_roll.radio_in; + g.channel_pitch.radio_trim = g.channel_pitch.radio_in; + //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; + g.channel_rudder.radio_trim = g.channel_rudder.radio_in; + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + + } else { + elevon1_trim = ch1_temp; + elevon2_trim = ch2_temp; + uint16_t center = 1500; + g.channel_roll.radio_trim = center; + g.channel_pitch.radio_trim = center; + g.channel_rudder.radio_trim = g.channel_rudder.radio_in; + } + + // save to eeprom + g.channel_roll.save_eeprom(); + g.channel_pitch.save_eeprom(); + //g.channel_throttle.save_eeprom(); + g.channel_rudder.save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/sensors.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// Sensors are not available in HIL_MODE_ATTITUDE +#if HIL_MODE != HIL_MODE_ATTITUDE + +void ReadSCP1000(void) {} + +static void init_barometer(void) +{ + int flashcount = 0; + long ground_pressure = 0; + int ground_temperature; + + while(ground_pressure == 0){ + barometer.Read(); // Get initial data from absolute pressure sensor + ground_pressure = barometer.Press; + ground_temperature = barometer.Temp; + mavlink_delay(20); + //Serial.printf("barometer.Press %ld\n", barometer.Press); + } + + for(int i = 0; i < 30; i++){ // We take some readings... + + #if HIL_MODE == HIL_MODE_SENSORS + gcs_update(); // look for inbound hil packets + #endif + + barometer.Read(); // Get initial data from absolute pressure sensor + ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l; + ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; + + mavlink_delay(20); + if(flashcount == 5) { + digitalWrite(C_LED_PIN, LOW); + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(B_LED_PIN, LOW); + } + + if(flashcount >= 10) { + flashcount = 0; + digitalWrite(C_LED_PIN, HIGH); + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, HIGH); + } + flashcount++; + } + + g.ground_pressure.set_and_save(ground_pressure); + g.ground_temperature.set_and_save(ground_temperature / 10.0f); + abs_pressure = ground_pressure; + + Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); +} + +static long read_barometer(void) +{ + float x, scaling, temp; + + barometer.Read(); // Get new data from absolute pressure sensor + + + //abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering + abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering + scaling = (float)g.ground_pressure / (float)abs_pressure; + temp = ((float)g.ground_temperature) + 273.15f; + x = log(scaling) * temp * 29271.267f; + return (x / 10); +} + +// in M/S * 100 +static void read_airspeed(void) +{ + #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed + if (g.airspeed_offset == 0) { + // runtime enabling of airspeed, we need to do instant + // calibration before we can use it. This isn't as + // accurate as the 50 point average in zero_airspeed(), + // but it is better than using it uncalibrated + airspeed_raw = (float)adc.Ch(AIRSPEED_CH); + g.airspeed_offset.set_and_save(airspeed_raw); + } + airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75); + airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0); + airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100; + #endif + + calc_airspeed_errors(); +} + +static void zero_airspeed(void) +{ + airspeed_raw = (float)adc.Ch(AIRSPEED_CH); + for(int c = 0; c < 10; c++){ + delay(20); + airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10); + } + g.airspeed_offset.set_and_save(airspeed_raw); +} + +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void read_battery(void) +{ + battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; + battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; + battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9; + battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9; + + if(g.battery_monitoring == 1) + battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream + if(g.battery_monitoring == 2) + battery_voltage = battery_voltage4; + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) + battery_voltage = battery_voltage1; + if(g.battery_monitoring == 4) { + current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin + current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; + } + + #if BATTERY_EVENT == ENABLED + if(battery_voltage < LOW_VOLTAGE) low_battery_event(); + if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); + #endif +} + +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/setup.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if CLI_ENABLED == ENABLED + +// Functions called from the setup menu +static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); +static int8_t setup_show (uint8_t argc, const Menu::arg *argv); +static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); +static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); +static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); +static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); +static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); +static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); + +// Command/function table for the setup menu +static const struct Menu::command setup_menu_commands[] PROGMEM = { + // command function called + // ======= =============== + {"reset", setup_factory}, + {"radio", setup_radio}, + {"modes", setup_flightmodes}, + {"compass", setup_compass}, + {"declination", setup_declination}, + {"battery", setup_batt_monitor}, + {"show", setup_show}, + {"erase", setup_erase}, +}; + +// Create the setup menu object. +MENU(setup_menu, "setup", setup_menu_commands); + +// Called from the top-level menu to run the setup menu. +static int8_t +setup_mode(uint8_t argc, const Menu::arg *argv) +{ + // Give the user some guidance + Serial.printf_P(PSTR("Setup Mode\n" + "\n" + "IMPORTANT: if you have not previously set this system up, use the\n" + "'reset' command to initialize the EEPROM to sensible default values\n" + "and then the 'radio' command to configure for your radio.\n" + "\n")); + + // Run the setup menu. When the menu exits, we will return to the main menu. + setup_menu.run(); + return 0; +} + +// Print the current configuration. +// Called by the setup menu 'show' command. +static int8_t +setup_show(uint8_t argc, const Menu::arg *argv) +{ + // clear the area + print_blanks(8); + + report_radio(); + report_batt_monitor(); + report_gains(); + report_xtrack(); + report_throttle(); + report_flight_modes(); + report_imu(); + report_compass(); + + Serial.printf_P(PSTR("Raw Values\n")); + print_divider(); + AP_Var_menu_show(argc, argv); + + return(0); +} + +// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). +// Called by the setup menu 'factoryreset' command. +static int8_t +setup_factory(uint8_t argc, const Menu::arg *argv) +{ + int c; + + Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); + + do { + c = Serial.read(); + } while (-1 == c); + + if (('y' != c) && ('Y' != c)) + return(-1); + AP_Var::erase_all(); + Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); + + //default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot() + + for (;;) { + } + // note, cannot actually return here + return(0); +} + + +// Perform radio setup. +// Called by the setup menu 'radio' command. +static int8_t +setup_radio(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("\n\nRadio Setup:\n")); + uint8_t i; + + for(i = 0; i < 100;i++){ + delay(20); + read_radio(); + } + + + if(g.channel_roll.radio_in < 500){ + while(1){ + Serial.printf_P(PSTR("\nNo radio; Check connectors.")); + delay(1000); + // stop here + } + } + + g.channel_roll.radio_min = g.channel_roll.radio_in; + g.channel_pitch.radio_min = g.channel_pitch.radio_in; + g.channel_throttle.radio_min = g.channel_throttle.radio_in; + g.channel_rudder.radio_min = g.channel_rudder.radio_in; + g.rc_5.radio_min = g.rc_5.radio_in; + g.rc_6.radio_min = g.rc_6.radio_in; + g.rc_7.radio_min = g.rc_7.radio_in; + g.rc_8.radio_min = g.rc_8.radio_in; + + g.channel_roll.radio_max = g.channel_roll.radio_in; + g.channel_pitch.radio_max = g.channel_pitch.radio_in; + g.channel_throttle.radio_max = g.channel_throttle.radio_in; + g.channel_rudder.radio_max = g.channel_rudder.radio_in; + g.rc_5.radio_max = g.rc_5.radio_in; + g.rc_6.radio_max = g.rc_6.radio_in; + g.rc_7.radio_max = g.rc_7.radio_in; + g.rc_8.radio_max = g.rc_8.radio_in; + + g.channel_roll.radio_trim = g.channel_roll.radio_in; + g.channel_pitch.radio_trim = g.channel_pitch.radio_in; + g.channel_rudder.radio_trim = g.channel_rudder.radio_in; + g.rc_5.radio_trim = 1500; + g.rc_6.radio_trim = 1500; + g.rc_7.radio_trim = 1500; + g.rc_8.radio_trim = 1500; + + Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n")); + while(1){ + + delay(20); + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + g.channel_roll.update_min_max(); + g.channel_pitch.update_min_max(); + g.channel_throttle.update_min_max(); + g.channel_rudder.update_min_max(); + g.rc_5.update_min_max(); + g.rc_6.update_min_max(); + g.rc_7.update_min_max(); + g.rc_8.update_min_max(); + + if(Serial.available() > 0){ + Serial.flush(); + g.channel_roll.save_eeprom(); + g.channel_pitch.save_eeprom(); + g.channel_throttle.save_eeprom(); + g.channel_rudder.save_eeprom(); + g.rc_5.save_eeprom(); + g.rc_6.save_eeprom(); + g.rc_7.save_eeprom(); + g.rc_8.save_eeprom(); + print_done(); + break; + } + } + trim_radio(); + report_radio(); + return(0); +} + + +static int8_t +setup_flightmodes(uint8_t argc, const Menu::arg *argv) +{ + byte switchPosition, mode = 0; + + Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); + print_hit_enter(); + trim_radio(); + + while(1){ + delay(20); + read_radio(); + switchPosition = readSwitch(); + + + // look for control switch change + if (oldSwitchPosition != switchPosition){ + // force position 5 to MANUAL + if (switchPosition > 4) { + flight_modes[switchPosition] = MANUAL; + } + // update our current mode + mode = flight_modes[switchPosition]; + + // update the user + print_switch(switchPosition, mode); + + // Remember switch position + oldSwitchPosition = switchPosition; + } + + // look for stick input + int radioInputSwitch = radio_input_switch(); + + if (radioInputSwitch != 0){ + + mode += radioInputSwitch; + + while ( + mode != MANUAL && + mode != CIRCLE && + mode != STABILIZE && + mode != FLY_BY_WIRE_A && + mode != FLY_BY_WIRE_B && + mode != AUTO && + mode != RTL && + mode != LOITER) + { + if (mode < MANUAL) + mode = LOITER; + else if (mode >LOITER) + mode = MANUAL; + else + mode += radioInputSwitch; + } + + // Override position 5 + if(switchPosition > 4) + mode = MANUAL; + + // save new mode + flight_modes[switchPosition] = mode; + + // print new mode + print_switch(switchPosition, mode); + } + + // escape hatch + if(Serial.available() > 0){ + // save changes + for (mode=0; mode<6; mode++) + flight_modes[mode].save(); + report_flight_modes(); + print_done(); + return (0); + } + } +} + +static int8_t +setup_declination(uint8_t argc, const Menu::arg *argv) +{ + compass.set_declination(radians(argv[1].f)); + report_compass(); + return 0; +} + + +static int8_t +setup_erase(uint8_t argc, const Menu::arg *argv) +{ + int c; + + Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ")); + + do { + c = Serial.read(); + } while (-1 == c); + + if (('y' != c) && ('Y' != c)) + return(-1); + zero_eeprom(); + return 0; +} + +static int8_t +setup_compass(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("on"))) { + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + if (!compass.init()) { + Serial.println_P(PSTR("Compass initialisation failed!")); + g.compass_enabled = false; + } else { + g.compass_enabled = true; + } + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { + g.compass_enabled = false; + + } else { + Serial.printf_P(PSTR("\nOptions:[on,off]\n")); + report_compass(); + return 0; + } + + g.compass_enabled.save(); + report_compass(); + return 0; +} + +static int8_t +setup_batt_monitor(uint8_t argc, const Menu::arg *argv) +{ + if(argv[1].i >= 0 && argv[1].i <= 4){ + g.battery_monitoring.set_and_save(argv[1].i); + + } else { + Serial.printf_P(PSTR("\nOptions: 0-4")); + } + + report_batt_monitor(); + return 0; +} + +/***************************************************************************/ +// CLI reports +/***************************************************************************/ + +static void report_batt_monitor() +{ + //print_blanks(2); + Serial.printf_P(PSTR("Batt Mointor\n")); + print_divider(); + if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled")); + if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell")); + if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell")); + if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts")); + if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); + print_blanks(2); +} +static void report_radio() +{ + //print_blanks(2); + Serial.printf_P(PSTR("Radio\n")); + print_divider(); + // radio + print_radio_values(); + print_blanks(2); +} + +static void report_gains() +{ + //print_blanks(2); + Serial.printf_P(PSTR("Gains\n")); + print_divider(); + + Serial.printf_P(PSTR("servo roll:\n")); + print_PID(&g.pidServoRoll); + + Serial.printf_P(PSTR("servo pitch:\n")); + print_PID(&g.pidServoPitch); + + Serial.printf_P(PSTR("servo rudder:\n")); + print_PID(&g.pidServoRudder); + + Serial.printf_P(PSTR("nav roll:\n")); + print_PID(&g.pidNavRoll); + + Serial.printf_P(PSTR("nav pitch airspeed:\n")); + print_PID(&g.pidNavPitchAirspeed); + + Serial.printf_P(PSTR("energry throttle:\n")); + print_PID(&g.pidTeThrottle); + + Serial.printf_P(PSTR("nav pitch alt:\n")); + print_PID(&g.pidNavPitchAltitude); + + print_blanks(2); +} + +static void report_xtrack() +{ + //print_blanks(2); + Serial.printf_P(PSTR("Crosstrack\n")); + print_divider(); + // radio + Serial.printf_P(PSTR("XTRACK: %4.2f\n" + "XTRACK angle: %d\n"), + (float)g.crosstrack_gain, + (int)g.crosstrack_entry_angle); + print_blanks(2); +} + +static void report_throttle() +{ + //print_blanks(2); + Serial.printf_P(PSTR("Throttle\n")); + print_divider(); + + Serial.printf_P(PSTR("min: %d\n" + "max: %d\n" + "cruise: %d\n" + "failsafe_enabled: %d\n" + "failsafe_value: %d\n"), + (int)g.throttle_min, + (int)g.throttle_max, + (int)g.throttle_cruise, + (int)g.throttle_fs_enabled, + (int)g.throttle_fs_value); + print_blanks(2); +} + +static void report_imu() +{ + //print_blanks(2); + Serial.printf_P(PSTR("IMU\n")); + print_divider(); + + print_gyro_offsets(); + print_accel_offsets(); + print_blanks(2); +} + +static void report_compass() +{ + //print_blanks(2); + Serial.printf_P(PSTR("Compass: ")); + + switch (compass.product_id) { + case AP_COMPASS_TYPE_HMC5883L: + Serial.println_P(PSTR("HMC5883L")); + break; + case AP_COMPASS_TYPE_HMC5843: + Serial.println_P(PSTR("HMC5843")); + break; + case AP_COMPASS_TYPE_HIL: + Serial.println_P(PSTR("HIL")); + break; + default: + Serial.println_P(PSTR("??")); + break; + } + + print_divider(); + + print_enabled(g.compass_enabled); + + // mag declination + Serial.printf_P(PSTR("Mag Declination: %4.4f\n"), + degrees(compass.get_declination())); + + Vector3f offsets = compass.get_offsets(); + + // mag offsets + Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"), + offsets.x, + offsets.y, + offsets.z); + print_blanks(2); +} + +static void report_flight_modes() +{ + //print_blanks(2); + Serial.printf_P(PSTR("Flight modes\n")); + print_divider(); + + for(int i = 0; i < 6; i++ ){ + print_switch(i, flight_modes[i]); + } + print_blanks(2); +} + +/***************************************************************************/ +// CLI utilities +/***************************************************************************/ + +static void +print_PID(PID * pid) +{ + Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), + pid->kP(), + pid->kI(), + pid->kD(), + (long)pid->imax()); +} + +static void +print_radio_values() +{ + Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); + Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max); + Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max); + Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max); + Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max); + Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max); + Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max); + Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max); + +} + +static void +print_switch(byte p, byte m) +{ + Serial.printf_P(PSTR("Pos %d: "),p); + Serial.println(flight_mode_strings[m]); +} + +static void +print_done() +{ + Serial.printf_P(PSTR("\nSaved Settings\n\n")); +} + +static void +print_blanks(int num) +{ + while(num > 0){ + num--; + Serial.println(""); + } +} + +static void +print_divider(void) +{ + for (int i = 0; i < 40; i++) { + Serial.printf_P(PSTR("-")); + } + Serial.println(""); +} + +static int8_t +radio_input_switch(void) +{ + static int8_t bouncer = 0; + + + if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) { + bouncer = 10; + } + if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) { + bouncer = -10; + } + if (bouncer >0) { + bouncer --; + } + if (bouncer <0) { + bouncer ++; + } + + if (bouncer == 1 || bouncer == -1) { + return bouncer; + } else { + return 0; + } +} + + +static void zero_eeprom(void) +{ + byte b = 0; + Serial.printf_P(PSTR("\nErasing EEPROM\n")); + for (int i = 0; i < EEPROM_MAX_ADDR; i++) { + eeprom_write_byte((uint8_t *) i, b); + } + Serial.printf_P(PSTR("done\n")); +} + +static void print_enabled(bool b) +{ + if(b) + Serial.printf_P(PSTR("en")); + else + Serial.printf_P(PSTR("dis")); + Serial.printf_P(PSTR("abled\n")); +} + +static void +print_accel_offsets(void) +{ + Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), + (float)imu.ax(), + (float)imu.ay(), + (float)imu.az()); +} + +static void +print_gyro_offsets(void) +{ + Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), + (float)imu.gx(), + (float)imu.gy(), + (float)imu.gz()); +} + + +#endif // CLI_ENABLED +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/system.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/***************************************************************************** +The init_ardupilot function processes everything we need for an in - air restart + We will determine later if we are actually on the ground and process a + ground start in that case. + +*****************************************************************************/ + +#if CLI_ENABLED == ENABLED + +// Functions called from the top-level menu +static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde +static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp +static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde + +// This is the help function +// PSTR is an AVR macro to read strings from flash memory +// printf_P is a version of print_f that reads from flash memory +static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Commands:\n" + " logs log readback/setup mode\n" + " setup setup mode\n" + " test test mode\n" + "\n" + "Move the slide switch and reset to FLY.\n" + "\n")); + return(0); +} + +// Command/function table for the top-level menu. +static const struct Menu::command main_menu_commands[] PROGMEM = { +// command function called +// ======= =============== + {"logs", process_logs}, + {"setup", setup_mode}, + {"test", test_mode}, + {"help", main_menu_help}, + {"planner", planner_mode} +}; + +// Create the top-level menu object. +MENU(main_menu, THISFIRMWARE, main_menu_commands); + +// the user wants the CLI. It never exits +static void run_cli(void) +{ + while (1) { + main_menu.run(); + } +} + +#endif // CLI_ENABLED + +static void init_ardupilot() +{ + // Console serial port + // + // The console port buffers are defined to be sufficiently large to support + // the console's use as a logging device, optionally as the GPS port when + // GPS_PROTOCOL_IMU is selected, and as the telemetry port. + // + // XXX This could be optimised to reduce the buffer sizes in the cases + // where they are not otherwise required. + // + Serial.begin(SERIAL0_BAUD, 128, 128); + + // GPS serial port. + // + // XXX currently the EM406 (SiRF receiver) is nominally configured + // at 57600, however it's not been supported to date. We should + // probably standardise on 38400. + // + // XXX the 128 byte receive buffer may be too small for NMEA, depending + // on the message set configured. + // + // standard gps running + Serial1.begin(38400, 128, 16); + + Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE + "\n\nFree RAM: %u\n"), + memcheck_available_memory()); + + // + // Check the EEPROM format version before loading any parameters from EEPROM. + // + if (!g.format_version.load()) { + + Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n")); + delay(100); // wait for serial msg to flush + + AP_Var::erase_all(); + + // save the current format version + g.format_version.set_and_save(Parameters::k_format_version); + + } else if (g.format_version != Parameters::k_format_version) { + + Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" + "\n\nForcing complete parameter reset..."), + g.format_version.get(), Parameters::k_format_version); + delay(100); // wait for serial msg to flush + + // erase all parameters + AP_Var::erase_all(); + + // save the new format version + g.format_version.set_and_save(Parameters::k_format_version); + + Serial.println_P(PSTR("done.")); + } else { + unsigned long before = micros(); + // Load all auto-loaded EEPROM variables + AP_Var::load_all(); + + Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); + Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"), + AP_Var::get_memory_use(), (unsigned)g.num_resets); + } + + // keep a record of how many resets have happened. This can be + // used to detect in-flight resets + g.num_resets.set_and_save(g.num_resets+1); + + // Telemetry port. + // + // Not used if telemetry is going to the console. + // + // XXX for unidirectional protocols, we could (should) minimize + // the receive buffer, and the transmit buffer could also be + // shrunk for protocols that don't send large messages. + // + Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + + mavlink_system.sysid = g.sysid_this_mav; + + +#if HIL_MODE != HIL_MODE_ATTITUDE + adc.Init(); // APM ADC library initialization + barometer.Init(); // APM Abs Pressure sensor initialization + + if (g.compass_enabled==true) { + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + if (!compass.init()) { + Serial.println_P(PSTR("Compass initialisation failed!")); + g.compass_enabled = false; + } else { + dcm.set_compass(&compass); + compass.get_offsets(); // load offsets to account for airframe magnetic interference + } + } + /* + Init is depricated - Jason + if(g.sonar_enabled){ + sonar.init(SONAR_PIN, &adc); + Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC); + } + */ +#endif + +#if LOGGING_ENABLED == ENABLED + DataFlash.Init(); // DataFlash log initialization +#endif + + // Do GPS init + g_gps = &g_gps_driver; + g_gps->init(); // GPS Initialization + g_gps->callback = mavlink_delay; + + // init the GCS + gcs0.init(&Serial); + gcs3.init(&Serial3); + + //mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav + mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id + mavlink_system.type = MAV_FIXED_WING; + + rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override + init_rc_in(); // sets up rc channels from radio + init_rc_out(); // sets up the timer libs + + pinMode(C_LED_PIN, OUTPUT); // GPS status LED + pinMode(A_LED_PIN, OUTPUT); // GPS status LED + pinMode(B_LED_PIN, OUTPUT); // GPS status LED + pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode + pinMode(PUSHBUTTON_PIN, INPUT); // unused + DDRL |= B00000100; // Set Port L, pin 2 to output for the relay + + // If the switch is in 'menu' mode, run the main menu. + // + // Since we can't be sure that the setup or test mode won't leave + // the system in an odd state, we don't let the user exit the top + // menu; they must reset in order to fly. + // +#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED + if (digitalRead(SLIDE_SWITCH_PIN) == 0) { + digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED + Serial.printf_P(PSTR("\n" + "Entering interactive setup mode...\n" + "\n" + "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n" + "Type 'help' to list commands, 'exit' to leave a submenu.\n" + "Visit the 'setup' menu for first-time configuration.\n")); + Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); + run_cli(); + } +#else + Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n")); +#endif // CLI_ENABLED + + if(g.log_bitmask != 0){ + // TODO - Here we will check on the length of the last log + // We don't want to create a bunch of little logs due to powering on and off + byte last_log_num = get_num_logs(); + start_new_log(last_log_num); + } + + // read in the flight switches + update_servo_switches(); + + if (ENABLE_AIR_START == 1) { + // Perform an air start and get back to flying + gcs_send_text_P(SEVERITY_LOW,PSTR(" AIR START")); + + // Get necessary data from EEPROM + //---------------- + //read_EEPROM_airstart_critical(); +#if HIL_MODE != HIL_MODE_ATTITUDE + imu.init(IMU::WARM_START); + dcm.set_centripetal(1); +#endif + + // This delay is important for the APM_RC library to work. + // We need some time for the comm between the 328 and 1280 to be established. + int old_pulse = 0; + while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(g.flight_mode_channel)) > 5 || + APM_RC.InputCh(g.flight_mode_channel) == 1000 || + APM_RC.InputCh(g.flight_mode_channel) == 1200)) { + old_pulse = APM_RC.InputCh(g.flight_mode_channel); + delay(25); + } + GPS_enabled = false; + g_gps->update(); + if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED) GPS_enabled = true; + + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_AIRSTART_MSG); + reload_commands_airstart(); // Get set to resume AUTO from where we left off + + }else { + startup_ground(); + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + } + + set_mode(MANUAL); + + // set the correct flight mode + // --------------------------- + reset_control_switch(); +} + +//******************************************************************************** +//This function does all the calibrations, etc. that we need during a ground start +//******************************************************************************** +static void startup_ground(void) +{ + set_mode(INITIALISING); + + gcs_send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); + + #if(GROUND_START_DELAY > 0) + gcs_send_text_P(SEVERITY_LOW,PSTR(" With Delay")); + delay(GROUND_START_DELAY * 1000); + #endif + + // Makes the servos wiggle + // step 1 = 1 wiggle + // ----------------------- + demo_servos(1); + + //IMU ground start + //------------------------ + // + startup_IMU_ground(); + + // read the radio to set trims + // --------------------------- + trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. + +#if HIL_MODE != HIL_MODE_ATTITUDE +if (g.airspeed_enabled == true) + { + // initialize airspeed sensor + // -------------------------- + zero_airspeed(); + gcs_send_text_P(SEVERITY_LOW,PSTR(" zero airspeed calibrated")); + } +else + { + gcs_send_text_P(SEVERITY_LOW,PSTR(" NO airspeed")); + } +#endif + + // Save the settings for in-air restart + // ------------------------------------ + //save_EEPROM_groundstart(); + + // initialize commands + // ------------------- + init_commands(); + + // Read in the GPS - see if one is connected + GPS_enabled = false; + for (byte counter = 0; ; counter++) { + g_gps->update(); + if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){ + GPS_enabled = true; + break; + } + + if (counter >= 2) { + GPS_enabled = false; + break; + } + } + + // Makes the servos wiggle - 3 times signals ready to fly + // ----------------------- + demo_servos(3); + + gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); +} + +static void set_mode(byte mode) +{ + if(control_mode == mode){ + // don't switch modes if we are already in the correct mode. + return; + } + if(g.auto_trim > 0 && control_mode == MANUAL) + trim_control_surfaces(); + + control_mode = mode; + crash_timer = 0; + + switch(control_mode) + { + case INITIALISING: + case MANUAL: + case CIRCLE: + case STABILIZE: + case FLY_BY_WIRE_A: + case FLY_BY_WIRE_B: + break; + + case AUTO: + update_auto(); + break; + + case RTL: + do_RTL(); + break; + + case LOITER: + do_loiter_at_location(); + break; + + case GUIDED: + set_guided_WP(); + break; + + default: + do_RTL(); + break; + } + + if (g.log_bitmask & MASK_LOG_MODE) + Log_Write_Mode(control_mode); +} + +static void check_long_failsafe() +{ + // only act on changes + // ------------------- + if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){ + if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { + failsafe = FAILSAFE_LONG; + failsafe_long_on_event(); + } + if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) { + failsafe = FAILSAFE_LONG; + failsafe_long_on_event(); + } + if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { + failsafe = FAILSAFE_GCS; + failsafe_long_on_event(); + } + } else { + // We do not change state but allow for user to change mode + if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE; + if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE; + if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE; + } +} + +static void check_short_failsafe() +{ + // only act on changes + // ------------------- + if(failsafe == FAILSAFE_NONE){ + if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde + failsafe_short_on_event(); + } + } + + if(failsafe == FAILSAFE_SHORT){ + if(!ch3_failsafe) { + failsafe_short_off_event(); + } + } +} + + +static void startup_IMU_ground(void) +{ +#if HIL_MODE != HIL_MODE_ATTITUDE + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); + mavlink_delay(500); + + // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! + // ----------------------- + demo_servos(2); + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); + mavlink_delay(1000); + + imu.init(IMU::COLD_START, mavlink_delay); + dcm.set_centripetal(1); + + // read Baro pressure at ground + //----------------------------- + init_barometer(); + +#endif // HIL_MODE_ATTITUDE + + digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready + digitalWrite(A_LED_PIN, LOW); + digitalWrite(C_LED_PIN, LOW); +} + + +static void update_GPS_light(void) +{ + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + switch (g_gps->status()) { + case(2): + digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case(1): + if (g_gps->valid_read == true){ + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light){ + digitalWrite(C_LED_PIN, LOW); + } else { + digitalWrite(C_LED_PIN, HIGH); + } + g_gps->valid_read = false; + } + break; + + default: + digitalWrite(C_LED_PIN, LOW); + break; + } +} + + +static void resetPerfData(void) { + mainLoop_count = 0; + G_Dt_max = 0; + dcm.gyro_sat_count = 0; + imu.adc_constraints = 0; + dcm.renorm_sqrt_count = 0; + dcm.renorm_blowup_count = 0; + gps_fix_count = 0; + pmTest1 = 0; + perf_mon_timer = millis(); +} + + +/* + map from a 8 bit EEPROM baud rate to a real baud rate + */ +static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) +{ + switch (rate) { + case 9: return 9600; + case 19: return 19200; + case 38: return 38400; + case 57: return 57600; + case 111: return 111100; + case 115: return 115200; + } + Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); + return default_baud; +} +#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/test.pde" +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if CLI_ENABLED == ENABLED + +// These are function definitions so the Menu can be constructed before the functions +// are defined below. Order matters to the compiler. +static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); +static int8_t test_radio(uint8_t argc, const Menu::arg *argv); +static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); +static int8_t test_gps(uint8_t argc, const Menu::arg *argv); +static int8_t test_adc(uint8_t argc, const Menu::arg *argv); +static int8_t test_imu(uint8_t argc, const Menu::arg *argv); +static int8_t test_gyro(uint8_t argc, const Menu::arg *argv); +static int8_t test_battery(uint8_t argc, const Menu::arg *argv); +static int8_t test_current(uint8_t argc, const Menu::arg *argv); +static int8_t test_relay(uint8_t argc, const Menu::arg *argv); +static int8_t test_wp(uint8_t argc, const Menu::arg *argv); +static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); +static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); +static int8_t test_mag(uint8_t argc, const Menu::arg *argv); +static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); +static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); +static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); +static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); +static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Common for implementation details +static const struct Menu::command test_menu_commands[] PROGMEM = { + {"pwm", test_radio_pwm}, + {"radio", test_radio}, + {"failsafe", test_failsafe}, + {"battery", test_battery}, + {"relay", test_relay}, + {"waypoints", test_wp}, + {"xbee", test_xbee}, + {"eedump", test_eedump}, + {"modeswitch", test_modeswitch}, + {"dipswitches", test_dipswitches}, + + // Tests below here are for hardware sensors only present + // when real sensors are attached or they are emulated +#if HIL_MODE == HIL_MODE_DISABLED + {"adc", test_adc}, + {"gps", test_gps}, + {"rawgps", test_rawgps}, + {"imu", test_imu}, + {"gyro", test_gyro}, + {"airspeed", test_airspeed}, + {"airpressure", test_pressure}, + {"compass", test_mag}, + {"current", test_current}, +#elif HIL_MODE == HIL_MODE_SENSORS + {"adc", test_adc}, + {"gps", test_gps}, + {"imu", test_imu}, + {"gyro", test_gyro}, + {"compass", test_mag}, +#elif HIL_MODE == HIL_MODE_ATTITUDE +#endif + +}; + +// A Macro to create the Menu +MENU(test_menu, "test", test_menu_commands); + +static int8_t +test_mode(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Test Mode\n\n")); + test_menu.run(); + return 0; +} + +static void print_hit_enter() +{ + Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); +} + +static int8_t +test_eedump(uint8_t argc, const Menu::arg *argv) +{ + int i, j; + + // hexdump the EEPROM + for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { + Serial.printf_P(PSTR("%04x:"), i); + for (j = 0; j < 16; j++) + Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); + Serial.println(); + } + return(0); +} + +static int8_t +test_radio_pwm(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(20); + + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + g.channel_roll.radio_in, + g.channel_pitch.radio_in, + g.channel_throttle.radio_in, + g.channel_rudder.radio_in, + g.rc_5.radio_in, + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_radio(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + #if THROTTLE_REVERSE == 1 + Serial.printf_P(PSTR("Throttle is reversed in config: \n")); + delay(1000); + #endif + + // read the radio to set trims + // --------------------------- + trim_radio(); + + while(1){ + delay(20); + read_radio(); + update_servo_switches(); + + g.channel_roll.calc_pwm(); + g.channel_pitch.calc_pwm(); + g.channel_throttle.calc_pwm(); + g.channel_rudder.calc_pwm(); + + // write out the servo PWM values + // ------------------------------ + set_servos(); + + Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + g.channel_roll.control_in, + g.channel_pitch.control_in, + g.channel_throttle.control_in, + g.channel_rudder.control_in, + g.rc_5.control_in, + g.rc_6.control_in, + g.rc_7.control_in, + g.rc_8.control_in); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_failsafe(uint8_t argc, const Menu::arg *argv) +{ + byte fail_test; + print_hit_enter(); + for(int i = 0; i < 50; i++){ + delay(20); + read_radio(); + } + + // read the radio to set trims + // --------------------------- + trim_radio(); + + oldSwitchPosition = readSwitch(); + + Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); + while(g.channel_throttle.control_in > 0){ + delay(20); + read_radio(); + } + + while(1){ + delay(20); + read_radio(); + + if(g.channel_throttle.control_in > 0){ + Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in); + fail_test++; + } + + if(oldSwitchPosition != readSwitch()){ + Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); + Serial.println(flight_mode_strings[readSwitch()]); + fail_test++; + } + + if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){ + Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in); + Serial.println(flight_mode_strings[readSwitch()]); + fail_test++; + } + + if(fail_test > 0){ + return (0); + } + if(Serial.available() > 0){ + Serial.printf_P(PSTR("LOS caused no change in APM.\n")); + return (0); + } + } +} + +static int8_t +test_battery(uint8_t argc, const Menu::arg *argv) +{ +if (g.battery_monitoring >=1 && g.battery_monitoring < 4) { + for (int i = 0; i < 80; i++){ // Need to get many samples for filter to stabilize + delay(20); + read_battery(); + } + Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"), + battery_voltage1, + battery_voltage2, + battery_voltage3, + battery_voltage4); +} else { + Serial.printf_P(PSTR("Not enabled\n")); +} + return (0); +} + +static int8_t +test_current(uint8_t argc, const Menu::arg *argv) +{ +if (g.battery_monitoring == 4) { + print_hit_enter(); + delta_ms_medium_loop = 100; + + while(1){ + delay(100); + read_radio(); + read_battery(); + Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), + battery_voltage, + current_amps, + current_total); + + // write out the servo PWM values + // ------------------------------ + set_servos(); + + if(Serial.available() > 0){ + return (0); + } + } +} else { + Serial.printf_P(PSTR("Not enabled\n")); + return (0); +} + +} + +static int8_t +test_relay(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + Serial.printf_P(PSTR("Relay on\n")); + relay.on(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } + + Serial.printf_P(PSTR("Relay off\n")); + relay.off(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_wp(uint8_t argc, const Menu::arg *argv) +{ + delay(1000); + + // save the alitude above home option + if(g.RTL_altitude < 0){ + Serial.printf_P(PSTR("Hold current altitude\n")); + }else{ + Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100); + } + + Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); + Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); + Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); + + for(byte i = 0; i <= g.command_total; i++){ + struct Location temp = get_cmd_with_index(i); + test_wp_print(&temp, i); + } + + return (0); +} + +static void +test_wp_print(struct Location *cmd, byte wp_index) +{ + Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), + (int)wp_index, + (int)cmd->id, + (int)cmd->options, + (int)cmd->p1, + cmd->alt, + cmd->lat, + cmd->lng); +} + +static int8_t +test_xbee(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); + + while(1){ + + if (Serial3.available()) + Serial3.write(Serial3.read()); + + if(Serial.available() > 0){ + return (0); + } + } +} + + +static int8_t +test_modeswitch(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + Serial.printf_P(PSTR("Control CH ")); + + Serial.println(FLIGHT_MODE_CHANNEL, DEC); + + while(1){ + delay(20); + byte switchPosition = readSwitch(); + if (oldSwitchPosition != switchPosition){ + Serial.printf_P(PSTR("Position %d\n"), switchPosition); + oldSwitchPosition = switchPosition; + } + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_dipswitches(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + if (!g.switch_enable) { + Serial.println_P(PSTR("dip switches disabled, using EEPROM")); + } + + while(1){ + delay(100); + update_servo_switches(); + + if (g.mix_mode == 0) { + Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), + (int)g.channel_roll.get_reverse(), + (int)g.channel_pitch.get_reverse(), + (int)g.channel_rudder.get_reverse()); + } else { + Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), + (int)g.reverse_elevons, + (int)g.reverse_ch1_elevon, + (int)g.reverse_ch2_elevon); + } + if(Serial.available() > 0){ + return (0); + } + } +} + +//------------------------------------------------------------------------------------------- +// tests in this section are for real sensors or sensors that have been simulated + +#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS +static int8_t +test_adc(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + adc.Init(); + delay(1000); + Serial.printf_P(PSTR("ADC\n")); + delay(1000); + + while(1){ + for (int i=0;i<9;i++) Serial.printf_P(PSTR("%u\t"),adc.Ch(i)); + Serial.println(); + delay(100); + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_gps(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + delay(333); + + // Blink GPS LED if we don't have a fix + // ------------------------------------ + update_GPS_light(); + + g_gps->update(); + + if (g_gps->new_data){ + Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), + g_gps->latitude, + g_gps->longitude, + g_gps->altitude/100, + g_gps->num_sats); + }else{ + Serial.printf_P(PSTR(".")); + } + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_imu(uint8_t argc, const Menu::arg *argv) +{ + //Serial.printf_P(PSTR("Calibrating.")); + + imu.init(IMU::COLD_START); + + print_hit_enter(); + delay(1000); + + while(1){ + delay(20); + if (millis() - fast_loopTimer > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer; + G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator + fast_loopTimer = millis(); + + // IMU + // --- + dcm.update_DCM(); + + if(g.compass_enabled) { + medium_loopCounter++; + if(medium_loopCounter == 5){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + medium_loopCounter = 0; + } + } + + // We are using the IMU + // --------------------- + Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), + (int)dcm.roll_sensor / 100, + (int)dcm.pitch_sensor / 100, + (uint16_t)dcm.yaw_sensor / 100); + } + if(Serial.available() > 0){ + return (0); + } + } +} + + +static int8_t +test_gyro(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + adc.Init(); + delay(1000); + Serial.printf_P(PSTR("Gyro | Accel\n")); + delay(1000); + + while(1){ + imu.update(); // need this because we are not calling the DCM + Vector3f gyros = imu.get_gyro(); + Vector3f accels = imu.get_accel(); + Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), + (int)gyros.x, + (int)gyros.y, + (int)gyros.z, + (int)accels.x, + (int)accels.y, + (int)accels.z); + delay(100); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_mag(uint8_t argc, const Menu::arg *argv) +{ + if (!g.compass_enabled) { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } + + compass.set_orientation(MAG_ORIENTATION); + if (!compass.init()) { + Serial.println_P(PSTR("Compass initialisation failed!")); + return 0; + } + dcm.set_compass(&compass); + report_compass(); + + // we need the DCM initialised for this test + imu.init(IMU::COLD_START); + + int counter = 0; + //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); + + print_hit_enter(); + + while(1) { + delay(20); + if (millis() - fast_loopTimer > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer; + G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator + fast_loopTimer = millis(); + + // IMU + // --- + dcm.update_DCM(); + + medium_loopCounter++; + if(medium_loopCounter == 5){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + medium_loopCounter = 0; + } + + counter++; + if (counter>20) { + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z, + maggy.x, + maggy.y, + maggy.z); + counter=0; + } + } + if (Serial.available() > 0) { + break; + } + } + + // save offsets. This allows you to get sane offset values using + // the CLI before you go flying. + Serial.println_P(PSTR("saving offsets")); + compass.save_offsets(); + return (0); +} + +#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS + +//------------------------------------------------------------------------------------------- +// real sensors that have not been simulated yet go here + +#if HIL_MODE == HIL_MODE_DISABLED + +static int8_t +test_airspeed(uint8_t argc, const Menu::arg *argv) +{ + unsigned airspeed_ch = adc.Ch(AIRSPEED_CH); + // Serial.println(adc.Ch(AIRSPEED_CH)); + Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch); + + if (g.airspeed_enabled == false){ + Serial.printf_P(PSTR("airspeed: ")); + print_enabled(false); + return (0); + + }else{ + print_hit_enter(); + zero_airspeed(); + Serial.printf_P(PSTR("airspeed: ")); + print_enabled(true); + + while(1){ + delay(20); + read_airspeed(); + Serial.printf_P(PSTR("%fm/s\n"), airspeed / 100.0); + + if(Serial.available() > 0){ + return (0); + } + } + } +} + + +static int8_t +test_pressure(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Uncalibrated relative airpressure\n")); + print_hit_enter(); + + home.alt = 0; + wp_distance = 0; + init_barometer(); + + while(1){ + delay(100); + current_loc.alt = read_barometer() + home.alt; + + Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld\n"), + current_loc.alt / 100.0, + abs_pressure); + + if(Serial.available() > 0){ + return (0); + } + } +} + +static int8_t +test_rawgps(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + + while(1){ + if (Serial3.available()){ + digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS + Serial1.write(Serial3.read()); + digitalWrite(B_LED_PIN, LOW); + } + if (Serial1.available()){ + digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS + Serial3.write(Serial1.read()); + digitalWrite(C_LED_PIN, LOW); + } + if(Serial.available() > 0){ + return (0); + } + } +} +#endif // HIL_MODE == HIL_MODE_DISABLED + +#endif // CLI_ENABLED diff --git a/CMakeLists.txt b/CMakeLists.txt index 74b8b01de6..41d70d7e69 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,7 +80,7 @@ macro(apm_project PROJECT_NAME BOARD SRCS) set(${PROJECT_NAME}_BOARD ${BOARD}) set(${PROJECT_NAME}_AFLAGS "-assembler-with-cpp") set(${PROJECT_NAME}_SRCS ${SRCS}) - set(${PROJECT_NAME}_LIBS c) + set(${PROJECT_NAME}_LIBS m c) generate_arduino_firmware(${PROJECT_NAME}) set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) install(FILES @@ -93,7 +93,5 @@ endmacro() apm_project(apo ${BOARD} apo/apo.cpp) apm_project(ArduRover ${BOARD} ArduRover/ArduRover.cpp) apm_project(ArduBoat ${BOARD} ArduBoat/ArduBoat.cpp) -#apm_project(ArduPlane ${BOARD} ArduPlane/ArduPlane.cpp) -#apm_project(ArduCopter ${BOARD} ArduCopter/ArduCopter.cpp) - - +apm_project(ArduPlane ${BOARD} ArduPlane/ArduPlane.cpp) +apm_project(ArduCopter ${BOARD} ArduCopter/ArduCopter.cpp)