From c914b1150491c5e55fb4ba38d0aa593b0fef87c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Oct 2011 19:13:55 +1100 Subject: [PATCH 1/8] use C++ tricks to minimise differences for MAVLink 1.0 this should make maintainence/testing a bit easier --- ArduPlane/GCS_Mavlink.pde | 76 ------------------------------- ArduPlane/Mavlink_compat.h | 91 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 91 insertions(+), 76 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 22bbd0dfe3..ee6d7d9169 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -284,7 +284,6 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan) 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(), @@ -296,16 +295,6 @@ static void NOINLINE send_location(mavlink_channel_t chan) 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) @@ -366,7 +355,6 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) // 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(), @@ -380,26 +368,11 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) 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(), @@ -413,25 +386,10 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan) 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(), @@ -444,18 +402,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan) 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) @@ -748,11 +694,7 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char 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 } } @@ -1731,7 +1673,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // 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, @@ -1739,14 +1680,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) 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; @@ -1959,7 +1892,6 @@ GCS_MAVLINK::queued_param_send() 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, @@ -1967,14 +1899,6 @@ GCS_MAVLINK::queued_param_send() 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++; } diff --git a/ArduPlane/Mavlink_compat.h b/ArduPlane/Mavlink_compat.h index a9884bf780..267c73832e 100644 --- a/ArduPlane/Mavlink_compat.h +++ b/ArduPlane/Mavlink_compat.h @@ -72,10 +72,101 @@ static uint8_t mav_var_type(AP_Meta_class::Type_id t) #else +static uint8_t mav_var_type(AP_Meta_class::Type_id t) +{ + return 0; +} + #define MAV_MISSION_ACCEPTED 0 #define MAV_MISSION_UNSUPPORTED 1 #define MAV_MISSION_UNSUPPORTED_FRAME 1 #define MAV_MISSION_ERROR 1 #define MAV_MISSION_INVALID_SEQUENCE 1 +/* + some functions have some extra params in MAVLink 1.0 + */ + +static void mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, + int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, + int16_t vz, uint16_t hdg) +{ + mavlink_msg_global_position_int_send( + chan, + lat, + lon, + alt, + vx, vy, vz); +} + +static void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, + int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, + int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, + int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ + mavlink_msg_rc_channels_scaled_send( + chan, + chan1_scaled, + chan2_scaled, + chan3_scaled, + chan4_scaled, + chan5_scaled, + chan6_scaled, + chan7_scaled, + chan8_scaled, + rssi); +} + +static void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, + uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, + uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, + uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ + mavlink_msg_rc_channels_raw_send( + chan, + chan1_raw, + chan2_raw, + chan3_raw, + chan4_raw, + chan5_raw, + chan6_raw, + chan7_raw, + chan8_raw, + rssi); +} + + +static void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, + uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, + uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, + uint16_t servo7_raw, uint16_t servo8_raw) +{ + mavlink_msg_servo_output_raw_send( + chan, + servo1_raw, + servo2_raw, + servo3_raw, + servo4_raw, + servo5_raw, + servo6_raw, + servo7_raw, + servo8_raw); +} + +static void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) +{ + mavlink_msg_statustext_send(chan, severity, (const int8_t*) text); +} + +static void mavlink_msg_param_value_send(mavlink_channel_t chan, const char *param_id, + float param_value, uint8_t param_type, + uint16_t param_count, uint16_t param_index) +{ + mavlink_msg_param_value_send( + chan, + (int8_t *)param_id, + param_value, + param_count, + param_index); +} #endif From 3f7d46b17e636bac24a95edaf08656dd7f5a20a2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Oct 2011 19:14:10 +1100 Subject: [PATCH 2/8] fixed uninitialised variable warning --- ArduPlane/commands_process.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 5ef18b36b1..a5dcffcc82 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -50,7 +50,7 @@ static void process_next_command() // and loads conditional or immediate commands if applicable struct Location temp; - byte old_index; + byte old_index = 0; // these are Navigation/Must commands // --------------------------------- From f6b736704e2abaa5958c29604c65936b4a16753f Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 30 Oct 2011 09:15:06 +0800 Subject: [PATCH 3/8] firmware build --- .../Firmware/AC2-HELHIL-1280.build.log | 65 +- .../Firmware/AC2-HELHIL-1280.size.txt | 70 ++- .../Firmware/AC2-HELHIL-2560.build.log | 65 +- .../Firmware/AC2-HELHIL-2560.size.txt | 70 ++- .../Firmware/AC2-Heli-1280.build.log | 37 +- .../Firmware/AC2-Heli-1280.size.txt | 72 ++- .../Firmware/AC2-Heli-2560.build.log | 37 +- .../Firmware/AC2-Heli-2560.size.txt | 72 ++- .../Firmware/firmware2.xml | 8 +- Tools/ArdupilotMegaPlanner/Firmware/git.log | 563 +++++++++++++++++- 10 files changed, 814 insertions(+), 245 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log index 10604a699c..3e86d02fc8 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log @@ -5,13 +5,15 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55: /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()': -/root/apm/ardupilot-mega/ArduCopter/Parameters.h:399: warning: overflow in implicit constant conversion +/root/apm/ardupilot-mega/ArduCopter/Parameters.h:407: warning: overflow in implicit constant conversion /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde: In function 'bool mavlink_try_send_message(mavlink_channel_t, ap_message, uint16_t)': -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:299: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:299: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:299: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch /root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'void heli_init_swash()': -/root/apm/ardupilot-mega/ArduCopter/heli.pde:60: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde:64: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'int heli_get_scaled_throttle(int)': +/root/apm/ardupilot-mega/ArduCopter/heli.pde:205: warning: unused variable 'scaled_throttle' /root/apm/ardupilot-mega/ArduCopter/setup.pde: In function 'int8_t setup_heli(uint8_t, const Menu::arg*)': /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function @@ -19,7 +21,9 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_roll' may be used uninitialized in this function -autogenerated: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:179: warning: 'int alt_hold_velocity()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:209: warning: 'int get_angle_boost(int)' defined but not used autogenerated:51: warning: 'void send_raw_imu1(mavlink_channel_t)' declared 'static' but never defined autogenerated:52: warning: 'void send_raw_imu2(mavlink_channel_t)' declared 'static' but never defined autogenerated:53: warning: 'void send_raw_imu3(mavlink_channel_t)' declared 'static' but never defined @@ -28,36 +32,39 @@ autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but nev autogenerated:111: warning: 'void Log_Write_Control_Tuning()' declared 'static' but never defined autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/Log.pde:776: warning: 'void Log_Write_Attitude()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/Log.pde:835: warning: 'void Log_Write_Attitude()' defined but not used autogenerated:121: warning: 'void decrement_WP_index()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used -autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used -autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined -autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined -autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined -autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used -autogenerated:246: warning: 'void ReadSCP1000()' declared 'static' but never defined -autogenerated:247: warning: 'void init_barometer()' declared 'static' but never defined -autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined -autogenerated:249: warning: 'long int read_barometer()' declared 'static' but never defined -autogenerated:250: warning: 'void read_airspeed()' declared 'static' but never defined -autogenerated:251: warning: 'void zero_airspeed()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined +autogenerated:215: warning: 'void debug_motors()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:95: warning: 'void calc_loiter2(int, int)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:213: warning: 'int get_loiter_angle()' defined but not used +autogenerated:237: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined +autogenerated:238: warning: 'long int cross_track_test()' declared 'static' but never defined +autogenerated:239: warning: 'void reset_crosstrack()' declared 'static' but never defined +autogenerated:241: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/radio.pde:134: warning: 'void throttle_failsafe(uint16_t)' defined but not used +autogenerated:250: warning: 'void ReadSCP1000()' declared 'static' but never defined +autogenerated:251: warning: 'void init_barometer()' declared 'static' but never defined +autogenerated:252: warning: 'long int read_baro_filtered()' declared 'static' but never defined +autogenerated:253: warning: 'long int read_barometer()' declared 'static' but never defined +autogenerated:254: warning: 'void read_airspeed()' declared 'static' but never defined +autogenerated:255: warning: 'void zero_airspeed()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/system.pde:456: warning: 'void set_failsafe(boolean)' defined but not used +autogenerated:290: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:297: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_min' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_max' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:344: warning: 'old_altitude' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:362: warning: 'abs_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:363: warning: 'ground_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:364: warning: 'ground_temperature' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:369: warning: 'baro_alt' defined but not used -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:347: warning: 'old_altitude' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:348: warning: 'old_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:366: warning: 'abs_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:367: warning: 'ground_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:368: warning: 'ground_temperature' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:373: warning: 'baro_alt' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:389: warning: 'angle_boost' defined but not used +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1727: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used /root/apm/ardupilot-mega/ArduCopter/test.pde:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:26: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined %% libraries/APM_BMP085/APM_BMP085.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.size.txt index e8146f2219..8c64ea4fd3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.size.txt @@ -10,6 +10,7 @@ 00000001 b land_complete 00000001 b throttle_mode 00000001 b command_may_ID +00000001 b mavlink_active 00000001 b wp_verify_byte 00000001 d altitude_sensor 00000001 b command_must_ID @@ -47,8 +48,10 @@ 00000002 b loiter_sum 00000002 b event_delay 00000002 b event_value +00000002 b CH7_wp_index 00000002 b event_repeat 00000002 b loiter_total +00000002 b manual_boost 00000002 b nav_throttle 00000002 b x_rate_error 00000002 b y_rate_error @@ -86,6 +89,7 @@ 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c 00000003 r print_enabled(unsigned char)::__c 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c +00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 d cos_roll_x 00000004 b land_start 00000004 b long_error @@ -124,6 +128,7 @@ 00000004 b fiftyhz_loopTimer 00000004 b old_target_bearing 00000004 b throttle_integrator +00000004 b heli_throttle_scaler 00000004 r __menu_name__log_menu 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing @@ -284,6 +289,8 @@ 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c 0000000a T setup 0000000b r test_relay(unsigned char, Menu::arg const*)::__c 0000000b r report_batt_monitor()::__c @@ -416,7 +423,6 @@ 00000014 r test_tri(unsigned char, Menu::arg const*)::__c 00000015 r setup_heli(unsigned char, Menu::arg const*)::__c 00000015 r init_ardupilot()::__c -00000015 r Log_Read_Motors()::__c 00000015 r print_hit_enter()::__c 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c 00000016 T piezo_beep() @@ -427,8 +433,8 @@ 00000018 r __menu_name__main_menu 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000019 r setup_gyro(unsigned char, Menu::arg const*)::__c +00000019 r Log_Read_Motors()::__c 0000001a r print_log_menu()::__c -0000001a r Log_Read_Nav_Tuning()::__c 0000001b r setup_heli(unsigned char, Menu::arg const*)::__c 0000001b r report_heli()::__c 0000001c W AP_VarS >::unserialize(void*, unsigned int) @@ -437,6 +443,7 @@ 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001d r Log_Read_Attitude()::__c +0000001d r Log_Read_Performance()::__c 0000001e r report_heli()::__c 0000001e r Log_Read_Optflow()::__c 00000020 t gcs_send_message(ap_message) @@ -446,10 +453,8 @@ 00000021 r print_log_menu()::__c 00000021 r report_compass()::__c 00000021 r Log_Read_Current()::__c -00000021 r Log_Read_Performance()::__c 00000022 t clear_leds() 00000022 t print_blanks(int) -00000022 t reset_hold_I() 00000022 W AP_Float16::~AP_Float16() 00000022 W AP_VarS >::~AP_VarS() 00000022 W AP_VarS >::~AP_VarS() @@ -459,13 +464,13 @@ 00000023 r setup_heli(unsigned char, Menu::arg const*)::__c 00000023 r setup_mode(unsigned char, Menu::arg const*)::__c 00000023 r print_gyro_offsets()::__c +00000024 t reset_hold_I() 00000024 r setup_heli(unsigned char, Menu::arg const*)::__c 00000024 r init_ardupilot()::__c 00000024 r print_accel_offsets()::__c 00000026 t print_done() 00000026 t print_hit_enter() 00000026 t Log_Read_Startup() -00000026 r Log_Read_Control_Tuning()::__c 00000028 t test_battery(unsigned char, Menu::arg const*) 00000028 t main_menu_help(unsigned char, Menu::arg const*) 00000028 t help_log(unsigned char, Menu::arg const*) @@ -476,14 +481,16 @@ 00000029 r setup_mode(unsigned char, Menu::arg const*)::__c 00000029 r test_gps(unsigned char, Menu::arg const*)::__c 0000002a t setup_declination(unsigned char, Menu::arg const*) +0000002a t _mav_put_int8_t_array 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c 0000002e t setup_motors(unsigned char, Menu::arg const*) 0000002e t print_divider() 0000002e t gcs_send_text_P(gcs_severity, prog_char_t const*) 0000002e t gcs_data_stream_send(unsigned int, unsigned int) 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char) +0000002e r init_ardupilot()::__c +0000002e r Log_Read_Nav_Tuning()::__c 0000002f r test_radio(unsigned char, Menu::arg const*)::__c -0000002f r init_ardupilot()::__c 00000030 t planner_mode(unsigned char, Menu::arg const*) 00000030 t send_heartbeat(mavlink_channel_t) 00000030 B imu @@ -496,19 +503,17 @@ 00000034 t _MAV_RETURN_float 00000034 t heli_get_servo(int) 00000034 W AP_Float16::serialize(void*, unsigned int) const -00000034 t _mav_put_int8_t_array 00000034 t mavlink_msg_statustext_send 00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c 00000036 t report_radio() +00000036 r Log_Read_Control_Tuning()::__c 00000037 r print_wp(Location*, unsigned char)::__c -00000038 t init_throttle_cruise() 00000038 t send_current_waypoint(mavlink_channel_t) 00000038 r setup_radio(unsigned char, Menu::arg const*)::__c 00000038 r setup_factory(unsigned char, Menu::arg const*)::__c 0000003a t report_gps() 0000003a t report_imu() 0000003a B g_gps_driver -0000003c W RC_Channel::~RC_Channel() 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*) 0000003e W AP_VarT::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char) 00000040 W AP_Float16::unserialize(void*, unsigned int) @@ -517,6 +522,7 @@ 00000042 t report_sonar() 00000042 r setup_heli(unsigned char, Menu::arg const*)::__c 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) +00000046 W RC_Channel::~RC_Channel() 00000048 t change_command(unsigned char) 00000048 t update_motor_leds() 00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c @@ -549,9 +555,9 @@ 00000064 t mavlink_msg_param_value_send 00000068 t zero_eeprom() 00000068 t find_last_log_page(int) +00000068 W GCS_MAVLINK::~GCS_MAVLINK() 0000006a T mavlink_send_text(mavlink_channel_t, gcs_severity, char const*) 0000006a t read_num_from_serial() -0000006a W GCS_MAVLINK::~GCS_MAVLINK() 0000006c t setup_sonar(unsigned char, Menu::arg const*) 00000074 t output_motors_armed() 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*) @@ -572,11 +578,11 @@ 00000092 t test_tuning(unsigned char, Menu::arg const*) 00000092 t report_tuning() 00000092 T GCS_MAVLINK::queued_param_send() +00000094 t init_throttle_cruise() 00000096 t print_wp(Location*, unsigned char) +00000098 B gcs0 +00000098 B gcs3 0000009a t planner_gcs(unsigned char, Menu::arg const*) -0000009a t Log_Read_Motors() -0000009b B gcs0 -0000009b B gcs3 0000009e t setup_mode(unsigned char, Menu::arg const*) 0000009e t Log_Write_Cmd(unsigned char, Location*) 000000a0 t Log_Read_Mode() @@ -585,12 +591,13 @@ 000000a4 T __vector_55 000000a6 t send_servo_out(mavlink_channel_t) 000000aa t test_sonar(unsigned char, Menu::arg const*) -000000aa t Log_Read_Nav_Tuning() 000000ab B compass +000000ae t Log_Read_Motors() 000000b0 t test_relay(unsigned char, Menu::arg const*) 000000b2 t erase_logs(unsigned char, Menu::arg const*) 000000b4 t read_radio() 000000b6 t get_log_boundaries(unsigned char, int&, int&) +000000b6 t Log_Read_Performance() 000000be t update_events() 000000c2 t setup_compass(unsigned char, Menu::arg const*) 000000c2 t send_radio_out(mavlink_channel_t) @@ -599,7 +606,6 @@ 000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c 000000c6 t test_eedump(unsigned char, Menu::arg const*) 000000c6 t send_radio_in(mavlink_channel_t) -000000c6 t Log_Read_Performance() 000000c6 t test_tri(unsigned char, Menu::arg const*) 000000d0 t get_bearing(Location*, Location*) 000000d2 t print_switch(unsigned char, unsigned char, bool) @@ -607,29 +613,31 @@ 000000d4 t get_stabilize_pitch(long) 000000d4 t Log_Read(int, int) 000000d8 t test_radio(unsigned char, Menu::arg const*) +000000de t Log_Read_Nav_Tuning() 000000e0 r test_menu_commands 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&) 000000e8 t Log_Read_Current() -000000ea t Log_Read_Control_Tuning() 000000ee t report_batt_monitor() 000000f6 t Log_Read_Cmd() +000000f8 t heli_get_angle_boost(int) +000000fa t calc_loiter_pitch_roll() 00000100 r setup_menu_commands +00000102 t Log_Read_Control_Tuning() 00000106 t setup_gyro(unsigned char, Menu::arg const*) 0000010a t mavlink_delay(unsigned long) 0000010a t test_gps(unsigned char, Menu::arg const*) 0000010c t test_current(unsigned char, Menu::arg const*) -0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000112 t set_next_WP(Location*) 00000112 t send_extended_status1(mavlink_channel_t, unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000118 t set_command_with_index(Location, int) 0000011c t get_command_with_index(int) -0000012c t calc_loiter_pitch_roll() 0000012e t arm_motors() 00000130 t report_compass() +00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) 00000148 t Log_Read_GPS() 00000156 t update_commands() 0000015c t update_trig() @@ -637,38 +645,38 @@ 00000160 t send_nav_controller_output(mavlink_channel_t) 00000166 t select_logs(unsigned char, Menu::arg const*) 00000166 t send_vfr_hud(mavlink_channel_t) -00000168 T GCS_MAVLINK::update() 0000016e t send_attitude(mavlink_channel_t) 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int) 00000178 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int) 00000184 t test_imu(unsigned char, Menu::arg const*) 00000188 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001a0 T GCS_MAVLINK::update() 000001a8 t print_radio_values() 000001cc t start_new_log() 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*) 000001e6 t verify_nav_wp() 000001ea t init_home() -00000216 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) +00000226 t set_mode(unsigned char) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000022a t send_gps_raw(mavlink_channel_t) 00000242 t calc_loiter(int, int) -0000030c t heli_init_swash() -00000312 W Parameters::~Parameters() +00000298 T update_throttle_mode() +00000326 W Parameters::~Parameters() 0000032a t report_heli() -00000330 t tuning() +0000033a t heli_init_swash() +0000036a t tuning() 00000384 t print_log_menu() -0000039a T update_throttle_mode() 0000039c t __static_initialization_and_destruction_0(int, int) +0000039e T update_roll_pitch_mode() 000003a0 t read_battery() 0000045c T update_yaw_mode() -0000046e T update_roll_pitch_mode() 0000052e t heli_move_swash(int, int, int, int) -00000622 t init_ardupilot() +00000600 t init_ardupilot() 0000071a t update_nav_wp() 000007ec t setup_heli(unsigned char, Menu::arg const*) 00000870 t process_next_command() -000009b8 W Parameters::Parameters() -00000a1f b g -0000100a T loop +00000a64 W Parameters::Parameters() +00000b2d b g +000010b6 T loop 00001604 T GCS_MAVLINK::handleMessage(__mavlink_message*) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log index 10604a699c..3e86d02fc8 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log @@ -5,13 +5,15 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55: /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()': -/root/apm/ardupilot-mega/ArduCopter/Parameters.h:399: warning: overflow in implicit constant conversion +/root/apm/ardupilot-mega/ArduCopter/Parameters.h:407: warning: overflow in implicit constant conversion /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde: In function 'bool mavlink_try_send_message(mavlink_channel_t, ap_message, uint16_t)': -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:299: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:299: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:299: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch /root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'void heli_init_swash()': -/root/apm/ardupilot-mega/ArduCopter/heli.pde:60: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde:64: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'int heli_get_scaled_throttle(int)': +/root/apm/ardupilot-mega/ArduCopter/heli.pde:205: warning: unused variable 'scaled_throttle' /root/apm/ardupilot-mega/ArduCopter/setup.pde: In function 'int8_t setup_heli(uint8_t, const Menu::arg*)': /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function @@ -19,7 +21,9 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_roll' may be used uninitialized in this function -autogenerated: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:179: warning: 'int alt_hold_velocity()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:209: warning: 'int get_angle_boost(int)' defined but not used autogenerated:51: warning: 'void send_raw_imu1(mavlink_channel_t)' declared 'static' but never defined autogenerated:52: warning: 'void send_raw_imu2(mavlink_channel_t)' declared 'static' but never defined autogenerated:53: warning: 'void send_raw_imu3(mavlink_channel_t)' declared 'static' but never defined @@ -28,36 +32,39 @@ autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but nev autogenerated:111: warning: 'void Log_Write_Control_Tuning()' declared 'static' but never defined autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/Log.pde:776: warning: 'void Log_Write_Attitude()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/Log.pde:835: warning: 'void Log_Write_Attitude()' defined but not used autogenerated:121: warning: 'void decrement_WP_index()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used -autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used -autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined -autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined -autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined -autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used -autogenerated:246: warning: 'void ReadSCP1000()' declared 'static' but never defined -autogenerated:247: warning: 'void init_barometer()' declared 'static' but never defined -autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined -autogenerated:249: warning: 'long int read_barometer()' declared 'static' but never defined -autogenerated:250: warning: 'void read_airspeed()' declared 'static' but never defined -autogenerated:251: warning: 'void zero_airspeed()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined +autogenerated:215: warning: 'void debug_motors()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:95: warning: 'void calc_loiter2(int, int)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:213: warning: 'int get_loiter_angle()' defined but not used +autogenerated:237: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined +autogenerated:238: warning: 'long int cross_track_test()' declared 'static' but never defined +autogenerated:239: warning: 'void reset_crosstrack()' declared 'static' but never defined +autogenerated:241: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/radio.pde:134: warning: 'void throttle_failsafe(uint16_t)' defined but not used +autogenerated:250: warning: 'void ReadSCP1000()' declared 'static' but never defined +autogenerated:251: warning: 'void init_barometer()' declared 'static' but never defined +autogenerated:252: warning: 'long int read_baro_filtered()' declared 'static' but never defined +autogenerated:253: warning: 'long int read_barometer()' declared 'static' but never defined +autogenerated:254: warning: 'void read_airspeed()' declared 'static' but never defined +autogenerated:255: warning: 'void zero_airspeed()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/system.pde:456: warning: 'void set_failsafe(boolean)' defined but not used +autogenerated:290: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:297: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_min' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_max' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:344: warning: 'old_altitude' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:362: warning: 'abs_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:363: warning: 'ground_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:364: warning: 'ground_temperature' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:369: warning: 'baro_alt' defined but not used -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:347: warning: 'old_altitude' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:348: warning: 'old_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:366: warning: 'abs_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:367: warning: 'ground_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:368: warning: 'ground_temperature' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:373: warning: 'baro_alt' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:389: warning: 'angle_boost' defined but not used +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1727: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used /root/apm/ardupilot-mega/ArduCopter/test.pde:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:26: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined %% libraries/APM_BMP085/APM_BMP085.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.size.txt index c3ae322200..d90c94c76f 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.size.txt @@ -10,6 +10,7 @@ 00000001 b land_complete 00000001 b throttle_mode 00000001 b command_may_ID +00000001 b mavlink_active 00000001 b wp_verify_byte 00000001 d altitude_sensor 00000001 b command_must_ID @@ -47,8 +48,10 @@ 00000002 b loiter_sum 00000002 b event_delay 00000002 b event_value +00000002 b CH7_wp_index 00000002 b event_repeat 00000002 b loiter_total +00000002 b manual_boost 00000002 b nav_throttle 00000002 b x_rate_error 00000002 b y_rate_error @@ -86,6 +89,7 @@ 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c 00000003 r print_enabled(unsigned char)::__c 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c +00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 d cos_roll_x 00000004 b land_start 00000004 b long_error @@ -124,6 +128,7 @@ 00000004 b fiftyhz_loopTimer 00000004 b old_target_bearing 00000004 b throttle_integrator +00000004 b heli_throttle_scaler 00000004 r __menu_name__log_menu 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing @@ -284,6 +289,8 @@ 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c 0000000a T setup 0000000b r test_relay(unsigned char, Menu::arg const*)::__c 0000000b r report_batt_monitor()::__c @@ -416,7 +423,6 @@ 00000014 r test_tri(unsigned char, Menu::arg const*)::__c 00000015 r setup_heli(unsigned char, Menu::arg const*)::__c 00000015 r init_ardupilot()::__c -00000015 r Log_Read_Motors()::__c 00000015 r print_hit_enter()::__c 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c 00000016 T piezo_beep() @@ -427,8 +433,8 @@ 00000018 r __menu_name__main_menu 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000019 r setup_gyro(unsigned char, Menu::arg const*)::__c +00000019 r Log_Read_Motors()::__c 0000001a r print_log_menu()::__c -0000001a r Log_Read_Nav_Tuning()::__c 0000001b r setup_heli(unsigned char, Menu::arg const*)::__c 0000001b r report_heli()::__c 0000001c W AP_VarS >::unserialize(void*, unsigned int) @@ -437,6 +443,7 @@ 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001d r Log_Read_Attitude()::__c +0000001d r Log_Read_Performance()::__c 0000001e r report_heli()::__c 0000001e r Log_Read_Optflow()::__c 00000020 t gcs_send_message(ap_message) @@ -446,10 +453,8 @@ 00000021 r print_log_menu()::__c 00000021 r report_compass()::__c 00000021 r Log_Read_Current()::__c -00000021 r Log_Read_Performance()::__c 00000022 t clear_leds() 00000022 t print_blanks(int) -00000022 t reset_hold_I() 00000022 W AP_Float16::~AP_Float16() 00000022 W AP_VarS >::~AP_VarS() 00000022 W AP_VarS >::~AP_VarS() @@ -459,13 +464,13 @@ 00000023 r setup_heli(unsigned char, Menu::arg const*)::__c 00000023 r setup_mode(unsigned char, Menu::arg const*)::__c 00000023 r print_gyro_offsets()::__c +00000024 t reset_hold_I() 00000024 r setup_heli(unsigned char, Menu::arg const*)::__c 00000024 r init_ardupilot()::__c 00000024 r print_accel_offsets()::__c 00000026 t print_done() 00000026 t print_hit_enter() 00000026 t Log_Read_Startup() -00000026 r Log_Read_Control_Tuning()::__c 00000028 t test_battery(unsigned char, Menu::arg const*) 00000028 t main_menu_help(unsigned char, Menu::arg const*) 00000028 t help_log(unsigned char, Menu::arg const*) @@ -476,14 +481,16 @@ 00000029 r setup_mode(unsigned char, Menu::arg const*)::__c 00000029 r test_gps(unsigned char, Menu::arg const*)::__c 0000002a t setup_declination(unsigned char, Menu::arg const*) +0000002a t _mav_put_int8_t_array 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c 0000002e t setup_motors(unsigned char, Menu::arg const*) 0000002e t print_divider() 0000002e t gcs_send_text_P(gcs_severity, prog_char_t const*) 0000002e t gcs_data_stream_send(unsigned int, unsigned int) 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char) +0000002e r init_ardupilot()::__c +0000002e r Log_Read_Nav_Tuning()::__c 0000002f r test_radio(unsigned char, Menu::arg const*)::__c -0000002f r init_ardupilot()::__c 00000030 t planner_mode(unsigned char, Menu::arg const*) 00000030 t send_heartbeat(mavlink_channel_t) 00000030 B imu @@ -496,19 +503,17 @@ 00000034 t _MAV_RETURN_float 00000034 t heli_get_servo(int) 00000034 W AP_Float16::serialize(void*, unsigned int) const -00000034 t _mav_put_int8_t_array 00000034 t mavlink_msg_statustext_send 00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c 00000036 t report_radio() +00000036 r Log_Read_Control_Tuning()::__c 00000037 r print_wp(Location*, unsigned char)::__c -00000038 t init_throttle_cruise() 00000038 t send_current_waypoint(mavlink_channel_t) 00000038 r setup_radio(unsigned char, Menu::arg const*)::__c 00000038 r setup_factory(unsigned char, Menu::arg const*)::__c 0000003a t report_gps() 0000003a t report_imu() 0000003a B g_gps_driver -0000003c W RC_Channel::~RC_Channel() 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*) 0000003e W AP_VarT::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char) 00000040 W AP_Float16::unserialize(void*, unsigned int) @@ -517,6 +522,7 @@ 00000042 t report_sonar() 00000042 r setup_heli(unsigned char, Menu::arg const*)::__c 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) +00000046 W RC_Channel::~RC_Channel() 00000048 t change_command(unsigned char) 00000048 t update_motor_leds() 00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c @@ -549,9 +555,9 @@ 00000064 t mavlink_msg_param_value_send 00000068 t zero_eeprom() 00000068 t find_last_log_page(int) +00000068 W GCS_MAVLINK::~GCS_MAVLINK() 0000006a T mavlink_send_text(mavlink_channel_t, gcs_severity, char const*) 0000006a t read_num_from_serial() -0000006a W GCS_MAVLINK::~GCS_MAVLINK() 0000006c t setup_sonar(unsigned char, Menu::arg const*) 00000074 t output_motors_armed() 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*) @@ -572,11 +578,11 @@ 00000090 t report_tuning() 00000092 t test_tuning(unsigned char, Menu::arg const*) 00000092 T GCS_MAVLINK::queued_param_send() +00000094 t init_throttle_cruise() 00000096 t print_wp(Location*, unsigned char) +00000098 B gcs0 +00000098 B gcs3 0000009a t planner_gcs(unsigned char, Menu::arg const*) -0000009a t Log_Read_Motors() -0000009b B gcs0 -0000009b B gcs3 0000009e t setup_mode(unsigned char, Menu::arg const*) 0000009e t Log_Read_Mode() 0000009e t Log_Write_Cmd(unsigned char, Location*) @@ -585,12 +591,13 @@ 000000a4 T __vector_55 000000a6 t send_servo_out(mavlink_channel_t) 000000a8 t test_sonar(unsigned char, Menu::arg const*) -000000aa t Log_Read_Nav_Tuning() 000000ab B compass +000000ae t Log_Read_Motors() 000000b0 t test_relay(unsigned char, Menu::arg const*) 000000b2 t erase_logs(unsigned char, Menu::arg const*) 000000b4 t read_radio() 000000b6 t get_log_boundaries(unsigned char, int&, int&) +000000b6 t Log_Read_Performance() 000000be t update_events() 000000c2 t test_eedump(unsigned char, Menu::arg const*) 000000c2 t setup_compass(unsigned char, Menu::arg const*) @@ -599,7 +606,6 @@ 000000c4 t get_distance(Location*, Location*) 000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c 000000c6 t send_radio_in(mavlink_channel_t) -000000c6 t Log_Read_Performance() 000000c6 t test_tri(unsigned char, Menu::arg const*) 000000d0 t get_bearing(Location*, Location*) 000000d0 t print_switch(unsigned char, unsigned char, bool) @@ -607,29 +613,31 @@ 000000d4 t get_stabilize_pitch(long) 000000d4 t Log_Read(int, int) 000000d8 t test_radio(unsigned char, Menu::arg const*) +000000de t Log_Read_Nav_Tuning() 000000e0 r test_menu_commands 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&) 000000e8 t Log_Read_Current() -000000ea t Log_Read_Control_Tuning() 000000ee t report_batt_monitor() 000000f6 t Log_Read_Cmd() +000000f8 t heli_get_angle_boost(int) +000000fa t calc_loiter_pitch_roll() 00000100 r setup_menu_commands +00000102 t Log_Read_Control_Tuning() 00000108 t setup_gyro(unsigned char, Menu::arg const*) 0000010a t mavlink_delay(unsigned long) 0000010a t test_gps(unsigned char, Menu::arg const*) 0000010c t test_current(unsigned char, Menu::arg const*) -0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000112 t set_next_WP(Location*) 00000112 t send_extended_status1(mavlink_channel_t, unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000118 t set_command_with_index(Location, int) 0000011c t get_command_with_index(int) -0000012c t calc_loiter_pitch_roll() 0000012e t arm_motors() 00000130 t report_compass() +00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) 00000148 t Log_Read_GPS() 00000156 t update_commands() 0000015c t update_trig() @@ -637,38 +645,38 @@ 00000160 t send_nav_controller_output(mavlink_channel_t) 00000166 t select_logs(unsigned char, Menu::arg const*) 00000166 t send_vfr_hud(mavlink_channel_t) -00000168 T GCS_MAVLINK::update() 0000016e t send_attitude(mavlink_channel_t) 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int) 00000178 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int) 00000184 t test_imu(unsigned char, Menu::arg const*) 00000188 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001a0 T GCS_MAVLINK::update() 000001a8 t print_radio_values() 000001cc t start_new_log() 000001e4 t verify_nav_wp() 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*) 000001ea t init_home() -00000216 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) +00000226 t set_mode(unsigned char) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000022a t send_gps_raw(mavlink_channel_t) 00000242 t calc_loiter(int, int) -0000030c t heli_init_swash() -00000312 W Parameters::~Parameters() +00000298 T update_throttle_mode() +00000326 W Parameters::~Parameters() 00000328 t report_heli() -00000330 t tuning() +0000033a t heli_init_swash() +0000036a t tuning() 00000382 t print_log_menu() -0000039a T update_throttle_mode() 0000039c t __static_initialization_and_destruction_0(int, int) +0000039e T update_roll_pitch_mode() 000003a0 t read_battery() 0000045c T update_yaw_mode() -0000046e T update_roll_pitch_mode() 0000052e t heli_move_swash(int, int, int, int) -00000620 t init_ardupilot() +000005fe t init_ardupilot() 0000071a t update_nav_wp() 000007e8 t setup_heli(unsigned char, Menu::arg const*) 00000870 t process_next_command() -000009b8 W Parameters::Parameters() -00000a1f b g -0000100a T loop +00000a64 W Parameters::Parameters() +00000b2d b g +000010b6 T loop 00001604 T GCS_MAVLINK::handleMessage(__mavlink_message*) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log index d420f23d8f..e2ae2c5243 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log @@ -5,17 +5,21 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55: /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()': -/root/apm/ardupilot-mega/ArduCopter/Parameters.h:399: warning: overflow in implicit constant conversion +/root/apm/ardupilot-mega/ArduCopter/Parameters.h:407: warning: overflow in implicit constant conversion /root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'void heli_init_swash()': -/root/apm/ardupilot-mega/ArduCopter/heli.pde:60: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde:64: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'int heli_get_scaled_throttle(int)': +/root/apm/ardupilot-mega/ArduCopter/heli.pde:205: warning: unused variable 'scaled_throttle' /root/apm/ardupilot-mega/ArduCopter/setup.pde: In function 'int8_t setup_heli(uint8_t, const Menu::arg*)': -/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function +/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_roll' may be used uninitialized in this function -autogenerated: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:179: warning: 'int alt_hold_velocity()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:209: warning: 'int get_angle_boost(int)' defined but not used autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined @@ -23,24 +27,25 @@ autogenerated:121: warning: 'void decrement_WP_index()' declared 'static' but ne /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used -autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used -autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined -autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined -autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined -autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used +autogenerated:215: warning: 'void debug_motors()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:95: warning: 'void calc_loiter2(int, int)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:213: warning: 'int get_loiter_angle()' defined but not used +autogenerated:237: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined +autogenerated:238: warning: 'long int cross_track_test()' declared 'static' but never defined +autogenerated:239: warning: 'void reset_crosstrack()' declared 'static' but never defined +autogenerated:241: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/radio.pde:134: warning: 'void throttle_failsafe(uint16_t)' defined but not used /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used -autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined +autogenerated:252: warning: 'long int read_baro_filtered()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/system.pde:456: warning: 'void set_failsafe(boolean)' defined but not used +autogenerated:290: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:297: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_min' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_max' defined but not used -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1727: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_BMP085/APM_BMP085_hil.o %% libraries/APM_PI/APM_PI.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt index 41607607b2..4f8bcc3fd3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt @@ -10,6 +10,7 @@ 00000001 b land_complete 00000001 b throttle_mode 00000001 b command_may_ID +00000001 b mavlink_active 00000001 b wp_verify_byte 00000001 d altitude_sensor 00000001 b command_must_ID @@ -45,10 +46,13 @@ 00000002 T userhook_init() 00000002 b climb_rate 00000002 b loiter_sum +00000002 b angle_boost 00000002 b event_delay 00000002 b event_value +00000002 b CH7_wp_index 00000002 b event_repeat 00000002 b loiter_total +00000002 b manual_boost 00000002 b nav_throttle 00000002 b x_rate_error 00000002 b y_rate_error @@ -71,6 +75,7 @@ 00000002 b g_gps 00000002 b airspeed 00000002 b baro_alt +00000002 b old_rate 00000002 b sonar_alt 00000002 b arm_motors()::arming_counter 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c @@ -85,6 +90,7 @@ 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c 00000003 r print_enabled(unsigned char)::__c 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c +00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 d cos_roll_x 00000004 b land_start 00000004 b long_error @@ -125,6 +131,7 @@ 00000004 b fiftyhz_loopTimer 00000004 b old_target_bearing 00000004 b throttle_integrator +00000004 b heli_throttle_scaler 00000004 r __menu_name__log_menu 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing @@ -287,6 +294,8 @@ 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c 0000000a T setup 0000000b r test_relay(unsigned char, Menu::arg const*)::__c 0000000b r report_batt_monitor()::__c @@ -418,7 +427,6 @@ 00000014 r test_tri(unsigned char, Menu::arg const*)::__c 00000015 r setup_heli(unsigned char, Menu::arg const*)::__c 00000015 r init_ardupilot()::__c -00000015 r Log_Read_Motors()::__c 00000015 r print_hit_enter()::__c 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c 00000016 T piezo_beep() @@ -429,8 +437,8 @@ 00000018 r __menu_name__main_menu 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000019 r setup_gyro(unsigned char, Menu::arg const*)::__c +00000019 r Log_Read_Motors()::__c 0000001a r print_log_menu()::__c -0000001a r Log_Read_Nav_Tuning()::__c 0000001b r setup_heli(unsigned char, Menu::arg const*)::__c 0000001b r report_heli()::__c 0000001c W AP_VarA::unserialize(void*, unsigned int) @@ -441,6 +449,7 @@ 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001d r Log_Read_Attitude()::__c +0000001d r Log_Read_Performance()::__c 0000001e r report_heli()::__c 0000001e r Log_Read_Optflow()::__c 00000020 t gcs_send_message(ap_message) @@ -450,10 +459,8 @@ 00000021 r print_log_menu()::__c 00000021 r report_compass()::__c 00000021 r Log_Read_Current()::__c -00000021 r Log_Read_Performance()::__c 00000022 t clear_leds() 00000022 t print_blanks(int) -00000022 t reset_hold_I() 00000022 t startup_ground() 00000022 W AP_Float16::~AP_Float16() 00000022 W AP_VarA::~AP_VarA() @@ -465,13 +472,13 @@ 00000023 r setup_heli(unsigned char, Menu::arg const*)::__c 00000023 r setup_mode(unsigned char, Menu::arg const*)::__c 00000023 r print_gyro_offsets()::__c +00000024 t reset_hold_I() 00000024 r setup_heli(unsigned char, Menu::arg const*)::__c 00000024 r init_ardupilot()::__c 00000024 r print_accel_offsets()::__c 00000026 t print_done() 00000026 t print_hit_enter() 00000026 t Log_Read_Startup() -00000026 r Log_Read_Control_Tuning()::__c 00000028 t test_battery(unsigned char, Menu::arg const*) 00000028 t main_menu_help(unsigned char, Menu::arg const*) 00000028 t help_log(unsigned char, Menu::arg const*) @@ -482,14 +489,16 @@ 00000029 r setup_mode(unsigned char, Menu::arg const*)::__c 00000029 r test_gps(unsigned char, Menu::arg const*)::__c 0000002a t setup_declination(unsigned char, Menu::arg const*) +0000002a t _mav_put_int8_t_array 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c 0000002e t setup_motors(unsigned char, Menu::arg const*) 0000002e t print_divider() 0000002e t gcs_send_text_P(gcs_severity, prog_char_t const*) 0000002e t gcs_data_stream_send(unsigned int, unsigned int) 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char) +0000002e r init_ardupilot()::__c +0000002e r Log_Read_Nav_Tuning()::__c 0000002f r test_radio(unsigned char, Menu::arg const*)::__c -0000002f r init_ardupilot()::__c 00000030 t planner_mode(unsigned char, Menu::arg const*) 00000030 t send_heartbeat(mavlink_channel_t) 00000031 r setup_heli(unsigned char, Menu::arg const*)::__c @@ -500,18 +509,16 @@ 00000033 b pending_status 00000034 t heli_get_servo(int) 00000034 W AP_Float16::serialize(void*, unsigned int) const -00000034 t _mav_put_int8_t_array 00000034 t mavlink_msg_statustext_send 00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c 00000036 t report_radio() +00000036 r Log_Read_Control_Tuning()::__c 00000037 r print_wp(Location*, unsigned char)::__c -00000038 t init_throttle_cruise() 00000038 t send_current_waypoint(mavlink_channel_t) 00000038 r setup_radio(unsigned char, Menu::arg const*)::__c 00000038 r setup_factory(unsigned char, Menu::arg const*)::__c 0000003a t report_gps() 0000003a t report_imu() -0000003c W RC_Channel::~RC_Channel() 0000003d B g_gps_driver 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*) 0000003e W AP_VarT::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char) @@ -523,6 +530,7 @@ 00000042 t report_sonar() 00000042 r setup_heli(unsigned char, Menu::arg const*)::__c 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) +00000046 W RC_Channel::~RC_Channel() 00000048 t change_command(unsigned char) 00000048 t update_motor_leds() 00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c @@ -553,9 +561,9 @@ 00000064 t mavlink_msg_param_value_send 00000068 t zero_eeprom() 00000068 t find_last_log_page(int) +00000068 W GCS_MAVLINK::~GCS_MAVLINK() 0000006a T mavlink_send_text(mavlink_channel_t, gcs_severity, char const*) 0000006a t read_num_from_serial() -0000006a W GCS_MAVLINK::~GCS_MAVLINK() 0000006c t setup_sonar(unsigned char, Menu::arg const*) 00000074 t output_motors_armed() 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*) @@ -579,12 +587,12 @@ 00000092 t test_tuning(unsigned char, Menu::arg const*) 00000092 t report_tuning() 00000092 T GCS_MAVLINK::queued_param_send() +00000094 t init_throttle_cruise() 00000096 t print_wp(Location*, unsigned char) +00000098 B gcs0 +00000098 B gcs3 0000009a t planner_gcs(unsigned char, Menu::arg const*) 0000009a t init_compass() -0000009a t Log_Read_Motors() -0000009b B gcs0 -0000009b B gcs3 0000009e t setup_mode(unsigned char, Menu::arg const*) 0000009e t Log_Write_Cmd(unsigned char, Location*) 000000a0 t Log_Read_Mode() @@ -593,11 +601,12 @@ 000000a4 T __vector_55 000000a6 t send_servo_out(mavlink_channel_t) 000000aa t test_sonar(unsigned char, Menu::arg const*) -000000aa t Log_Read_Nav_Tuning() +000000ae t Log_Read_Motors() 000000b0 t test_relay(unsigned char, Menu::arg const*) 000000b2 t erase_logs(unsigned char, Menu::arg const*) 000000b4 t read_radio() 000000b6 t get_log_boundaries(unsigned char, int&, int&) +000000b6 t Log_Read_Performance() 000000b7 B compass 000000be t update_events() 000000c2 t setup_compass(unsigned char, Menu::arg const*) @@ -607,7 +616,6 @@ 000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c 000000c6 t test_eedump(unsigned char, Menu::arg const*) 000000c6 t send_radio_in(mavlink_channel_t) -000000c6 t Log_Read_Performance() 000000c6 t test_tri(unsigned char, Menu::arg const*) 000000c7 B dcm 000000ca t init_barometer() @@ -618,30 +626,32 @@ 000000d4 t Log_Read(int, int) 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() +000000de t Log_Read_Nav_Tuning() 000000de t test_adc(unsigned char, Menu::arg const*) 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&) 000000e8 t Log_Read_Current() -000000ea t Log_Read_Control_Tuning() 000000ee t report_batt_monitor() 000000f6 t Log_Read_Cmd() +000000f8 t heli_get_angle_boost(int) +000000fa t calc_loiter_pitch_roll() 00000100 r test_menu_commands 00000100 r setup_menu_commands +00000102 t Log_Read_Control_Tuning() 00000106 t setup_gyro(unsigned char, Menu::arg const*) 0000010a t mavlink_delay(unsigned long) 0000010a t send_raw_imu2(mavlink_channel_t) 0000010a t test_gps(unsigned char, Menu::arg const*) 0000010c t test_current(unsigned char, Menu::arg const*) -0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000112 t set_next_WP(Location*) 00000112 t send_extended_status1(mavlink_channel_t, unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000118 t set_command_with_index(Location, int) 0000011c t get_command_with_index(int) -0000012c t calc_loiter_pitch_roll() 00000130 t report_compass() +00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) 00000148 t Log_Read_GPS() 00000156 t update_commands() 0000015c t update_trig() @@ -650,10 +660,10 @@ 00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) 00000166 t select_logs(unsigned char, Menu::arg const*) 00000166 t send_vfr_hud(mavlink_channel_t) -00000168 T GCS_MAVLINK::update() 0000016c t test_imu(unsigned char, Menu::arg const*) 0000016e t send_attitude(mavlink_channel_t) 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int) +000001a0 T GCS_MAVLINK::update() 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int) 000001a8 t print_radio_values() 000001be t arm_motors() @@ -661,29 +671,29 @@ 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*) 000001e6 t verify_nav_wp() 000001ea t init_home() -00000216 t set_mode(unsigned char) 0000021a t send_raw_imu1(mavlink_channel_t) 00000220 t test_wp(unsigned char, Menu::arg const*) +00000226 t set_mode(unsigned char) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000022a t send_gps_raw(mavlink_channel_t) 00000242 t calc_loiter(int, int) 00000268 t send_raw_imu3(mavlink_channel_t) -0000030c t heli_init_swash() -00000312 W Parameters::~Parameters() +00000298 T update_throttle_mode() +00000326 W Parameters::~Parameters() 0000032a t report_heli() -00000330 t tuning() +0000033a t heli_init_swash() +0000036a t tuning() 00000384 t print_log_menu() -0000039a T update_throttle_mode() +0000039e T update_roll_pitch_mode() 000003a0 t read_battery() 0000045c T update_yaw_mode() -0000046e T update_roll_pitch_mode() 0000053e t heli_move_swash(int, int, int, int) 000005cc t __static_initialization_and_destruction_0(int, int) -00000620 t init_ardupilot() +000005fe t init_ardupilot() 0000071a t update_nav_wp() 000007ec t setup_heli(unsigned char, Menu::arg const*) 00000870 t process_next_command() -000009b8 W Parameters::Parameters() -00000a1f b g +00000a64 W Parameters::Parameters() +00000b2d b g 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*) -00001598 T loop +000016c8 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log index d420f23d8f..e2ae2c5243 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log @@ -5,17 +5,21 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55: /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()': -/root/apm/ardupilot-mega/ArduCopter/Parameters.h:399: warning: overflow in implicit constant conversion +/root/apm/ardupilot-mega/ArduCopter/Parameters.h:407: warning: overflow in implicit constant conversion /root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'void heli_init_swash()': -/root/apm/ardupilot-mega/ArduCopter/heli.pde:60: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde:64: warning: comparisons like X<=Y<=Z do not have their mathematical meaning +/root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'int heli_get_scaled_throttle(int)': +/root/apm/ardupilot-mega/ArduCopter/heli.pde:205: warning: unused variable 'scaled_throttle' /root/apm/ardupilot-mega/ArduCopter/setup.pde: In function 'int8_t setup_heli(uint8_t, const Menu::arg*)': -/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function +/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_roll' may be used uninitialized in this function -autogenerated: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde: At global scope: +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:179: warning: 'int alt_hold_velocity()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:209: warning: 'int get_angle_boost(int)' defined but not used autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined @@ -23,24 +27,25 @@ autogenerated:121: warning: 'void decrement_WP_index()' declared 'static' but ne /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used -autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used -autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined -autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined -autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined -autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used +autogenerated:215: warning: 'void debug_motors()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:95: warning: 'void calc_loiter2(int, int)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/navigation.pde:213: warning: 'int get_loiter_angle()' defined but not used +autogenerated:237: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined +autogenerated:238: warning: 'long int cross_track_test()' declared 'static' but never defined +autogenerated:239: warning: 'void reset_crosstrack()' declared 'static' but never defined +autogenerated:241: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/radio.pde:134: warning: 'void throttle_failsafe(uint16_t)' defined but not used /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used -autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined +autogenerated:252: warning: 'long int read_baro_filtered()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/system.pde:456: warning: 'void set_failsafe(boolean)' defined but not used +autogenerated:290: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:297: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_min' defined but not used /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:277: warning: 'heli_servo_max' defined but not used -/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1727: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_BMP085/APM_BMP085_hil.o %% libraries/APM_PI/APM_PI.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt index 0f00a831cc..5f1c4d5346 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt @@ -10,6 +10,7 @@ 00000001 b land_complete 00000001 b throttle_mode 00000001 b command_may_ID +00000001 b mavlink_active 00000001 b wp_verify_byte 00000001 d altitude_sensor 00000001 b command_must_ID @@ -45,10 +46,13 @@ 00000002 T userhook_init() 00000002 b climb_rate 00000002 b loiter_sum +00000002 b angle_boost 00000002 b event_delay 00000002 b event_value +00000002 b CH7_wp_index 00000002 b event_repeat 00000002 b loiter_total +00000002 b manual_boost 00000002 b nav_throttle 00000002 b x_rate_error 00000002 b y_rate_error @@ -71,6 +75,7 @@ 00000002 b g_gps 00000002 b airspeed 00000002 b baro_alt +00000002 b old_rate 00000002 b sonar_alt 00000002 b arm_motors()::arming_counter 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c @@ -85,6 +90,7 @@ 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c 00000003 r print_enabled(unsigned char)::__c 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c +00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 d cos_roll_x 00000004 b land_start 00000004 b long_error @@ -125,6 +131,7 @@ 00000004 b fiftyhz_loopTimer 00000004 b old_target_bearing 00000004 b throttle_integrator +00000004 b heli_throttle_scaler 00000004 r __menu_name__log_menu 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing @@ -287,6 +294,8 @@ 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c 0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c +0000000a V Parameters::Parameters()::__c 0000000a T setup 0000000b r test_relay(unsigned char, Menu::arg const*)::__c 0000000b r report_batt_monitor()::__c @@ -418,7 +427,6 @@ 00000014 r test_tri(unsigned char, Menu::arg const*)::__c 00000015 r setup_heli(unsigned char, Menu::arg const*)::__c 00000015 r init_ardupilot()::__c -00000015 r Log_Read_Motors()::__c 00000015 r print_hit_enter()::__c 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c 00000016 T piezo_beep() @@ -429,8 +437,8 @@ 00000018 r __menu_name__main_menu 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000019 r setup_gyro(unsigned char, Menu::arg const*)::__c +00000019 r Log_Read_Motors()::__c 0000001a r print_log_menu()::__c -0000001a r Log_Read_Nav_Tuning()::__c 0000001b r setup_heli(unsigned char, Menu::arg const*)::__c 0000001b r report_heli()::__c 0000001c W AP_VarA::unserialize(void*, unsigned int) @@ -441,6 +449,7 @@ 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001c W AP_VarS >::serialize(void*, unsigned int) const 0000001d r Log_Read_Attitude()::__c +0000001d r Log_Read_Performance()::__c 0000001e r report_heli()::__c 0000001e r Log_Read_Optflow()::__c 00000020 t gcs_send_message(ap_message) @@ -450,10 +459,8 @@ 00000021 r print_log_menu()::__c 00000021 r report_compass()::__c 00000021 r Log_Read_Current()::__c -00000021 r Log_Read_Performance()::__c 00000022 t clear_leds() 00000022 t print_blanks(int) -00000022 t reset_hold_I() 00000022 t startup_ground() 00000022 W AP_Float16::~AP_Float16() 00000022 W AP_VarA::~AP_VarA() @@ -465,13 +472,13 @@ 00000023 r setup_heli(unsigned char, Menu::arg const*)::__c 00000023 r setup_mode(unsigned char, Menu::arg const*)::__c 00000023 r print_gyro_offsets()::__c +00000024 t reset_hold_I() 00000024 r setup_heli(unsigned char, Menu::arg const*)::__c 00000024 r init_ardupilot()::__c 00000024 r print_accel_offsets()::__c 00000026 t print_done() 00000026 t print_hit_enter() 00000026 t Log_Read_Startup() -00000026 r Log_Read_Control_Tuning()::__c 00000028 t test_battery(unsigned char, Menu::arg const*) 00000028 t main_menu_help(unsigned char, Menu::arg const*) 00000028 t help_log(unsigned char, Menu::arg const*) @@ -482,14 +489,16 @@ 00000029 r setup_mode(unsigned char, Menu::arg const*)::__c 00000029 r test_gps(unsigned char, Menu::arg const*)::__c 0000002a t setup_declination(unsigned char, Menu::arg const*) +0000002a t _mav_put_int8_t_array 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c 0000002e t setup_motors(unsigned char, Menu::arg const*) 0000002e t print_divider() 0000002e t gcs_send_text_P(gcs_severity, prog_char_t const*) 0000002e t gcs_data_stream_send(unsigned int, unsigned int) 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char) +0000002e r init_ardupilot()::__c +0000002e r Log_Read_Nav_Tuning()::__c 0000002f r test_radio(unsigned char, Menu::arg const*)::__c -0000002f r init_ardupilot()::__c 00000030 t planner_mode(unsigned char, Menu::arg const*) 00000030 t send_heartbeat(mavlink_channel_t) 00000031 r setup_heli(unsigned char, Menu::arg const*)::__c @@ -500,18 +509,16 @@ 00000033 b pending_status 00000034 t heli_get_servo(int) 00000034 W AP_Float16::serialize(void*, unsigned int) const -00000034 t _mav_put_int8_t_array 00000034 t mavlink_msg_statustext_send 00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c 00000036 t report_radio() +00000036 r Log_Read_Control_Tuning()::__c 00000037 r print_wp(Location*, unsigned char)::__c -00000038 t init_throttle_cruise() 00000038 t send_current_waypoint(mavlink_channel_t) 00000038 r setup_radio(unsigned char, Menu::arg const*)::__c 00000038 r setup_factory(unsigned char, Menu::arg const*)::__c 0000003a t report_gps() 0000003a t report_imu() -0000003c W RC_Channel::~RC_Channel() 0000003d B g_gps_driver 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*) 0000003e W AP_VarT::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char) @@ -523,6 +530,7 @@ 00000042 t report_sonar() 00000042 r setup_heli(unsigned char, Menu::arg const*)::__c 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) +00000046 W RC_Channel::~RC_Channel() 00000048 t change_command(unsigned char) 00000048 t update_motor_leds() 00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c @@ -553,9 +561,9 @@ 00000064 t mavlink_msg_param_value_send 00000068 t zero_eeprom() 00000068 t find_last_log_page(int) +00000068 W GCS_MAVLINK::~GCS_MAVLINK() 0000006a T mavlink_send_text(mavlink_channel_t, gcs_severity, char const*) 0000006a t read_num_from_serial() -0000006a W GCS_MAVLINK::~GCS_MAVLINK() 0000006c t setup_sonar(unsigned char, Menu::arg const*) 00000074 t output_motors_armed() 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*) @@ -579,12 +587,12 @@ 00000090 t report_tuning() 00000092 t test_tuning(unsigned char, Menu::arg const*) 00000092 T GCS_MAVLINK::queued_param_send() +00000094 t init_throttle_cruise() 00000096 t print_wp(Location*, unsigned char) +00000098 B gcs0 +00000098 B gcs3 0000009a t planner_gcs(unsigned char, Menu::arg const*) 0000009a t init_compass() -0000009a t Log_Read_Motors() -0000009b B gcs0 -0000009b B gcs3 0000009e t setup_mode(unsigned char, Menu::arg const*) 0000009e t Log_Read_Mode() 0000009e t Log_Write_Cmd(unsigned char, Location*) @@ -593,11 +601,12 @@ 000000a4 T __vector_55 000000a6 t send_servo_out(mavlink_channel_t) 000000a8 t test_sonar(unsigned char, Menu::arg const*) -000000aa t Log_Read_Nav_Tuning() +000000ae t Log_Read_Motors() 000000b0 t test_relay(unsigned char, Menu::arg const*) 000000b2 t erase_logs(unsigned char, Menu::arg const*) 000000b4 t read_radio() 000000b6 t get_log_boundaries(unsigned char, int&, int&) +000000b6 t Log_Read_Performance() 000000b7 B compass 000000be t update_events() 000000c2 t test_eedump(unsigned char, Menu::arg const*) @@ -607,7 +616,6 @@ 000000c4 t get_distance(Location*, Location*) 000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c 000000c6 t send_radio_in(mavlink_channel_t) -000000c6 t Log_Read_Performance() 000000c6 t test_tri(unsigned char, Menu::arg const*) 000000c7 B dcm 000000ca t init_barometer() @@ -619,29 +627,31 @@ 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000dc t test_adc(unsigned char, Menu::arg const*) +000000de t Log_Read_Nav_Tuning() 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&) 000000e8 t Log_Read_Current() -000000ea t Log_Read_Control_Tuning() 000000ee t report_batt_monitor() 000000f6 t Log_Read_Cmd() +000000f8 t heli_get_angle_boost(int) +000000fa t calc_loiter_pitch_roll() 00000100 r test_menu_commands 00000100 r setup_menu_commands +00000102 t Log_Read_Control_Tuning() 00000108 t setup_gyro(unsigned char, Menu::arg const*) 0000010a t mavlink_delay(unsigned long) 0000010a t send_raw_imu2(mavlink_channel_t) 0000010a t test_gps(unsigned char, Menu::arg const*) 0000010c t test_current(unsigned char, Menu::arg const*) -0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) +00000110 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000112 t set_next_WP(Location*) 00000112 t send_extended_status1(mavlink_channel_t, unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) 00000118 t set_command_with_index(Location, int) 0000011c t get_command_with_index(int) -0000012c t calc_loiter_pitch_roll() 00000130 t report_compass() +00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) 00000148 t Log_Read_GPS() 00000156 t update_commands() 0000015c t update_trig() @@ -650,10 +660,10 @@ 00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) 00000166 t select_logs(unsigned char, Menu::arg const*) 00000166 t send_vfr_hud(mavlink_channel_t) -00000168 T GCS_MAVLINK::update() 0000016c t test_imu(unsigned char, Menu::arg const*) 0000016e t send_attitude(mavlink_channel_t) 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int) +000001a0 T GCS_MAVLINK::update() 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int) 000001a8 t print_radio_values() 000001be t arm_motors() @@ -661,29 +671,29 @@ 000001e4 t verify_nav_wp() 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*) 000001ea t init_home() -00000216 t set_mode(unsigned char) 0000021a t send_raw_imu1(mavlink_channel_t) 0000021c t test_wp(unsigned char, Menu::arg const*) +00000226 t set_mode(unsigned char) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000022a t send_gps_raw(mavlink_channel_t) 00000242 t calc_loiter(int, int) 00000268 t send_raw_imu3(mavlink_channel_t) -0000030c t heli_init_swash() -00000312 W Parameters::~Parameters() +00000298 T update_throttle_mode() +00000326 W Parameters::~Parameters() 00000328 t report_heli() -00000330 t tuning() +0000033a t heli_init_swash() +0000036a t tuning() 00000382 t print_log_menu() -0000039a T update_throttle_mode() +0000039e T update_roll_pitch_mode() 000003a0 t read_battery() 0000045c T update_yaw_mode() -0000046e T update_roll_pitch_mode() 0000053e t heli_move_swash(int, int, int, int) 000005cc t __static_initialization_and_destruction_0(int, int) -0000061e t init_ardupilot() +000005fc t init_ardupilot() 0000071a t update_nav_wp() 000007e8 t setup_heli(unsigned char, Menu::arg const*) 00000870 t process_next_command() -000009b8 W Parameters::Parameters() -00000a1f b g +00000a64 W Parameters::Parameters() +00000b2d b g 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*) -00001598 T loop +000016c8 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml index c2dccc3239..e338f2b167 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml +++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml @@ -95,7 +95,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.hex - ArduCopter V2.0.49 Beta Heli (2560 only) + ArduCopter V2.0.50 Beta Heli (2560 only) #define AUTO_RESET_LOITER 0 #define FRAME_CONFIG HELI_FRAME @@ -137,7 +137,7 @@ #define NAV_LOITER_IMAX 10 - 111 + 112 http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex @@ -157,7 +157,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.hex - ArduCopter V2.0.49 Beta Heli Hil + ArduCopter V2.0.50 Beta Heli Hil #define HIL_MODE HIL_MODE_ATTITUDE @@ -203,6 +203,6 @@ - 111 + 112 diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index 04b288c7e4..ca7ab87b26 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1,36 +1,545 @@ From https://code.google.com/p/ardupilot-mega - 8a21477..b0bfa54 APM_Camera -> origin/APM_Camera - 6f44fff..076459c master -> origin/master -Updating 6f44fff..076459c + b0bfa54..8d3fb35 APM_Camera -> origin/APM_Camera + cd1bcd6..34c9a18 master -> origin/master +Updating cd1bcd6..34c9a18 Fast-forward - ArduCopter/control_modes.pde | 16 + + .gitignore | 4 +- + ArduBoat/ArduBoat.cpp | 7 +- + ArduBoat/ArduBoat.pde | 2 +- + ArduBoat/BoatGeneric.h | 39 +- + ArduBoat/ControllerBoat.h | 157 +- + ArduCopter/APM_Config.h | 7 +- + ArduCopter/ArduCopter.pde | 64 +- + ArduCopter/Attitude.pde | 54 +- + ArduCopter/CMakeLists.txt | 165 - + ArduCopter/GCS.h | 7 +- + ArduCopter/GCS_Mavlink.pde | 23 +- + ArduCopter/Log.pde | 261 +- + ArduCopter/Parameters.h | 12 +- + ArduCopter/config.h | 45 +- + ArduCopter/control_modes.pde | 8 +- ArduCopter/defines.h | 1 + - ArduCopter/system.pde | 4 + - ArduPlane/ArduPlane.pde | 10 +- - ArduPlane/Parameters.h | 3 + - ArduPlane/config.h | 8 + - Tools/ArduTracker/tags |148411 -------------------- - Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs | 2 + - Tools/ArdupilotMegaPlanner/Log.cs | 174 +- - Tools/ArdupilotMegaPlanner/MainV2.cs | 9 +- + ArduCopter/heli.pde | 25 + + ArduCopter/motors.pde | 4 +- + ArduCopter/navigation.pde | 42 +- + ArduCopter/radio.pde | 14 +- + ArduCopter/system.pde | 36 +- + ArduPlane/.gitignore | 1 - + ArduPlane/ArduPlane.pde | 27 +- + ArduPlane/CMakeLists.txt | 168 - + ArduPlane/GCS.h | 7 +- + ArduPlane/GCS_Mavlink.pde | 464 +++- + ArduPlane/Log.pde | 10 +- + ArduPlane/Mavlink_compat.h | 172 + + ArduPlane/Parameters.h | 12 +- + ArduPlane/commands.pde | 65 +- + ArduPlane/commands_logic.pde | 152 +- + ArduPlane/commands_process.pde | 183 +- + ArduPlane/config.h | 5 + + ArduPlane/defines.h | 3 +- + ArduPlane/navigation.pde | 2 +- + ArduPlane/system.pde | 21 +- + ArduPlane/test.pde | 6 +- + ArduRover/ArduRover.cpp | 6 +- + ArduRover/ArduRover.pde | 1 + + ArduRover/CarStampede.h | 31 +- + ArduRover/ControllerCar.h | 158 +- + ArduRover/ControllerTank.h | 176 +- + ArduRover/TankGeneric.h | 16 +- + CMakeLists.txt | 9 +- + Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj | 9 + + Tools/ArdupilotMegaPlanner/Camera.Designer.cs | 421 +++ + Tools/ArdupilotMegaPlanner/Camera.cs | 139 + + Tools/ArdupilotMegaPlanner/Camera.resx | 120 + + Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs | 12 +- + Tools/ArdupilotMegaPlanner/CurrentState.cs | 81 +- + .../GCSViews/Configuration.Designer.cs | 137 +- + .../ArdupilotMegaPlanner/GCSViews/Configuration.cs | 47 +- + .../GCSViews/Configuration.resx | 2088 +++++------- + Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs | 12 +- + .../GCSViews/FlightData.Designer.cs | 155 +- + Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs | 164 +- + .../ArdupilotMegaPlanner/GCSViews/FlightData.resx | 604 ++-- + .../ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs | 4 +- + Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs | 18 +- + Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs | 7 + + Tools/ArdupilotMegaPlanner/Joystick.cs | 50 +- + .../ArdupilotMegaPlanner/JoystickSetup.Designer.cs | 276 ++- + Tools/ArdupilotMegaPlanner/JoystickSetup.cs | 125 +- + Tools/ArdupilotMegaPlanner/JoystickSetup.resx | 763 ++++- + Tools/ArdupilotMegaPlanner/MAVLink.cs | 11 +- + Tools/ArdupilotMegaPlanner/MainV2.cs | 87 +- + Tools/ArdupilotMegaPlanner/MavlinkLog.cs | 9 +- + Tools/ArdupilotMegaPlanner/Program.cs | 1 + .../Properties/AssemblyInfo.cs | 2 +- + Tools/ArdupilotMegaPlanner/SerialOutput.cs | 13 +- + Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 4 +- + Tools/ArdupilotMegaPlanner/Updater/Updater.csproj | 2 +- .../bin/Release/ArdupilotMegaPlanner.application | 2 +- - .../bin/Release/ArdupilotMegaPlanner.exe | Bin 2194432 -> 2194944 bytes + .../bin/Release/ArdupilotMegaPlanner.exe | Bin 2194944 -> 2188288 bytes + .../bin/Release/GCSViews/Configuration.resx | 2088 +++++------- + .../bin/Release/GCSViews/FlightData.resx | 604 ++-- + .../bin/Release/JoystickSetup.resx | 763 ++++- Tools/ArdupilotMegaPlanner/bin/Release/Updater.exe | Bin 8192 -> 8192 bytes Tools/ArdupilotMegaPlanner/bin/Release/resedit.exe | Bin 20480 -> 20480 bytes .../ru-RU/ArdupilotMegaPlanner.resources.dll | Bin 53248 -> 53248 bytes .../zh-Hans/ArdupilotMegaPlanner.resources.dll | Bin 380928 -> 380928 bytes - apo/ControllerQuad.h | 51 +- - apo/QuadArducopter.h | 10 +- - libraries/APO/AP_ArmingMechanism.cpp | 57 + - libraries/APO/AP_ArmingMechanism.h | 67 + - libraries/APO/AP_BatteryMonitor.cpp | 2 + - libraries/APO/AP_BatteryMonitor.h | 58 +- - libraries/APO/AP_Guide.h | 1 + - libraries/APO/AP_HardwareAbstractionLayer.h | 13 + - libraries/APO/AP_Navigator.cpp | 20 +- - libraries/Desktop/support/FastSerial.cpp | 283 +- - 27 files changed, 517 insertions(+), 148687 deletions(-) - delete mode 100644 Tools/ArduTracker/tags - create mode 100644 libraries/APO/AP_ArmingMechanism.cpp - create mode 100644 libraries/APO/AP_ArmingMechanism.h + Tools/scripts/format.sh | 13 + + apo/ControllerPlane.h | 329 +- + apo/ControllerQuad.h | 414 +-- + apo/PlaneEasystar.h | 18 +- + apo/QuadArducopter.h | 17 +- + apo/apo.pde | 5 +- + libraries/APM_BMP085/APM_BMP085.h | 2 +- + libraries/APM_PI/APM_PI.cpp | 8 +- + libraries/APO/APO.h | 6 +- + libraries/APO/APO_DefaultSetup.h | 321 +- + libraries/APO/AP_ArmingMechanism.cpp | 14 +- + libraries/APO/AP_ArmingMechanism.h | 10 +- + libraries/APO/AP_Autopilot.cpp | 461 ++-- + libraries/APO/AP_Autopilot.h | 195 +- + libraries/APO/AP_BatteryMonitor.h | 1 - + libraries/APO/AP_CommLink.cpp | 1280 ++++---- + libraries/APO/AP_CommLink.h | 126 +- + libraries/APO/AP_Controller.cpp | 111 +- + libraries/APO/AP_Controller.h | 402 ++- + libraries/APO/AP_Guide.cpp | 429 ++- + libraries/APO/AP_Guide.h | 203 +- + libraries/APO/AP_HardwareAbstractionLayer.cpp | 1 + + libraries/APO/AP_HardwareAbstractionLayer.h | 258 +- + libraries/APO/AP_MavlinkCommand.cpp | 304 +- + libraries/APO/AP_MavlinkCommand.h | 654 ++-- + libraries/APO/AP_Navigator.cpp | 298 +- + libraries/APO/AP_Navigator.h | 358 +- + libraries/APO/AP_RcChannel.cpp | 125 +- + libraries/APO/AP_RcChannel.h | 103 +- + libraries/APO/AP_Var_keys.h | 21 +- + libraries/APO/constants.h | 1 + + libraries/APO/examples/MavlinkTest/MavlinkTest.pde | 64 +- + libraries/APO/examples/ServoManual/ServoManual.pde | 144 +- + libraries/APO/examples/ServoSweep/ServoSweep.pde | 184 +- + libraries/AP_Common/AP_Common.h | 20 +- + libraries/AP_Common/AP_Test.h | 4 +- + libraries/AP_Common/AP_Var.cpp | 80 +- + libraries/AP_Common/AP_Var.h | 12 +- + libraries/AP_Common/c++.cpp | 22 +- + libraries/AP_Common/examples/menu/menu.pde | 22 +- + libraries/AP_Common/include/menu.h | 186 +- + libraries/AP_Common/menu.cpp | 198 +- + libraries/AP_DCM/AP_DCM_HIL.cpp | 12 +- + libraries/AP_GPS/AP_GPS_406.cpp | 72 +- + libraries/AP_GPS/AP_GPS_406.h | 8 +- + libraries/AP_GPS/AP_GPS_Auto.cpp | 316 +- + libraries/AP_GPS/AP_GPS_Auto.h | 58 +- + libraries/AP_GPS/AP_GPS_HIL.cpp | 18 +- + libraries/AP_GPS/AP_GPS_HIL.h | 10 +- + libraries/AP_GPS/AP_GPS_IMU.cpp | 294 +- + libraries/AP_GPS/AP_GPS_IMU.h | 68 +- + libraries/AP_GPS/AP_GPS_MTK.cpp | 226 +- + libraries/AP_GPS/AP_GPS_MTK.h | 74 +- + libraries/AP_GPS/AP_GPS_MTK16.cpp | 240 +- + libraries/AP_GPS/AP_GPS_MTK16.h | 78 +- + libraries/AP_GPS/AP_GPS_NMEA.cpp | 528 ++-- + libraries/AP_GPS/AP_GPS_NMEA.h | 118 +- + libraries/AP_GPS/AP_GPS_None.h | 8 +- + libraries/AP_GPS/AP_GPS_SIRF.cpp | 280 +- + libraries/AP_GPS/AP_GPS_SIRF.h | 130 +- + libraries/AP_GPS/AP_GPS_Shim.h | 38 +- + libraries/AP_GPS/AP_GPS_UBLOX.cpp | 292 +- + libraries/AP_GPS/AP_GPS_UBLOX.h | 184 +- + libraries/AP_GPS/GPS.cpp | 50 +- + libraries/AP_GPS/GPS.h | 330 +- + .../AP_GPS/examples/GPS_406_test/GPS_406_test.pde | 62 +- + .../examples/GPS_AUTO_test/GPS_AUTO_test.pde | 42 +- + .../AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde | 60 +- + .../examples/GPS_NMEA_test/GPS_NMEA_test.pde | 81 +- + .../examples/GPS_UBLOX_test/GPS_UBLOX_test.pde | 60 +- + libraries/Desktop/Desktop.mk | 2 +- + libraries/Desktop/Makefile.desktop | 3 + + libraries/GCS_MAVLink/GCS_MAVLink.cpp | 6 +- + libraries/GCS_MAVLink/GCS_MAVLink.h | 18 +- + .../include/ardupilotmega/ardupilotmega.h | 74 +- + .../include/ardupilotmega/mavlink_msg_ap_adc.h | 254 ++ + .../ardupilotmega/mavlink_msg_digicam_configure.h | 364 ++ + .../ardupilotmega/mavlink_msg_digicam_control.h | 342 ++ + .../ardupilotmega/mavlink_msg_mount_configure.h | 254 ++ + .../ardupilotmega/mavlink_msg_mount_control.h | 254 ++ + .../ardupilotmega/mavlink_msg_mount_status.h | 232 ++ + .../GCS_MAVLink/include/ardupilotmega/testsuite.h | 340 ++ + .../GCS_MAVLink/include/ardupilotmega/version.h | 2 +- + libraries/GCS_MAVLink/include/bittest.c | 39 - + libraries/GCS_MAVLink/include/common/common.h | 52 +- + .../mavlink_msg_attitude_controller_output.h | 169 - + .../include/common/mavlink_msg_attitude_new.h | 268 -- + .../include/common/mavlink_msg_auth_key.h | 6 +- + .../common/mavlink_msg_change_operator_control.h | 6 +- + .../include/common/mavlink_msg_debug_vect.h | 6 +- + .../include/common/mavlink_msg_full_state.h | 428 --- + .../include/common/mavlink_msg_gps_status.h | 30 +- + .../include/common/mavlink_msg_named_value_float.h | 6 +- + .../include/common/mavlink_msg_named_value_int.h | 6 +- + .../common/mavlink_msg_object_detection_event.h | 6 +- + .../common/mavlink_msg_param_request_read.h | 6 +- + .../include/common/mavlink_msg_param_set.h | 6 +- + .../include/common/mavlink_msg_param_value.h | 6 +- + .../mavlink_msg_position_controller_output.h | 169 - + .../mavlink_msg_request_dynamic_gyro_calibration.h | 177 - + .../mavlink_msg_request_static_calibration.h | 138 - + .../common/mavlink_msg_set_roll_pitch_yaw.h | 184 - + .../common/mavlink_msg_set_roll_pitch_yaw_speed.h | 184 - + .../include/common/mavlink_msg_statustext.h | 6 +- + .../mavlink_msg_waypoint_set_global_reference.h | 294 -- + libraries/GCS_MAVLink/include/common/testsuite.h | 30 +- + libraries/GCS_MAVLink/include/common/version.h | 2 +- + libraries/GCS_MAVLink/include/documentation.dox | 41 - + libraries/GCS_MAVLink/include/mavlink_helpers.h | 8 +- + libraries/GCS_MAVLink/include/minimal/mavlink.h | 11 - + .../include/minimal/mavlink_msg_heartbeat.h | 132 - + libraries/GCS_MAVLink/include/minimal/minimal.h | 45 - + libraries/GCS_MAVLink/include/pixhawk/mavlink.h | 11 - + .../include/pixhawk/mavlink_msg_attitude_control.h | 257 -- + .../include/pixhawk/mavlink_msg_aux_status.h | 204 - + .../include/pixhawk/mavlink_msg_brief_feature.h | 249 -- + .../include/pixhawk/mavlink_msg_control_status.h | 203 - + .../mavlink_msg_data_transmission_handshake.h | 174 - + .../include/pixhawk/mavlink_msg_debug_vect.h | 156 - + .../pixhawk/mavlink_msg_encapsulated_data.h | 124 - + .../pixhawk/mavlink_msg_encapsulated_image.h | 76 - + .../include/pixhawk/mavlink_msg_get_image_ack.h | 104 - + .../include/pixhawk/mavlink_msg_image_available.h | 586 --- + .../pixhawk/mavlink_msg_image_trigger_control.h | 101 - + .../include/pixhawk/mavlink_msg_image_triggered.h | 352 -- + .../include/pixhawk/mavlink_msg_manual_control.h | 191 - + .../include/pixhawk/mavlink_msg_marker.h | 236 -- + .../include/pixhawk/mavlink_msg_pattern_detected.h | 160 - + .../pixhawk/mavlink_msg_point_of_interest.h | 241 -- + .../mavlink_msg_point_of_interest_connection.h | 307 -- + .../mavlink_msg_position_control_offset_set.h | 206 - + .../mavlink_msg_position_control_setpoint.h | 192 - + .../mavlink_msg_position_control_setpoint_set.h | 226 -- + .../include/pixhawk/mavlink_msg_raw_aux.h | 226 -- + .../pixhawk/mavlink_msg_request_data_stream.h | 118 - + .../mavlink_msg_request_dynamic_gyro_calibration.h | 123 - + .../mavlink_msg_request_static_calibration.h | 90 - + .../include/pixhawk/mavlink_msg_set_altitude.h | 78 - + .../include/pixhawk/mavlink_msg_set_cam_shutter.h | 197 - + .../include/pixhawk/mavlink_msg_watchdog_command.h | 158 - + .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 124 - + .../pixhawk/mavlink_msg_watchdog_process_info.h | 186 - + .../pixhawk/mavlink_msg_watchdog_process_status.h | 200 - + libraries/GCS_MAVLink/include/pixhawk/pixhawk.h | 79 - + libraries/GCS_MAVLink/include/protocol.h | 37 +- + libraries/GCS_MAVLink/include/slugs/mavlink.h | 11 - + .../include/slugs/mavlink_msg_air_data.h | 148 - + .../include/slugs/mavlink_msg_cpu_load.h | 138 - + .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 121 - + .../include/slugs/mavlink_msg_data_log.h | 216 -- + .../include/slugs/mavlink_msg_diagnostic.h | 210 -- + .../include/slugs/mavlink_msg_filtered_data.h | 216 -- + .../include/slugs/mavlink_msg_gps_date_time.h | 203 - + .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 167 - + .../GCS_MAVLink/include/slugs/mavlink_msg_pid.h | 130 - + .../include/slugs/mavlink_msg_pilot_console.h | 145 - + .../include/slugs/mavlink_msg_pwm_commands.h | 235 -- + .../include/slugs/mavlink_msg_sensor_bias.h | 216 -- + .../include/slugs/mavlink_msg_slugs_action.h | 138 - + .../include/slugs/mavlink_msg_slugs_navigation.h | 272 -- + libraries/GCS_MAVLink/include/slugs/slugs.h | 56 - + libraries/GCS_MAVLink/include/ualberta/mavlink.h | 11 - + .../include/ualberta/mavlink_msg_nav_filter_bias.h | 242 -- + .../ualberta/mavlink_msg_radio_calibration.h | 204 - + .../mavlink_msg_request_radio_calibration.h | 59 - + .../ualberta/mavlink_msg_request_rc_channels.h | 101 - + .../ualberta/mavlink_msg_ualberta_sys_status.h | 135 - + libraries/GCS_MAVLink/include/ualberta/ualberta.h | 79 - + .../include_v1.0/ardupilotmega/ardupilotmega.h | 122 + + .../include_v1.0/ardupilotmega/mavlink.h | 27 + + .../ardupilotmega/mavlink_msg_ap_adc.h | 254 ++ + .../ardupilotmega/mavlink_msg_digicam_configure.h | 364 ++ + .../ardupilotmega/mavlink_msg_digicam_control.h | 342 ++ + .../ardupilotmega/mavlink_msg_meminfo.h | 166 + + .../ardupilotmega/mavlink_msg_mount_configure.h | 254 ++ + .../ardupilotmega/mavlink_msg_mount_control.h | 254 ++ + .../ardupilotmega/mavlink_msg_mount_status.h | 232 ++ + .../ardupilotmega/mavlink_msg_sensor_offsets.h | 386 ++ + .../ardupilotmega/mavlink_msg_set_mag_offsets.h | 232 ++ + .../include_v1.0/ardupilotmega/testsuite.h | 538 +++ + .../include_v1.0/ardupilotmega/version.h | 12 + + libraries/GCS_MAVLink/include_v1.0/checksum.h | 89 + + libraries/GCS_MAVLink/include_v1.0/common/common.h | 429 +++ + .../GCS_MAVLink/include_v1.0/common/mavlink.h | 27 + + .../include_v1.0/common/mavlink_msg_attitude.h | 276 ++ + .../common/mavlink_msg_attitude_quaternion.h | 298 ++ + .../include_v1.0/common/mavlink_msg_auth_key.h | 144 + + .../common/mavlink_msg_change_operator_control.h | 204 + + .../mavlink_msg_change_operator_control_ack.h | 188 + + .../include_v1.0/common/mavlink_msg_command_ack.h | 166 + + .../include_v1.0/common/mavlink_msg_command_long.h | 364 ++ + .../include_v1.0/common/mavlink_msg_data_stream.h | 188 + + .../include_v1.0/common/mavlink_msg_debug.h | 188 + + .../include_v1.0/common/mavlink_msg_debug_vect.h | 226 ++ + .../common/mavlink_msg_extended_message.h | 188 + + .../common/mavlink_msg_global_position_int.h | 320 ++ + .../mavlink_msg_global_position_setpoint_int.h | 232 ++ + .../mavlink_msg_global_vision_position_estimate.h | 276 ++ + .../common/mavlink_msg_gps_global_origin.h | 188 + + .../include_v1.0/common/mavlink_msg_gps_raw_int.h | 342 ++ + .../include_v1.0/common/mavlink_msg_gps_status.h | 252 ++ + .../include_v1.0/common/mavlink_msg_heartbeat.h | 251 ++ + .../include_v1.0/common/mavlink_msg_hil_controls.h | 364 ++ + .../common/mavlink_msg_hil_rc_inputs_raw.h | 430 +++ + .../include_v1.0/common/mavlink_msg_hil_state.h | 474 +++ + .../common/mavlink_msg_local_position_ned.h | 276 ++ + .../common/mavlink_msg_local_position_setpoint.h | 232 ++ + .../common/mavlink_msg_manual_control.h | 320 ++ + .../include_v1.0/common/mavlink_msg_memory_vect.h | 204 + + .../include_v1.0/common/mavlink_msg_mission_ack.h | 188 + + .../common/mavlink_msg_mission_clear_all.h | 166 + + .../common/mavlink_msg_mission_count.h | 188 + + .../common/mavlink_msg_mission_current.h | 144 + + .../include_v1.0/common/mavlink_msg_mission_item.h | 430 +++ + .../common/mavlink_msg_mission_item_reached.h | 144 + + .../common/mavlink_msg_mission_request.h | 188 + + .../common/mavlink_msg_mission_request_list.h | 166 + + .../mavlink_msg_mission_request_partial_list.h | 210 ++ + .../common/mavlink_msg_mission_set_current.h | 188 + + .../mavlink_msg_mission_write_partial_list.h | 210 ++ + .../common/mavlink_msg_named_value_float.h | 182 + + .../common/mavlink_msg_named_value_int.h | 182 + + .../common/mavlink_msg_nav_controller_output.h | 298 ++ + .../include_v1.0/common/mavlink_msg_optical_flow.h | 254 ++ + .../common/mavlink_msg_param_request_list.h | 166 + + .../common/mavlink_msg_param_request_read.h | 204 + + .../include_v1.0/common/mavlink_msg_param_set.h | 226 ++ + .../include_v1.0/common/mavlink_msg_param_value.h | 226 ++ + .../include_v1.0/common/mavlink_msg_ping.h | 210 ++ + .../include_v1.0/common/mavlink_msg_raw_imu.h | 342 ++ + .../include_v1.0/common/mavlink_msg_raw_pressure.h | 232 ++ + .../common/mavlink_msg_rc_channels_override.h | 342 ++ + .../common/mavlink_msg_rc_channels_raw.h | 364 ++ + .../common/mavlink_msg_rc_channels_scaled.h | 364 ++ + .../common/mavlink_msg_request_data_stream.h | 232 ++ + ...link_msg_roll_pitch_yaw_speed_thrust_setpoint.h | 232 ++ + .../mavlink_msg_roll_pitch_yaw_thrust_setpoint.h | 232 ++ + .../common/mavlink_msg_safety_allowed_area.h | 276 ++ + .../common/mavlink_msg_safety_set_allowed_area.h | 320 ++ + .../include_v1.0/common/mavlink_msg_scaled_imu.h | 342 ++ + .../common/mavlink_msg_scaled_pressure.h | 210 ++ + .../common/mavlink_msg_servo_output_raw.h | 342 ++ + .../mavlink_msg_set_global_position_setpoint_int.h | 232 ++ + .../common/mavlink_msg_set_gps_global_origin.h | 210 ++ + .../mavlink_msg_set_local_position_setpoint.h | 276 ++ + .../include_v1.0/common/mavlink_msg_set_mode.h | 188 + + .../mavlink_msg_set_roll_pitch_yaw_speed_thrust.h | 254 ++ + .../common/mavlink_msg_set_roll_pitch_yaw_thrust.h | 254 ++ + .../common/mavlink_msg_state_correction.h | 320 ++ + .../include_v1.0/common/mavlink_msg_statustext.h | 160 + + .../include_v1.0/common/mavlink_msg_sys_status.h | 408 ++ + .../include_v1.0/common/mavlink_msg_system_time.h | 166 + + .../include_v1.0/common/mavlink_msg_vfr_hud.h | 254 ++ + .../common}/mavlink_msg_vicon_position_estimate.h | 198 +- + .../common}/mavlink_msg_vision_position_estimate.h | 198 +- + .../common}/mavlink_msg_vision_speed_estimate.h | 148 +- + .../GCS_MAVLink/include_v1.0/common/testsuite.h | 3908 ++++++++++++++++++++ + .../GCS_MAVLink/include_v1.0/common/version.h | 12 + + .../GCS_MAVLink/include_v1.0/mavlink_helpers.h | 488 +++ + libraries/GCS_MAVLink/include_v1.0/mavlink_types.h | 182 + + libraries/GCS_MAVLink/include_v1.0/protocol.h | 319 ++ + .../message_definitions/ardupilotmega.xml | 132 + + libraries/GCS_MAVLink/message_definitions/test.xml | 31 + + .../message_definitions_v1.0/ardupilotmega.xml | 177 + + .../message_definitions_v1.0/common.xml | 1536 ++++++++ + .../message_definitions_v1.0/minimal.xml | 16 + + .../message_definitions_v1.0/pixhawk.xml | 193 + + .../GCS_MAVLink/message_definitions_v1.0/slugs.xml | 144 + + .../GCS_MAVLink/message_definitions_v1.0/test.xml | 31 + + .../message_definitions_v1.0/ualberta.xml | 54 + + libraries/RC_Channel/RC_Channel.cpp | 2 +- + libraries/RC_Channel/RC_Channel.h | 8 +- + libraries/RC_Channel/RC_Channel_aux.cpp | 2 +- + libraries/RC_Channel/RC_Channel_aux.h | 1 + + 355 files changed, 43388 insertions(+), 22115 deletions(-) + delete mode 100644 ArduCopter/CMakeLists.txt + delete mode 100644 ArduPlane/.gitignore + delete mode 100644 ArduPlane/CMakeLists.txt + create mode 100644 ArduPlane/Mavlink_compat.h + create mode 100644 Tools/ArdupilotMegaPlanner/Camera.Designer.cs + create mode 100644 Tools/ArdupilotMegaPlanner/Camera.cs + create mode 100644 Tools/ArdupilotMegaPlanner/Camera.resx + create mode 100755 Tools/scripts/format.sh + create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h + create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_configure.h + create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_digicam_control.h + create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_configure.h + create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_control.h + create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_mount_status.h + delete mode 100644 libraries/GCS_MAVLink/include/bittest.c + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_controller_output.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_full_state.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_position_controller_output.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_request_dynamic_gyro_calibration.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_request_static_calibration.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h + delete mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_global_reference.h + delete mode 100644 libraries/GCS_MAVLink/include/documentation.dox + delete mode 100644 libraries/GCS_MAVLink/include/minimal/mavlink.h + delete mode 100644 libraries/GCS_MAVLink/include/minimal/mavlink_msg_heartbeat.h + delete mode 100644 libraries/GCS_MAVLink/include/minimal/minimal.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_attitude_control.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_aux_status.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_control_status.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_data_transmission_handshake.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_debug_vect.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_data.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_encapsulated_image.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_get_image_ack.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_available.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_trigger_control.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_triggered.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_manual_control.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_marker.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_pattern_detected.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_point_of_interest_connection.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_offset_set.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_raw_aux.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_data_stream.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_dynamic_gyro_calibration.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_request_static_calibration.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_altitude.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_set_cam_shutter.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_command.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_info.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_watchdog_process_status.h + delete mode 100644 libraries/GCS_MAVLink/include/pixhawk/pixhawk.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_air_data.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_cpu_load.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_ctrl_srfc_pt.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_data_log.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_diagnostic.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_filtered_data.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_gps_date_time.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_mid_lvl_cmds.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_pid.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_pilot_console.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_pwm_commands.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_sensor_bias.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_action.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/mavlink_msg_slugs_navigation.h + delete mode 100644 libraries/GCS_MAVLink/include/slugs/slugs.h + delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink.h + delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_nav_filter_bias.h + delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_radio_calibration.h + delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_radio_calibration.h + delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_request_rc_channels.h + delete mode 100644 libraries/GCS_MAVLink/include/ualberta/mavlink_msg_ualberta_sys_status.h + delete mode 100644 libraries/GCS_MAVLink/include/ualberta/ualberta.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_ap_adc.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_configure.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_digicam_control.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_meminfo.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_configure.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_control.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_mount_status.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/ardupilotmega/version.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/checksum.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/common.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_attitude.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_attitude_quaternion.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_auth_key.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_change_operator_control.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_change_operator_control_ack.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_ack.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_command_long.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_data_stream.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_debug.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_debug_vect.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_extended_message.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position_int.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position_setpoint_int.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_vision_position_estimate.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_global_origin.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw_int.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_status.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_heartbeat.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_controls.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_rc_inputs_raw.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_state.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position_ned.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position_setpoint.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_manual_control.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_memory_vect.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_ack.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_clear_all.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_count.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_current.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_item.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_item_reached.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_request.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_request_list.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_request_partial_list.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_set_current.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_mission_write_partial_list.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_named_value_float.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_named_value_int.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_nav_controller_output.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_optical_flow.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_param_request_list.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_param_request_read.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_param_set.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_param_value.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_ping.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_raw_imu.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_raw_pressure.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_rc_channels_override.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_rc_channels_raw.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_rc_channels_scaled.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_request_data_stream.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_safety_allowed_area.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_safety_set_allowed_area.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_scaled_imu.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_scaled_pressure.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_servo_output_raw.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_global_position_setpoint_int.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_gps_global_origin.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_local_position_setpoint.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_mode.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_state_correction.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_statustext.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_sys_status.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/mavlink_msg_vfr_hud.h + rename libraries/GCS_MAVLink/{include/pixhawk => include_v1.0/common}/mavlink_msg_vicon_position_estimate.h (54%) + rename libraries/GCS_MAVLink/{include/pixhawk => include_v1.0/common}/mavlink_msg_vision_position_estimate.h (54%) + rename libraries/GCS_MAVLink/{include/pixhawk => include_v1.0/common}/mavlink_msg_vision_speed_estimate.h (57%) + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/testsuite.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/common/version.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/mavlink_helpers.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/mavlink_types.h + create mode 100644 libraries/GCS_MAVLink/include_v1.0/protocol.h + create mode 100644 libraries/GCS_MAVLink/message_definitions/test.xml + create mode 100644 libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml + create mode 100644 libraries/GCS_MAVLink/message_definitions_v1.0/common.xml + create mode 100644 libraries/GCS_MAVLink/message_definitions_v1.0/minimal.xml + create mode 100644 libraries/GCS_MAVLink/message_definitions_v1.0/pixhawk.xml + create mode 100644 libraries/GCS_MAVLink/message_definitions_v1.0/slugs.xml + create mode 100644 libraries/GCS_MAVLink/message_definitions_v1.0/test.xml + create mode 100644 libraries/GCS_MAVLink/message_definitions_v1.0/ualberta.xml From d74a223f7efd4d1c355007fcde18f00084a9edbc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 30 Oct 2011 12:40:54 +1100 Subject: [PATCH 4/8] make desktop serial more responsive --- libraries/Desktop/support/FastSerial.cpp | 17 +++++++++++++++++ libraries/Desktop/support/main.cpp | 19 +++++++++++-------- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/libraries/Desktop/support/FastSerial.cpp b/libraries/Desktop/support/FastSerial.cpp index fb66de60af..04623fa8d0 100644 --- a/libraries/Desktop/support/FastSerial.cpp +++ b/libraries/Desktop/support/FastSerial.cpp @@ -45,6 +45,7 @@ #include #include #include +#include "desktop.h" #define LISTEN_BASE_PORT 5760 #define BUFFER_SIZE 128 @@ -305,3 +306,19 @@ void FastSerial::_freeBuffer(Buffer *buffer) { } +/* + return true if any bytes are pending + */ +void desktop_serial_select_setup(fd_set *fds, int *fd_high) +{ + int i; + + for (i=0; i *fd_high) { + *fd_high = tcp_state[i].fd; + } + } + } +} diff --git a/libraries/Desktop/support/main.cpp b/libraries/Desktop/support/main.cpp index b6eef2e63b..9e16c1ff70 100644 --- a/libraries/Desktop/support/main.cpp +++ b/libraries/Desktop/support/main.cpp @@ -4,6 +4,7 @@ #include #include #include +#include "desktop.h" void setup(void); void loop(void); @@ -16,15 +17,17 @@ int main(void) setup(); while (true) { struct timeval tv; - unsigned long t1, t2; - t1 = micros(); + fd_set fds; + int fd_high = 0; + + FD_ZERO(&fds); loop(); - t2 = micros(); - if (t2 - t1 < 2) { - tv.tv_sec = 0; - tv.tv_usec = 1; - select(0, NULL, NULL, NULL, &tv); - } + + desktop_serial_select_setup(&fds, &fd_high); + tv.tv_sec = 0; + tv.tv_usec = 100; + + select(fd_high+1, &fds, NULL, NULL, &tv); } return 0; } From e6f7bf9df81b60bbc42e36211a46e4296cd0e8f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 30 Oct 2011 12:41:32 +1100 Subject: [PATCH 5/8] MAVLink: use new MAV_MODE_FLAG_CUSTOM_MODE_ENABLED flag this enables us to tell if custom_mode is set --- ArduPlane/GCS_Mavlink.pde | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index ee6d7d9169..ea0366efad 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -28,16 +28,11 @@ static bool mavlink_active; static NOINLINE void send_heartbeat(mavlink_channel_t chan) { #ifdef MAVLINK10 - uint8_t base_mode = 0; + uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; uint8_t system_status = MAV_STATE_ACTIVE; + uint32_t custom_mode = control_mode; - // 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 + // work out the base_mode. This value is not very useful // 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 @@ -93,6 +88,9 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } + // indicate we have set a custom mode + base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + mavlink_msg_heartbeat_send( chan, MAV_TYPE_FIXED_WING, @@ -1140,13 +1138,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) 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) { + if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) { + // 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. + break; + } + switch (packet.custom_mode) { case MANUAL: case CIRCLE: case STABILIZE: @@ -1156,7 +1154,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case AUTO: case RTL: case LOITER: - set_mode(adjusted_mode); + set_mode(packet.custom_mode); break; } From e1f8609b788bdfbf63788a0f985c81ca82560250 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 30 Oct 2011 01:17:51 -0400 Subject: [PATCH 6/8] Cmake working for ArduCoptre/ ArduPlane Need to get pde processing working. --- ArduCopter/ArduCopter.cpp | 11568 ++++++++++++++++++++++++++++++++++++ ArduPlane/ArduPlane.cpp | 7973 +++++++++++++++++++++++++ CMakeLists.txt | 8 +- 3 files changed, 19544 insertions(+), 5 deletions(-) create mode 100644 ArduCopter/ArduCopter.cpp create mode 100644 ArduPlane/ArduPlane.cpp 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) From 2561ef3a840ff9f9b57dd764217af5347711f576 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 30 Oct 2011 01:36:45 -0400 Subject: [PATCH 7/8] Added cmake uploading. --- CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 41d70d7e69..6b5c21454d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,12 @@ if (NOT DEFINED BOARD) set(BOARD "mega") endif() +if (NOT DEFINED PORT) + message(STATUS "please define the upload port (for example: cmake + -DPORT=/dev/ttyUSB0, assuming /dev/ttyUSB0") + set(PORT "/dev/ttyUSB0") +endif() + # cpack settings set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "A universal autopilot system for the ArduPilotMega board.") set(CPACK_PACKAGE_VENDOR "DIYDRONES") @@ -78,7 +84,7 @@ set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) macro(apm_project PROJECT_NAME BOARD SRCS) message(STATUS "creating apo project ${PROJECT_NAME}") set(${PROJECT_NAME}_BOARD ${BOARD}) - set(${PROJECT_NAME}_AFLAGS "-assembler-with-cpp") + set(${PROJECT_NAME}_PORT ${PORT}) set(${PROJECT_NAME}_SRCS ${SRCS}) set(${PROJECT_NAME}_LIBS m c) generate_arduino_firmware(${PROJECT_NAME}) From 71789ab796a4bfbf47f809d5d240761496b9caab Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 30 Oct 2011 02:21:32 -0400 Subject: [PATCH 8/8] Working on auto generating cpp files for sketch. Just have to add prototypes now and #defines for debugging. --- ArduBoat/ArduBoat.cpp | 25 - ArduCopter/ArduCopter.cpp | 11568 ------------------------------------ ArduPlane/ArduPlane.cpp | 7973 ------------------------- ArduRover/ArduRover.cpp | 27 - CMakeLists.txt | 45 +- 5 files changed, 28 insertions(+), 19610 deletions(-) delete mode 100644 ArduBoat/ArduBoat.cpp delete mode 100644 ArduCopter/ArduCopter.cpp delete mode 100644 ArduPlane/ArduPlane.cpp delete mode 100644 ArduRover/ArduRover.cpp diff --git a/ArduBoat/ArduBoat.cpp b/ArduBoat/ArduBoat.cpp deleted file mode 100644 index 09f7119736..0000000000 --- a/ArduBoat/ArduBoat.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduBoat/ArduBoat.pde" -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Vehicle Configuration -#include "BoatGeneric.h" - -// ArduPilotOne Default Setup -#include "APO_DefaultSetup.h" -// vim:ts=4:sw=4:expandtab -#line 1 "autogenerated" -#include "WProgram.h" diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp deleted file mode 100644 index f9166d0eac..0000000000 --- a/ArduCopter/ArduCopter.cpp +++ /dev/null @@ -1,11568 +0,0 @@ -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/ArduCopter.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#define THISFIRMWARE "ArduCopter V2.0.50 Beta" -/* -ArduCopter Version 2.0 Beta -Authors: Jason Short -Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen -Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier - - -This firmware is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -Special Thanks for Contributors: - -Hein Hollander :Octo Support -Dani Saez :V Ocoto Support -Max Levine :Tri Support, Graphics -Jose Julio :Stabilization Control laws -Randy MacKay :Heli Support -Jani Hiriven :Testing feedback -Andrew Tridgell :Mavlink Support -James Goppert :Mavlink Support -Doug Weibel :Libraries -Mike Smith :Libraries, Coding support -HappyKillmore :Mavlink GCS -Michael Oborne :Mavlink GCS -Jack Dunkle :Alpha testing -Christof Schmid :Alpha testing -Oliver :Piezo support -Guntars :Arming safety suggestion - -And much more so PLEASE PM me on DIYDRONES to add your contribution to the List - -*/ - -//////////////////////////////////////////////////////////////////////////////// -// Header includes -//////////////////////////////////////////////////////////////////////////////// - -// AVR runtime -#include -#include -#include -#include - -// Libraries -#include -#include -#include // ArduPilot Mega RC Library -#include // ArduPilot GPS library -#include // Arduino I2C lib -#include // Arduino SPI lib -#include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include // ArduPilot Mega BMP085 Library -#include // ArduPilot Mega Magnetometer Library -#include // ArduPilot Mega Vector/Matrix math Library -#include // ArduPilot Mega IMU Library -#include // ArduPilot Mega DCM Library -#include // PI library -#include // RC Channel Library -#include // Range finder library -#include // Optical Flow library -#include -#include // APM relay -#include // MAVLink GCS definitions -#include - -// Configuration -#include "defines.h" -#include "config.h" - -// Local modules -#include "Parameters.h" -#include "GCS.h" - -//////////////////////////////////////////////////////////////////////////////// -// Serial ports -//////////////////////////////////////////////////////////////////////////////// -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -#line 1 "autogenerated" -#include "WProgram.h" - void setup() ; - void loop() ; - static void fast_loop() ; - static void medium_loop() ; - static void fifty_hz_loop() ; - static void slow_loop() ; - static void super_slow_loop() ; - static void update_GPS(void) ; - void update_yaw_mode(void) ; - void update_roll_pitch_mode(void) ; - void update_throttle_mode(void) ; - static void update_navigation() ; - static void read_AHRS(void) ; - static void update_trig(void); - static void update_altitude() ; - static void adjust_altitude() ; - static void tuning(); - static void update_nav_wp() ; - static void update_auto_yaw() ; - static int get_stabilize_roll(long target_angle) ; - static int get_stabilize_pitch(long target_angle) ; - static int get_stabilize_yaw(long target_angle) ; - static int get_nav_throttle(long z_error) ; - static int get_rate_roll(long target_rate) ; - static int get_rate_pitch(long target_rate) ; - static int get_rate_yaw(long target_rate) ; - static void reset_hold_I(void) ; - static void reset_nav(void) ; - static long get_nav_yaw_offset(int yaw_input, int reset) ; - static int alt_hold_velocity() ; - static int get_angle_boost(int value) ; - static void init_camera() ; - static void camera_stabilization() ; - void write_byte(byte val) ; - void write_int(int val ) ; - void write_float(float val) ; - void write_long(long val) ; - void flush(byte id) ; - static NOINLINE void send_heartbeat(mavlink_channel_t chan) ; - static NOINLINE void send_attitude(mavlink_channel_t chan) ; - static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) ; - static void NOINLINE send_meminfo(mavlink_channel_t chan) ; - static void NOINLINE send_location(mavlink_channel_t chan) ; - static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) ; - static void NOINLINE send_gps_raw(mavlink_channel_t chan) ; - static void NOINLINE send_servo_out(mavlink_channel_t chan) ; - static void NOINLINE send_radio_in(mavlink_channel_t chan) ; - static void NOINLINE send_radio_out(mavlink_channel_t chan) ; - static void NOINLINE send_vfr_hud(mavlink_channel_t chan) ; - static void NOINLINE send_raw_imu1(mavlink_channel_t chan) ; - static void NOINLINE send_raw_imu2(mavlink_channel_t chan) ; - static void NOINLINE send_raw_imu3(mavlink_channel_t chan) ; - static void NOINLINE send_gps_status(mavlink_channel_t chan) ; - static void NOINLINE send_current_waypoint(mavlink_channel_t chan) ; - static void NOINLINE send_statustext(mavlink_channel_t chan) ; - static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; - static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; - void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) ; - static void mavlink_delay(unsigned long t) ; - static void gcs_send_message(enum ap_message id) ; - static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) ; - static void gcs_update(void) ; - static void gcs_send_text(gcs_severity severity, const char *str) ; - static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) ; - static bool print_log_menu(void) ; - static void clear_header() ; - static byte get_num_logs(void) ; - static void start_new_log() ; - static void get_log_boundaries(byte log_num, int & start_page, int & end_page) ; - static int find_last_log_page(int bottom_page) ; - static void Log_Write_GPS() ; - static void Log_Read_GPS() ; - static void Log_Write_Raw() ; - static void Log_Read_Raw() ; - static void Log_Write_Current() ; - static void Log_Read_Current() ; - static void Log_Write_Motors() ; - static void Log_Read_Motors() ; - static void Log_Write_Optflow() ; - static void Log_Read_Optflow() ; - static void Log_Write_Nav_Tuning() ; - static void Log_Read_Nav_Tuning() ; - static void Log_Write_Control_Tuning() ; - static void Log_Read_Control_Tuning() ; - static void Log_Write_Performance() ; - static void Log_Read_Performance() ; - static void Log_Write_Cmd(byte num, struct Location *wp) ; - static void Log_Read_Cmd() ; - static void Log_Write_Attitude2() ; - static void Log_Read_Attitude2() ; - static void Log_Write_Attitude() ; - static void Log_Read_Attitude() ; - static void Log_Write_Mode(byte mode) ; - static void Log_Read_Mode() ; - static void Log_Write_Startup() ; - static void Log_Read_Startup() ; - static void Log_Read(int start_page, int end_page) ; - static void Log_Write_Startup() ; - static void Log_Read_Startup() ; - static void Log_Read(int start_page, int end_page) ; - static void Log_Write_Cmd(byte num, struct Location *wp) ; - static void Log_Write_Mode(byte mode) ; - static void start_new_log() ; - static void Log_Write_Raw() ; - static void Log_Write_GPS() ; - static void Log_Write_Current() ; - static void Log_Write_Attitude() ; - static void Log_Write_Optflow() ; - static void Log_Write_Nav_Tuning() ; - static void Log_Write_Control_Tuning() ; - static void Log_Write_Motors() ; - static void Log_Write_Performance() ; - void userhook_init() ; - void userhook_50Hz() ; - static void init_commands() ; - static void clear_command_queue(); - static struct Location get_command_with_index(int i) ; - static void set_command_with_index(struct Location temp, int i) ; - static void increment_WP_index() ; - static void decrement_WP_index() ; - static long read_alt_to_hold() ; - static Location get_LOITER_home_wp() ; - static void set_next_WP(struct Location *wp) ; - static void init_home() ; - static void handle_process_must() ; - static void handle_process_may() ; - static void handle_process_now() ; - static void handle_no_commands() ; - static bool verify_must() ; - static bool verify_may() ; - static void do_RTL(void) ; - static void do_takeoff() ; - static void do_nav_wp() ; - static void do_land() ; - static void do_loiter_unlimited() ; - static void do_loiter_turns() ; - static void do_loiter_time() ; - static bool verify_takeoff() ; - static bool verify_land() ; - static bool verify_nav_wp() ; - static bool verify_loiter_unlim() ; - static bool verify_loiter_time() ; - static bool verify_loiter_turns() ; - static bool verify_RTL() ; - static void do_wait_delay() ; - static void do_change_alt() ; - static void do_within_distance() ; - static void do_yaw() ; - static bool verify_wait_delay() ; - static bool verify_change_alt() ; - static bool verify_within_distance() ; - static bool verify_yaw() ; - static void do_change_speed() ; - static void do_target_yaw() ; - static void do_loiter_at_location() ; - static void do_jump() ; - static void do_set_home() ; - static void do_set_servo() ; - static void do_set_relay() ; - static void do_repeat_servo() ; - static void do_repeat_relay() ; - static void change_command(uint8_t index) ; - static void update_commands(void) ; - static void verify_commands(void) ; - static bool process_next_command() ; - static void process_must() ; - static void process_may() ; - static void process_now() ; - static void read_control_switch() ; - static byte readSwitch(void); - static void reset_control_switch() ; - static void read_trim_switch() ; - static void auto_trim() ; - static void trim_accel() ; - static void failsafe_on_event() ; - static void failsafe_off_event() ; - static void low_battery_event(void) ; - void piezo_on() ; - void piezo_off() ; - void piezo_beep() ; - void roll_flip() ; - static void heli_init_swash() ; - static void heli_move_servos_to_mid() ; - static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) ; - static void init_motors_out() ; - static void output_motors_armed() ; - static void output_motors_disarmed() ; - static void output_motor_test() ; - static int heli_get_scaled_throttle(int throttle) ; - static int heli_get_angle_boost(int pwm_out) ; -static void update_lights() ; - static void update_GPS_light(void) ; - static void update_motor_light(void) ; - static void dancing_light() ; - static void clear_leds() ; - static void update_motor_leds(void) ; - static void arm_motors() ; - static void set_servos_4() ; - static void init_motors_out() ; - static void output_motors_armed() ; - static void output_motors_disarmed() ; - static void output_motor_test() ; - static void init_motors_out() ; - static void output_motors_armed() ; - static void output_motors_disarmed() ; - static void output_motor_test() ; - static void init_motors_out() ; - static void output_motors_armed() ; - static void output_motors_disarmed() ; - static void output_motor_test() ; - static void init_motors_out() ; - static void output_motors_armed() ; - static void output_motors_disarmed() ; - static void debug_motors() ; - static void output_motor_test() ; - static void init_motors_out() ; - static void output_motors_armed() ; - static void output_motors_disarmed() ; - static void output_motor_test() ; - static void init_motors_out() ; - static void output_motors_armed() ; - static void output_motors_disarmed() ; - static void output_motor_test() ; - static byte navigate() ; - static bool check_missed_wp() ; - static void calc_location_error(struct Location *next_loc) ; - static void calc_loiter(int x_error, int y_error) ; - static void calc_loiter2(int x_error, int y_error) ; - static void calc_loiter_pitch_roll() ; - static void calc_nav_rate(int max_speed) ; - static void calc_nav_pitch_roll() ; - static long get_altitude_error() ; - static int get_loiter_angle() ; - static long wrap_360(long error) ; - static long wrap_180(long error) ; - static long get_crosstrack_correction(void) ; - static long cross_track_test() ; - static void reset_crosstrack() ; - static long get_distance(struct Location *loc1, struct Location *loc2) ; - static long get_alt_distance(struct Location *loc1, struct Location *loc2) ; - static long get_bearing(struct Location *loc1, struct Location *loc2) ; - static void default_dead_zones() ; - static void init_rc_in() ; - static void init_rc_out() ; - void output_min() ; - static void read_radio() ; - static void throttle_failsafe(uint16_t pwm) ; - static void trim_radio() ; - static void ReadSCP1000(void) ; - static void init_barometer(void) ; - static long read_baro_filtered(void) ; - static long read_barometer(void) ; - static void read_airspeed(void) ; - static void zero_airspeed(void) ; - static void read_battery(void) ; - static void clear_offsets() ; - static void report_batt_monitor() ; - static void report_sonar() ; - static void report_frame() ; - static void report_radio() ; - static void report_imu() ; - static void report_compass() ; - static void report_flight_modes() ; - void report_optflow() ; - static void report_heli() ; - static void report_gyro() ; - static void print_radio_values() ; - static void print_switch(byte p, byte m, bool b) ; - static void print_done() ; - static void zero_eeprom(void) ; - static void print_accel_offsets(void) ; - static void print_gyro_offsets(void) ; - static RC_Channel * heli_get_servo(int servo_num); - static int read_num_from_serial() ; - static void print_blanks(int num) ; - static void print_divider(void) ; - static void print_enabled(boolean b) ; - static void init_esc() ; - static void print_wp(struct Location *cmd, byte index) ; - static void report_gps() ; - static void report_version() ; - static void report_tuning() ; - static void run_cli(void) ; - static void init_ardupilot() ; - static void startup_ground(void) ; - static void set_mode(byte mode) ; - static void set_failsafe(boolean mode) ; - static void init_compass() ; - static void init_optflow() ; - static void init_simple_bearing() ; - static void init_throttle_cruise() ; - static boolean check_startup_for_CLI() ; - static boolean check_startup_for_CLI() ; - static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) ; - static void print_hit_enter() ; - static void fake_out_gps() ; - static void print_motor_out(); -#line 88 "/home/jgoppert/Projects/ardupilotone/ArduCopter/ArduCopter.pde" -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port -FastSerialPort3(Serial3); // Telemetry port - -//////////////////////////////////////////////////////////////////////////////// -// Parameters -//////////////////////////////////////////////////////////////////////////////// -// -// Global parameters are all contained within the 'g' class. -// -static Parameters g; - - -//////////////////////////////////////////////////////////////////////////////// -// prototypes -static void update_events(void); - - -//////////////////////////////////////////////////////////////////////////////// -// Sensors -//////////////////////////////////////////////////////////////////////////////// -// -// There are three basic options related to flight sensor selection. -// -// - Normal flight mode. Real sensors are used. -// - HIL Attitude mode. Most sensors are disabled, as the HIL -// protocol supplies attitude information directly. -// - HIL Sensors mode. Synthetic sensors are configured that -// supply data from the simulation. -// - -// All GPS access should be through this pointer. -static GPS *g_gps; - -// flight modes convenience array -static AP_Int8 *flight_modes = &g.flight_mode1; - -#if HIL_MODE == HIL_MODE_DISABLED - - // real sensors - AP_ADC_ADS7844 adc; - APM_BMP085_Class barometer; - AP_Compass_HMC5843 compass(Parameters::k_param_compass); - - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; - #endif - - // real GPS selection - #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO - AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); - - #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA - AP_GPS_NMEA g_gps_driver(&Serial1); - - #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF - AP_GPS_SIRF g_gps_driver(&Serial1); - - #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX - AP_GPS_UBLOX g_gps_driver(&Serial1); - - #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK - AP_GPS_MTK g_gps_driver(&Serial1); - - #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 - AP_GPS_MTK16 g_gps_driver(&Serial1); - - #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE - AP_GPS_None g_gps_driver(NULL); - - #else - #error Unrecognised GPS_PROTOCOL setting. - #endif // GPS PROTOCOL - -#elif HIL_MODE == HIL_MODE_SENSORS - // sensor emulators - AP_ADC_HIL adc; - APM_BMP085_HIL_Class barometer; - AP_Compass_HIL compass; - AP_GPS_HIL g_gps_driver(NULL); - -#elif HIL_MODE == HIL_MODE_ATTITUDE - AP_ADC_HIL adc; - AP_DCM_HIL dcm; - AP_GPS_HIL g_gps_driver(NULL); - AP_Compass_HIL compass; // never used - AP_IMU_Shim imu; // never used - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; - #endif - static int32_t gps_base_alt; -#else - #error Unrecognised HIL_MODE setting. -#endif // HIL MODE - -#if HIL_MODE != HIL_MODE_ATTITUDE - #if HIL_MODE != HIL_MODE_SENSORS - // Normal - AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); - #else - // hil imu - AP_IMU_Shim imu; - #endif - // normal dcm - AP_DCM dcm(&imu, g_gps); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// GCS selection -//////////////////////////////////////////////////////////////////////////////// -GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0); -GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3); - -//////////////////////////////////////////////////////////////////////////////// -// SONAR selection -//////////////////////////////////////////////////////////////////////////////// -// -ModeFilter sonar_mode_filter; - -#if SONAR_TYPE == MAX_SONAR_XL - AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); -#else - #error Unrecognised SONAR_TYPE setting. -#endif - -// agmatthews USERHOOKS -//////////////////////////////////////////////////////////////////////////////// -// User variables -//////////////////////////////////////////////////////////////////////////////// -#ifdef USERHOOK_VARIABLES -#include USERHOOK_VARIABLES -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Global variables -//////////////////////////////////////////////////////////////////////////////// -static const char *comma = ","; - -static const char* flight_mode_strings[] = { - "STABILIZE", - "ACRO", - "ALT_HOLD", - "AUTO", - "GUIDED", - "LOITER", - "RTL", - "CIRCLE", - "POSITION"}; - -/* Radio values - Channel assignments - 1 Ailerons (rudder if no ailerons) - 2 Elevator - 3 Throttle - 4 Rudder (if we have ailerons) - 5 Mode - 3 position switch - 6 User assignable - 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second) - 8 TBD -*/ - -// test -#if ACCEL_ALT_HOLD == 1 -Vector3f accels_rot; -static int accels_rot_count; -static float accels_rot_sum; -static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN; -#endif - -// temp -static int y_actual_speed; -static int y_rate_error; - -// calc the -static int x_actual_speed; -static int x_rate_error; - -// Radio -// ----- -static byte control_mode = STABILIZE; -static byte old_control_mode = STABILIZE; -static byte oldSwitchPosition; // for remembering the control mode switch -static int motor_out[8]; -static bool do_simple = false; - -// Heli -// ---- -#if FRAME_CONFIG == HELI_FRAME -static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos -static int heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly -static long heli_servo_out[4]; // used for servo averaging for analog servos -static int heli_servo_out_count = 0; // use for servo averaging -#endif - -// Failsafe -// -------- -static boolean failsafe; // did our throttle dip below the failsafe value? -static boolean ch3_failsafe; -static boolean motor_armed; -static boolean motor_auto_armed; // if true, - -// PIDs -// ---- -static Vector3f omega; -float tuning_value; - -// LED output -// ---------- -static boolean motor_light; // status of the Motor safety -static boolean GPS_light; // status of the GPS light -static byte led_mode = NORMAL_LEDS; - -// GPS variables -// ------------- -static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage -static float scaleLongUp = 1; // used to reverse longitude scaling -static float scaleLongDown = 1; // used to reverse longitude scaling -static byte ground_start_count = 10; // have we achieved first lock and set Home? -static bool did_ground_start = false; // have we ground started after first arming - -// Location & Navigation -// --------------------- -static const float radius_of_earth = 6378100; // meters -static const float gravity = 9.81; // meters/ sec^2 -static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target - -static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate -static byte wp_control; // used to control - navgation or loiter - -static byte command_must_index; // current command memory location -static byte command_may_index; // current command memory location -static byte command_must_ID; // current command ID -static byte command_may_ID; // current command ID -static byte wp_verify_byte; // used for tracking state of navigating waypoints - -static float cos_roll_x = 1; -static float cos_pitch_x = 1; -static float cos_yaw_x = 1; -static float sin_pitch_y, sin_yaw_y, sin_roll_y; -static long initial_simple_bearing; // used for Simple mode -static float simple_sin_y, simple_cos_x; -static byte jump = -10; // used to track loops in jump command -static int waypoint_speed_gov; - -// Acro -#if CH7_OPTION == CH7_FLIP -static bool do_flip = false; -#endif - -static boolean trim_flag; -static int CH7_wp_index = 0; - -// Airspeed -// -------- -static int airspeed; // m/s * 100 - -// Location Errors -// --------------- -static long altitude_error; // meters * 100 we are off in altitude -static long old_altitude; -static int old_rate; -static long yaw_error; // how off are we pointed -static long long_error, lat_error; // temp for debugging - -// Battery Sensors -// --------------- -static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter -static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter -static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter - -static float current_amps; -static float current_total; -static bool low_batt = false; - -// Barometer Sensor variables -// -------------------------- -static long abs_pressure; -static long ground_pressure; -static int ground_temperature; - -// Altitude Sensor variables -// ---------------------- -static int sonar_alt; -static int baro_alt; -static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR -static int altitude_rate; - -// flight mode specific -// -------------------- -static byte yaw_mode; -static byte roll_pitch_mode; -static byte throttle_mode; - -static boolean takeoff_complete; // Flag for using take-off controls -static boolean land_complete; -static long old_alt; // used for managing altitude rates -static int velocity_land; -static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target -static int manual_boost; // used in adjust altitude to make changing alt faster -static int angle_boost; // used in adjust altitude to make changing alt faster - -// Loiter management -// ----------------- -static long original_target_bearing; // deg * 100, used to check we are not passing the WP -static long old_target_bearing; // used to track difference in angle - -static int loiter_total; // deg : how many times to loiter * 360 -static int loiter_sum; // deg : how far we have turned around a waypoint -static unsigned long loiter_time; // millis : when we started LOITER mode -static unsigned loiter_time_max; // millis : how long to stay in LOITER mode - - -// these are the values for navigation control functions -// ---------------------------------------------------- -static long nav_roll; // deg * 100 : target roll angle -static long nav_pitch; // deg * 100 : target pitch angle -static long nav_yaw; // deg * 100 : target yaw angle -static long auto_yaw; // deg * 100 : target yaw angle -static long nav_lat; // for error calcs -static long nav_lon; // for error calcs -static int nav_throttle; // 0-1000 for throttle control - -static unsigned long throttle_integrator; // used to integrate throttle output to predict battery life -static bool invalid_throttle; // used to control when we calculate nav_throttle -//static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value - -static long command_yaw_start; // what angle were we to begin with -static unsigned long command_yaw_start_time; // when did we start turning -static unsigned int command_yaw_time; // how long we are turning -static long command_yaw_end; // what angle are we trying to be -static long command_yaw_delta; // how many degrees will we turn -static int command_yaw_speed; // how fast to turn -static byte command_yaw_dir; -static byte command_yaw_relative; - -static int auto_level_counter; - -// Waypoints -// --------- -static long wp_distance; // meters - distance between plane and next waypoint -static long wp_totalDistance; // meters - distance between old and next waypoint -//static byte next_wp_index; // Current active command index - -// repeating event control -// ----------------------- -static byte event_id; // what to do - see defines -static unsigned long event_timer; // when the event was asked for in ms -static unsigned int event_delay; // how long to delay the next firing of event in millis -static int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice -static int event_value; // per command value, such as PWM for servos -static int event_undo_value; // the value used to undo commands -//static byte repeat_forever; -//static byte undo_event; // counter for timing the undo - -// delay command -// -------------- -static long condition_value; // used in condition commands (eg delay, change alt, etc.) -static long condition_start; -//static int condition_rate; - -// land command -// ------------ -static long land_start; // when we intiated command in millis() -static long original_alt; // altitide reference for start of command - -// 3D Location vectors -// ------------------- -static struct Location home; // home location -static struct Location prev_WP; // last waypoint -static struct Location current_loc; // current location -static struct Location next_WP; // next waypoint -static struct Location target_WP; // where do we want to you towards? -static struct Location next_command; // command preloaded -static struct Location guided_WP; // guided mode waypoint -static long target_altitude; // used for -static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location - -// IMU variables -// ------------- -static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) - -// Performance monitoring -// ---------------------- -static long perf_mon_timer; -//static float imu_health; // Metric based on accel gain deweighting -static int gps_fix_count; -static byte gps_watchdog; - -// System Timers -// -------------- -static unsigned long fast_loopTimer; // Time in miliseconds of main control loop -static byte medium_loopCounter; // Counters for branching from main control loop to slower loops - -static unsigned long fiftyhz_loopTimer; - -static byte slow_loopCounter; -static int superslow_loopCounter; -static byte simple_timer; // for limiting the execution of flight mode thingys - - -static float dTnav; // Delta Time in milliseconds for navigation computations -static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav - -static byte counter_one_herz; -static bool GPS_enabled = false; -static bool new_radio_frame; - -AP_Relay relay; - -//////////////////////////////////////////////////////////////////////////////// -// Top-level logic -//////////////////////////////////////////////////////////////////////////////// - -void setup() { - memcheck_init(); - init_ardupilot(); -} - -void loop() -{ - long timer = micros(); - // We want this to execute fast - // ---------------------------- - if ((timer - fast_loopTimer) >= 4000) { - //PORTK |= B00010000; - G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops - fast_loopTimer = timer; - - // Execute the fast loop - // --------------------- - fast_loop(); - } - //PORTK &= B11101111; - - if ((timer - fiftyhz_loopTimer) >= 20000) { - fiftyhz_loopTimer = timer; - //PORTK |= B01000000; - - // reads all of the necessary trig functions for cameras, throttle, etc. - update_trig(); - - // perform 10hz tasks - medium_loop(); - - // Stuff to run at full 50hz, but after the loops - fifty_hz_loop(); - - counter_one_herz++; - - if(counter_one_herz == 50){ - super_slow_loop(); - counter_one_herz = 0; - } - - if (millis() - perf_mon_timer > 1200 /*20000*/) { - if (g.log_bitmask & MASK_LOG_PM) - Log_Write_Performance(); - - gps_fix_count = 0; - perf_mon_timer = millis(); - } - //PORTK &= B10111111; - } -} -// PORTK |= B01000000; -// PORTK &= B10111111; - -// Main loop -static void fast_loop() -{ - // try to send any deferred messages if the serial port now has - // some space available - gcs_send_message(MSG_RETRY_DEFERRED); - - // Read radio - // ---------- - read_radio(); - - // IMU DCM Algorithm - read_AHRS(); - - // custom code/exceptions for flight modes - // --------------------------------------- - update_yaw_mode(); - update_roll_pitch_mode(); - - // write out the servo PWM values - // ------------------------------ - set_servos_4(); - - //if(motor_armed) - //Log_Write_Attitude(); - -// agmatthews - USERHOOKS -#ifdef USERHOOK_FASTLOOP - USERHOOK_FASTLOOP -#endif - -} - -static void medium_loop() -{ - // This is the start of the medium (10 Hz) loop pieces - // ----------------------------------------- - switch(medium_loopCounter) { - - // This case deals with the GPS and Compass - //----------------------------------------- - case 0: - medium_loopCounter++; - - #ifdef OPTFLOW_ENABLED - if(g.optflow_enabled){ - optflow.read(); - optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow - - // write to log - if (g.log_bitmask & MASK_LOG_OPTFLOW){ - Log_Write_Optflow(); - } - } - #endif - - if(GPS_enabled){ - update_GPS(); - } - - //readCommands(); - - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.compass_enabled){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - } - #endif - - // auto_trim, uses an auto_level algorithm - auto_trim(); - - // record throttle output - // ------------------------------ - throttle_integrator += g.rc_3.servo_out; - break; - - // This case performs some navigation computations - //------------------------------------------------ - case 1: - medium_loopCounter++; - - // Auto control modes: - if(g_gps->new_data && g_gps->fix){ - - // invalidate GPS data - g_gps->new_data = false; - - // we are not tracking I term on navigation, so this isn't needed - dTnav = (float)(millis() - nav_loopTimer)/ 1000.0; - nav_loopTimer = millis(); - - // prevent runup from bad GPS - dTnav = min(dTnav, 1.0); - - // calculate the copter's desired bearing and WP distance - // ------------------------------------------------------ - if(navigate()){ - - // control mode specific updates - // ----------------------------- - update_navigation(); - - if (g.log_bitmask & MASK_LOG_NTUN) - Log_Write_Nav_Tuning(); - } - }else{ - g_gps->new_data = false; - } - break; - - // command processing - //------------------- - case 2: - medium_loopCounter++; - - // Read altitude from sensors - // -------------------------- - update_altitude(); - - // invalidate the throttle hold value - // ---------------------------------- - invalid_throttle = true; - - break; - - // This case deals with sending high rate telemetry - //------------------------------------------------- - case 3: - medium_loopCounter++; - - // perform next command - // -------------------- - if(control_mode == AUTO){ - update_commands(); - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - if(motor_armed){ - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) - Log_Write_Attitude(); - - if (g.log_bitmask & MASK_LOG_CTUN) - Log_Write_Control_Tuning(); - } - #endif - - // send all requested output streams with rates requested - // between 5 and 45 Hz - gcs_data_stream_send(5,45); - - if (g.log_bitmask & MASK_LOG_MOTORS) - Log_Write_Motors(); - - break; - - // This case controls the slow loop - //--------------------------------- - case 4: - medium_loopCounter = 0; - - if (g.battery_monitoring != 0){ - read_battery(); - } - - // Accel trims = hold > 2 seconds - // Throttle cruise = switch less than 1 second - // -------------------------------------------- - read_trim_switch(); - - // Check for engine arming - // ----------------------- - arm_motors(); - - - slow_loop(); - break; - - default: - // this is just a catch all - // ------------------------ - medium_loopCounter = 0; - break; - } -// agmatthews - USERHOOKS -#ifdef USERHOOK_MEDIUMLOOP - USERHOOK_MEDIUMLOOP -#endif - -} - -// stuff that happens at 50 hz -// --------------------------- -static void fifty_hz_loop() -{ - // moved to slower loop - // -------------------- - update_throttle_mode(); - - // Read Sonar - // ---------- - if(g.sonar_enabled){ - sonar_alt = sonar.read(); - } - // agmatthews - USERHOOKS - #ifdef USERHOOK_50HZLOOP - USERHOOK_50HZLOOP - #endif - - #if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME - // HIL for a copter needs very fast update of the servo values - gcs_send_message(MSG_RADIO_OUT); - #endif - - camera_stabilization(); - - # if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) - Log_Write_Attitude(); - - if (g.log_bitmask & MASK_LOG_RAW) - Log_Write_Raw(); - #endif - - // kick the GCS to process uplink data - gcs_update(); - gcs_data_stream_send(45,1000); - - #if FRAME_CONFIG == TRI_FRAME - // servo Yaw - g.rc_4.calc_pwm(); - APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - #endif -} - - -static void slow_loop() -{ - // This is the slow (3 1/3 Hz) loop pieces - //---------------------------------------- - switch (slow_loopCounter){ - case 0: - slow_loopCounter++; - superslow_loopCounter++; - - if(superslow_loopCounter > 1200){ - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled){ - compass.save_offsets(); - superslow_loopCounter = 0; - } - #endif - } - break; - - case 1: - slow_loopCounter++; - - // Read 3-position switch on radio - // ------------------------------- - read_control_switch(); - - // Read main battery voltage if hooked up - does not read the 5v from radio - // ------------------------------------------------------------------------ - //#if BATTERY_EVENT == 1 - // read_battery(); - //#endif - - #if AUTO_RESET_LOITER == 1 - if(control_mode == LOITER){ - //if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ - // reset LOITER to current position - //next_WP = current_loc; - //} - } - #endif - - break; - - case 2: - slow_loopCounter = 0; - update_events(); - - // blink if we are armed - update_lights(); - - // send all requested output streams with rates requested - // between 1 and 5 Hz - gcs_data_stream_send(1,5); - - if(g.radio_tuning > 0) - tuning(); - - #if MOTOR_LEDS == 1 - update_motor_leds(); - #endif - - break; - - default: - slow_loopCounter = 0; - break; - - } - // agmatthews - USERHOOKS - #ifdef USERHOOK_SLOWLOOP - USERHOOK_SLOWLOOP - #endif - -} - -// 1Hz loop -static void super_slow_loop() -{ - if (g.log_bitmask & MASK_LOG_CUR) - Log_Write_Current(); - - gcs_send_message(MSG_HEARTBEAT); - // agmatthews - USERHOOKS - #ifdef USERHOOK_SUPERSLOWLOOP - USERHOOK_SUPERSLOWLOOP - #endif -} - -static void update_GPS(void) -{ - g_gps->update(); - update_GPS_light(); - - //current_loc.lng = 377697000; // Lon * 10 * *7 - //current_loc.lat = -1224318000; // Lat * 10 * *7 - //current_loc.alt = 100; // alt * 10 * *7 - //return; - if(gps_watchdog < 12){ - gps_watchdog++; - }else{ - // we have lost GPS signal for a moment. Reduce our error to avoid flyaways - // commented temporarily - //nav_roll >>= 1; - //nav_pitch >>= 1; - } - - if (g_gps->new_data && g_gps->fix) { - gps_watchdog = 0; - - // for performance - // --------------- - gps_fix_count++; - - if(ground_start_count > 1){ - ground_start_count--; - - } else if (ground_start_count == 1) { - - // We countdown N number of good GPS fixes - // so that the altitude is more accurate - // ------------------------------------- - if (current_loc.lat == 0) { - ground_start_count = 5; - - }else{ - init_home(); - ground_start_count = 0; - } - } - - current_loc.lng = g_gps->longitude; // Lon * 10 * *7 - current_loc.lat = g_gps->latitude; // Lat * 10 * *7 - - if (g.log_bitmask & MASK_LOG_GPS){ - Log_Write_GPS(); - } - } -} - - -void update_yaw_mode(void) -{ - switch(yaw_mode){ - case YAW_ACRO: - g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in); - return; - break; - - case YAW_HOLD: - // calcualte new nav_yaw offset - if (control_mode <= STABILIZE){ - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); - }else{ - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); - } - break; - - case YAW_LOOK_AT_HOME: - //nav_yaw updated in update_navigation() - break; - - case YAW_AUTO: - nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -20, 20); - nav_yaw = wrap_360(nav_yaw); - break; - } - - // Yaw control - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); - - //Serial.printf("4: %d\n",g.rc_4.servo_out); -} - -void update_roll_pitch_mode(void) -{ - #if CH7_OPTION == CH7_FLIP - if (do_flip){ - roll_flip(); - return; - } - #endif - - int control_roll = 0, control_pitch = 0; - - //read_radio(); - if(do_simple && new_radio_frame){ - new_radio_frame = false; - simple_timer++; - - int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; - - if (simple_timer == 1){ - // roll - simple_cos_x = sin(radians(90 - delta)); - - }else if (simple_timer > 2){ - // pitch - simple_sin_y = cos(radians(90 - delta)); - simple_timer = 0; - } - - // Rotate input by the initial bearing - control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; - control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); - - g.rc_1.control_in = control_roll; - g.rc_2.control_in = control_pitch; - } - - switch(roll_pitch_mode){ - case ROLL_PITCH_ACRO: - g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); - break; - - case ROLL_PITCH_STABLE: - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); - break; - - case ROLL_PITCH_AUTO: - // mix in user control with Nav control - control_roll = g.rc_1.control_mix(nav_roll); - control_pitch = g.rc_2.control_mix(nav_pitch); - g.rc_1.servo_out = get_stabilize_roll(control_roll); - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); - break; - } -} - -// 50 hz update rate, not 250 -void update_throttle_mode(void) -{ - switch(throttle_mode){ - - case THROTTLE_MANUAL: - if (g.rc_3.control_in > 0){ - #if FRAME_CONFIG == HELI_FRAME - g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.rc_3.control_in)); - #else - angle_boost = get_angle_boost(g.rc_3.control_in); - g.rc_3.servo_out = g.rc_3.control_in + angle_boost; - #endif - }else{ - g.pi_stabilize_roll.reset_I(); - g.pi_stabilize_pitch.reset_I(); - g.pi_rate_roll.reset_I(); - g.pi_rate_pitch.reset_I(); - g.rc_3.servo_out = 0; - } - break; - - case THROTTLE_HOLD: - // allow interactive changing of atitude - adjust_altitude(); - // fall through - - case THROTTLE_AUTO: - // 10hz, don't run up i term - if(invalid_throttle && motor_auto_armed == true){ - - // how far off are we - altitude_error = get_altitude_error(); - - // get the AP throttle - nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s - //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); - - // clear the new data flag - invalid_throttle = false; - } - #if FRAME_CONFIG == HELI_FRAME - g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle); - #else - angle_boost = get_angle_boost(g.throttle_cruise); - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost; - #endif - break; - } -} - -// called after a GPS read -static void update_navigation() -{ - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ - switch(control_mode){ - case AUTO: - verify_commands(); - // note: wp_control is handled by commands_logic - - // calculates desired Yaw - update_auto_yaw(); - - // calculates the desired Roll and Pitch - update_nav_wp(); - break; - - case GUIDED: - wp_control = WP_MODE; - // check if we are close to point > loiter - wp_verify_byte = 0; - verify_nav_wp(); - - if (wp_control == WP_MODE) { - update_auto_yaw(); - } else { - set_mode(LOITER); - } - update_nav_wp(); - break; - - case RTL: - if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ - // lets just jump to Loiter Mode after RTL - set_mode(LOITER); - }else{ - // calculates desired Yaw - // XXX this is an experiment - #if FRAME_CONFIG == HELI_FRAME - update_auto_yaw(); - #endif - - wp_control = WP_MODE; - } - - // calculates the desired Roll and Pitch - update_nav_wp(); - break; - - // switch passthrough to LOITER - case LOITER: - case POSITION: - wp_control = LOITER_MODE; - - // calculates the desired Roll and Pitch - update_nav_wp(); - break; - - case CIRCLE: - yaw_tracking = MAV_ROI_WPNEXT; - wp_control = CIRCLE_MODE; - - // calculates desired Yaw - update_auto_yaw(); - update_nav_wp(); - break; - - } - - if(yaw_mode == YAW_LOOK_AT_HOME){ - if(home_is_set){ - //nav_yaw = point_at_home_yaw(); - nav_yaw = get_bearing(¤t_loc, &home); - } else { - nav_yaw = 0; - } - } -} - -static void read_AHRS(void) -{ - // Perform IMU calculations and get attitude info - //----------------------------------------------- - #if HIL_MODE == HIL_MODE_SENSORS - // update hil before dcm update - gcs_update(); - #endif - - dcm.update_DCM_fast(); - omega = dcm.get_gyro(); -} - -static void update_trig(void){ - Vector2f yawvector; - Matrix3f temp = dcm.get_dcm_matrix(); - - yawvector.x = temp.a.x; // sin - yawvector.y = temp.b.x; // cos - yawvector.normalize(); - - - sin_pitch_y = -temp.c.x; - cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x)); - - cos_roll_x = temp.c.z / cos_pitch_x; - sin_roll_y = temp.c.y / cos_pitch_x; - - cos_yaw_x = yawvector.y; // 0 x = north - sin_yaw_y = yawvector.x; // 1 y - - //flat: - // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, - // 90° = cos_yaw: 1.00, sin_yaw: 0.00, - // 180 = cos_yaw: 0.00, sin_yaw: -1.00, - // 270 = cos_yaw: -1.00, sin_yaw: 0.00, -} - -// updated at 10hz -static void update_altitude() -{ - altitude_sensor = BARO; - - #if HIL_MODE == HIL_MODE_ATTITUDE - current_loc.alt = g_gps->altitude - gps_base_alt; - return; - #else - - if(g.sonar_enabled){ - // filter out offset - float scale; - - // read barometer - baro_alt = read_barometer(); - - if(baro_alt < 1000){ - - #if SONAR_TILT_CORRECTION == 1 - // correct alt for angle of the sonar - float temp = cos_pitch_x * cos_roll_x; - temp = max(temp, 0.707); - sonar_alt = (float)sonar_alt * temp; - #endif - - scale = (sonar_alt - 400) / 200; - scale = constrain(scale, 0, 1); - current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; - }else{ - current_loc.alt = baro_alt + home.alt; - } - - }else{ - baro_alt = read_barometer(); - // no sonar altitude - current_loc.alt = baro_alt + home.alt; - } - - // calc the accel rate limit to 2m/s - altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer - - // rate limiter to reduce some of the motor pulsing - if (altitude_rate > 0){ - // going up - altitude_rate = min(altitude_rate, old_rate + 20); - }else{ - // going down - altitude_rate = max(altitude_rate, old_rate - 20); - } - - old_rate = altitude_rate; - old_altitude = current_loc.alt; - #endif -} - -static void -adjust_altitude() -{ - if(g.rc_3.control_in <= 200){ - next_WP.alt -= 1; // 1 meter per second - next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location - next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter - //manual_boost = (g.rc_3.control_in == 0) ? -20 : 0; - - }else if (g.rc_3.control_in > 700){ - next_WP.alt += 1; // 1 meter per second - next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location - //manual_boost = (g.rc_3.control_in == 800) ? 20 : 0; - } -} - -static void tuning(){ - tuning_value = (float)g.rc_6.control_in / 1000.0; - - switch(g.radio_tuning){ - - /*case CH6_STABILIZE_KP: - g.rc_6.set_range(0,2000); // 0 to 8 - tuning_value = (float)g.rc_6.control_in / 100.0; - alt_hold_gain = tuning_value; - break;*/ - - case CH6_STABILIZE_KP: - g.rc_6.set_range(0,8000); // 0 to 8 - g.pi_stabilize_roll.kP(tuning_value); - g.pi_stabilize_pitch.kP(tuning_value); - break; - - case CH6_STABILIZE_KI: - g.rc_6.set_range(0,300); // 0 to .3 - tuning_value = (float)g.rc_6.control_in / 1000.0; - g.pi_stabilize_roll.kI(tuning_value); - g.pi_stabilize_pitch.kI(tuning_value); - break; - - case CH6_RATE_KP: - g.rc_6.set_range(0,300); // 0 to .3 - g.pi_rate_roll.kP(tuning_value); - g.pi_rate_pitch.kP(tuning_value); - break; - - case CH6_RATE_KI: - g.rc_6.set_range(0,300); // 0 to .3 - g.pi_rate_roll.kI(tuning_value); - g.pi_rate_pitch.kI(tuning_value); - break; - - case CH6_YAW_KP: - g.rc_6.set_range(0,1000); - g.pi_stabilize_yaw.kP(tuning_value); - break; - - case CH6_YAW_RATE_KP: - g.rc_6.set_range(0,1000); - g.pi_rate_yaw.kP(tuning_value); - break; - - case CH6_THROTTLE_KP: - g.rc_6.set_range(0,1000); - g.pi_throttle.kP(tuning_value); - break; - - case CH6_TOP_BOTTOM_RATIO: - g.rc_6.set_range(800,1000); // .8 to 1 - g.top_bottom_ratio = tuning_value; - break; - - case CH6_RELAY: - g.rc_6.set_range(0,1000); - if (g.rc_6.control_in > 525) relay.on(); - if (g.rc_6.control_in < 475) relay.off(); - break; - - case CH6_TRAVERSE_SPEED: - g.rc_6.set_range(0,1000); - g.waypoint_speed_max = g.rc_6.control_in; - break; - - case CH6_LOITER_P: - g.rc_6.set_range(0,1000); - g.pi_loiter_lat.kP(tuning_value); - g.pi_loiter_lon.kP(tuning_value); - break; - - case CH6_NAV_P: - g.rc_6.set_range(0,6000); - g.pi_nav_lat.kP(tuning_value); - g.pi_nav_lon.kP(tuning_value); - break; - - #if FRAME_CONFIG == HELI_FRAME - case CH6_HELI_EXTERNAL_GYRO: - g.rc_6.set_range(1000,2000); - g.heli_ext_gyro_gain = tuning_value * 1000; - break; - #endif - } -} - -static void update_nav_wp() -{ - if(wp_control == LOITER_MODE){ - - // calc a pitch to the target - calc_location_error(&next_WP); - - // use error as the desired rate towards the target - calc_loiter(long_error, lat_error); - - // rotate pitch and roll to the copter frame of reference - calc_loiter_pitch_roll(); - - }else if(wp_control == CIRCLE_MODE){ - - // check if we have missed the WP - int loiter_delta = (target_bearing - old_target_bearing)/100; - - // reset the old value - old_target_bearing = target_bearing; - - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; - - // sum the angle around the WP - loiter_sum += loiter_delta; - - // create a virtual waypoint that circles the next_WP - // Count the degrees we have circulated the WP - int circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100; - - target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle))); - target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle))); - - // calc the lat and long error to the target - calc_location_error(&target_WP); - - // use error as the desired rate towards the target - // nav_lon, nav_lat is calculated - calc_loiter(long_error, lat_error); - - // rotate pitch and roll to the copter frame of reference - calc_loiter_pitch_roll(); - - } else { - // use error as the desired rate towards the target - calc_nav_rate(g.waypoint_speed_max); - // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll(); - } -} - -static void update_auto_yaw() -{ - // this tracks a location so the copter is always pointing towards it. - if(yaw_tracking == MAV_ROI_LOCATION){ - auto_yaw = get_bearing(¤t_loc, &target_WP); - - }else if(yaw_tracking == MAV_ROI_WPNEXT){ - auto_yaw = target_bearing; - } - // MAV_ROI_NONE = basic Yaw hold -} - - - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/Attitude.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static int -get_stabilize_roll(long target_angle) -{ - long error; - long rate; - - error = wrap_180(target_angle - dcm.roll_sensor); - - // limit the error we're feeding to the PID - error = constrain(error, -2500, 2500); - - // desired Rate: - rate = g.pi_stabilize_roll.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); - -#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters - // Rate P: - error = rate - (long)(degrees(omega.x) * 100.0); - rate = g.pi_rate_roll.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\n", (int)error, (int)rate); -#endif - - // output control: - return (int)constrain(rate, -2500, 2500); -} - -static int -get_stabilize_pitch(long target_angle) -{ - long error; - long rate; - - error = wrap_180(target_angle - dcm.pitch_sensor); - - // limit the error we're feeding to the PID - error = constrain(error, -2500, 2500); - - // desired Rate: - rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); - -#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters - // Rate P: - error = rate - (long)(degrees(omega.y) * 100.0); - rate = g.pi_rate_pitch.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\n", (int)error, (int)rate); -#endif - - // output control: - return (int)constrain(rate, -2500, 2500); -} - - -#define YAW_ERROR_MAX 2000 -static int -get_stabilize_yaw(long target_angle) -{ - long error; - long rate; - - yaw_error = wrap_180(target_angle - dcm.yaw_sensor); - - // limit the error we're feeding to the PID - yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX); - rate = g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt); - //Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate); - -#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters - if( ! g.heli_ext_gyro_enabled ) { - // Rate P: - error = rate - (long)(degrees(omega.z) * 100.0); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); - } -#else - // Rate P: - error = rate - (long)(degrees(omega.z) * 100.0); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\n", (int)error, (int)rate); -#endif - - // output control: - return (int)constrain(rate, -2500, 2500); -} - -#define ALT_ERROR_MAX 500 -static int -get_nav_throttle(long z_error) -{ - // limit error to prevent I term run up - z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); - int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 - - rate_error = rate_error - altitude_rate; - - // limit the rate - rate_error = constrain(rate_error, -120, 140); - return (int)g.pi_throttle.get_pi(rate_error, .1); -} - -static int -get_rate_roll(long target_rate) -{ - long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0); - return g.pi_acro_roll.get_pi(error, G_Dt); -} - -static int -get_rate_pitch(long target_rate) -{ - long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0); - return g.pi_acro_pitch.get_pi(error, G_Dt); -} - -static int -get_rate_yaw(long target_rate) -{ - long error; - error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0); - target_rate = g.pi_rate_yaw.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); -} - - -// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. -// Keeps outdated data out of our calculations -static void reset_hold_I(void) -{ - g.pi_loiter_lat.reset_I(); - g.pi_loiter_lon.reset_I(); - g.pi_crosstrack.reset_I(); -} - -// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. -// Keeps outdated data out of our calculations -static void reset_nav(void) -{ - nav_throttle = 0; - invalid_throttle = true; - - g.pi_nav_lat.reset_I(); - g.pi_nav_lon.reset_I(); - - long_error = 0; - lat_error = 0; -} - - -/************************************************************* -throttle control -****************************************************************/ - -static long -get_nav_yaw_offset(int yaw_input, int reset) -{ - long _yaw; - - if(reset == 0){ - // we are on the ground - return dcm.yaw_sensor; - - }else{ - // re-define nav_yaw if we have stick input - if(yaw_input != 0){ - // set nav_yaw + or - the current location - _yaw = (long)yaw_input + dcm.yaw_sensor; - // we need to wrap our value so we can be 0 to 360 (*100) - return wrap_360(_yaw); - - }else{ - // no stick input, lets not change nav_yaw - return nav_yaw; - } - } -} - -static int alt_hold_velocity() -{ - #if ACCEL_ALT_HOLD == 1 - Vector3f accel_filt; - float error; - - // subtract filtered Accel - error = abs(next_WP.alt - current_loc.alt) - 25; - error = min(error, 50.0); - error = max(error, 0.0); - error = 1 - (error/ 50.0); - - accel_filt = imu.get_accel_filtered(); - accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); - int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12 - - //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); - return constrain(output, -70, 70); - -// fast rise -//s: -17.6241, g:0.0000, e:1.0000, o:0 -//s: -18.4990, g:0.0000, e:1.0000, o:0 -//s: -19.3193, g:0.0000, e:1.0000, o:0 -//s: -13.1310, g:47.8700, e:1.0000, o:-158 - - #else - return 0; - #endif -} - -static int get_angle_boost(int value) -{ - float temp = cos_pitch_x * cos_roll_x; - temp = 1.0 - constrain(temp, .5, 1.0); - return (int)(temp * value); -} - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/Camera.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -//#if CAMERA_STABILIZER == ENABLED - -static void init_camera() -{ - // ch 6 high(right) is down. - g.rc_camera_pitch.set_angle(4500); - g.rc_camera_roll.set_angle(4500); - - g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW); - g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW); -} - -static void -camera_stabilization() -{ - // PITCH - // ----- - // allow control mixing - g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. - g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor); - - g.rc_camera_pitch.servo_out = (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain; - - // limit - //g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500); - - // dont allow control mixing - /* - g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1; - */ - - - // ROLL - // ----- - // allow control mixing - /* - g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. - g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-dcm.roll_sensor); - */ - - // dont allow control mixing - g.rc_camera_roll.servo_out = (float)-dcm.roll_sensor * g.camera_roll_gain; - - // limit - //g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500); - - // Output - // ------ - g.rc_camera_pitch.calc_pwm(); - g.rc_camera_roll.calc_pwm(); - - APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out); - APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out); - //Serial.printf("c:%d\n", g.rc_camera_pitch.radio_out); -} - -//#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/GCS.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* -GCS Protocol - -4 Ardupilot Header -D -5 Payload length -1 Message ID -1 Message Version -9 Payload byte 1 -8 Payload byte 2 -7 Payload byte 3 -A Checksum byte 1 -B Checksum byte 2 - -*/ - -/* -#if GCS_PORT == 3 -# define SendSerw Serial3.write -# define SendSer Serial3.print -#else -# define SendSerw Serial.write -# define SendSer Serial.print -#endif - -byte mess_buffer[60]; -byte buff_pointer; - -// Unions for getting byte values -union f_out{ - byte bytes[4]; - float value; -} floatOut; - -union i_out { - byte bytes[2]; - int value; -} intOut; - -union l_out{ - byte bytes[4]; - long value; -} longOut; - -// Add binary values to the buffer -void write_byte(byte val) -{ - mess_buffer[buff_pointer++] = val; -} - -void write_int(int val ) -{ - intOut.value = val; - mess_buffer[buff_pointer++] = intOut.bytes[0]; - mess_buffer[buff_pointer++] = intOut.bytes[1]; -} - -void write_float(float val) -{ - floatOut.value = val; - mess_buffer[buff_pointer++] = floatOut.bytes[0]; - mess_buffer[buff_pointer++] = floatOut.bytes[1]; - mess_buffer[buff_pointer++] = floatOut.bytes[2]; - mess_buffer[buff_pointer++] = floatOut.bytes[3]; -} - -void write_long(long val) -{ - longOut.value = val; - mess_buffer[buff_pointer++] = longOut.bytes[0]; - mess_buffer[buff_pointer++] = longOut.bytes[1]; - mess_buffer[buff_pointer++] = longOut.bytes[2]; - mess_buffer[buff_pointer++] = longOut.bytes[3]; -} - -void flush(byte id) -{ - byte mess_ck_a = 0; - byte mess_ck_b = 0; - byte i; - - SendSer("4D"); // This is the message preamble - SendSerw(buff_pointer); // Length - SendSerw(2); // id - - for (i = 0; i < buff_pointer; i++) { - SendSerw(mess_buffer[i]); - } - - buff_pointer = 0; -} -*/ -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/GCS_Mavlink.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// use this to prevent recursion during sensor init -static bool in_mavlink_delay; - - -// this costs us 51 bytes, but means that low priority -// messages don't block the CPU -static mavlink_statustext_t pending_status; - -// true when we have received at least 1 MAVLink packet -static bool mavlink_active; - - -// check if a message will fit in the payload space available -#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false - -/* - !!NOTE!! - - the use of NOINLINE separate functions for each message type avoids - a compiler bug in gcc that would cause it to use far more stack - space than is needed. Without the NOINLINE we use the sum of the - stack needed for each message type. Please be careful to follow the - pattern below when adding any new messages - */ - -static NOINLINE void send_heartbeat(mavlink_channel_t chan) -{ - mavlink_msg_heartbeat_send( - chan, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); -} - -static NOINLINE void send_attitude(mavlink_channel_t chan) -{ - mavlink_msg_attitude_send( - chan, - micros(), - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); -} - -static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) -{ - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; - - switch(control_mode) { - case LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_HOLD; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case GUIDED: - mode = MAV_MODE_GUIDED; - break; - default: - mode = control_mode + 100; - } - - uint8_t status = MAV_STATE_ACTIVE; - uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 - - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - 0, - battery_voltage * 1000, - battery_remaining, - packet_drops); -} - -static void NOINLINE send_meminfo(mavlink_channel_t chan) -{ - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); -} - -static void NOINLINE send_location(mavlink_channel_t chan) -{ - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_global_position_int_send( - chan, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, - g_gps->ground_speed * rot.a.x, - g_gps->ground_speed * rot.b.x, - g_gps->ground_speed * rot.c.x); -} - -static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) -{ - mavlink_msg_nav_controller_output_send( - chan, - nav_roll / 1.0e2, - nav_pitch / 1.0e2, - target_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - 0, - 0); -} - -static void NOINLINE send_gps_raw(mavlink_channel_t chan) -{ - mavlink_msg_gps_raw_send( - chan, - micros(), - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); -} - -static void NOINLINE send_servo_out(mavlink_channel_t chan) -{ - const uint8_t rssi = 1; - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with HIL maintainers - - #if FRAME_CONFIG == HELI_FRAME - - mavlink_msg_rc_channels_scaled_send( - chan, - g.rc_1.servo_out, - g.rc_2.servo_out, - g.rc_3.radio_out, - g.rc_4.servo_out, - 0, - 0, - 0, - 0, - rssi); - - #else - - mavlink_msg_rc_channels_scaled_send( - chan, - g.rc_1.servo_out, - g.rc_2.servo_out, - g.rc_3.radio_out, - g.rc_4.servo_out, - 10000 * g.rc_1.norm_output(), - 10000 * g.rc_2.norm_output(), - 10000 * g.rc_3.norm_output(), - 10000 * g.rc_4.norm_output(), - rssi); - - #endif -} - -static void NOINLINE send_radio_in(mavlink_channel_t chan) -{ - const uint8_t rssi = 1; - mavlink_msg_rc_channels_raw_send( - chan, - g.rc_1.radio_in, - g.rc_2.radio_in, - g.rc_3.radio_in, - g.rc_4.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); -} - -static void NOINLINE send_radio_out(mavlink_channel_t chan) -{ - mavlink_msg_servo_output_raw_send( - chan, - motor_out[0], - motor_out[1], - motor_out[2], - motor_out[3], - motor_out[4], - motor_out[5], - motor_out[6], - motor_out[7]); -} - -static void NOINLINE send_vfr_hud(mavlink_channel_t chan) -{ - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, - g.rc_3.servo_out/10, - current_loc.alt / 100.0, - climb_rate); -} - -#if HIL_MODE != HIL_MODE_ATTITUDE -static void NOINLINE send_raw_imu1(mavlink_channel_t chan) -{ - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); - mavlink_msg_raw_imu_send( - chan, - micros(), - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); -} - -static void NOINLINE send_raw_imu2(mavlink_channel_t chan) -{ - mavlink_msg_scaled_pressure_send( - chan, - micros(), - (float)barometer.Press/100.0, - (float)(barometer.Press-ground_pressure)/100.0, - (int)(barometer.Temp*10)); -} - -static void NOINLINE send_raw_imu3(mavlink_channel_t chan) -{ - Vector3f mag_offsets = compass.get_offsets(); - - mavlink_msg_sensor_offsets_send(chan, - mag_offsets.x, - mag_offsets.y, - mag_offsets.z, - compass.get_declination(), - barometer.RawPress, - barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), - imu.ax(), imu.ay(), imu.az()); -} -#endif // HIL_MODE != HIL_MODE_ATTITUDE - -static void NOINLINE send_gps_status(mavlink_channel_t chan) -{ - mavlink_msg_gps_status_send( - chan, - g_gps->num_sats, - NULL, - NULL, - NULL, - NULL, - NULL); -} - -static void NOINLINE send_current_waypoint(mavlink_channel_t chan) -{ - mavlink_msg_waypoint_current_send( - chan, - g.waypoint_index); -} - -static void NOINLINE send_statustext(mavlink_channel_t chan) -{ - mavlink_msg_statustext_send( - chan, - pending_status.severity, - pending_status.text); -} - - -// try to send a message, return false if it won't fit in the serial tx buffer -static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) -{ - int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; - - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // defer any messages on the telemetry port for 1 second after - // bootup, to try to prevent bricking of Xbees - return false; - } - - switch(id) { - case MSG_HEARTBEAT: - CHECK_PAYLOAD_SIZE(HEARTBEAT); - send_heartbeat(chan); - return true; - - case MSG_EXTENDED_STATUS1: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - send_extended_status1(chan, packet_drops); - break; - - case MSG_EXTENDED_STATUS2: - CHECK_PAYLOAD_SIZE(MEMINFO); - send_meminfo(chan); - break; - - case MSG_ATTITUDE: - CHECK_PAYLOAD_SIZE(ATTITUDE); - send_attitude(chan); - break; - - case MSG_LOCATION: - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - send_location(chan); - break; - - case MSG_NAV_CONTROLLER_OUTPUT: - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - send_nav_controller_output(chan); - break; - - case MSG_GPS_RAW: - CHECK_PAYLOAD_SIZE(GPS_RAW); - send_gps_raw(chan); - break; - - case MSG_SERVO_OUT: - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - send_servo_out(chan); - break; - - case MSG_RADIO_IN: - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - send_radio_in(chan); - break; - - case MSG_RADIO_OUT: - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - send_radio_out(chan); - break; - - case MSG_VFR_HUD: - CHECK_PAYLOAD_SIZE(VFR_HUD); - send_vfr_hud(chan); - break; - -#if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU1: - CHECK_PAYLOAD_SIZE(RAW_IMU); - send_raw_imu1(chan); - break; - - case MSG_RAW_IMU2: - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - send_raw_imu2(chan); - break; - - case MSG_RAW_IMU3: - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - send_raw_imu3(chan); - break; -#endif // HIL_MODE != HIL_MODE_ATTITUDE - - case MSG_GPS_STATUS: - CHECK_PAYLOAD_SIZE(GPS_STATUS); - send_gps_status(chan); - break; - - case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); - send_current_waypoint(chan); - break; - - case MSG_NEXT_PARAM: - CHECK_PAYLOAD_SIZE(PARAM_VALUE); - if (chan == MAVLINK_COMM_0) { - gcs0.queued_param_send(); - } else { - gcs3.queued_param_send(); - } - break; - - case MSG_NEXT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); - if (chan == MAVLINK_COMM_0) { - gcs0.queued_waypoint_send(); - } else { - gcs3.queued_waypoint_send(); - } - break; - - case MSG_STATUSTEXT: - CHECK_PAYLOAD_SIZE(STATUSTEXT); - send_statustext(chan); - break; - - case MSG_RETRY_DEFERRED: - break; // just here to prevent a warning - } - return true; -} - - -#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED -static struct mavlink_queue { - enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES]; - uint8_t next_deferred_message; - uint8_t num_deferred_messages; -} mavlink_queue[2]; - -// send a message using mavlink -static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) -{ - uint8_t i, nextid; - struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; - - // see if we can send the deferred messages, if any - while (q->num_deferred_messages != 0) { - if (!mavlink_try_send_message(chan, - q->deferred_messages[q->next_deferred_message], - packet_drops)) { - break; - } - q->next_deferred_message++; - if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { - q->next_deferred_message = 0; - } - q->num_deferred_messages--; - } - - if (id == MSG_RETRY_DEFERRED) { - return; - } - - // this message id might already be deferred - for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { - if (q->deferred_messages[nextid] == id) { - // its already deferred, discard - return; - } - nextid++; - if (nextid == MAX_DEFERRED_MESSAGES) { - nextid = 0; - } - } - - if (q->num_deferred_messages != 0 || - !mavlink_try_send_message(chan, id, packet_drops)) { - // can't send it now, so defer it - if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { - // the defer buffer is full, discard - return; - } - nextid = q->next_deferred_message + q->num_deferred_messages; - if (nextid >= MAX_DEFERRED_MESSAGES) { - nextid -= MAX_DEFERRED_MESSAGES; - } - q->deferred_messages[nextid] = id; - q->num_deferred_messages++; - } -} - -void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) -{ - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // don't send status MAVLink messages for 2 seconds after - // bootup, to try to prevent Xbee bricking - return; - } - - if (severity == SEVERITY_LOW) { - // send via the deferred queuing system - pending_status.severity = (uint8_t)severity; - strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); - mavlink_send_message(chan, MSG_STATUSTEXT, 0); - } else { - // send immediately - mavlink_msg_statustext_send( - chan, - severity, - (const int8_t*) str); -} -} - - -GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : -packet_drops(0), - - -// parameters -// note, all values not explicitly initialised here are zeroed -waypoint_send_timeout(1000), // 1 second -waypoint_receive_timeout(1000), // 1 second - -// stream rates -_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), - // AP_VAR //ref //index, default, name - streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), - streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), - streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), - streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), - streamRatePosition (&_group, 4, 0, PSTR("POSITION")), - streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), - streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), - streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) -{ - -} - -void -GCS_MAVLINK::init(FastSerial * port) -{ - GCS_Class::init(port); - if (port == &Serial) { - mavlink_comm_0_port = port; - chan = MAVLINK_COMM_0; - }else{ - mavlink_comm_1_port = port; - chan = MAVLINK_COMM_1; - } - _queued_parameter = NULL; -} - -void -GCS_MAVLINK::update(void) -{ - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; - status.packet_rx_drop_count = 0; - - // process received bytes - while(comm_get_available(chan)) - { - uint8_t c = comm_receive_ch(chan); - -#if CLI_ENABLED == ENABLED - /* allow CLI to be started by hitting enter 3 times, if no - heartbeat packets have been received */ - if (mavlink_active == false) { - if (c == '\n' || c == '\r') { - crlf_count++; - } else { - crlf_count = 0; - } - if (crlf_count == 3) { - run_cli(); - } - } -#endif - - // Try to get a new message - if (mavlink_parse_char(chan, c, &msg, &status)) { - mavlink_active = true; - handleMessage(&msg); - } - } - - // Update packet drops counter - packet_drops += status.packet_rx_drop_count; - - // send out queued params/ waypoints - if (NULL != _queued_parameter) { - send_message(MSG_NEXT_PARAM); - } - - if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { - send_message(MSG_NEXT_WAYPOINT); - } - - // stop waypoint sending if timeout - if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ - waypoint_sending = false; - } - - // stop waypoint receiving if timeout - if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ - waypoint_receiving = false; - } -} - -void -GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) -{ - if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { - - if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ - send_message(MSG_RAW_IMU1); - send_message(MSG_RAW_IMU2); - send_message(MSG_RAW_IMU3); - //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); - } - - if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { - send_message(MSG_EXTENDED_STATUS1); - send_message(MSG_EXTENDED_STATUS2); - send_message(MSG_GPS_STATUS); - send_message(MSG_CURRENT_WAYPOINT); - send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working - send_message(MSG_NAV_CONTROLLER_OUTPUT); - //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); - } - - if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { - // sent with GPS read -#if HIL_MODE == HIL_MODE_ATTITUDE - send_message(MSG_LOCATION); -#endif - //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); - } - - if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { - // This is used for HIL. Do not change without discussing with HIL maintainers - send_message(MSG_SERVO_OUT); - //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); - } - - if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { - send_message(MSG_RADIO_OUT); - send_message(MSG_RADIO_IN); - //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); - } - - if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info - send_message(MSG_ATTITUDE); - //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); - } - - if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info - send_message(MSG_VFR_HUD); - //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); - } - - if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - // Available datastream - //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); - } - } -} - -void -GCS_MAVLINK::send_message(enum ap_message id) -{ - mavlink_send_message(chan,id, packet_drops); -} - -void -GCS_MAVLINK::send_text(gcs_severity severity, const char *str) -{ - mavlink_send_text(chan,severity,str); -} - -void -GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) -{ - mavlink_statustext_t m; - uint8_t i; - for (i=0; imsgid) { - - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - { - // decode - mavlink_request_data_stream_t packet; - mavlink_msg_request_data_stream_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - int freq = 0; // packet frequency - - if (packet.start_stop == 0) - freq = 0; // stop sending - else if (packet.start_stop == 1) - freq = packet.req_message_rate; // start sending - else - break; - - switch(packet.req_stream_id){ - - case MAV_DATA_STREAM_ALL: - streamRateRawSensors = freq; - streamRateExtendedStatus = freq; - streamRateRCChannels = freq; - streamRateRawController = freq; - streamRatePosition = freq; - streamRateExtra1 = freq; - streamRateExtra2 = freq; - //streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. - streamRateExtra3 = freq; // Don't save!! - break; - - case MAV_DATA_STREAM_RAW_SENSORS: - streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly - // we will not continue to broadcast raw sensor data at 50Hz. - break; - case MAV_DATA_STREAM_EXTENDED_STATUS: - //streamRateExtendedStatus.set_and_save(freq); - streamRateExtendedStatus = freq; - break; - - case MAV_DATA_STREAM_RC_CHANNELS: - streamRateRCChannels = freq; - break; - - case MAV_DATA_STREAM_RAW_CONTROLLER: - streamRateRawController = freq; - break; - - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; - - case MAV_DATA_STREAM_POSITION: - streamRatePosition = freq; - break; - - case MAV_DATA_STREAM_EXTRA1: - streamRateExtra1 = freq; - break; - - case MAV_DATA_STREAM_EXTRA2: - streamRateExtra2 = freq; - break; - - case MAV_DATA_STREAM_EXTRA3: - streamRateExtra3 = freq; - break; - - default: - break; - } - break; - } - - case MAVLINK_MSG_ID_ACTION: - { - // decode - mavlink_action_t packet; - mavlink_msg_action_decode(msg, &packet); - if (mavlink_check_target(packet.target,packet.target_component)) break; - - if (in_mavlink_delay) { - // don't execute action commands while in sensor - // initialisation - break; - } - - uint8_t result = 0; - - // do action - send_text(SEVERITY_LOW,PSTR("action received: ")); -//Serial.println(packet.action); - switch(packet.action){ - - case MAV_ACTION_LAUNCH: - //set_mode(TAKEOFF); - break; - - case MAV_ACTION_RETURN: - set_mode(RTL); - result=1; - break; - - case MAV_ACTION_EMCY_LAND: - //set_mode(LAND); - break; - - case MAV_ACTION_HALT: - do_loiter_at_location(); - result=1; - break; - - /* No mappable implementation in APM 2.0 - case MAV_ACTION_MOTORS_START: - case MAV_ACTION_CONFIRM_KILL: - case MAV_ACTION_EMCY_KILL: - case MAV_ACTION_MOTORS_STOP: - case MAV_ACTION_SHUTDOWN: - break; - */ - - case MAV_ACTION_CONTINUE: - process_next_command(); - result=1; - break; - - case MAV_ACTION_SET_MANUAL: - set_mode(STABILIZE); - result=1; - break; - - case MAV_ACTION_SET_AUTO: - set_mode(AUTO); - result=1; - break; - - case MAV_ACTION_STORAGE_READ: - AP_Var::load_all(); - result=1; - break; - - case MAV_ACTION_STORAGE_WRITE: - AP_Var::save_all(); - result=1; - break; - - case MAV_ACTION_CALIBRATE_RC: break; - trim_radio(); - result=1; - break; - - case MAV_ACTION_CALIBRATE_GYRO: - case MAV_ACTION_CALIBRATE_MAG: - case MAV_ACTION_CALIBRATE_PRESSURE: - break; - - case MAV_ACTION_CALIBRATE_ACC: - imu.init_accel(mavlink_delay); - result=1; - break; - - - //case MAV_ACTION_REBOOT: // this is a rough interpretation - //startup_IMU_ground(); - //result=1; - // break; - - /* For future implemtation - case MAV_ACTION_REC_START: break; - case MAV_ACTION_REC_PAUSE: break; - case MAV_ACTION_REC_STOP: break; - */ - - /* Takeoff is not an implemented flight mode in APM 2.0 - case MAV_ACTION_TAKEOFF: - set_mode(TAKEOFF); - break; - */ - - case MAV_ACTION_NAVIGATE: - set_mode(AUTO); - result=1; - break; - - /* Land is not an implemented flight mode in APM 2.0 - case MAV_ACTION_LAND: - set_mode(LAND); - break; - */ - - case MAV_ACTION_LOITER: - set_mode(LOITER); - result=1; - break; - - default: break; - } - - mavlink_msg_action_ack_send( - chan, - packet.action, - result - ); - - break; - } - - case MAVLINK_MSG_ID_SET_MODE: - { - // decode - mavlink_set_mode_t packet; - mavlink_msg_set_mode_decode(msg, &packet); - - switch(packet.mode){ - - case MAV_MODE_MANUAL: - set_mode(STABILIZE); - break; - - case MAV_MODE_GUIDED: - set_mode(GUIDED); - break; - - case MAV_MODE_AUTO: - if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); - if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); - if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); - mav_nav = 255; - break; - - case MAV_MODE_TEST1: - set_mode(STABILIZE); - break; - } - } - - /*case MAVLINK_MSG_ID_SET_NAV_MODE: - { - // decode - mavlink_set_nav_mode_t packet; - mavlink_msg_set_nav_mode_decode(msg, &packet); - // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message - mav_nav = packet.nav_mode; - break; - } - */ - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); - - // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - // Start sending waypoints - mavlink_msg_waypoint_count_send( - chan,msg->sysid, - msg->compid, - g.waypoint_total + 1); // + home - - waypoint_timelast_send = millis(); - waypoint_sending = true; - waypoint_receiving = false; - waypoint_dest_sysid = msg->sysid; - waypoint_dest_compid = msg->compid; - break; - } - - // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); - - // Check if sending waypiont - //if (!waypoint_sending) break; - // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW - - // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - // send waypoint - tell_command = get_command_with_index(packet.seq); - - // set frame of waypoint - uint8_t frame; - - if (tell_command.options & WP_OPTION_ALT_RELATIVE) { - frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame - } else { - frame = MAV_FRAME_GLOBAL; // reference frame - } - - float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; - - // time that the mav should loiter in milliseconds - uint8_t current = 0; // 1 (true), 0 (false) - - if (packet.seq == (uint16_t)g.waypoint_index) - current = 1; - - uint8_t autocontinue = 1; // 1 (true), 0 (false) - - float x = 0, y = 0, z = 0; - - if (tell_command.id < MAV_CMD_NAV_LAST) { - // command needs scaling - x = tell_command.lat/1.0e7; // local (x), global (latitude) - y = tell_command.lng/1.0e7; // local (y), global (longitude) - // ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM - z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) - } - - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_CONDITION_CHANGE_ALT: - case MAV_CMD_DO_SET_HOME: - param1 = tell_command.p1; - break; - - case MAV_CMD_NAV_TAKEOFF: - param1 = 0; - break; - - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1; // ACM loiter time is in 1 second increments - break; - - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; - - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - param1 = tell_command.p1; - break; - - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - param3 = tell_command.lat; - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - - case MAV_CMD_NAV_WAYPOINT: - param1 = tell_command.p1; - break; - - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - } - - mavlink_msg_waypoint_send(chan,msg->sysid, - msg->compid, - packet.seq, - frame, - tell_command.id, - current, - autocontinue, - param1, - param2, - param3, - param4, - x, - y, - z); - - // update last waypoint comm stamp - waypoint_timelast_send = millis(); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint ack")); - - // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // turn off waypoint send - waypoint_sending = false; - break; - } - - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - { - //send_text_P(SEVERITY_LOW,PSTR("param request list")); - - // decode - mavlink_param_request_list_t packet; - mavlink_msg_param_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // Start sending parameters - next call to ::update will kick the first one out - - _queued_parameter = AP_Var::first(); - _queued_parameter_index = 0; - _queued_parameter_count = _count_parameters(); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all")); - - // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; - - // clear all waypoints - uint8_t type = 0; // ok (0), error(1) - g.waypoint_total.set_and_save(0); - - // send acknowledgement 3 times to makes sure it is received - for (int i=0;i<3;i++) - mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type); - - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint set current")); - - // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // set current command - change_command(packet.seq); - - mavlink_msg_waypoint_current_send(chan, g.waypoint_index); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint count")); - - // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // start waypoint receiving - if (packet.count > MAX_WAYPOINTS) { - packet.count = MAX_WAYPOINTS; - } - g.waypoint_total.set_and_save(packet.count - 1); - - waypoint_timelast_receive = millis(); - waypoint_receiving = true; - waypoint_sending = false; - waypoint_request_i = 0; - break; - } - -#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS - case MAVLINK_MSG_ID_SET_MAG_OFFSETS: - { - mavlink_set_mag_offsets_t packet; - mavlink_msg_set_mag_offsets_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); - break; - } -#endif - - // XXX receive a WP from GCS and store in EEPROM - case MAVLINK_MSG_ID_WAYPOINT: - { - // decode - mavlink_waypoint_t packet; - mavlink_msg_waypoint_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // defaults - tell_command.id = packet.command; - - /* - switch (packet.frame){ - - case MAV_FRAME_MISSION: - case MAV_FRAME_GLOBAL: - { - tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm - tell_command.options = 0; // absolute altitude - break; - } - - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; - break; - } - //case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - default: - { - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! - break; - } - } - */ - - // we only are supporting Abs position, relative Alt - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; - tell_command.options = 1; // store altitude relative!! Always!! - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_DO_SET_ROI: - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_NAV_TAKEOFF: - tell_command.p1 = 0; - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.p1 = packet.param1 * 100; - break; - - case MAV_CMD_NAV_LOITER_TIME: - tell_command.p1 = packet.param1; // APM loiter time is in ten second increments - break; - - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - tell_command.lat = packet.param1; - break; - - case MAV_CMD_DO_JUMP: - tell_command.lat = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_REPEAT_SERVO: - tell_command.lng = packet.param4; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - tell_command.lat = packet.param3; - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_NAV_WAYPOINT: - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - } - - if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission - guided_WP = tell_command; - - // add home alt if needed - if (guided_WP.options & WP_OPTION_ALT_RELATIVE){ - guided_WP.alt += home.alt; - } - - set_mode(GUIDED); - - // make any new wp uploaded instant (in case we are already in Guided mode) - set_next_WP(&guided_WP); - - // verify we recevied the command - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - 0); - - } else { - // Check if receiving waypoints (mission upload expected) - if (!waypoint_receiving) break; - - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) break; - set_command_with_index(tell_command, packet.seq); - - // update waypoint receiving state machine - waypoint_timelast_receive = millis(); - waypoint_request_i++; - - if (waypoint_request_i > (uint16_t)g.waypoint_total){ - uint8_t type = 0; // ok (0), error(1) - - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - type); - - send_text(SEVERITY_LOW,PSTR("flight plan received")); - waypoint_receiving = false; - // XXX ignores waypoint radius for individual waypoints, can - // only set WP_RADIUS parameter - } - } - break; - } - - case MAVLINK_MSG_ID_PARAM_SET: - { - AP_Var *vp; - AP_Meta_class::Type_id var_type; - - // decode - mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - // set parameter - - char key[ONBOARD_PARAM_NAME_LENGTH+1]; - strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); - key[ONBOARD_PARAM_NAME_LENGTH] = 0; - - // find the requested parameter - vp = AP_Var::find(key); - if ((NULL != vp) && // exists - !isnan(packet.param_value) && // not nan - !isinf(packet.param_value)) { // not inf - - // add a small amount before casting parameter values - // from float to integer to avoid truncating to the - // next lower integer value. - float rounding_addition = 0.01; - - // fetch the variable type ID - var_type = vp->meta_type_id(); - - // handle variables with standard type IDs - if (var_type == AP_Var::k_typeid_float) { - ((AP_Float *)vp)->set_and_save(packet.param_value); - - } else if (var_type == AP_Var::k_typeid_float16) { - ((AP_Float16 *)vp)->set_and_save(packet.param_value); - - } else if (var_type == AP_Var::k_typeid_int32) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else if (var_type == AP_Var::k_typeid_int16) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else if (var_type == AP_Var::k_typeid_int8) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else { - // we don't support mavlink set on this parameter - break; - } - - // Report back the new value if we accepted the change - // we send the value we actually set, which could be - // different from the value sent, in case someone sent - // a fractional value to an integer type - mavlink_msg_param_value_send( - chan, - (int8_t *)key, - vp->cast_to_float(), - _count_parameters(), - -1); // XXX we don't actually know what its index is... - } - - break; - } // end case - - case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: - { - // allow override of RC channel values for HIL - // or for complete GCS control of switch position - // and RC PWM values. - if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs - mavlink_rc_channels_override_t packet; - int16_t v[8]; - mavlink_msg_rc_channels_override_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system,packet.target_component)) - break; - - v[0] = packet.chan1_raw; - v[1] = packet.chan2_raw; - v[2] = packet.chan3_raw; - v[3] = packet.chan4_raw; - v[4] = packet.chan5_raw; - v[5] = packet.chan6_raw; - v[6] = packet.chan7_raw; - v[7] = packet.chan8_raw; - APM_RC.setHIL(v); - break; - } - -#if HIL_MODE != HIL_MODE_DISABLED - // This is used both as a sensor and to pass the location - // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: - { - // decode - mavlink_gps_raw_t packet; - mavlink_msg_gps_raw_decode(msg, &packet); - - // set gps hil sensor - g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, - packet.v,packet.hdg,0,0); - if (gps_base_alt == 0) { - gps_base_alt = packet.alt*100; - } - current_loc.lng = packet.lon * T7; - current_loc.lat = packet.lat * T7; - current_loc.alt = g_gps->altitude - gps_base_alt; - if (!home_is_set) { - init_home(); - } - break; - } -#if HIL_MODE == HIL_MODE_ATTITUDE - case MAVLINK_MSG_ID_ATTITUDE: - { - // decode - mavlink_attitude_t packet; - mavlink_msg_attitude_decode(msg, &packet); - - // set dcm hil sensor - dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); - break; - } -#endif -#endif -/* - case MAVLINK_MSG_ID_HEARTBEAT: - { - // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes - if(msg->sysid != g.sysid_my_gcs) break; - rc_override_fs_timer = millis(); - //pmTest1++; - break; - } - - #if HIL_MODE != HIL_MODE_DISABLED - // This is used both as a sensor and to pass the location - // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: - { - // decode - mavlink_gps_raw_t packet; - mavlink_msg_gps_raw_decode(msg, &packet); - - // set gps hil sensor - g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, - packet.v,packet.hdg,0,0); - break; - } - - // Is this resolved? - MAVLink protocol change..... - case MAVLINK_MSG_ID_VFR_HUD: - { - // decode - mavlink_vfr_hud_t packet; - mavlink_msg_vfr_hud_decode(msg, &packet); - - // set airspeed - airspeed = 100*packet.airspeed; - break; - } - -#endif -#if HIL_MODE == HIL_MODE_ATTITUDE - case MAVLINK_MSG_ID_ATTITUDE: - { - // decode - mavlink_attitude_t packet; - mavlink_msg_attitude_decode(msg, &packet); - - // set dcm hil sensor - dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); - break; - } -#endif -*/ -#if HIL_MODE == HIL_MODE_SENSORS - - case MAVLINK_MSG_ID_RAW_IMU: - { - // decode - mavlink_raw_imu_t packet; - mavlink_msg_raw_imu_decode(msg, &packet); - - // set imu hil sensors - // TODO: check scaling for temp/absPress - float temp = 70; - float absPress = 1; - // Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); - // Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); - - // rad/sec - Vector3f gyros; - gyros.x = (float)packet.xgyro / 1000.0; - gyros.y = (float)packet.ygyro / 1000.0; - gyros.z = (float)packet.zgyro / 1000.0; - // m/s/s - Vector3f accels; - accels.x = (float)packet.xacc / 1000.0; - accels.y = (float)packet.yacc / 1000.0; - accels.z = (float)packet.zacc / 1000.0; - - imu.set_gyro(gyros); - - imu.set_accel(accels); - - compass.setHIL(packet.xmag,packet.ymag,packet.zmag); - break; - } - - case MAVLINK_MSG_ID_RAW_PRESSURE: - { - // decode - mavlink_raw_pressure_t packet; - mavlink_msg_raw_pressure_decode(msg, &packet); - - // set pressure hil sensor - // TODO: check scaling - float temp = 70; - barometer.setHIL(temp,packet.press_diff1); - break; - } -#endif // HIL_MODE - } // end switch -} // end handle mavlink - -uint16_t -GCS_MAVLINK::_count_parameters() -{ - // if we haven't cached the parameter count yet... - if (0 == _parameter_count) { - AP_Var *vp; - - vp = AP_Var::first(); - do { - // if a parameter responds to cast_to_float then we are going to be able to report it - if (!isnan(vp->cast_to_float())) { - _parameter_count++; - } - } while (NULL != (vp = vp->next())); - } - return _parameter_count; -} - -AP_Var * -GCS_MAVLINK::_find_parameter(uint16_t index) -{ - AP_Var *vp; - - vp = AP_Var::first(); - while (NULL != vp) { - - // if the parameter is reportable - if (!(isnan(vp->cast_to_float()))) { - // if we have counted down to the index we want - if (0 == index) { - // return the parameter - return vp; - } - // count off this parameter, as it is reportable but not - // the one we want - index--; - } - // and move to the next parameter - vp = vp->next(); - } - return NULL; -} - -/** -* @brief Send the next pending parameter, called from deferred message -* handling code -*/ -void -GCS_MAVLINK::queued_param_send() -{ - // Check to see if we are sending parameters - if (NULL == _queued_parameter) return; - - AP_Var *vp; - float value; - - // copy the current parameter and prepare to move to the next - vp = _queued_parameter; - _queued_parameter = _queued_parameter->next(); - - // if the parameter can be cast to float, report it here and break out of the loop - value = vp->cast_to_float(); - if (!isnan(value)) { - char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK - vp->copy_name(param_name, sizeof(param_name)); - - mavlink_msg_param_value_send( - chan, - (int8_t*)param_name, - value, - _queued_parameter_count, - _queued_parameter_index); - - _queued_parameter_index++; - } -} - -/** -* @brief Send the next pending waypoint, called from deferred message -* handling code -*/ -void -GCS_MAVLINK::queued_waypoint_send() -{ - if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { - mavlink_msg_waypoint_request_send( - chan, - waypoint_dest_sysid, - waypoint_dest_compid, - waypoint_request_i); - } -} - -/* - a delay() callback that processes MAVLink packets. We set this as the - callback in long running library initialisation routines to allow - MAVLink to process packets while waiting for the initialisation to - complete -*/ -static void mavlink_delay(unsigned long t) -{ - unsigned long tstart; - static unsigned long last_1hz, last_50hz; - - if (in_mavlink_delay) { - // this should never happen, but let's not tempt fate by - // letting the stack grow too much - delay(t); - return; - } - - in_mavlink_delay = true; - - tstart = millis(); - do { - unsigned long tnow = millis(); - if (tnow - last_1hz > 1000) { - last_1hz = tnow; - gcs_send_message(MSG_HEARTBEAT); - gcs_send_message(MSG_EXTENDED_STATUS1); - } - if (tnow - last_50hz > 20) { - last_50hz = tnow; - gcs_update(); - } - delay(1); - } while (millis() - tstart < t); - - in_mavlink_delay = false; -} - -/* - send a message on both GCS links - */ -static void gcs_send_message(enum ap_message id) -{ - gcs0.send_message(id); - gcs3.send_message(id); -} - -/* - send data streams in the given rate range on both links - */ -static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) -{ - gcs0.data_stream_send(freqMin, freqMax); - gcs3.data_stream_send(freqMin, freqMax); -} - -/* - look for incoming commands on the GCS links - */ -static void gcs_update(void) -{ - gcs0.update(); - gcs3.update(); -} - -static void gcs_send_text(gcs_severity severity, const char *str) -{ - gcs0.send_text(severity, str); - gcs3.send_text(severity, str); -} - -static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) -{ - gcs0.send_text(severity, str); - gcs3.send_text(severity, str); -} - -/* - send a low priority formatted message to the GCS - only one fits in the queue, so if you send more than one before the - last one gets into the serial buffer then the old one will be lost - */ -static void gcs_send_text_fmt(const prog_char_t *fmt, ...) -{ - char fmtstr[40]; - va_list ap; - uint8_t i; - for (i=0; i" - " erase (all logs)\n" - " enable | all\n" - " disable | all\n" - "\n")); - return 0; -} - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -const struct Menu::command log_menu_commands[] PROGMEM = { - {"dump", dump_log}, - {"erase", erase_logs}, - {"enable", select_logs}, - {"disable", select_logs}, - {"help", help_log} -}; - -// A Macro to create the Menu -MENU2(log_menu, "Log", log_menu_commands, print_log_menu); - -static void get_log_boundaries(byte log_num, int & start_page, int & end_page); - -static bool -print_log_menu(void) -{ - int log_start; - int log_end; - byte last_log_num = get_num_logs(); - - Serial.printf_P(PSTR("logs enabled: ")); - - if (0 == g.log_bitmask) { - Serial.printf_P(PSTR("none")); - }else{ - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Serial.printf_P(PSTR(" ATTITUDE_FAST")); - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Serial.printf_P(PSTR(" ATTITUDE_MED")); - if (g.log_bitmask & MASK_LOG_GPS) Serial.printf_P(PSTR(" GPS")); - if (g.log_bitmask & MASK_LOG_PM) Serial.printf_P(PSTR(" PM")); - if (g.log_bitmask & MASK_LOG_CTUN) Serial.printf_P(PSTR(" CTUN")); - if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN")); - if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW")); - if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD")); - if (g.log_bitmask & MASK_LOG_CUR) Serial.printf_P(PSTR(" CURRENT")); - if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS")); - if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW")); - } - - Serial.println(); - - if (last_log_num == 0) { - Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n")); - }else{ - Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); - - for(int i = 1; i < last_log_num + 1; i++) { - get_log_boundaries(i, log_start, log_end); - //Serial.printf_P(PSTR("last_num %d "), last_log_num); - Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"), i, log_start, log_end); - } - Serial.println(); - } - return(true); -} - -static int8_t -dump_log(uint8_t argc, const Menu::arg *argv) -{ - byte dump_log; - int dump_log_start; - int dump_log_end; - - // check that the requested log number can be read - dump_log = argv[1].i; - - if (/*(argc != 2) || */ (dump_log < 1)) { - Serial.printf_P(PSTR("bad log # %d\n"), dump_log); - Log_Read(0, 4095); - erase_logs(NULL, NULL); - return(-1); - } - - get_log_boundaries(dump_log, dump_log_start, dump_log_end); - /*Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"), - dump_log, - dump_log_start, - dump_log_end); - */ - Log_Read(dump_log_start, dump_log_end); - //Serial.printf_P(PSTR("Done\n")); - return (0); -} - -static int8_t -erase_logs(uint8_t argc, const Menu::arg *argv) -{ - //for(int i = 10 ; i > 0; i--) { - // Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds.\n"), i); - // delay(1000); - //} - - // lay down a bunch of "log end" messages. - Serial.printf_P(PSTR("\nErasing log...\n")); - for(int j = 1; j < 4096; j++) - DataFlash.PageErase(j); - - clear_header(); - - Serial.printf_P(PSTR("\nLog erased.\n")); - return (0); -} - -static void clear_header() -{ - DataFlash.StartWrite(1); - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_INDEX_MSG); - DataFlash.WriteByte(0); - DataFlash.WriteByte(END_BYTE); - DataFlash.FinishWrite(); -} - -static int8_t -select_logs(uint8_t argc, const Menu::arg *argv) -{ - uint16_t bits; - - if (argc != 2) { - Serial.printf_P(PSTR("missing log type\n")); - return(-1); - } - - bits = 0; - - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // that name as the argument to the command, and set the bit in - // bits accordingly. - // - if (!strcasecmp_P(argv[1].str, PSTR("all"))) { - bits = ~0; - } else { - #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s - TARG(ATTITUDE_FAST); - TARG(ATTITUDE_MED); - TARG(GPS); - TARG(PM); - TARG(CTUN); - TARG(NTUN); - TARG(MODE); - TARG(RAW); - TARG(CMD); - TARG(CUR); - TARG(MOTORS); - TARG(OPTFLOW); - #undef TARG - } - - if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { - g.log_bitmask.set_and_save(g.log_bitmask | bits); - }else{ - g.log_bitmask.set_and_save(g.log_bitmask & ~bits); - } - - return(0); -} - -static int8_t -process_logs(uint8_t argc, const Menu::arg *argv) -{ - log_menu.run(); - return 0; -} - - -// finds out how many logs are available -static byte get_num_logs(void) -{ - int page = 1; - byte data; - byte log_step = 0; - - DataFlash.StartRead(1); - - while (page == 1) { - data = DataFlash.ReadByte(); - - switch(log_step){ //This is a state machine to read the packets - case 0: - if(data==HEAD_BYTE1) // Head byte 1 - log_step++; - break; - - case 1: - if(data==HEAD_BYTE2) // Head byte 2 - log_step++; - else - log_step = 0; - break; - - case 2: - if(data == LOG_INDEX_MSG){ - byte num_logs = DataFlash.ReadByte(); - //Serial.printf("num_logs, %d\n", num_logs); - - return num_logs; - }else{ - //Serial.printf("* %d\n", data); - log_step = 0; // Restart, we have a problem... - } - break; - } - page = DataFlash.GetPage(); - } - return 0; -} - -// send the number of the last log? -static void start_new_log() -{ - byte num_existing_logs = get_num_logs(); - - int start_pages[50] = {0,0,0}; - int end_pages[50] = {0,0,0}; - - if(num_existing_logs > 0){ - for(int i = 0; i < num_existing_logs; i++) { - get_log_boundaries(i + 1, start_pages[i], end_pages[i]); - } - end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]); - } - - if((end_pages[num_existing_logs - 1] < 4095) && (num_existing_logs < MAX_NUM_LOGS /*50*/)) { - - if(num_existing_logs > 0) - start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1; - else - start_pages[0] = 2; - - num_existing_logs++; - - DataFlash.StartWrite(1); - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_INDEX_MSG); - DataFlash.WriteByte(num_existing_logs); - - for(int i = 0; i < MAX_NUM_LOGS; i++) { - DataFlash.WriteInt(start_pages[i]); - DataFlash.WriteInt(end_pages[i]); - } - - DataFlash.WriteByte(END_BYTE); - DataFlash.FinishWrite(); - DataFlash.StartWrite(start_pages[num_existing_logs - 1]); - - }else{ - gcs_send_text_P(SEVERITY_LOW,PSTR("Logs full")); - } -} - -// All log data is stored in page 1? -static void get_log_boundaries(byte log_num, int & start_page, int & end_page) -{ - int page = 1; - byte data; - byte log_step = 0; - - DataFlash.StartRead(1); - while (page == 1) { - data = DataFlash.ReadByte(); - switch(log_step) //This is a state machine to read the packets - { - case 0: - if(data==HEAD_BYTE1) // Head byte 1 - log_step++; - break; - case 1: - if(data==HEAD_BYTE2) // Head byte 2 - log_step++; - else - log_step = 0; - break; - case 2: - if(data==LOG_INDEX_MSG){ - byte num_logs = DataFlash.ReadByte(); - for(int i=0;i 1) { - look_page = (top_page + bottom_page) / 2; - DataFlash.StartRead(look_page); - check = DataFlash.ReadLong(); - - //Serial.printf("look page:%d, check:%d\n", look_page, check); - - if(check == (long)0xFFFFFFFF) - top_page = look_page; - else - bottom_page = look_page; - } - return top_page; -} - -// Write an GPS packet. Total length : 30 bytes -static void Log_Write_GPS() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_GPS_MSG); - - DataFlash.WriteLong(g_gps->time); // 1 - DataFlash.WriteByte(g_gps->num_sats); // 2 - - DataFlash.WriteLong(current_loc.lat); // 3 - DataFlash.WriteLong(current_loc.lng); // 4 - DataFlash.WriteLong(current_loc.alt); // 5 - DataFlash.WriteLong(g_gps->altitude); // 6 - - DataFlash.WriteInt(g_gps->ground_speed); // 7 - DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8 - - DataFlash.WriteByte(END_BYTE); -} - -// Read a GPS packet -static void Log_Read_GPS() -{ - Serial.printf_P(PSTR("GPS, %ld, %d, " - "%4.7f, %4.7f, %4.4f, %4.4f, " - "%d, %u\n"), - - DataFlash.ReadLong(), // 1 time - (int)DataFlash.ReadByte(), // 2 sats - - (float)DataFlash.ReadLong() / t7, // 3 lat - (float)DataFlash.ReadLong() / t7, // 4 lon - (float)DataFlash.ReadLong() / 100.0, // 5 gps alt - (float)DataFlash.ReadLong() / 100.0, // 6 sensor alt - - DataFlash.ReadInt(), // 7 ground speed - (uint16_t)DataFlash.ReadInt()); // 8 ground course -} - -// Write an raw accel/gyro data packet. Total length : 28 bytes -#if HIL_MODE != HIL_MODE_ATTITUDE -static void Log_Write_Raw() -{ - Vector3f gyro = imu.get_gyro(); - Vector3f accel = imu.get_accel(); - //Vector3f accel_filt = imu.get_accel_filtered(); - - gyro *= t7; // Scale up for storage as long integers - accel *= t7; - //accel_filt *= t7; - - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_RAW_MSG); - - DataFlash.WriteLong((long)gyro.x); - DataFlash.WriteLong((long)gyro.y); - DataFlash.WriteLong((long)gyro.z); - - - //DataFlash.WriteLong((long)(accels_rot.x * t7)); - //DataFlash.WriteLong((long)(accels_rot.y * t7)); - //DataFlash.WriteLong((long)(accels_rot.z * t7)); - - DataFlash.WriteLong((long)accel.x); - DataFlash.WriteLong((long)accel.y); - DataFlash.WriteLong((long)accel.z); - - DataFlash.WriteByte(END_BYTE); -} -#endif - -// Read a raw accel/gyro packet -static void Log_Read_Raw() -{ - float logvar; - Serial.printf_P(PSTR("RAW,")); - for (int y = 0; y < 6; y++) { - logvar = (float)DataFlash.ReadLong() / t7; - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -static void Log_Write_Current() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CURRENT_MSG); - - DataFlash.WriteInt(g.rc_3.control_in); - DataFlash.WriteLong(throttle_integrator); - - DataFlash.WriteInt((int)(battery_voltage * 100.0)); - DataFlash.WriteInt((int)(current_amps * 100.0)); - DataFlash.WriteInt((int)current_total); - - DataFlash.WriteByte(END_BYTE); -} - -// Read a Current packet -static void Log_Read_Current() -{ - Serial.printf_P(PSTR("CURR: %d, %ld, %4.4f, %4.4f, %d\n"), - DataFlash.ReadInt(), - DataFlash.ReadLong(), - - ((float)DataFlash.ReadInt() / 100.f), - ((float)DataFlash.ReadInt() / 100.f), - DataFlash.ReadInt()); -} - -static void Log_Write_Motors() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_MOTORS_MSG); - - #if FRAME_CONFIG == TRI_FRAME - DataFlash.WriteInt(motor_out[CH_1]);//1 - DataFlash.WriteInt(motor_out[CH_2]);//2 - DataFlash.WriteInt(motor_out[CH_4]);//3 - DataFlash.WriteInt(g.rc_4.radio_out);//4 - - #elif FRAME_CONFIG == HEXA_FRAME - DataFlash.WriteInt(motor_out[CH_1]);//1 - DataFlash.WriteInt(motor_out[CH_2]);//2 - DataFlash.WriteInt(motor_out[CH_3]);//3 - DataFlash.WriteInt(motor_out[CH_4]);//4 - DataFlash.WriteInt(motor_out[CH_7]);//5 - DataFlash.WriteInt(motor_out[CH_8]);//6 - - #elif FRAME_CONFIG == Y6_FRAME - //left - DataFlash.WriteInt(motor_out[CH_2]);//1 - DataFlash.WriteInt(motor_out[CH_3]);//2 - //right - DataFlash.WriteInt(motor_out[CH_7]);//3 - DataFlash.WriteInt(motor_out[CH_1]);//4 - //back - DataFlash.WriteInt(motor_out[CH_8]);//5 - DataFlash.WriteInt(motor_out[CH_4]);//6 - - #elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME - DataFlash.WriteInt(motor_out[CH_1]);//1 - DataFlash.WriteInt(motor_out[CH_2]);//2 - DataFlash.WriteInt(motor_out[CH_3]);//3 - DataFlash.WriteInt(motor_out[CH_4]);//4 - DataFlash.WriteInt(motor_out[CH_7]);//5 - DataFlash.WriteInt(motor_out[CH_8]); //6 - DataFlash.WriteInt(motor_out[CH_10]);//7 - DataFlash.WriteInt(motor_out[CH_11]);//8 - - #elif FRAME_CONFIG == HELI_FRAME - DataFlash.WriteInt(heli_servo_out[0]);//1 - DataFlash.WriteInt(heli_servo_out[1]);//2 - DataFlash.WriteInt(heli_servo_out[2]);//3 - DataFlash.WriteInt(heli_servo_out[3]);//4 - DataFlash.WriteInt(g.heli_ext_gyro_gain);//5 - - #else // quads - DataFlash.WriteInt(motor_out[CH_1]);//1 - DataFlash.WriteInt(motor_out[CH_2]);//2 - DataFlash.WriteInt(motor_out[CH_3]);//3 - DataFlash.WriteInt(motor_out[CH_4]);//4 - #endif - - DataFlash.WriteByte(END_BYTE); -} - -// Read a Current packet -static void Log_Read_Motors() -{ - #if FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME - // 1 2 3 4 5 6 - Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d\n"), - DataFlash.ReadInt(), //1 - DataFlash.ReadInt(), //2 - DataFlash.ReadInt(), //3 - DataFlash.ReadInt(), //4 - DataFlash.ReadInt(), //5 - DataFlash.ReadInt()); //6 - - #elif FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME - // 1 2 3 4 5 6 7 8 - Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d, %d, %d, %d\n"), - DataFlash.ReadInt(), //1 - DataFlash.ReadInt(), //2 - DataFlash.ReadInt(), //3 - DataFlash.ReadInt(), //4 - - DataFlash.ReadInt(), //5 - DataFlash.ReadInt(), //6 - DataFlash.ReadInt(), //7 - DataFlash.ReadInt()); //8 - - #elif FRAME_CONFIG == HELI_FRAME - // 1 2 3 4 5 - Serial.printf_P(PSTR("MOT: %d, %d, %d, %d, %d\n"), - DataFlash.ReadInt(), //1 - DataFlash.ReadInt(), //2 - DataFlash.ReadInt(), //3 - DataFlash.ReadInt(), //4 - DataFlash.ReadInt()); //5 - - #else // quads, TRIs - // 1 2 3 4 - Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"), - DataFlash.ReadInt(), //1 - DataFlash.ReadInt(), //2 - DataFlash.ReadInt(), //3 - DataFlash.ReadInt()); //4; - #endif -} - -#ifdef OPTFLOW_ENABLED -// Write an optical flow packet. Total length : 18 bytes -static void Log_Write_Optflow() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_OPTFLOW_MSG); - DataFlash.WriteInt((int)optflow.dx); - DataFlash.WriteInt((int)optflow.dy); - DataFlash.WriteInt((int)optflow.surface_quality); - DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); - DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); - DataFlash.WriteByte(END_BYTE); -} -#endif - - -static void Log_Read_Optflow() -{ - Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - (float)DataFlash.ReadLong(),// / t7, - (float)DataFlash.ReadLong() // / t7 - ); -} - -static void Log_Write_Nav_Tuning() -{ - //Matrix3f tempmat = dcm.get_dcm_matrix(); - - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_NAV_TUNING_MSG); - - DataFlash.WriteInt((int)wp_distance); // 1 - DataFlash.WriteInt((int)(target_bearing/100)); // 2 - DataFlash.WriteInt((int)long_error); // 3 - DataFlash.WriteInt((int)lat_error); // 4 - DataFlash.WriteInt((int)nav_lon); // 5 - DataFlash.WriteInt((int)nav_lat); // 6 - DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7 - DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8 - DataFlash.WriteInt((int)g.pi_loiter_lon.get_integrator()); // 9 - DataFlash.WriteInt((int)g.pi_loiter_lat.get_integrator()); // 10 - - DataFlash.WriteByte(END_BYTE); -} - - -static void Log_Read_Nav_Tuning() -{ - // 1 2 3 4 5 6 7 8 9 10 - Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), - DataFlash.ReadInt(), // 1 - DataFlash.ReadInt(), // 2 - DataFlash.ReadInt(), // 3 - DataFlash.ReadInt(), // 4 - DataFlash.ReadInt(), // 5 - DataFlash.ReadInt(), // 6 - DataFlash.ReadInt(), // 7 - DataFlash.ReadInt(), // 8 - DataFlash.ReadInt(), // 9 - DataFlash.ReadInt()); // 10 -} - - -// Write a control tuning packet. Total length : 22 bytes -#if HIL_MODE != HIL_MODE_ATTITUDE -static void Log_Write_Control_Tuning() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); - - // yaw - DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1 - DataFlash.WriteInt((int)(nav_yaw/100)); //2 - DataFlash.WriteInt((int)yaw_error/100); //3 - - // Alt hold - DataFlash.WriteInt(sonar_alt); //4 - DataFlash.WriteInt(baro_alt); //5 - DataFlash.WriteInt((int)next_WP.alt); //6 - - DataFlash.WriteInt(nav_throttle); //7 - DataFlash.WriteInt(angle_boost); //8 - DataFlash.WriteInt(manual_boost); //9 - - DataFlash.WriteInt(g.rc_3.servo_out); //10 - DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //11 - DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //12 - - DataFlash.WriteByte(END_BYTE); -} -#endif - -// Read an control tuning packet -static void Log_Read_Control_Tuning() -{ - // 1 2 3 4 5 6 7 8 9 10 11 12 - Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), - - // Control - //DataFlash.ReadByte(), - //DataFlash.ReadInt(), - - // yaw - DataFlash.ReadInt(), //1 - DataFlash.ReadInt(), //2 - DataFlash.ReadInt(), //3 - - // Alt Hold - DataFlash.ReadInt(), //4 - DataFlash.ReadInt(), //5 - DataFlash.ReadInt(), //6 - - DataFlash.ReadInt(), //7 - DataFlash.ReadInt(), //8 - DataFlash.ReadInt(), //9 - - DataFlash.ReadInt(), //10 - DataFlash.ReadInt(), //11 - DataFlash.ReadInt()); //12 -} - -// Write a performance monitoring packet. Total length : 19 bytes -static void Log_Write_Performance() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - - //DataFlash.WriteByte( delta_ms_fast_loop); - //DataFlash.WriteByte( loop_step); - - - //* - //DataFlash.WriteLong( millis()- perf_mon_timer); - - //DataFlash.WriteByte( dcm.gyro_sat_count); //2 - //DataFlash.WriteByte( imu.adc_constraints); //3 - //DataFlash.WriteByte( dcm.renorm_sqrt_count); //4 - //DataFlash.WriteByte( dcm.renorm_blowup_count); //5 - //DataFlash.WriteByte( gps_fix_count); //6 - - - - //DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7 - - - - // control_mode - DataFlash.WriteByte(control_mode); //1 - DataFlash.WriteByte(yaw_mode); //2 - DataFlash.WriteByte(roll_pitch_mode); //3 - DataFlash.WriteByte(throttle_mode); //4 - DataFlash.WriteInt(g.throttle_cruise.get()); //5 - DataFlash.WriteLong(throttle_integrator); //6 - DataFlash.WriteByte(END_BYTE); -} - -// Read a performance packet -static void Log_Read_Performance() -{ //1 2 3 4 5 6 - Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d, %ld\n"), - - // Control - //DataFlash.ReadLong(), - //DataFlash.ReadInt(), - DataFlash.ReadByte(), //1 - DataFlash.ReadByte(), //2 - DataFlash.ReadByte(), //3 - DataFlash.ReadByte(), //4 - DataFlash.ReadInt(), //5 - DataFlash.ReadLong()); //6 -} - -// Write a command processing packet. -static void Log_Write_Cmd(byte num, struct Location *wp) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CMD_MSG); - - DataFlash.WriteByte(g.waypoint_total); - - DataFlash.WriteByte(num); - DataFlash.WriteByte(wp->id); - DataFlash.WriteByte(wp->options); - DataFlash.WriteByte(wp->p1); - DataFlash.WriteLong(wp->alt); - DataFlash.WriteLong(wp->lat); - DataFlash.WriteLong(wp->lng); - - DataFlash.WriteByte(END_BYTE); -} -//CMD, 3, 0, 16, 8, 1, 800, 340440192, -1180692736 - - -// Read a command processing packet -static void Log_Read_Cmd() -{ - Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"), - - // WP total - DataFlash.ReadByte(), - - // num, id, p1, options - DataFlash.ReadByte(), - DataFlash.ReadByte(), - DataFlash.ReadByte(), - DataFlash.ReadByte(), - - // Alt, lat long - DataFlash.ReadLong(), - DataFlash.ReadLong(), - DataFlash.ReadLong()); -} -/* -// Write an attitude packet. Total length : 10 bytes -static void Log_Write_Attitude2() -{ - Vector3f gyro = imu.get_gyro(); - Vector3f accel = imu.get_accel(); - - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_ATTITUDE_MSG); - - DataFlash.WriteInt((int)dcm.roll_sensor); - DataFlash.WriteInt((int)dcm.pitch_sensor); - - DataFlash.WriteLong((long)(degrees(omega.x) * 100.0)); - DataFlash.WriteLong((long)(degrees(omega.y) * 100.0)); - - DataFlash.WriteLong((long)(accel.x * 100000)); - DataFlash.WriteLong((long)(accel.y * 100000)); - - //DataFlash.WriteLong((long)(accel.z * 100000)); - - DataFlash.WriteByte(END_BYTE); -}*/ -/* -// Read an attitude packet -static void Log_Read_Attitude2() -{ - Serial.printf_P(PSTR("ATT, %d, %d, %ld, %ld, %1.4f, %1.4f\n"), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - - DataFlash.ReadLong(), - DataFlash.ReadLong(), - - (float)DataFlash.ReadLong()/100000.0, - (float)DataFlash.ReadLong()/100000.0 ); -} -*/ - -// Write an attitude packet. Total length : 10 bytes -static void Log_Write_Attitude() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_ATTITUDE_MSG); - - DataFlash.WriteInt((int)dcm.roll_sensor); - DataFlash.WriteInt((int)dcm.pitch_sensor); - DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); - - DataFlash.WriteInt((int)g.rc_1.servo_out); - DataFlash.WriteInt((int)g.rc_2.servo_out); - DataFlash.WriteInt((int)g.rc_4.servo_out); - - DataFlash.WriteByte(END_BYTE); -} - -// Read an attitude packet -static void Log_Read_Attitude() -{ - Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - (uint16_t)DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt()); -} - -// Write a mode packet. Total length : 5 bytes -static void Log_Write_Mode(byte mode) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_MODE_MSG); - DataFlash.WriteByte(mode); - DataFlash.WriteInt(g.throttle_cruise); - DataFlash.WriteByte(END_BYTE); -} - -// Read a mode packet -static void Log_Read_Mode() -{ - Serial.printf_P(PSTR("MOD:")); - Serial.print(flight_mode_strings[DataFlash.ReadByte()]); - Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt()); -} - -static void Log_Write_Startup() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_STARTUP_MSG); - DataFlash.WriteByte(END_BYTE); -} - -// Read a mode packet -static void Log_Read_Startup() -{ - Serial.printf_P(PSTR("START UP\n")); -} - - -// Read the DataFlash log memory : Packet Parser -static void Log_Read(int start_page, int end_page) -{ - byte data; - byte log_step = 0; - int page = start_page; - - DataFlash.StartRead(start_page); - - while (page < end_page && page != -1){ - - data = DataFlash.ReadByte(); - - // This is a state machine to read the packets - switch(log_step){ - case 0: - if(data == HEAD_BYTE1) // Head byte 1 - log_step++; - break; - - case 1: - if(data == HEAD_BYTE2) // Head byte 2 - log_step++; - else{ - log_step = 0; - Serial.println("."); - } - break; - - case 2: - log_step = 0; - switch(data){ - case LOG_ATTITUDE_MSG: - Log_Read_Attitude(); - break; - - case LOG_MODE_MSG: - Log_Read_Mode(); - break; - - case LOG_CONTROL_TUNING_MSG: - Log_Read_Control_Tuning(); - break; - - case LOG_NAV_TUNING_MSG: - Log_Read_Nav_Tuning(); - break; - - case LOG_PERFORMANCE_MSG: - Log_Read_Performance(); - break; - - case LOG_RAW_MSG: - Log_Read_Raw(); - break; - - case LOG_CMD_MSG: - Log_Read_Cmd(); - break; - - case LOG_CURRENT_MSG: - Log_Read_Current(); - break; - - case LOG_STARTUP_MSG: - Log_Read_Startup(); - break; - - case LOG_MOTORS_MSG: - Log_Read_Motors(); - break; - - case LOG_OPTFLOW_MSG: - Log_Read_Optflow(); - break; - - case LOG_GPS_MSG: - Log_Read_GPS(); - break; - } - break; - } - page = DataFlash.GetPage(); - } -} - -#else // LOGGING_ENABLED - -static void Log_Write_Startup() {} -static void Log_Read_Startup() {} -static void Log_Read(int start_page, int end_page) {} -static void Log_Write_Cmd(byte num, struct Location *wp) {} -static void Log_Write_Mode(byte mode) {} -static void start_new_log() {} -static void Log_Write_Raw() {} -static void Log_Write_GPS() {} -static void Log_Write_Current() {} -static void Log_Write_Attitude() {} -#ifdef OPTFLOW_ENABLED -static void Log_Write_Optflow() {} -#endif -static void Log_Write_Nav_Tuning() {} -static void Log_Write_Control_Tuning() {} -static void Log_Write_Motors() {} -static void Log_Write_Performance() {} -static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } - -#endif // LOGGING_ENABLED -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/UserCode.pde" -// agmatthews USERHOOKS - -void userhook_init() -{ - // put your initialisation code here - - -} - -void userhook_50Hz() -{ - // put your 50Hz code here - - -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -static void init_commands() -{ - // zero is home, but we always load the next command (1), in the code. - g.waypoint_index = 0; - - // This are registers for the current may and must commands - // setting to zero will allow them to be written to by new commands - command_must_index = NO_COMMAND; - command_may_index = NO_COMMAND; - - // clear the command queue - clear_command_queue(); -} - -// forces the loading of a new command -// queue is emptied after a new command is processed -static void clear_command_queue(){ - next_command.id = NO_COMMAND; -} - -// Getters -// ------- -static struct Location get_command_with_index(int i) -{ - struct Location temp; - - // Find out proper location in memory by using the start_byte position + the index - // -------------------------------------------------------------------------------- - if (i > g.waypoint_total) { - // we do not have a valid command to load - // return a WP with a "Blank" id - temp.id = CMD_BLANK; - - // no reason to carry on - return temp; - - }else{ - // we can load a command, we don't process it yet - // read WP position - long mem = (WP_START_BYTE) + (i * WP_SIZE); - - temp.id = eeprom_read_byte((uint8_t*)mem); - - mem++; - temp.options = eeprom_read_byte((uint8_t*)mem); - - mem++; - temp.p1 = eeprom_read_byte((uint8_t*)mem); - - mem++; - temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative! - - mem += 4; - temp.lat = (long)eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000 - - mem += 4; - temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000 - } - - // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative - //if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ - //temp.alt += home.alt; - //} - - if(temp.options & WP_OPTION_RELATIVE){ - // If were relative, just offset from home - temp.lat += home.lat; - temp.lng += home.lng; - } - - return temp; -} - -// Setters -// ------- -static void set_command_with_index(struct Location temp, int i) -{ - i = constrain(i, 0, g.waypoint_total.get()); - uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - - eeprom_write_byte((uint8_t *) mem, temp.id); - - mem++; - eeprom_write_byte((uint8_t *) mem, temp.options); - - mem++; - eeprom_write_byte((uint8_t *) mem, temp.p1); - - mem++; - eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM! - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7 - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7 -} - -static void increment_WP_index() -{ - if (g.waypoint_index < g.waypoint_total) { - g.waypoint_index++; - } - - SendDebugln(g.waypoint_index,DEC); -} - -/* -static void decrement_WP_index() -{ - if (g.waypoint_index > 0) { - g.waypoint_index.set_and_save(g.waypoint_index - 1); - } -}*/ - -static long read_alt_to_hold() -{ - if(g.RTL_altitude < 0) - return current_loc.alt; - else - return g.RTL_altitude;// + home.alt; -} - - -//******************************************************************************** -// This function sets the waypoint and modes for Return to Launch -// It's not currently used -//******************************************************************************** - -static Location get_LOITER_home_wp() -{ - //so we know where we are navigating from - next_WP = current_loc; - - // read home position - struct Location temp = get_command_with_index(0); // 0 = home - temp.id = MAV_CMD_NAV_LOITER_UNLIM; - temp.alt = read_alt_to_hold(); - return temp; -} - -/* -This function sets the next waypoint command -It precalculates all the necessary stuff. -*/ - -static void set_next_WP(struct Location *wp) -{ - //SendDebug("MSG wp_index: "); - //SendDebugln(g.waypoint_index, DEC); - - // copy the current WP into the OldWP slot - // --------------------------------------- - prev_WP = next_WP; - - // Load the next_WP slot - // --------------------- - next_WP = *wp; - - // used to control and limit the rate of climb - not used right now! - // ----------------------------------------------------------------- - target_altitude = current_loc.alt; - - // this is used to offset the shrinking longitude as we go towards the poles - float rads = (abs(next_WP.lat)/t7) * 0.0174532925; - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); - - // this is handy for the groundstation - // ----------------------------------- - wp_totalDistance = get_distance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - target_bearing = get_bearing(¤t_loc, &next_WP); - - // to check if we have missed the WP - // --------------------------------- - original_target_bearing = target_bearing; - - // reset speed governer - // -------------------- - waypoint_speed_gov = 0; -} - - -// run this at setup on the ground -// ------------------------------- -static void init_home() -{ - // block until we get a good fix - // ----------------------------- - while (!g_gps->new_data || !g_gps->fix) { - g_gps->update(); - } - - home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = g_gps->longitude; // Lon * 10**7 - home.lat = g_gps->latitude; // Lat * 10**7 - //home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid - home.alt = 0; // Home is always 0 - home_is_set = true; - - // to point yaw towards home until we set it with Mavlink - target_WP = home; - - // Save Home to EEPROM - // ------------------- - // no need to save this to EPROM - set_command_with_index(home, 0); - print_wp(&home, 0); - - // Save prev loc this makes the calcs look better before commands are loaded - prev_WP = home; - - // this is dangerous since we can get GPS lock at any time. - //next_WP = home; - - // Load home for a default guided_WP - // ------------- - guided_WP = home; - guided_WP.alt += g.RTL_altitude; -} - - - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands_logic.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/********************************************************************************/ -// Command Event Handlers -/********************************************************************************/ -static void handle_process_must() -{ - switch(next_command.id){ - - case MAV_CMD_NAV_TAKEOFF: - do_takeoff(); - break; - - case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint - do_nav_wp(); - break; - - case MAV_CMD_NAV_LAND: // LAND to Waypoint - do_land(); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely - do_loiter_unlimited(); - break; - - case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times - do_loiter_turns(); - break; - - case MAV_CMD_NAV_LOITER_TIME: // 19 - do_loiter_time(); - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - do_RTL(); - break; - - default: - break; - } - -} - -static void handle_process_may() -{ - switch(next_command.id){ - - case MAV_CMD_CONDITION_DELAY: - do_wait_delay(); - break; - - case MAV_CMD_CONDITION_DISTANCE: - do_within_distance(); - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - do_change_alt(); - break; - - case MAV_CMD_CONDITION_YAW: - do_yaw(); - break; - - default: - break; - } -} - -static void handle_process_now() -{ - switch(next_command.id){ - - case MAV_CMD_DO_JUMP: - do_jump(); - break; - - case MAV_CMD_DO_CHANGE_SPEED: - do_change_speed(); - break; - - case MAV_CMD_DO_SET_HOME: - do_set_home(); - break; - - case MAV_CMD_DO_SET_SERVO: - do_set_servo(); - break; - - case MAV_CMD_DO_SET_RELAY: - do_set_relay(); - break; - - case MAV_CMD_DO_REPEAT_SERVO: - do_repeat_servo(); - break; - - case MAV_CMD_DO_REPEAT_RELAY: - do_repeat_relay(); - break; - - case MAV_CMD_DO_SET_ROI: - do_target_yaw(); - } -} - -static void handle_no_commands() -{ - /* - switch (control_mode){ - default: - set_mode(RTL); - break; - }*/ - //return; - //Serial.println("Handle No CMDs"); -} - -/********************************************************************************/ -// Verify command Handlers -/********************************************************************************/ - -static bool verify_must() -{ - //Serial.printf("vmust: %d\n", command_must_ID); - - switch(command_must_ID) { - - case MAV_CMD_NAV_TAKEOFF: - return verify_takeoff(); - break; - - case MAV_CMD_NAV_LAND: - return verify_land(); - break; - - case MAV_CMD_NAV_WAYPOINT: - return verify_nav_wp(); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: - return false; - break; - - case MAV_CMD_NAV_LOITER_TURNS: - return verify_loiter_turns(); - break; - - case MAV_CMD_NAV_LOITER_TIME: - return verify_loiter_time(); - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - return verify_RTL(); - break; - - default: - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current Must commands")); - return false; - break; - } -} - -static bool verify_may() -{ - switch(command_may_ID) { - - case MAV_CMD_CONDITION_DELAY: - return verify_wait_delay(); - break; - - case MAV_CMD_CONDITION_DISTANCE: - return verify_within_distance(); - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - return verify_change_alt(); - break; - - case MAV_CMD_CONDITION_YAW: - return verify_yaw(); - break; - - default: - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" No current May commands")); - return false; - break; - } -} - -/********************************************************************************/ -// -/********************************************************************************/ - -static void do_RTL(void) -{ - // TODO: Altitude option from mission planner - Location temp = home; - temp.alt = read_alt_to_hold(); - - //so we know where we are navigating from - // -------------------------------------- - next_WP = current_loc; - - // Loads WP from Memory - // -------------------- - set_next_WP(&temp); - - // output control mode to the ground station - // ----------------------------------------- - gcs_send_message(MSG_HEARTBEAT); -} - -/********************************************************************************/ -// Nav (Must) commands -/********************************************************************************/ - -static void do_takeoff() -{ - wp_control = LOITER_MODE; - - // Start with current location - Location temp = current_loc; - - // next_command.alt is a relative altitude!!! - if (next_command.options & WP_OPTION_ALT_RELATIVE) { - temp.alt = next_command.alt + home.alt; - //Serial.printf("rel alt: %ld",temp.alt); - } else { - temp.alt = next_command.alt; - //Serial.printf("abs alt: %ld",temp.alt); - } - - takeoff_complete = false; - // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction - - // Set our waypoint - set_next_WP(&temp); -} - -static void do_nav_wp() -{ - wp_control = WP_MODE; - - // next_command.alt is a relative altitude!!! - if (next_command.options & WP_OPTION_ALT_RELATIVE) { - next_command.alt += home.alt; - } - set_next_WP(&next_command); - - - // this is our bitmask to verify we have met all conditions to move on - wp_verify_byte = 0; - - // this will be used to remember the time in millis after we reach or pass the WP. - loiter_time = 0; - - // this is the delay, stored in seconds and expanded to millis - loiter_time_max = next_command.p1 * 1000; - - // if we don't require an altitude minimum, we save this flag as passed (1) - if((next_WP.options & WP_OPTION_ALT_REQUIRED) == 0){ - // we don't need to worry about it - wp_verify_byte |= NAV_ALTITUDE; - } -} - -static void do_land() -{ - wp_control = LOITER_MODE; - - //Serial.println("dlnd "); - - // not really used right now, might be good for debugging - land_complete = false; - - // A value that drives to 0 when the altitude doesn't change - velocity_land = 2000; - - // used to limit decent rate - land_start = millis(); - - // used to limit decent rate - original_alt = current_loc.alt; - - // hold at our current location - set_next_WP(¤t_loc); -} - -static void do_loiter_unlimited() -{ - wp_control = LOITER_MODE; - - //Serial.println("dloi "); - if(next_command.lat == 0) - set_next_WP(¤t_loc); - else - set_next_WP(&next_command); -} - -static void do_loiter_turns() -{ - wp_control = CIRCLE_MODE; - - if(next_command.lat == 0) - set_next_WP(¤t_loc); - else - set_next_WP(&next_command); - - loiter_total = next_command.p1 * 360; - loiter_sum = 0; -} - -static void do_loiter_time() -{ - if(next_command.lat == 0){ - wp_control = LOITER_MODE; - loiter_time = millis(); - set_next_WP(¤t_loc); - }else{ - wp_control = WP_MODE; - set_next_WP(&next_command); - } - - loiter_time_max = next_command.p1 * 1000; // units are (seconds) -} - -/********************************************************************************/ -// Verify Nav (Must) commands -/********************************************************************************/ - -static bool verify_takeoff() -{ - - // wait until we are ready! - if(g.rc_3.control_in == 0){ - return false; - } - - if (current_loc.alt > next_WP.alt){ - //Serial.println("Y"); - takeoff_complete = true; - return true; - - }else{ - - //Serial.println("N"); - return false; - } -} - -static bool verify_land() -{ - // land at 1 meter per second - next_WP.alt = original_alt - ((millis() - land_start) / 20); // condition_value = our initial - - velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8); - old_alt = current_loc.alt; - - if(g.sonar_enabled){ - // decide which sensor we're using - if(sonar_alt < 300){ - next_WP = current_loc; // don't pitch or roll - next_WP.alt = -200; // force us down - } - if(sonar_alt < 40){ - land_complete = true; - //Serial.println("Y"); - //return true; - } - } - - if(velocity_land <= 0){ - land_complete = true; - //return true; - } - //Serial.printf("N, %d\n", velocity_land); - //Serial.printf("N_alt, %ld\n", next_WP.alt); - - return false; -} - -static bool verify_nav_wp() -{ - // Altitude checking - if(next_WP.options & WP_OPTION_ALT_REQUIRED){ - // we desire a certain minimum altitude - if (current_loc.alt > next_WP.alt){ - // we have reached that altitude - wp_verify_byte |= NAV_ALTITUDE; - } - } - - // Did we pass the WP? // Distance checking - if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ - - // if we have a distance calc error, wp_distance may be less than 0 - if(wp_distance > 0){ - wp_verify_byte |= NAV_LOCATION; - - if(loiter_time == 0){ - loiter_time = millis(); - } - } - } - - // Hold at Waypoint checking, we cant move on until this is OK - if(wp_verify_byte & NAV_LOCATION){ - // we have reached our goal - - // loiter at the WP - wp_control = LOITER_MODE; - - if ((millis() - loiter_time) > loiter_time_max) { - wp_verify_byte |= NAV_DELAY; - //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); - //Serial.println("vlt done"); - } - } - - if(wp_verify_byte >= 7){ - //if(wp_verify_byte & NAV_LOCATION){ - char message[30]; - sprintf(message,"Reached Command #%i",command_must_index); - gcs_send_text(SEVERITY_LOW,message); - wp_verify_byte = 0; - return true; - }else{ - return false; - } -} - -static bool verify_loiter_unlim() -{ - return false; -} - -static bool verify_loiter_time() -{ - if(wp_control == LOITER_MODE){ - if ((millis() - loiter_time) > loiter_time_max) { - return true; - } - } - if(wp_control == WP_MODE && wp_distance <= g.waypoint_radius){ - // reset our loiter time - loiter_time = millis(); - // switch to position hold - wp_control = LOITER_MODE; - } - return false; -} - -static bool verify_loiter_turns() -{ - // have we rotated around the center enough times? - // ----------------------------------------------- - if(abs(loiter_sum) > loiter_total) { - loiter_total = 0; - loiter_sum = 0; - //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); - // clear the command queue; - return true; - } - return false; -} - -static bool verify_RTL() -{ - if (wp_distance <= g.waypoint_radius) { - //gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); - return true; - }else{ - return false; - } -} - -/********************************************************************************/ -// Condition (May) commands -/********************************************************************************/ - -static void do_wait_delay() -{ - //Serial.print("dwd "); - condition_start = millis(); - condition_value = next_command.lat * 1000; // convert to milliseconds - //Serial.println(condition_value,DEC); -} - -static void do_change_alt() -{ - Location temp = next_WP; - condition_start = current_loc.alt; - condition_value = next_command.alt; - temp.alt = next_command.alt; - set_next_WP(&temp); -} - -static void do_within_distance() -{ - condition_value = next_command.lat; -} - -static void do_yaw() -{ - //Serial.println("dyaw "); - yaw_tracking = MAV_ROI_NONE; - - // target angle in degrees - command_yaw_start = nav_yaw; // current position - command_yaw_start_time = millis(); - - command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise - command_yaw_speed = next_command.lat * 100; // ms * 100 - command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute - - - - // if unspecified go 30° a second - if(command_yaw_speed == 0) - command_yaw_speed = 3000; - - // if unspecified go counterclockwise - if(command_yaw_dir == 0) - command_yaw_dir = -1; - else - command_yaw_dir = 1; - - if (command_yaw_relative == 1){ - // relative - command_yaw_delta = next_command.alt * 100; - - }else{ - // absolute - command_yaw_end = next_command.alt * 100; - - // calculate the delta travel in deg * 100 - if(command_yaw_dir == 1){ - if(command_yaw_start >= command_yaw_end){ - command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); - }else{ - command_yaw_delta = command_yaw_end - command_yaw_start; - } - }else{ - if(command_yaw_start > command_yaw_end){ - command_yaw_delta = command_yaw_start - command_yaw_end; - }else{ - command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); - } - } - command_yaw_delta = wrap_360(command_yaw_delta); - } - - - // rate to turn deg per second - default is ten - command_yaw_time = (command_yaw_delta / command_yaw_speed) * 1000; -} - - -/********************************************************************************/ -// Verify Condition (May) commands -/********************************************************************************/ - -static bool verify_wait_delay() -{ - //Serial.print("vwd"); - if ((unsigned)(millis() - condition_start) > condition_value){ - //Serial.println("y"); - condition_value = 0; - return true; - } - //Serial.println("n"); - return false; -} - -static bool verify_change_alt() -{ - if (condition_start < next_WP.alt){ - // we are going higer - if(current_loc.alt > next_WP.alt){ - condition_value = 0; - return true; - } - }else{ - // we are going lower - if(current_loc.alt < next_WP.alt){ - condition_value = 0; - return true; - } - } - return false; -} - -static bool verify_within_distance() -{ - if (wp_distance < condition_value){ - condition_value = 0; - return true; - } - return false; -} - -static bool verify_yaw() -{ - //Serial.print("vyaw "); - - if((millis() - command_yaw_start_time) > command_yaw_time){ - // time out - // make sure we hold at the final desired yaw angle - nav_yaw = command_yaw_end; - auto_yaw = nav_yaw; - - //Serial.println("Y"); - return true; - - }else{ - // else we need to be at a certain place - // power is a ratio of the time : .5 = half done - float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time; - - nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir); - nav_yaw = wrap_360(nav_yaw); - auto_yaw = nav_yaw; - //Serial.printf("ny %ld\n",nav_yaw); - return false; - } -} - -/********************************************************************************/ -// Do (Now) commands -/********************************************************************************/ - -static void do_change_speed() -{ - g.waypoint_speed_max = next_command.p1 * 100; -} - -static void do_target_yaw() -{ - yaw_tracking = next_command.p1; - - if(yaw_tracking == MAV_ROI_LOCATION){ - target_WP = next_command; - } -} - -static void do_loiter_at_location() -{ - next_WP = current_loc; -} - -static void do_jump() -{ - if(jump == -10){ - jump = next_command.lat; - } - - if(jump > 0) { - jump--; - command_must_index = 0; - command_may_index = 0; - - // set pointer to desired index - g.waypoint_index = next_command.p1 - 1; - - } else if (jump == 0){ - // we're done, move along - jump = -10; - - } else if (jump == -1) { - // repeat forever - g.waypoint_index = next_command.p1 - 1; - } -} - -static void do_set_home() -{ - if(next_command.p1 == 1) { - init_home(); - } else { - home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = next_command.lng; // Lon * 10**7 - home.lat = next_command.lat; // Lat * 10**7 - home.alt = max(next_command.alt, 0); - home_is_set = true; - } -} - -static void do_set_servo() -{ - APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); -} - -static void do_set_relay() -{ - if (next_command.p1 == 1) { - relay.on(); - } else if (next_command.p1 == 0) { - relay.off(); - }else{ - relay.toggle(); - } -} - -static void do_repeat_servo() -{ - event_id = next_command.p1 - 1; - - if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) { - - event_timer = 0; - event_value = next_command.alt; - event_repeat = next_command.lat * 2; - event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - - switch(next_command.p1) { - case CH_5: - event_undo_value = g.rc_5.radio_trim; - break; - case CH_6: - event_undo_value = g.rc_6.radio_trim; - break; - case CH_7: - event_undo_value = g.rc_7.radio_trim; - break; - case CH_8: - event_undo_value = g.rc_8.radio_trim; - break; - } - update_events(); - } -} - -static void do_repeat_relay() -{ - event_id = RELAY_TOGGLE; - event_timer = 0; - event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_command.alt * 2; - update_events(); -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/commands_process.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// For changing active command mid-mission -//---------------------------------------- -static void change_command(uint8_t index) -{ - struct Location temp = get_command_with_index(index); - - if (temp.id > MAV_CMD_NAV_LAST ){ - gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd")); - } else { - command_must_index = NO_COMMAND; - next_command.id = NO_COMMAND; - g.waypoint_index = index - 1; - update_commands(); - } -} - -// called by 10 Hz Medium loop -// --------------------------- -static void update_commands(void) -{ - // fill command queue with a new command if available - if(next_command.id == NO_COMMAND){ - - // fetch next command if the next command queue is empty - // ----------------------------------------------------- - if (g.waypoint_index < g.waypoint_total) { - - // only if we have a cmd stored in EEPROM - next_command = get_command_with_index(g.waypoint_index + 1); - //Serial.printf("queue CMD %d\n", next_command.id); - } - } - - // Are we out of must commands and the queue is empty? - if(next_command.id == NO_COMMAND && command_must_index == NO_COMMAND){ - // if no commands were available from EEPROM - // And we have no nav commands - // -------------------------------------------- - if (command_must_ID == NO_COMMAND){ - gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); - handle_no_commands(); - } - } - - // check to see if we need to act on our command queue - if (process_next_command()){ - //Serial.printf("did PNC: %d\n", next_command.id); - - // We acted on the queue - let's debug that - // ---------------------------------------- - print_wp(&next_command, g.waypoint_index); - - // invalidate command queue so a new one is loaded - // ----------------------------------------------- - clear_command_queue(); - - // make sure we load the next command index - // ---------------------------------------- - increment_WP_index(); - } -} - -// called with GPS navigation update - not constantly -static void verify_commands(void) -{ - if(verify_must()){ - //Serial.printf("verified must cmd %d\n" , command_must_index); - command_must_index = NO_COMMAND; - }else{ - //Serial.printf("verified must false %d\n" , command_must_index); - } - - if(verify_may()){ - //Serial.printf("verified may cmd %d\n" , command_may_index); - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; - } -} - -static bool -process_next_command() -{ - // these are Navigation/Must commands - // --------------------------------- - if (command_must_index == NO_COMMAND){ // no current command loaded - if (next_command.id < MAV_CMD_NAV_LAST ){ - - // we remember the index of our mission here - command_must_index = g.waypoint_index + 1; - - // Save CMD to Log - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index + 1, &next_command); - - // Act on the new command - process_must(); - return true; - } - } - - // these are Condition/May commands - // ---------------------- - if (command_may_index == NO_COMMAND){ - if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ - - // we remember the index of our mission here - command_may_index = g.waypoint_index + 1; - - //SendDebug("MSG new may "); - //SendDebugln(next_command.id,DEC); - //Serial.print("new command_may_index "); - //Serial.println(command_may_index,DEC); - - // Save CMD to Log - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index + 1, &next_command); - - process_may(); - return true; - } - - // these are Do/Now commands - // --------------------------- - if (next_command.id > MAV_CMD_CONDITION_LAST){ - //SendDebug("MSG new now "); - //SendDebugln(next_command.id,DEC); - - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index + 1, &next_command); - process_now(); - return true; - } - } - // we did not need any new commands - return false; -} - -/**************************************************/ -// These functions implement the commands. -/**************************************************/ -static void process_must() -{ - //gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); - //Serial.printf("pmst %d\n", (int)next_command.id); - - // clear May indexes to force loading of more commands - // existing May commands are tossed. - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; - - // remember our command ID - command_must_ID = next_command.id; - - // implements the Flight Logic - handle_process_must(); - -} - -static void process_may() -{ - //gcs_send_text_P(SEVERITY_LOW,PSTR("")); - //Serial.print("pmay"); - - command_may_ID = next_command.id; - handle_process_may(); -} - -static void process_now() -{ - //Serial.print("pnow"); - handle_process_now(); -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/control_modes.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -static void read_control_switch() -{ - static bool switch_debouncer = false; - byte switchPosition = readSwitch(); - - if (oldSwitchPosition != switchPosition){ - if(switch_debouncer){ - // remember the prev location for GS - prev_WP = current_loc; - oldSwitchPosition = switchPosition; - switch_debouncer = false; - - set_mode(flight_modes[switchPosition]); - - #if CH7_OPTION != CH7_SIMPLE_MODE - // setup Simple mode - // do we enable simple mode? - do_simple = (g.simple_modes & (1 << switchPosition)); - #endif - }else{ - switch_debouncer = true; - } - } -} - -static byte readSwitch(void){ - int pulsewidth = g.rc_5.radio_in; // default for Arducopter - - if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; - if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; - if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; - if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual - if (pulsewidth >= 1750) return 5; // Hardware Manual - return 0; -} - -static void reset_control_switch() -{ - oldSwitchPosition = -1; - read_control_switch(); -} - -// read at 10 hz -// set this to your trainer switch -static void read_trim_switch() -{ - #if CH7_OPTION == CH7_FLIP - if (g.rc_7.control_in > 800 && g.rc_3.control_in != 0){ - do_flip = true; - } - - #elif CH7_OPTION == CH7_SIMPLE_MODE - do_simple = (g.rc_7.control_in > 800); - //Serial.println(g.rc_7.control_in, DEC); - - #elif CH7_OPTION == CH7_RTL - static bool ch7_rtl_flag = false; - - if (ch7_rtl_flag == false && g.rc_7.control_in > 800){ - ch7_rtl_flag = true; - set_mode(RTL); - } - - if (ch7_rtl_flag == true && g.rc_7.control_in < 800){ - ch7_rtl_flag = false; - if (control_mode == RTL || control_mode == LOITER){ - reset_control_switch(); - } - } - - #elif CH7_OPTION == CH7_SET_HOVER - // switch is engaged - if (g.rc_7.control_in > 800){ - trim_flag = true; - - }else{ // switch is disengaged - - if(trim_flag){ - - // set the throttle nominal - if(g.rc_3.control_in > 150){ - g.throttle_cruise.set_and_save(g.rc_3.control_in); - //Serial.printf("tnom %d\n", g.throttle_cruise.get()); - } - trim_flag = false; - } - } - - #elif CH7_OPTION == CH7_SAVE_WP - - if (g.rc_7.control_in > 800){ - trim_flag = true; - - }else{ // switch is disengaged - - if(trim_flag){ - // set the next_WP - CH7_wp_index++; - current_loc.id = MAV_CMD_NAV_WAYPOINT; - g.waypoint_total.set_and_save(CH7_wp_index); - set_command_with_index(current_loc, CH7_wp_index); - } - } - - #elif CH7_OPTION == CH7_ADC_FILTER - if (g.rc_7.control_in > 800){ - adc.filter_result = true; - }else{ - adc.filter_result = false; - } - - #elif CH7_OPTION == CH7_AUTO_TRIM - if (g.rc_7.control_in > 800){ - auto_level_counter = 10; - } - #endif - -} - -static void auto_trim() -{ - if(auto_level_counter > 0){ - //g.rc_1.dead_zone = 60; // 60 = .6 degrees - //g.rc_2.dead_zone = 60; - - auto_level_counter--; - trim_accel(); - led_mode = AUTO_TRIM_LEDS; - - if(auto_level_counter == 1){ - //g.rc_1.dead_zone = 0; // 60 = .6 degrees - //g.rc_2.dead_zone = 0; - led_mode = NORMAL_LEDS; - clear_leds(); - imu.save(); - - //Serial.println("Done"); - auto_level_counter = 0; - - // set TC - init_throttle_cruise(); - } - } -} - - - -static void trim_accel() -{ - g.pi_stabilize_roll.reset_I(); - g.pi_stabilize_pitch.reset_I(); - - if(g.rc_1.control_in > 0){ // Roll RIght - imu.ay(imu.ay() + 1); - }else if (g.rc_1.control_in < 0){ - imu.ay(imu.ay() - 1); - } - - if(g.rc_2.control_in > 0){ // Pitch Back - imu.ax(imu.ax() + 1); - }else if (g.rc_2.control_in < 0){ - imu.ax(imu.ax() - 1); - } - - /* - Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"), - dcm.roll_sensor, - dcm.pitch_sensor, - (float)imu.ax(), - (float)imu.ay(), - (float)imu.az()); - //*/ -} - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/events.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - This event will be called when the failsafe changes - boolean failsafe reflects the current state -*/ -static void failsafe_on_event() -{ - // This is how to handle a failsafe. - switch(control_mode) - { - case AUTO: - if (g.throttle_fs_action == 1) { - set_mode(RTL); - } - // 2 = Stay in AUTO and ignore failsafe - - default: - // not ready to enable yet w/o more testing - //set_mode(RTL); - break; - } -} - -static void failsafe_off_event() -{ - if (g.throttle_fs_action == 2){ - // We're back in radio contact - // return to AP - // --------------------------- - - // re-read the switch so we can return to our preferred mode - // -------------------------------------------------------- - reset_control_switch(); - - - }else if (g.throttle_fs_action == 1){ - // We're back in radio contact - // return to Home - // we should already be in RTL and throttle set to cruise - // ------------------------------------------------------ - set_mode(RTL); - } -} - -static void low_battery_event(void) -{ - gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); - low_batt = true; - - // if we are in Auto mode, come home - if(control_mode >= AUTO) - set_mode(RTL); -} - - -static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY -{ - if(event_repeat == 0 || (millis() - event_timer) < event_delay) - return; - - if (event_repeat > 0){ - event_repeat --; - } - - if(event_repeat != 0) { // event_repeat = -1 means repeat forever - event_timer = millis(); - - if (event_id >= CH_5 && event_id <= CH_8) { - if(event_repeat%2) { - APM_RC.OutputCh(event_id, event_value); // send to Servos - } else { - APM_RC.OutputCh(event_id, event_undo_value); - } - } - - if (event_id == RELAY_TOGGLE) { - relay.toggle(); - } - } -} - -#if PIEZO == ENABLED -void piezo_on() -{ - digitalWrite(PIEZO_PIN,HIGH); - //PORTF |= B00100000; -} - -void piezo_off() -{ - digitalWrite(PIEZO_PIN,LOW); - //PORTF &= ~B00100000; -} - -void piezo_beep() -{ - // Note: This command should not be used in time sensitive loops - piezo_on(); - delay(100); - piezo_off(); -} -#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/flip.pde" -// 2010 Jose Julio -// 2011 Adapted for AC2 by Jason Short -// -// Automatic Acrobatic Procedure (AAP) v1 : Roll flip -// State machine aproach: -// Some states are fixed commands (for a fixed time) -// Some states are fixed commands (until some IMU condition) -// Some states include controls inside -#if CH7_OPTION == CH7_FLIP -void roll_flip() -{ - #define AAP_THR_INC 180 - #define AAP_THR_DEC 90 - #define AAP_ROLL_OUT 200 - #define AAP_ROLL_RATE 3000 // up to 1250 - - static int AAP_timer = 0; - static byte AAP_state = 0; - - // Yaw - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); - // Pitch - g.rc_2.servo_out = get_stabilize_pitch(0); - - // State machine - switch (AAP_state){ - case 0: // Step 1 : Initialize - AAP_timer = 0; - AAP_state++; - break; - case 1: // Step 2 : Increase throttle to start maneuver - if (AAP_timer < 95){ // .5 seconds - g.rc_1.servo_out = get_stabilize_roll(0); - g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; - //g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); - AAP_timer++; - }else{ - AAP_state++; - AAP_timer = 0; - } - break; - - case 2: // Step 3 : ROLL (until we reach a certain angle [45º]) - if (dcm.roll_sensor < 4500){ - // Roll control - g.rc_1.servo_out = AAP_ROLL_OUT; //get_rate_roll(AAP_ROLL_RATE); - g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; - }else{ - AAP_state++; - } - break; - - case 3: // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45º]) - if ((dcm.roll_sensor >= 4500) || (dcm.roll_sensor < -4500)){ - g.rc_1.servo_out = 150; //get_rate_roll(AAP_ROLL_RATE); - g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; - }else{ - AAP_state++; - } - break; - - case 4: // Step 5 : Increase throttle to stop the descend - if (AAP_timer < 90){ // .5 seconds - g.rc_1.servo_out = get_stabilize_roll(0); - g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; - AAP_timer++; - }else{ - AAP_state++; - AAP_timer = 0; - } - break; - - case 5: // exit mode - //control_mode = - do_flip = false; - break; - } -} -#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/heli.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if FRAME_CONFIG == HELI_FRAME - -#define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz -#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz - -static int heli_manual_override = false; -static float heli_throttle_scaler = 0; - -// heli_servo_averaging: -// 0 or 1 = no averaging, 250hz -// 2 = average two samples, 125hz -// 3 = averaging three samples = 83.3 hz -// 4 = averaging four samples = 62.5 hz -// 5 = averaging 5 samples = 50hz -// digital = 0 / 250hz, analog = 2 / 83.3 - -static void heli_init_swash() -{ - int i; - int tilt_max[CH_3+1]; - int total_tilt_max = 0; - - // swash servo initialisation - g.heli_servo_1.set_range(0,1000); - g.heli_servo_2.set_range(0,1000); - g.heli_servo_3.set_range(0,1000); - g.heli_servo_4.set_angle(4500); - - // pitch factors - heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos)); - heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos)); - heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos)); - - // roll factors - heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90)); - heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90)); - heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90)); - - // collective min / max - total_tilt_max = 0; - for( i=CH_1; i<=CH_3; i++ ) { - tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100; - total_tilt_max = max(total_tilt_max,tilt_max[i]); - } - - // servo min/max values - or should I use set_range? - g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1]; - g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1]; - g.heli_servo_2.radio_min = g.heli_coll_min - tilt_max[CH_2]; - g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2]; - g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3]; - g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3]; - - // scaler for changing channel 3 radio input into collective range - heli_throttle_scaler = ((float)(g.heli_coll_max - g.heli_coll_min))/1000; - - // reset the servo averaging - for( i=0; i<=3; i++ ) - heli_servo_out[i] = 0; - - // double check heli_servo_averaging is reasonable - if( g.heli_servo_averaging < 0 || g.heli_servo_averaging < 0 > 5 ) { - g.heli_servo_averaging = 0; - g.heli_servo_averaging.save(); - } -} - -static void heli_move_servos_to_mid() -{ - heli_move_swash(0,0,1500,0); -} - -// -// heli_move_swash - moves swash plate to attitude of parameters passed in -// - expected ranges: -// roll : -4500 ~ 4500 -// pitch: -4500 ~ 4500 -// collective: 1000 ~ 2000 -// yaw: -4500 ~ 4500 -// -static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) -{ - // ensure values are acceptable: - roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); - pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); - coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max); - - // swashplate servos - g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); - if( g.heli_servo_1.get_reverse() ) - g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out; - - g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); - if( g.heli_servo_2.get_reverse() ) - g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out; - - g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); - if( g.heli_servo_3.get_reverse() ) - g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out; - - if( g.heli_servo_4.get_reverse() ) - g.heli_servo_4.servo_out = -yaw_out; // should probably just use rc_4 directly like we do for a tricopter - else - g.heli_servo_4.servo_out = yaw_out; - - // use servo_out to calculate pwm_out and radio_out - g.heli_servo_1.calc_pwm(); - g.heli_servo_2.calc_pwm(); - g.heli_servo_3.calc_pwm(); - g.heli_servo_4.calc_pwm(); - - // add the servo values to the averaging - heli_servo_out[0] += g.heli_servo_1.servo_out; - heli_servo_out[1] += g.heli_servo_2.servo_out; - heli_servo_out[2] += g.heli_servo_3.servo_out; - heli_servo_out[3] += g.heli_servo_4.radio_out; - heli_servo_out_count++; - - // is it time to move the servos? - if( heli_servo_out_count >= g.heli_servo_averaging ) { - - // average the values if necessary - if( g.heli_servo_averaging >= 2 ) { - heli_servo_out[0] /= g.heli_servo_averaging; - heli_servo_out[1] /= g.heli_servo_averaging; - heli_servo_out[2] /= g.heli_servo_averaging; - heli_servo_out[3] /= g.heli_servo_averaging; - } - - // actually move the servos - APM_RC.OutputCh(CH_1, heli_servo_out[0]); - APM_RC.OutputCh(CH_2, heli_servo_out[1]); - APM_RC.OutputCh(CH_3, heli_servo_out[2]); - APM_RC.OutputCh(CH_4, heli_servo_out[3]); - - // output gyro value - if( g.heli_ext_gyro_enabled ) { - APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain); - } - - #if INSTANT_PWM == 1 - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - #endif - - // reset the averaging - heli_servo_out_count = 0; - heli_servo_out[0] = 0; - heli_servo_out[1] = 0; - heli_servo_out[2] = 0; - heli_servo_out[3] = 0; - } -} - -static void init_motors_out() -{ - #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 - ICR3 = 40000; // 50 hz output CH 7, 8, 11 - #endif -} - -// these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better -static void output_motors_armed() -{ - //static int counter = 0; - g.rc_1.calc_pwm(); - g.rc_2.calc_pwm(); - g.rc_3.calc_pwm(); - g.rc_4.calc_pwm(); - - if( heli_manual_override ) { - // straight pass through from radio inputs to swash plate - heli_move_swash( g.rc_1.control_in, g.rc_2.control_in, g.rc_3.radio_in, g.rc_4.control_in ); - }else{ - // source inputs from attitude controller - heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); - } -} - -// for helis - armed or disarmed we allow servos to move -static void output_motors_disarmed() -{ - if(g.rc_3.control_in > 0){ - // we have pushed up the throttle, remove safety - motor_auto_armed = true; - } - - output_motors_armed(); -} - -static void output_motor_test() -{ -} - -// heli_get_scaled_throttle - user's throttle scaled to collective range -// input is expected to be in the range of 0~1000 (ie. pwm) -// also does equivalent of angle_boost -static int heli_get_scaled_throttle(int throttle) -{ - float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler; - return g.heli_coll_min - 1000 + (throttle * heli_throttle_scaler); -} - -// heli_angle_boost - takes servo_out position -// adds a boost depending on roll/pitch values -// equivalent of quad's angle_boost function -// pwm_out value should be 0 ~ 1000 -static int heli_get_angle_boost(int pwm_out) -{ - float angle_boost_factor = cos_pitch_x * cos_roll_x; - angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); - int throttle_above_center = max(1000 + pwm_out - g.heli_coll_mid,0); - return pwm_out + throttle_above_center*angle_boost_factor; -} - -#endif // HELI_FRAME -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/leds.pde" -static void update_lights() -{ - switch(led_mode){ - case NORMAL_LEDS: - update_motor_light(); - update_GPS_light(); - break; - - case AUTO_TRIM_LEDS: - dancing_light(); - break; - } -} - -static void update_GPS_light(void) -{ - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - switch (g_gps->status()){ - - case(2): - digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case(1): - if (g_gps->valid_read == true){ - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light){ - digitalWrite(C_LED_PIN, LOW); - }else{ - digitalWrite(C_LED_PIN, HIGH); - } - g_gps->valid_read = false; - } - break; - - default: - digitalWrite(C_LED_PIN, LOW); - break; - } -} - -static void update_motor_light(void) -{ - if(motor_armed == false){ - motor_light = !motor_light; - - // blink - if(motor_light){ - digitalWrite(A_LED_PIN, HIGH); - }else{ - digitalWrite(A_LED_PIN, LOW); - } - }else{ - if(!motor_light){ - motor_light = true; - digitalWrite(A_LED_PIN, HIGH); - } - } -} - -static void dancing_light() -{ - static byte step; - - if (step++ == 3) - step = 0; - - switch(step) - { - case 0: - digitalWrite(C_LED_PIN, LOW); - digitalWrite(A_LED_PIN, HIGH); - break; - - case 1: - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, HIGH); - break; - - case 2: - digitalWrite(B_LED_PIN, LOW); - digitalWrite(C_LED_PIN, HIGH); - break; - } -} -static void clear_leds() -{ - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, LOW); - digitalWrite(C_LED_PIN, LOW); - motor_light = false; - led_mode = NORMAL_LEDS; -} - -#if MOTOR_LEDS == 1 -static void update_motor_leds(void) -{ - if (motor_armed == true){ - if (low_batt == true){ - // blink rear - static bool blink = false; - - if (blink){ - digitalWrite(RE_LED, HIGH); - digitalWrite(FR_LED, HIGH); - digitalWrite(RI_LED, LOW); - digitalWrite(LE_LED, LOW); - }else{ - digitalWrite(RE_LED, LOW); - digitalWrite(FR_LED, LOW); - digitalWrite(RI_LED, HIGH); - digitalWrite(LE_LED, HIGH); - } - blink = !blink; - }else{ - digitalWrite(RE_LED, HIGH); - digitalWrite(FR_LED, HIGH); - digitalWrite(RI_LED, HIGH); - digitalWrite(LE_LED, HIGH); - } - }else { - digitalWrite(RE_LED, LOW); - digitalWrite(FR_LED, LOW); - digitalWrite(RI_LED, LOW); - digitalWrite(LE_LED, LOW); - } -} -#endif - - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#define ARM_DELAY 10 // one second -#define DISARM_DELAY 10 // one second -#define LEVEL_DELAY 70 // twelve seconds -#define AUTO_LEVEL_DELAY 90 // twentyfive seconds - - -// called at 10hz -static void arm_motors() -{ - static int arming_counter; - - // Arm motor output : Throttle down and full yaw right for more than 2 seconds - if (g.rc_3.control_in == 0){ - - // full right - if (g.rc_4.control_in > 4000) { - - // don't allow arming in anything but manual - if (control_mode < ALT_HOLD){ - - if (arming_counter > AUTO_LEVEL_DELAY){ - auto_level_counter = 155; - arming_counter = 0; - - }else if (arming_counter == ARM_DELAY){ - #if HIL_MODE != HIL_MODE_DISABLED - gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); - #endif - motor_armed = true; - arming_counter = ARM_DELAY; - - #if PIEZO_ARMING == 1 - piezo_beep(); - piezo_beep(); - #endif - - // Tune down DCM - // ------------------- - #if HIL_MODE != HIL_MODE_ATTITUDE - dcm.kp_roll_pitch(0.030000); - dcm.ki_roll_pitch(0.00001278), // 50 hz I term - //dcm.ki_roll_pitch(0.000006); - #endif - - // tune down compass - // ----------------- - dcm.kp_yaw(0.08); - dcm.ki_yaw(0); - - // Remember Orientation - // -------------------- - init_simple_bearing(); - - // Reset home position - // ---------------------- - if(home_is_set) - init_home(); - - if(did_ground_start == false){ - did_ground_start = true; - startup_ground(); - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - // read Baro pressure at ground - - // this resets Baro for more accuracy - //----------------------------------- - init_barometer(); - #endif - - // temp hack - motor_light = true; - digitalWrite(A_LED_PIN, HIGH); - - arming_counter++; - } else{ - arming_counter++; - } - } - - // full left - }else if (g.rc_4.control_in < -4000) { - //Serial.print(arming_counter, DEC); - if (arming_counter > LEVEL_DELAY){ - //Serial.print("init"); - imu.init_accel(mavlink_delay); - arming_counter = 0; - - }else if (arming_counter == DISARM_DELAY){ - #if HIL_MODE != HIL_MODE_DISABLED - gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); - #endif - - motor_armed = false; - arming_counter = DISARM_DELAY; - compass.save_offsets(); - - #if PIEZO_ARMING == 1 - piezo_beep(); - #endif - - // Tune down DCM - // ------------------- - #if HIL_MODE != HIL_MODE_ATTITUDE - //dcm.kp_roll_pitch(0.12); // higher for fast recovery - //dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop - #endif - - // tune up compass - // ----------------- - dcm.kp_yaw(0.8); - dcm.ki_yaw(0.00004); - - // Clear throttle slew - // ------------------- - //throttle_slew = 0; - - arming_counter++; - - }else{ - arming_counter++; - } - // centered - }else{ - arming_counter = 0; - } - }else{ - arming_counter = 0; - } -} - - -/***************************************** - * Set the flight control servos based on the current calculated values - *****************************************/ -static void -set_servos_4() -{ - if (motor_armed == true && motor_auto_armed == true) { - // creates the radio_out and pwm_out values - output_motors_armed(); - } else{ - output_motors_disarmed(); - } -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_hexa.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if FRAME_CONFIG == HEXA_FRAME - -static void init_motors_out() -{ - #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 - ICR3 = 5000; // 50 hz output CH 7, 8, 11 - #endif -} - -static void output_motors_armed() -{ - int roll_out, pitch_out; - int out_min = g.rc_3.radio_min; - int out_max = g.rc_3.radio_max; - - // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); - - if(g.rc_3.servo_out > 0) - out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; - - g.rc_1.calc_pwm(); - g.rc_2.calc_pwm(); - g.rc_3.calc_pwm(); - g.rc_4.calc_pwm(); - - if(g.frame_orientation == X_FRAME){ - roll_out = g.rc_1.pwm_out / 2; - pitch_out = (float)g.rc_2.pwm_out * .866; - - //left side - motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle - motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front - motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back - - //right side - motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle - motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front - motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back - - }else{ - roll_out = (float)g.rc_1.pwm_out * .866; - pitch_out = g.rc_2.pwm_out / 2; - - //Front side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT - motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT - motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT - - //Back side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK - motor_out[CH_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT - motor_out[CH_8] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT - } - - // Yaw - motor_out[CH_2] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_4] += g.rc_4.pwm_out; // CCW - - motor_out[CH_3] -= g.rc_4.pwm_out; // CW - motor_out[CH_1] -= g.rc_4.pwm_out; // CW - motor_out[CH_8] -= g.rc_4.pwm_out; // CW - - - // Tridge's stability patch - for (int i = CH_1; i<=CH_8; i++) { - if(i == CH_5 || i == CH_6) - break; - if (motor_out[i] > out_max) { - // note that i^1 is the opposite motor - motor_out[i^1] -= motor_out[i] - out_max; - motor_out[i] = out_max; - } - } - - // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); - - #if CUT_MOTORS == ENABLED - // if we are not sending a throttle output, we cut the motors - if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - } - #endif - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); - - #if INSTANT_PWM == 1 - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - APM_RC.Force_Out6_Out7(); - #endif - -} - -static void output_motors_disarmed() -{ - if(g.rc_3.control_in > 0){ - // we have pushed up the throttle - // remove safety - motor_auto_armed = true; - } - - // fill the motor_out[] array for HIL use - for (unsigned char i = 0; i < 8; i++) { - motor_out[i] = g.rc_3.radio_min; - } - - // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); -} - -static void output_motor_test() -{ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - - - if(g.frame_orientation == X_FRAME){ -// 31 -// 24 - if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 100; - } - - if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 100; - } - - if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 100; - motor_out[CH_4] += 100; - } - - if(g.rc_2.control_in < -3000){ // front - motor_out[CH_7] += 100; - motor_out[CH_3] += 100; - } - - }else{ -// 3 -// 2 1 -// 4 - if(g.rc_1.control_in > 3000){ // right - motor_out[CH_4] += 100; - motor_out[CH_8] += 100; - } - - if(g.rc_1.control_in < -3000){ // left - motor_out[CH_7] += 100; - motor_out[CH_3] += 100; - } - - if(g.rc_2.control_in > 3000){ // back - motor_out[CH_2] += 100; - } - - if(g.rc_2.control_in < -3000){ // front - motor_out[CH_1] += 100; - } - - } - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); -} - -/* - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); - delay(1000); -} -*/ - -#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_octa.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if FRAME_CONFIG == OCTA_FRAME - -static void init_motors_out() -{ - #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 - ICR3 = 5000; // 50 hz output CH 7, 8, 11 - #endif -} - -static void output_motors_armed() -{ - int roll_out, pitch_out; - int out_min = g.rc_3.radio_min; - int out_max = g.rc_3.radio_max; - - // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); - - if(g.rc_3.servo_out > 0) - out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; - - g.rc_1.calc_pwm(); - g.rc_2.calc_pwm(); - g.rc_3.calc_pwm(); - g.rc_4.calc_pwm(); - - if(g.frame_orientation == X_FRAME){ - roll_out = (float)g.rc_1.pwm_out * 0.4; - pitch_out = (float)g.rc_2.pwm_out * 0.4; - - //Front side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT - motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT - - //Back side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT - - //Left side - motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT - motor_out[CH_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK - - //Right side - motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK - motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT - - }else if(g.frame_orientation == PLUS_FRAME){ - roll_out = (float)g.rc_1.pwm_out * 0.71; - pitch_out = (float)g.rc_2.pwm_out * 0.71; - - //Front side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT - motor_out[CH_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT - motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT - - //Left side - motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT - - //Right side - motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT - - //Back side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK - motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT - motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT - - }else if(g.frame_orientation == V_FRAME){ - - int roll_out2, pitch_out2; - int roll_out3, pitch_out3; - int roll_out4, pitch_out4; - - roll_out = g.rc_1.pwm_out; - pitch_out = g.rc_2.pwm_out; - roll_out2 = (float)g.rc_1.pwm_out * 0.833; - pitch_out2 = (float)g.rc_2.pwm_out * 0.34; - roll_out3 = (float)g.rc_1.pwm_out * 0.666; - pitch_out3 = (float)g.rc_2.pwm_out * 0.32; - roll_out4 = g.rc_1.pwm_out / 2; - pitch_out4 = (float)g.rc_2.pwm_out * 0.98; - - //Front side - motor_out[CH_10] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT - motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT - - //Left side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT - motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK - - //Right side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK - motor_out[CH_8] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT - - //Back side - motor_out[CH_11] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT - - } - - // Yaw - motor_out[CH_3] += g.rc_4.pwm_out; // CCW - motor_out[CH_4] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_8] += g.rc_4.pwm_out; // CCW - - motor_out[CH_1] -= g.rc_4.pwm_out; // CW - motor_out[CH_2] -= g.rc_4.pwm_out; // CW - motor_out[CH_10] -= g.rc_4.pwm_out; // CW - motor_out[CH_11] -= g.rc_4.pwm_out; // CW - - - // TODO add stability patch - motor_out[CH_1] = min(motor_out[CH_1], out_max); - motor_out[CH_2] = min(motor_out[CH_2], out_max); - motor_out[CH_3] = min(motor_out[CH_3], out_max); - motor_out[CH_4] = min(motor_out[CH_4], out_max); - motor_out[CH_7] = min(motor_out[CH_7], out_max); - motor_out[CH_8] = min(motor_out[CH_8], out_max); - motor_out[CH_10] = min(motor_out[CH_10], out_max); - motor_out[CH_11] = min(motor_out[CH_11], out_max); - - - // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); - motor_out[CH_10] = max(motor_out[CH_10], out_min); - motor_out[CH_11] = max(motor_out[CH_11], out_min); - - - #if CUT_MOTORS == ENABLED - // if we are not sending a throttle output, we cut the motors - if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - motor_out[CH_10] = g.rc_3.radio_min; - motor_out[CH_11] = g.rc_3.radio_min; - } - #endif - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); - APM_RC.OutputCh(CH_10, motor_out[CH_10]); - APM_RC.OutputCh(CH_11, motor_out[CH_11]); - - #if INSTANT_PWM == 1 - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - APM_RC.Force_Out6_Out7(); - #endif -} - -static void output_motors_disarmed() -{ - if(g.rc_3.control_in > 0){ - // we have pushed up the throttle - // remove safety - motor_auto_armed = true; - } - - // fill the motor_out[] array for HIL use - for (unsigned char i = 0; i < 11; i++) { - motor_out[i] = g.rc_3.radio_min; - } - - // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); -} - -static void output_motor_test() -{ - if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME ) - { - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); - delay(1000); - } - - if( g.frame_orientation == V_FRAME ) - { - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); - delay(1000); - } -} - -#endif - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_octa_quad.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if FRAME_CONFIG == OCTA_QUAD_FRAME - -static void init_motors_out() -{ - #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 - ICR3 = 5000; // 50 hz output CH 7, 8, 11 - #endif -} - -static void output_motors_armed() -{ - int roll_out, pitch_out; - int out_min = g.rc_3.radio_min; - int out_max = g.rc_3.radio_max; - - // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); - - if(g.rc_3.servo_out > 0) - out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; - - g.rc_1.calc_pwm(); - g.rc_2.calc_pwm(); - g.rc_3.calc_pwm(); - g.rc_4.calc_pwm(); - - if(g.frame_orientation == X_FRAME){ - roll_out = (float)g.rc_1.pwm_out * .707; - pitch_out = (float)g.rc_2.pwm_out * .707; - - // Front Left - motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP - motor_out[CH_8] = g.rc_3.radio_out + roll_out + pitch_out; // CW - - // Front Right - motor_out[CH_10] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP - motor_out[CH_11] = g.rc_3.radio_out - roll_out + pitch_out; // CW - - // Back Left - motor_out[CH_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP - motor_out[CH_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW - - // Back Right - motor_out[CH_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP - motor_out[CH_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW - - - - }if(g.frame_orientation == PLUS_FRAME){ - roll_out = g.rc_1.pwm_out; - pitch_out = g.rc_2.pwm_out; - - // Left - motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP - motor_out[CH_8] = g.rc_3.radio_out - roll_out; // CW - - // Right - motor_out[CH_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP - motor_out[CH_2] = g.rc_3.radio_out + roll_out; // CW - - // Front - motor_out[CH_10] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP - motor_out[CH_11] = g.rc_3.radio_out + pitch_out; // CW - - // Back - motor_out[CH_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP - motor_out[CH_4] = g.rc_3.radio_out - pitch_out; // CW - - } - - // Yaw - motor_out[CH_1] += g.rc_4.pwm_out; // CCW - motor_out[CH_3] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_10] += g.rc_4.pwm_out; // CCW - - motor_out[CH_2] -= g.rc_4.pwm_out; // CW - motor_out[CH_4] -= g.rc_4.pwm_out; // CW - motor_out[CH_8] -= g.rc_4.pwm_out; // CW - motor_out[CH_11] -= g.rc_4.pwm_out; // CW - - // TODO add stability patch - motor_out[CH_1] = min(motor_out[CH_1], out_max); - motor_out[CH_2] = min(motor_out[CH_2], out_max); - motor_out[CH_3] = min(motor_out[CH_3], out_max); - motor_out[CH_4] = min(motor_out[CH_4], out_max); - motor_out[CH_7] = min(motor_out[CH_7], out_max); - motor_out[CH_8] = min(motor_out[CH_8], out_max); - motor_out[CH_10] = min(motor_out[CH_10], out_max); - motor_out[CH_11] = min(motor_out[CH_11], out_max); - - // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); - motor_out[CH_10] = max(motor_out[CH_10], out_min); - motor_out[CH_11] = max(motor_out[CH_11], out_min); - - #if CUT_MOTORS == ENABLED - // if we are not sending a throttle output, we cut the motors - if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - motor_out[CH_10] = g.rc_3.radio_min; - motor_out[CH_11] = g.rc_3.radio_min; - } - #endif - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); - APM_RC.OutputCh(CH_10, motor_out[CH_10]); - APM_RC.OutputCh(CH_11, motor_out[CH_11]); - - #if INSTANT_PWM == 1 - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - APM_RC.Force_Out6_Out7(); - #endif -} - -static void output_motors_disarmed() -{ - if(g.rc_3.control_in > 0){ - // we have pushed up the throttle - // remove safety - motor_auto_armed = true; - } - - // fill the motor_out[] array for HIL use - for (unsigned char i = 0; i < 11; i++) { - motor_out[i] = g.rc_3.radio_min; - } - - // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); -} - -static void output_motor_test() -{ - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); - delay(1000); - - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); - delay(1000); -} - -#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_quad.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if FRAME_CONFIG == QUAD_FRAME - -static void init_motors_out() -{ - #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 - ICR3 = 40000; // 50 hz output CH 7, 8, 11 - #endif -} - -static void output_motors_armed() -{ - int roll_out, pitch_out; - int out_min = g.rc_3.radio_min; - int out_max = g.rc_3.radio_max; - - // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); - - if(g.rc_3.servo_out > 0) - out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; - - g.rc_1.calc_pwm(); - g.rc_2.calc_pwm(); - g.rc_3.calc_pwm(); - g.rc_4.calc_pwm(); - - if(g.frame_orientation == X_FRAME){ - roll_out = g.rc_1.pwm_out * .707; - pitch_out = g.rc_2.pwm_out * .707; - - // left - motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT - motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK - - // right - motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT - motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK - - }else{ - - roll_out = g.rc_1.pwm_out; - pitch_out = g.rc_2.pwm_out; - - // left - motor_out[CH_1] = g.rc_3.radio_out - roll_out; - // right - motor_out[CH_2] = g.rc_3.radio_out + roll_out; - // front - motor_out[CH_3] = g.rc_3.radio_out + pitch_out; - // back - motor_out[CH_4] = g.rc_3.radio_out - pitch_out; - } - - // Yaw input - motor_out[CH_1] += g.rc_4.pwm_out; // CCW - motor_out[CH_2] += g.rc_4.pwm_out; // CCW - motor_out[CH_3] -= g.rc_4.pwm_out; // CW - motor_out[CH_4] -= g.rc_4.pwm_out; // CW - - /* We need to clip motor output at out_max. When cipping a motors - * output we also need to compensate for the instability by - * lowering the opposite motor by the same proportion. This - * ensures that we retain control when one or more of the motors - * is at its maximum output - */ - for (int i=CH_1; i<=CH_4; i++) { - if (motor_out[i] > out_max) { - // note that i^1 is the opposite motor - motor_out[i^1] -= motor_out[i] - out_max; - motor_out[i] = out_max; - } - } - - // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - - #if CUT_MOTORS == ENABLED - // if we are not sending a throttle output, we cut the motors - if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - } - #endif - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - - #if INSTANT_PWM == 1 - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - #endif -} - -static void output_motors_disarmed() -{ - if(g.rc_3.control_in > 0){ - // we have pushed up the throttle - // remove safety - motor_auto_armed = true; - } - - // fill the motor_out[] array for HIL use - for (unsigned char i = 0; i < 8; i++) { - motor_out[i] = g.rc_3.radio_min; - } - - // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); -} - -/* -static void debug_motors() -{ - Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n", - motor_out[CH_1], - motor_out[CH_2], - motor_out[CH_3], - motor_out[CH_4]); -} -*/ - -static void output_motor_test() -{ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - - - if(g.frame_orientation == X_FRAME){ -// 31 -// 24 - if(g.rc_1.control_in > 3000){ - motor_out[CH_1] += 100; - motor_out[CH_4] += 100; - } - - if(g.rc_1.control_in < -3000){ - motor_out[CH_2] += 100; - motor_out[CH_3] += 100; - } - - if(g.rc_2.control_in > 3000){ - motor_out[CH_2] += 100; - motor_out[CH_4] += 100; - } - - if(g.rc_2.control_in < -3000){ - motor_out[CH_1] += 100; - motor_out[CH_3] += 100; - } - - }else{ -// 3 -// 2 1 -// 4 - if(g.rc_1.control_in > 3000) - motor_out[CH_1] += 100; - - if(g.rc_1.control_in < -3000) - motor_out[CH_2] += 100; - - if(g.rc_2.control_in > 3000) - motor_out[CH_4] += 100; - - if(g.rc_2.control_in < -3000) - motor_out[CH_3] += 100; - } - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); -} - -#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_tri.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if FRAME_CONFIG == TRI_FRAME - -static void init_motors_out() -{ - #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 - ICR3 = 40000; // 50 hz output CH 7, 8, 11 - #endif -} - - -static void output_motors_armed() -{ - int out_min = g.rc_3.radio_min; - int out_max = g.rc_3.radio_max; - - // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); - - if(g.rc_3.servo_out > 0) - out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; - - g.rc_1.calc_pwm(); - g.rc_2.calc_pwm(); - g.rc_3.calc_pwm(); - - int roll_out = (float)g.rc_1.pwm_out * .866; - int pitch_out = g.rc_2.pwm_out / 2; - - //left front - motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out; - //right front - motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; - // rear - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; - - //motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013; - - // Tridge's stability patch - if (motor_out[CH_1] > out_max) { - motor_out[CH_2] -= (motor_out[CH_1] - out_max) >> 1; - motor_out[CH_4] -= (motor_out[CH_1] - out_max) >> 1; - motor_out[CH_1] = out_max; - } - - if (motor_out[CH_2] > out_max) { - motor_out[CH_1] -= (motor_out[CH_2] - out_max) >> 1; - motor_out[CH_4] -= (motor_out[CH_2] - out_max) >> 1; - motor_out[CH_2] = out_max; - } - - if (motor_out[CH_4] > out_max) { - motor_out[CH_1] -= (motor_out[CH_4] - out_max) >> 1; - motor_out[CH_2] -= (motor_out[CH_4] - out_max) >> 1; - motor_out[CH_4] = out_max; - } - - // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - - #if CUT_MOTORS == ENABLED - // if we are not sending a throttle output, we cut the motors - if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - } - #endif - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - - #if INSTANT_PWM == 1 - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - #endif -} - -static void output_motors_disarmed() -{ - if(g.rc_3.control_in > 0){ - // we have pushed up the throttle - // remove safety - motor_auto_armed = true; - } - - // fill the motor_out[] array for HIL use - for (unsigned char i = 0; i < 8; i++) { - motor_out[i] = g.rc_3.radio_min; - } - - // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); -} - -static void output_motor_test() -{ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - - - if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 100; - } - - if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 100; - } - - if(g.rc_2.control_in > 3000){ // back - motor_out[CH_4] += 100; - } - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); -} - -#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/motors_y6.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if FRAME_CONFIG == Y6_FRAME - -#define YAW_DIRECTION 1 - - -static void init_motors_out() -{ - #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 - ICR3 = 5000; // 50 hz output CH 7, 8, 11 - #endif -} - -static void output_motors_armed() -{ - int out_min = g.rc_3.radio_min; - int out_max = g.rc_3.radio_max; - - // Throttle is 0 to 1000 only - g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); - - if(g.rc_3.servo_out > 0) - out_min = g.rc_3.radio_min + MINIMUM_THROTTLE; - - g.rc_1.calc_pwm(); - g.rc_2.calc_pwm(); - g.rc_3.calc_pwm(); - g.rc_4.calc_pwm(); - - // Multi-Wii Mix - //left - motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW - motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW - //right - motor_out[CH_7] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW - motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW - //back - motor_out[CH_8] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW - motor_out[CH_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW - - //left - motor_out[CH_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW - motor_out[CH_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW - //right - motor_out[CH_7] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW - motor_out[CH_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW - //back - motor_out[CH_8] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW - motor_out[CH_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW - - - /* - int roll_out = (float)g.rc_1.pwm_out * .866; - int pitch_out = g.rc_2.pwm_out / 2; - - //left - motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP - motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW - - //right - motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP - motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW - - //back - motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW - - // Yaw - //top - motor_out[CH_2] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_8] += g.rc_4.pwm_out; // CCW - - //bottom - motor_out[CH_3] -= g.rc_4.pwm_out; // CW - motor_out[CH_1] -= g.rc_4.pwm_out; // CW - motor_out[CH_4] -= g.rc_4.pwm_out; // CW - */ - - // TODO: add stability patch - motor_out[CH_1] = min(motor_out[CH_1], out_max); - motor_out[CH_2] = min(motor_out[CH_2], out_max); - motor_out[CH_3] = min(motor_out[CH_3], out_max); - motor_out[CH_4] = min(motor_out[CH_4], out_max); - motor_out[CH_7] = min(motor_out[CH_7], out_max); - motor_out[CH_8] = min(motor_out[CH_8], out_max); - - // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); - - #if CUT_MOTORS == ENABLED - // if we are not sending a throttle output, we cut the motors - if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - } - #endif - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); - - #if INSTANT_PWM == 1 - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - APM_RC.Force_Out6_Out7(); - #endif -} - -static void output_motors_disarmed() -{ - if(g.rc_3.control_in > 0){ - // we have pushed up the throttle - // remove safety - motor_auto_armed = true; - } - - // fill the motor_out[] array for HIL use - for (unsigned char i = 0; i < 8; i++) { - motor_out[i] = g.rc_3.radio_min; - } - - // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); -} - -static void output_motor_test() -{ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - - - if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 100; - motor_out[CH_7] += 100; - } - - if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 100; - motor_out[CH_3] += 100; - } - - if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 100; - motor_out[CH_4] += 100; - } - - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_4]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); -} - -#endif -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/navigation.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -//**************************************************************** -// Function that will calculate the desired direction to fly and distance -//**************************************************************** -static byte navigate() -{ - if(next_WP.lat == 0){ - return 0; - } - - // waypoint distance from plane - // ---------------------------- - wp_distance = get_distance(¤t_loc, &next_WP); - - if (wp_distance < 0){ - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); - //Serial.println(wp_distance,DEC); - //print_current_waypoints(); - return 0; - } - - // target_bearing is where we should be heading - // -------------------------------------------- - target_bearing = get_bearing(¤t_loc, &next_WP); - return 1; -} - -static bool check_missed_wp() -{ - long temp = target_bearing - original_target_bearing; - temp = wrap_180(temp); - return (abs(temp) > 10000); //we pased the waypoint by 10 ° -} - -// ------------------------------ - -// long_error, lat_error -static void calc_location_error(struct Location *next_loc) -{ - /* - Becuase we are using lat and lon to do our distance errors here's a quick chart: - 100 = 1m - 1000 = 11m = 36 feet - 1800 = 19.80m = 60 feet - 3000 = 33m - 10000 = 111m - pitch_max = 22° (2200) - */ - - // X ROLL - long_error = (float)(next_loc->lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST - - // Y PITCH - lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH -} - -#define NAV_ERR_MAX 800 -static void calc_loiter(int x_error, int y_error) -{ - x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); - y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); - - int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); - int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); - - // find the rates: - float temp = radians((float)g_gps->ground_course/100.0); - - #ifdef OPTFLOW_ENABLED - // calc the cos of the error to tell how fast we are moving towards the target in cm - if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){ - x_actual_speed = optflow.vlon * 10; - y_actual_speed = optflow.vlat * 10; - }else{ - x_actual_speed = (float)g_gps->ground_speed * sin(temp); - y_actual_speed = (float)g_gps->ground_speed * cos(temp); - } - #else - x_actual_speed = (float)g_gps->ground_speed * sin(temp); - y_actual_speed = (float)g_gps->ground_speed * cos(temp); - #endif - - y_rate_error = y_target_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum - nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); - nav_lat = constrain(nav_lat, -3500, 3500); - - x_rate_error = x_target_speed - x_actual_speed; - x_rate_error = constrain(x_rate_error, -250, 250); - nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav); - nav_lon = constrain(nav_lon, -3500, 3500); -} - -static void calc_loiter2(int x_error, int y_error) -{ - static int last_x_error = 0; - static int last_y_error = 0; - - x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); - y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); - - int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); - int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); - - // find the rates: - x_actual_speed = (float)(last_x_error - x_error)/dTnav; - y_actual_speed = (float)(last_y_error - y_error)/dTnav; - - // save speeds - last_x_error = x_error; - last_y_error = y_error; - - y_rate_error = y_target_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum - nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); - nav_lat = constrain(nav_lat, -3500, 3500); - - x_rate_error = x_target_speed - x_actual_speed; - x_rate_error = constrain(x_rate_error, -250, 250); - nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav); - nav_lon = constrain(nav_lon, -3500, 3500); -} - -// nav_roll, nav_pitch -static void calc_loiter_pitch_roll() -{ - - //float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); - //float _cos_yaw_x = cos(temp); - //float _sin_yaw_y = sin(temp); - - //Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y); - - // rotate the vector - nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; - nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y; - - // flip pitch because forward is negative - nav_pitch = -nav_pitch; -} - -static void calc_nav_rate(int max_speed) -{ - /* - 0 1 2 3 4 5 6 7 8 - ...|...|...|...|...|...|...|...| - 100 200 300 400 - +|+ - */ - max_speed = min(max_speed, (wp_distance * 50)); - - // limit the ramp up of the speed - if(waypoint_speed_gov < max_speed){ - - waypoint_speed_gov += (int)(150.0 * dTnav); // increase at 1.5/ms - - // go at least 1m/s - max_speed = max(100, waypoint_speed_gov); - // limit with governer - max_speed = min(max_speed, waypoint_speed_gov); - } - - // XXX target_angle should be the original desired target angle! - float temp = radians((original_target_bearing - g_gps->ground_course)/100.0); - - x_actual_speed = -sin(temp) * (float)g_gps->ground_speed; - x_rate_error = -x_actual_speed; - x_rate_error = constrain(x_rate_error, -800, 800); - nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); - - y_actual_speed = cos(temp) * (float)g_gps->ground_speed; - y_rate_error = max_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -800, 800); // added a rate error limit to keep pitching down to a minimum - nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); - - /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", - max_speed, - x_actual_speed, - y_actual_speed, - x_rate_error, - y_rate_error, - nav_lon, - nav_lat);*/ -} - -// nav_roll, nav_pitch -static void calc_nav_pitch_roll() -{ - float temp = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0); - float _cos_yaw_x = cos(temp); - float _sin_yaw_y = sin(temp); - - // rotate the vector - nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x; - nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y; - - // flip pitch because forward is negative - nav_pitch = -nav_pitch; - - /*Serial.printf("_cos_yaw_x:%1.4f, _sin_yaw_y:%1.4f, nav_roll:%ld, nav_pitch:%ld\n", - _cos_yaw_x, - _sin_yaw_y, - nav_roll, - nav_pitch);*/ -} - -static long get_altitude_error() -{ - return next_WP.alt - current_loc.alt; -} - -static int get_loiter_angle() -{ - float power; - int angle; - - if(wp_distance <= g.loiter_radius){ - power = float(wp_distance) / float(g.loiter_radius); - power = constrain(power, 0.5, 1); - angle = 90.0 * (2.0 + power); - }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ - power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); - angle = power * 90; - } - - return angle; -} - -static long wrap_360(long error) -{ - if (error > 36000) error -= 36000; - if (error < 0) error += 36000; - return error; -} - -static long wrap_180(long error) -{ - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; -} - -/* -static long get_crosstrack_correction(void) -{ - // Crosstrack Error - // ---------------- - if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following - - // Meters we are off track line - float error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; - - // take meters * 100 to get adjustment to nav_bearing - long _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100; - - // constrain answer to 30° to avoid overshoot - return constrain(_crosstrack_correction, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); - } - return 0; -} -*/ -/* -static long cross_track_test() -{ - long temp = wrap_180(target_bearing - crosstrack_bearing); - return abs(temp); -} -*/ -/* -static void reset_crosstrack() -{ - crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following -} -*/ -/*static long get_altitude_above_home(void) -{ - // This is the altitude above the home location - // The GPS gives us altitude at Sea Level - // if you slope soar, you should see a negative number sometimes - // ------------------------------------------------------------- - return current_loc.alt - home.alt; -} -*/ -// distance is returned in meters -static long get_distance(struct Location *loc1, struct Location *loc2) -{ - //if(loc1->lat == 0 || loc1->lng == 0) - // return -1; - //if(loc2->lat == 0 || loc2->lng == 0) - // return -1; - float dlat = (float)(loc2->lat - loc1->lat); - float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; - return sqrt(sq(dlat) + sq(dlong)) * .01113195; -} -/* -static long get_alt_distance(struct Location *loc1, struct Location *loc2) -{ - return abs(loc1->alt - loc2->alt); -} -*/ -static long get_bearing(struct Location *loc1, struct Location *loc2) -{ - long off_x = loc2->lng - loc1->lng; - long off_y = (loc2->lat - loc1->lat) * scaleLongUp; - long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; - if (bearing < 0) bearing += 36000; - return bearing; -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/planner.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Common for implementation details -const struct Menu::command planner_menu_commands[] PROGMEM = { - {"gcs", planner_gcs}, -}; - -// A Macro to create the Menu -MENU(planner_menu, "planner", planner_menu_commands); - -static int8_t -planner_mode(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Planner Mode\nNot intended for manual use\n\n")); - planner_menu.run(); - return (0); -} - -static int8_t -planner_gcs(uint8_t argc, const Menu::arg *argv) -{ - gcs0.init(&Serial); - - int loopcount = 0; - - while (1) { - if (millis()-fast_loopTimer > 19) { - fast_loopTimer = millis(); - - gcs_update(); - - gcs_data_stream_send(45, 1000); - - if ((loopcount % 5) == 0) // 10 hz - gcs_data_stream_send(5, 45); - - if ((loopcount % 16) == 0) { // 3 hz - gcs_data_stream_send(1, 5); - gcs_send_message(MSG_HEARTBEAT); - } - - loopcount++; - } - } - return 0; -} - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/radio.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -//Function that will read the radio data, limit servos and trigger a failsafe -// ---------------------------------------------------------------------------- -static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling - -static void default_dead_zones() -{ - g.rc_1.set_dead_zone(60); - g.rc_2.set_dead_zone(60); - g.rc_3.set_dead_zone(60); - g.rc_4.set_dead_zone(200); -} - -static void init_rc_in() -{ - // set rc channel ranges - g.rc_1.set_angle(4500); - g.rc_2.set_angle(4500); - g.rc_3.set_range(0,1000); - #if FRAME_CONFIG != HELI_FRAME - g.rc_3.scale_output = .9; - #endif - g.rc_4.set_angle(4500); - - // reverse: CW = left - // normal: CW = left??? - - - g.rc_1.set_type(RC_CHANNEL_ANGLE_RAW); - g.rc_2.set_type(RC_CHANNEL_ANGLE_RAW); - g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); - - // set rc dead zones - /*g.rc_1.dead_zone = 60; - g.rc_2.dead_zone = 60; - g.rc_3.dead_zone = 60; - g.rc_4.dead_zone = 300; - */ - - - //set auxiliary ranges - g.rc_5.set_range(0,1000); - g.rc_6.set_range(0,1000); - g.rc_7.set_range(0,1000); - g.rc_8.set_range(0,1000); - -} - -static void init_rc_out() -{ - #if ARM_AT_STARTUP == 1 - motor_armed = 1; - #endif - - - APM_RC.Init(); // APM Radio initialization - init_motors_out(); - - // fix for crazy output - OCR1B = 0xFFFF; // PB6, OUT3 - OCR1C = 0xFFFF; // PB7, OUT4 - OCR5B = 0xFFFF; // PL4, OUT1 - OCR5C = 0xFFFF; // PL5, OUT2 - OCR4B = 0xFFFF; // PH4, OUT6 - OCR4C = 0xFFFF; // PH5, OUT5 - - // this is the camera pitch5 and roll6 - APM_RC.OutputCh(CH_5, 1500); - APM_RC.OutputCh(CH_6, 1500); - - // don't fuss if we are calibrating - if(g.esc_calibrate == 1) - return; - - if(g.rc_3.radio_min <= 1200){ - output_min(); - } - - for(byte i = 0; i < 5; i++){ - delay(20); - read_radio(); - } - - // sanity check - if(g.rc_3.radio_min >= 1300){ - g.rc_3.radio_min = g.rc_3.radio_in; - output_min(); - } -} - -void output_min() -{ - #if FRAME_CONFIG == HELI_FRAME - heli_move_servos_to_mid(); - #else - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - #endif - - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - - #if FRAME_CONFIG == OCTA_FRAME - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - #endif - -} -static void read_radio() -{ - if (APM_RC.GetState() == 1){ - new_radio_frame = true; - g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); - g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); - g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); - g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); - g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); - g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); - g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); - - #if FRAME_CONFIG != HELI_FRAME - // limit our input to 800 so we can still pitch and roll - g.rc_3.control_in = min(g.rc_3.control_in, 800); - #endif - - //throttle_failsafe(g.rc_3.radio_in); - } -} - -static void throttle_failsafe(uint16_t pwm) -{ - if(g.throttle_fs_enabled == 0) - return; - - //check for failsafe and debounce funky reads - // ------------------------------------------ - if (pwm < (unsigned)g.throttle_fs_value){ - // we detect a failsafe from radio - // throttle has dropped below the mark - failsafeCounter++; - if (failsafeCounter == 9){ - SendDebug("MSG FS ON "); - SendDebugln(pwm, DEC); - }else if(failsafeCounter == 10) { - ch3_failsafe = true; - //set_failsafe(true); - //failsafeCounter = 10; - }else if (failsafeCounter > 10){ - failsafeCounter = 11; - } - - }else if(failsafeCounter > 0){ - // we are no longer in failsafe condition - // but we need to recover quickly - failsafeCounter--; - if (failsafeCounter > 3){ - failsafeCounter = 3; - } - if (failsafeCounter == 1){ - SendDebug("MSG FS OFF "); - SendDebugln(pwm, DEC); - }else if(failsafeCounter == 0) { - ch3_failsafe = false; - //set_failsafe(false); - //failsafeCounter = -1; - }else if (failsafeCounter <0){ - failsafeCounter = -1; - } - } -} - -static void trim_radio() -{ - for (byte i = 0; i < 30; i++){ - read_radio(); - } - - g.rc_1.trim(); // roll - g.rc_2.trim(); // pitch - g.rc_4.trim(); // yaw - - g.rc_1.save_eeprom(); - g.rc_2.save_eeprom(); - g.rc_4.save_eeprom(); -} - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/sensors.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// Sensors are not available in HIL_MODE_ATTITUDE -#if HIL_MODE != HIL_MODE_ATTITUDE - -static void ReadSCP1000(void) {} - -static void init_barometer(void) -{ - #if HIL_MODE == HIL_MODE_SENSORS - gcs_update(); // look for inbound hil packets for initialization - #endif - - ground_temperature = barometer.Temp; - int i; - - // We take some readings... - for(i = 0; i < 60; i++){ - delay(20); - - // get new data from absolute pressure sensor - barometer.Read(); - - //Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press); - } - - for(i = 0; i < 20; i++){ - delay(20); - - #if HIL_MODE == HIL_MODE_SENSORS - gcs_update(); // look for inbound hil packets - #endif - - // Get initial data from absolute pressure sensor - barometer.Read(); - ground_pressure = barometer.Press; - ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; - //Serial.printf("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press, ground_temperature, ground_pressure); - } - - abs_pressure = ground_pressure; - - //Serial.printf("init %ld\n", abs_pressure); - //SendDebugln("barometer calibration complete."); -} - -/* -static long read_baro_filtered(void) -{ - - // get new data from absolute pressure sensor - barometer.Read(); - - return barometer.Press; - - long pressure = 0; - // add new data into our filter - baro_filter[baro_filter_index] = barometer.Press; - baro_filter_index++; - - // loop our filter - if(baro_filter_index >= BARO_FILTER_SIZE) - baro_filter_index = 0; - - // zero out our accumulator - - // sum our filter - for(byte i = 0; i < BARO_FILTER_SIZE; i++){ - pressure += baro_filter[i]; - } - - - // average our sampels - return pressure /= BARO_FILTER_SIZE; - // -} -*/ -static long read_barometer(void) -{ - float x, scaling, temp; - - barometer.Read(); - abs_pressure = barometer.Press; - - - //Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure); - - scaling = (float)ground_pressure / (float)abs_pressure; - temp = ((float)ground_temperature / 10.0f) + 273.15f; - x = log(scaling) * temp * 29271.267f; - return (x / 10); -} - -// in M/S * 100 -static void read_airspeed(void) -{ - -} - -static void zero_airspeed(void) -{ - -} - -#endif // HIL_MODE != HIL_MODE_ATTITUDE - -static void read_battery(void) -{ - battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; - battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; - battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9; - battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9; - - if(g.battery_monitoring == 1) - battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream - - if(g.battery_monitoring == 2) - battery_voltage = battery_voltage4; - - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) - battery_voltage = battery_voltage1; - - if(g.battery_monitoring == 4) { - current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin - current_total += current_amps * 0.0278; // called at 100ms on average - } - - #if BATTERY_EVENT == 1 - //if(battery_voltage < g.low_voltage) - // low_battery_event(); - - if((battery_voltage < g.low_voltage) || (g.battery_monitoring == 4 && current_total > g.pack_capacity)){ - low_battery_event(); - - #if PIEZO_LOW_VOLTAGE == 1 - // Only Activate if a battery is connected to avoid alarm on USB only - if (battery_voltage1 > 1){ - piezo_on(); - }else{ - piezo_off(); - } - #endif - - }else{ - #if PIEZO_LOW_VOLTAGE == 1 - piezo_off(); - #endif - } - #endif -} - -//v: 10.9453, a: 17.4023, mah: 8.2 -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/setup.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CLI_ENABLED == ENABLED - -// Functions called from the setup menu -static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); -static int8_t setup_motors (uint8_t argc, const Menu::arg *argv); -static int8_t setup_accel (uint8_t argc, const Menu::arg *argv); -static int8_t setup_frame (uint8_t argc, const Menu::arg *argv); -static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); -static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); -static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); -static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); -static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); -static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); -static int8_t setup_tune (uint8_t argc, const Menu::arg *argv); -//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); -static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); -static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); -#ifdef OPTFLOW_ENABLED -static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv); -#endif -static int8_t setup_show (uint8_t argc, const Menu::arg *argv); - -#if FRAME_CONFIG == HELI_FRAME - static int8_t setup_heli (uint8_t argc, const Menu::arg *argv); - static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv); -#endif - -// Command/function table for the setup menu -const struct Menu::command setup_menu_commands[] PROGMEM = { - // command function called - // ======= =============== - {"erase", setup_erase}, - {"reset", setup_factory}, - {"radio", setup_radio}, - {"frame", setup_frame}, - {"motors", setup_motors}, - {"esc", setup_esc}, - {"level", setup_accel}, - {"modes", setup_flightmodes}, - {"battery", setup_batt_monitor}, - {"sonar", setup_sonar}, - {"compass", setup_compass}, - {"tune", setup_tune}, -// {"offsets", setup_mag_offset}, - {"declination", setup_declination}, -#ifdef OPTFLOW_ENABLED - {"optflow", setup_optflow}, -#endif -#if FRAME_CONFIG == HELI_FRAME - {"heli", setup_heli}, - {"gyro", setup_gyro}, -#endif - {"show", setup_show} -}; - -// Create the setup menu object. -MENU(setup_menu, "setup", setup_menu_commands); - -// Called from the top-level menu to run the setup menu. -static int8_t -setup_mode(uint8_t argc, const Menu::arg *argv) -{ - // Give the user some guidance - Serial.printf_P(PSTR("Setup Mode\n\n\n")); - //"\n" - //"IMPORTANT: if you have not previously set this system up, use the\n" - //"'reset' command to initialize the EEPROM to sensible default values\n" - //"and then the 'radio' command to configure for your radio.\n" - //"\n")); - - if(g.rc_1.radio_min >= 1300){ - delay(1000); - Serial.printf_P(PSTR("\n!Warning, your radio is not configured!")); - delay(1000); - Serial.printf_P(PSTR("\n Type 'radio' to configure now.\n\n")); - } - - // Run the setup menu. When the menu exits, we will return to the main menu. - setup_menu.run(); - return 0; -} - -// Print the current configuration. -// Called by the setup menu 'show' command. -static int8_t -setup_show(uint8_t argc, const Menu::arg *argv) -{ - // clear the area - print_blanks(8); - - report_version(); - report_radio(); - report_frame(); - report_batt_monitor(); - report_sonar(); - //report_gains(); - //report_xtrack(); - //report_throttle(); - report_flight_modes(); - report_imu(); - report_compass(); - -#ifdef OPTFLOW_ENABLED - report_optflow(); -#endif - -#if FRAME_CONFIG == HELI_FRAME - report_heli(); - report_gyro(); -#endif - - AP_Var_menu_show(argc, argv); - return(0); -} - -// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). -// Called by the setup menu 'factoryreset' command. -static int8_t -setup_factory(uint8_t argc, const Menu::arg *argv) -{ - int c; - - Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n")); - - do { - c = Serial.read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) - return(-1); - - AP_Var::erase_all(); - Serial.printf_P(PSTR("\nReboot APM")); - - delay(1000); - //default_gains(); - - for (;;) { - } - // note, cannot actually return here - return(0); -} - -// Perform radio setup. -// Called by the setup menu 'radio' command. -static int8_t -setup_radio(uint8_t argc, const Menu::arg *argv) -{ - Serial.println("\n\nRadio Setup:"); - uint8_t i; - - for(i = 0; i < 100;i++){ - delay(20); - read_radio(); - } - - if(g.rc_1.radio_in < 500){ - while(1){ - //Serial.printf_P(PSTR("\nNo radio; Check connectors.")); - delay(1000); - // stop here - } - } - - g.rc_1.radio_min = g.rc_1.radio_in; - g.rc_2.radio_min = g.rc_2.radio_in; - g.rc_3.radio_min = g.rc_3.radio_in; - g.rc_4.radio_min = g.rc_4.radio_in; - g.rc_5.radio_min = g.rc_5.radio_in; - g.rc_6.radio_min = g.rc_6.radio_in; - g.rc_7.radio_min = g.rc_7.radio_in; - g.rc_8.radio_min = g.rc_8.radio_in; - - g.rc_1.radio_max = g.rc_1.radio_in; - g.rc_2.radio_max = g.rc_2.radio_in; - g.rc_3.radio_max = g.rc_3.radio_in; - g.rc_4.radio_max = g.rc_4.radio_in; - g.rc_5.radio_max = g.rc_5.radio_in; - g.rc_6.radio_max = g.rc_6.radio_in; - g.rc_7.radio_max = g.rc_7.radio_in; - g.rc_8.radio_max = g.rc_8.radio_in; - - g.rc_1.radio_trim = g.rc_1.radio_in; - g.rc_2.radio_trim = g.rc_2.radio_in; - g.rc_4.radio_trim = g.rc_4.radio_in; - // 3 is not trimed - g.rc_5.radio_trim = 1500; - g.rc_6.radio_trim = 1500; - g.rc_7.radio_trim = 1500; - g.rc_8.radio_trim = 1500; - - - Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: ")); - while(1){ - - delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - g.rc_1.update_min_max(); - g.rc_2.update_min_max(); - g.rc_3.update_min_max(); - g.rc_4.update_min_max(); - g.rc_5.update_min_max(); - g.rc_6.update_min_max(); - g.rc_7.update_min_max(); - g.rc_8.update_min_max(); - - if(Serial.available() > 0){ - delay(20); - Serial.flush(); - - g.rc_1.save_eeprom(); - g.rc_2.save_eeprom(); - g.rc_3.save_eeprom(); - g.rc_4.save_eeprom(); - g.rc_5.save_eeprom(); - g.rc_6.save_eeprom(); - g.rc_7.save_eeprom(); - g.rc_8.save_eeprom(); - - print_done(); - break; - } - } - report_radio(); - return(0); -} - -static int8_t -setup_esc(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("\nESC Calibration:\n" - "-1 Unplug USB and battery\n" - "-2 Move CLI/FLY switch to FLY mode\n" - "-3 Move throttle to max, connect battery\n" - "-4 After two long beeps, throttle to 0, then test\n\n" - " Press Enter to cancel.\n")); - - - g.esc_calibrate.set_and_save(1); - - while(1){ - delay(20); - - if(Serial.available() > 0){ - g.esc_calibrate.set_and_save(0); - return(0); - } - } -} - -static int8_t -setup_motors(uint8_t argc, const Menu::arg *argv) -{ - while(1){ - delay(20); - read_radio(); - output_motor_test(); - if(Serial.available() > 0){ - g.esc_calibrate.set_and_save(0); - return(0); - } - } -} - -static int8_t -setup_accel(uint8_t argc, const Menu::arg *argv) -{ - imu.init_accel(); - print_accel_offsets(); - report_imu(); - return(0); -} - -static int8_t -setup_frame(uint8_t argc, const Menu::arg *argv) -{ - if (!strcmp_P(argv[1].str, PSTR("x"))) { - g.frame_orientation.set_and_save(X_FRAME); - } else if (!strcmp_P(argv[1].str, PSTR("p"))) { - g.frame_orientation.set_and_save(PLUS_FRAME); - } else if (!strcmp_P(argv[1].str, PSTR("+"))) { - g.frame_orientation.set_and_save(PLUS_FRAME); - } else if (!strcmp_P(argv[1].str, PSTR("v"))) { - g.frame_orientation.set_and_save(V_FRAME); - }else{ - Serial.printf_P(PSTR("\nOptions:[x,+,v]\n")); - report_frame(); - return 0; - } - - report_frame(); - return 0; -} - -static int8_t -setup_flightmodes(uint8_t argc, const Menu::arg *argv) -{ - byte _switchPosition = 0; - byte _oldSwitchPosition = 0; - byte mode = 0; - - Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n")); - print_hit_enter(); - - while(1){ - delay(20); - read_radio(); - _switchPosition = readSwitch(); - - - // look for control switch change - if (_oldSwitchPosition != _switchPosition){ - - mode = flight_modes[_switchPosition]; - mode = constrain(mode, 0, NUM_MODES-1); - - // update the user - print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); - - // Remember switch position - _oldSwitchPosition = _switchPosition; - } - - // look for stick input - if (abs(g.rc_1.control_in) > 3000){ - mode++; - if(mode >= NUM_MODES) - mode = 0; - - // save new mode - flight_modes[_switchPosition] = mode; - - // print new mode - print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); - delay(500); - } - - // look for stick input - if (g.rc_4.control_in > 3000){ - g.simple_modes |= (1<<_switchPosition); - // print new mode - print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); - delay(500); - } - - // look for stick input - if (g.rc_4.control_in < -3000){ - g.simple_modes &= ~(1<<_switchPosition); - // print new mode - print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); - delay(500); - } - - // escape hatch - if(Serial.available() > 0){ - for (mode = 0; mode < 6; mode++) - flight_modes[mode].save(); - - g.simple_modes.save(); - print_done(); - report_flight_modes(); - return (0); - } - } -} - -static int8_t -setup_declination(uint8_t argc, const Menu::arg *argv) -{ - compass.set_declination(radians(argv[1].f)); - report_compass(); - return 0; -} - -static int8_t -setup_tune(uint8_t argc, const Menu::arg *argv) -{ - g.radio_tuning.set_and_save(argv[1].i); - report_tuning(); - return 0; -} - - - -static int8_t -setup_erase(uint8_t argc, const Menu::arg *argv) -{ - zero_eeprom(); - return 0; -} - -static int8_t -setup_compass(uint8_t argc, const Menu::arg *argv) -{ - if (!strcmp_P(argv[1].str, PSTR("on"))) { - g.compass_enabled.set_and_save(true); - init_compass(); - - } else if (!strcmp_P(argv[1].str, PSTR("off"))) { - clear_offsets(); - g.compass_enabled.set_and_save(false); - - }else{ - Serial.printf_P(PSTR("\nOptions:[on,off]\n")); - report_compass(); - return 0; - } - - g.compass_enabled.save(); - report_compass(); - return 0; -} - -static int8_t -setup_batt_monitor(uint8_t argc, const Menu::arg *argv) -{ - if (!strcmp_P(argv[1].str, PSTR("off"))) { - g.battery_monitoring.set_and_save(0); - - } else if(argv[1].i > 0 && argv[1].i <= 4){ - g.battery_monitoring.set_and_save(argv[1].i); - - } else { - Serial.printf_P(PSTR("\nOptions: off, 1-4")); - } - - report_batt_monitor(); - return 0; -} - -static int8_t -setup_sonar(uint8_t argc, const Menu::arg *argv) -{ - if (!strcmp_P(argv[1].str, PSTR("on"))) { - g.sonar_enabled.set_and_save(true); - - } else if (!strcmp_P(argv[1].str, PSTR("off"))) { - g.sonar_enabled.set_and_save(false); - - }else{ - Serial.printf_P(PSTR("\nOptions:[on, off]\n")); - report_sonar(); - return 0; - } - - report_sonar(); - return 0; -} - -#if FRAME_CONFIG == HELI_FRAME - -// Perform heli setup. -// Called by the setup menu 'radio' command. -static int8_t -setup_heli(uint8_t argc, const Menu::arg *argv) -{ - - uint8_t active_servo = 0; - int value = 0; - int temp; - int state = 0; // 0 = set rev+pos, 1 = capture min/max - int max_roll, max_pitch, min_coll, max_coll, min_tail, max_tail; - - // initialise swash plate - heli_init_swash(); - - // source swash plate movements directly from - heli_manual_override = true; - - // display initial settings - report_heli(); - - // display help - Serial.printf_P(PSTR("Instructions:")); - print_divider(); - Serial.printf_P(PSTR("\td\t\tdisplay settings\n")); - Serial.printf_P(PSTR("\t1~4\t\tselect servo\n")); - Serial.printf_P(PSTR("\ta or z\t\tmove mid up/down\n")); - Serial.printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n")); - Serial.printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n")); - Serial.printf_P(PSTR("\tp\tset pos (i.e. p0 = front, p90 = right)\n")); - Serial.printf_P(PSTR("\tr\t\treverse servo\n")); - Serial.printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n")); - Serial.printf_P(PSTR("\tt\tset trim (-500 ~ 500)\n")); - Serial.printf_P(PSTR("\tx\t\texit & save\n")); - - // start capturing - while( value != 'x' ) { - - // read radio although we don't use it yet - read_radio(); - - // record min/max - if( state == 1 ) { - if( abs(g.rc_1.control_in) > max_roll ) - max_roll = abs(g.rc_1.control_in); - if( abs(g.rc_2.control_in) > max_pitch ) - max_pitch = abs(g.rc_2.control_in); - if( g.rc_3.radio_in < min_coll ) - min_coll = g.rc_3.radio_in; - if( g.rc_3.radio_in > max_coll ) - max_coll = g.rc_3.radio_in; - min_tail = min(g.rc_4.radio_in, min_tail); - max_tail = max(g.rc_4.radio_in, max_tail); - //Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out); - } - - if( Serial.available() ) { - value = Serial.read(); - - // process the user's input - switch( value ) { - case '1': - active_servo = CH_1; - break; - case '2': - active_servo = CH_2; - break; - case '3': - active_servo = CH_3; - break; - case '4': - active_servo = CH_4; - break; - case 'a': - case 'A': - heli_get_servo(active_servo)->radio_trim += 10; - break; - case 'c': - case 'C': - if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) { - g.heli_coll_mid = g.rc_3.radio_in; - Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid); - } - break; - case 'd': - case 'D': - // display settings - report_heli(); - break; - case 'm': - case 'M': - if( state == 0 ) { - state = 1; // switch to capture min/max mode - Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n")); - - // reset servo ranges - g.heli_roll_max = g.heli_pitch_max = 4500; - g.heli_coll_min = 1000; - g.heli_coll_max = 2000; - g.heli_servo_4.radio_min = 1000; - g.heli_servo_4.radio_max = 2000; - - // set sensible values in temp variables - max_roll = abs(g.rc_1.control_in); - max_pitch = abs(g.rc_2.control_in); - min_coll = 2000; - max_coll = 1000; - min_tail = max_tail = abs(g.rc_4.radio_in); - }else{ - state = 0; // switch back to normal mode - // double check values aren't totally terrible - if( max_roll <= 1000 || max_pitch <= 1000 || (max_coll - min_coll < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 ) - Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_coll,max_coll,min_tail,max_tail); - else{ - g.heli_roll_max = max_roll; - g.heli_pitch_max = max_pitch; - g.heli_coll_min = min_coll; - g.heli_coll_max = max_coll; - g.heli_servo_4.radio_min = min_tail; - g.heli_servo_4.radio_max = max_tail; - - // reinitialise swash - heli_init_swash(); - - // display settings - report_heli(); - } - } - break; - case 'p': - case 'P': - temp = read_num_from_serial(); - if( temp >= -360 && temp <= 360 ) { - if( active_servo == CH_1 ) - g.heli_servo1_pos = temp; - if( active_servo == CH_2 ) - g.heli_servo2_pos = temp; - if( active_servo == CH_3 ) - g.heli_servo3_pos = temp; - heli_init_swash(); - Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp); - } - break; - case 'r': - case 'R': - heli_get_servo(active_servo)->set_reverse(!heli_get_servo(active_servo)->get_reverse()); - break; - case 't': - case 'T': - temp = read_num_from_serial(); - if( temp > 1000 ) - temp -= 1500; - if( temp > -500 && temp < 500 ) { - heli_get_servo(active_servo)->radio_trim = 1500 + temp; - heli_init_swash(); - Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); - } - break; - case 'u': - case 'U': - temp = 0; - // delay up to 2 seconds for servo type from user - while( !Serial.available() && temp < 20 ) { - temp++; - delay(100); - } - if( Serial.available() ) { - value = Serial.read(); - if( value == 'a' || value == 'A' ) { - g.heli_servo_averaging = HELI_SERVO_AVERAGING_ANALOG; - Serial.printf_P(PSTR("Analog Servo %dhz\n"),250 / HELI_SERVO_AVERAGING_ANALOG); - } - if( value == 'd' || value == 'D' ) { - g.heli_servo_averaging = HELI_SERVO_AVERAGING_DIGITAL; - Serial.printf_P(PSTR("Digital Servo 250hz\n")); - } - } - break; - case 'z': - case 'Z': - heli_get_servo(active_servo)->radio_trim -= 10; - break; - } - } - - // allow swash plate to move - output_motors_armed(); - - delay(20); - } - - // display final settings - report_heli(); - - // save to eeprom - g.heli_servo_1.save_eeprom(); - g.heli_servo_2.save_eeprom(); - g.heli_servo_3.save_eeprom(); - g.heli_servo_4.save_eeprom(); - g.heli_servo1_pos.save(); - g.heli_servo2_pos.save(); - g.heli_servo3_pos.save(); - g.heli_roll_max.save(); - g.heli_pitch_max.save(); - g.heli_coll_min.save(); - g.heli_coll_max.save(); - g.heli_coll_mid.save(); - g.heli_servo_averaging.save(); - - // return swash plate movements to attitude controller - heli_manual_override = false; - - return(0); -} - -// setup for external tail gyro (for heli only) -static int8_t -setup_gyro(uint8_t argc, const Menu::arg *argv) -{ - if (!strcmp_P(argv[1].str, PSTR("on"))) { - g.heli_ext_gyro_enabled.set_and_save(true); - - // optionally capture the gain - if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) { - g.heli_ext_gyro_gain = argv[2].i; - g.heli_ext_gyro_gain.save(); - } - - } else if (!strcmp_P(argv[1].str, PSTR("off"))) { - g.heli_ext_gyro_enabled.set_and_save(false); - - // capture gain if user simply provides a number - } else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) { - g.heli_ext_gyro_enabled.set_and_save(true); - g.heli_ext_gyro_gain = argv[1].i; - g.heli_ext_gyro_gain.save(); - - }else{ - Serial.printf_P(PSTR("\nOptions:[on, off] gain\n")); - } - - report_gyro(); - return 0; -} - -#endif // FRAME_CONFIG == HELI - -static void clear_offsets() -{ - Vector3f _offsets(0.0,0.0,0.0); - compass.set_offsets(_offsets); - compass.save_offsets(); -} - -/*static int8_t -setup_mag_offset(uint8_t argc, const Menu::arg *argv) -{ - Vector3f _offsets; - - if (!strcmp_P(argv[1].str, PSTR("c"))) { - clear_offsets(); - report_compass(); - return (0); - } - - print_hit_enter(); - init_compass(); - - int _min[3] = {0,0,0}; - int _max[3] = {0,0,0}; - - compass.read(); - compass.calculate(0,0); // roll = 0, pitch = 0 - - while(1){ - delay(50); - - compass.read(); - compass.calculate(0,0); // roll = 0, pitch = 0 - - if(compass.mag_x < _min[0]) _min[0] = compass.mag_x; - if(compass.mag_y < _min[1]) _min[1] = compass.mag_y; - if(compass.mag_z < _min[2]) _min[2] = compass.mag_z; - - // capture max - if(compass.mag_x > _max[0]) _max[0] = compass.mag_x; - if(compass.mag_y > _max[1]) _max[1] = compass.mag_y; - if(compass.mag_z > _max[2]) _max[2] = compass.mag_z; - - // calculate offsets - _offsets.x = (float)(_max[0] + _min[0]) / -2; - _offsets.y = (float)(_max[1] + _min[1]) / -2; - _offsets.z = (float)(_max[2] + _min[2]) / -2; - - // display all to user - Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"), - - (uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100, - - compass.mag_x, - compass.mag_y, - compass.mag_z, - - _offsets.x, - _offsets.y, - _offsets.z); - - if(Serial.available() > 1){ - compass.set_offsets(_offsets); - //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); - report_compass(); - return 0; - } - } - return 0; -} -*/ - -#ifdef OPTFLOW_ENABLED -static int8_t -setup_optflow(uint8_t argc, const Menu::arg *argv) -{ - if (!strcmp_P(argv[1].str, PSTR("on"))) { - g.optflow_enabled = true; - init_optflow(); - - } else if (!strcmp_P(argv[1].str, PSTR("off"))) { - g.optflow_enabled = false; - - }else{ - Serial.printf_P(PSTR("\nOptions:[on, off]\n")); - report_optflow(); - return 0; - } - - g.optflow_enabled.save(); - report_optflow(); - return 0; -} -#endif - - -/***************************************************************************/ -// CLI reports -/***************************************************************************/ - -static void report_batt_monitor() -{ - Serial.printf_P(PSTR("\nBatt Mointor\n")); - print_divider(); - if(g.battery_monitoring == 0) print_enabled(false); - if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3 cells")); - if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4 cells")); - if(g.battery_monitoring == 3) Serial.printf_P(PSTR("batt volts")); - if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur")); - print_blanks(2); -} - -static void report_wp(byte index = 255) -{ - if(index == 255){ - for(byte i = 0; i <= g.waypoint_total; i++){ - struct Location temp = get_command_with_index(i); - print_wp(&temp, i); - } - }else{ - struct Location temp = get_command_with_index(index); - print_wp(&temp, index); - } -} - -static void report_sonar() -{ - g.sonar_enabled.load(); - Serial.printf_P(PSTR("Sonar\n")); - print_divider(); - print_enabled(g.sonar_enabled.get()); - print_blanks(2); -} - -static void report_frame() -{ - Serial.printf_P(PSTR("Frame\n")); - print_divider(); - -#if FRAME_CONFIG == QUAD_FRAME - Serial.printf_P(PSTR("Quad frame\n")); -#elif FRAME_CONFIG == TRI_FRAME - Serial.printf_P(PSTR("TRI frame\n")); -#elif FRAME_CONFIG == HEXA_FRAME - Serial.printf_P(PSTR("Hexa frame\n")); -#elif FRAME_CONFIG == Y6_FRAME - Serial.printf_P(PSTR("Y6 frame\n")); -#elif FRAME_CONFIG == OCTA_FRAME - Serial.printf_P(PSTR("Octa frame\n")); -#elif FRAME_CONFIG == HELI_FRAME - Serial.printf_P(PSTR("Heli frame\n")); -#endif - -#if FRAME_CONFIG != HELI_FRAME - if(g.frame_orientation == X_FRAME) - Serial.printf_P(PSTR("X mode\n")); - else if(g.frame_orientation == PLUS_FRAME) - Serial.printf_P(PSTR("+ mode\n")); - else if(g.frame_orientation == V_FRAME) - Serial.printf_P(PSTR("V mode\n")); -#endif - - print_blanks(2); -} - -static void report_radio() -{ - Serial.printf_P(PSTR("Radio\n")); - print_divider(); - // radio - print_radio_values(); - print_blanks(2); -} - -static void report_imu() -{ - Serial.printf_P(PSTR("IMU\n")); - print_divider(); - - print_gyro_offsets(); - print_accel_offsets(); - print_blanks(2); -} - -static void report_compass() -{ - Serial.printf_P(PSTR("Compass\n")); - print_divider(); - - print_enabled(g.compass_enabled); - - // mag declination - Serial.printf_P(PSTR("Mag Dec: %4.4f\n"), - degrees(compass.get_declination())); - - Vector3f offsets = compass.get_offsets(); - - // mag offsets - Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"), - offsets.x, - offsets.y, - offsets.z); - print_blanks(2); -} - -static void report_flight_modes() -{ - Serial.printf_P(PSTR("Flight modes\n")); - print_divider(); - - for(int i = 0; i < 6; i++ ){ - print_switch(i, flight_modes[i], (g.simple_modes & (1<kP(), - pid->kI(), - (long)pid->imax()); -} -*/ - -static void -print_radio_values() -{ - Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); - Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max); - Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max); - Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max); - Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); - Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); - Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); - //Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); -} - -static void -print_switch(byte p, byte m, bool b) -{ - Serial.printf_P(PSTR("Pos %d:\t"),p); - Serial.print(flight_mode_strings[m]); - Serial.printf_P(PSTR(",\t\tSimple: ")); - if(b) - Serial.printf_P(PSTR("ON\n")); - else - Serial.printf_P(PSTR("OFF\n")); -} - -static void -print_done() -{ - Serial.printf_P(PSTR("\nSaved Settings\n\n")); -} - - -static void zero_eeprom(void) -{ - byte b = 0; - - Serial.printf_P(PSTR("\nErasing EEPROM\n")); - - for (int i = 0; i < EEPROM_MAX_ADDR; i++) { - eeprom_write_byte((uint8_t *) i, b); - } - - Serial.printf_P(PSTR("done\n")); -} - -static void -print_accel_offsets(void) -{ - Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), - (float)imu.ax(), - (float)imu.ay(), - (float)imu.az()); -} - -static void -print_gyro_offsets(void) -{ - Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), - (float)imu.gx(), - (float)imu.gy(), - (float)imu.gz()); -} - -#if FRAME_CONFIG == HELI_FRAME - -static RC_Channel * -heli_get_servo(int servo_num){ - if( servo_num == CH_1 ) - return &g.heli_servo_1; - if( servo_num == CH_2 ) - return &g.heli_servo_2; - if( servo_num == CH_3 ) - return &g.heli_servo_3; - if( servo_num == CH_4 ) - return &g.heli_servo_4; - return NULL; -} - -// Used to read integer values from the serial port -static int read_num_from_serial() { - byte index = 0; - byte timeout = 0; - char data[5] = ""; - - do { - if (Serial.available() == 0) { - delay(10); - timeout++; - }else{ - data[index] = Serial.read(); - timeout = 0; - index++; - } - }while (timeout < 5 && index < 5); - - return atoi(data); -} -#endif - -#endif // CLI_ENABLED - -static void -print_blanks(int num) -{ - while(num > 0){ - num--; - Serial.println(""); - } -} - -static void -print_divider(void) -{ - for (int i = 0; i < 40; i++) { - Serial.print("-"); - } - Serial.println(""); -} - -static void print_enabled(boolean b) -{ - if(b) - Serial.printf_P(PSTR("en")); - else - Serial.printf_P(PSTR("dis")); - Serial.printf_P(PSTR("abled\n")); -} - - -static void -init_esc() -{ - g.esc_calibrate.set_and_save(0); - while(1){ - read_radio(); - delay(100); - dancing_light(); - APM_RC.OutputCh(CH_1, g.rc_3.radio_in); - APM_RC.OutputCh(CH_2, g.rc_3.radio_in); - APM_RC.OutputCh(CH_3, g.rc_3.radio_in); - APM_RC.OutputCh(CH_4, g.rc_3.radio_in); - APM_RC.OutputCh(CH_7, g.rc_3.radio_in); - APM_RC.OutputCh(CH_8, g.rc_3.radio_in); - - #if FRAME_CONFIG == OCTA_FRAME - APM_RC.OutputCh(CH_10, g.rc_3.radio_in); - APM_RC.OutputCh(CH_11, g.rc_3.radio_in); - #endif - - } -} - -static void print_wp(struct Location *cmd, byte index) -{ - Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), - (int)index, - (int)cmd->id, - (int)cmd->options, - (int)cmd->p1, - cmd->alt, - cmd->lat, - cmd->lng); -} - -static void report_gps() -{ - Serial.printf_P(PSTR("\nGPS\n")); - print_divider(); - print_enabled(GPS_enabled); - print_blanks(2); -} - -static void report_version() -{ - Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get()); - print_divider(); - print_blanks(2); -} - - -static void report_tuning() -{ - Serial.printf_P(PSTR("\nTUNE:\n")); - print_divider(); - if (g.radio_tuning == 0){ - print_enabled(g.radio_tuning.get()); - }else{ - Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get()); - } - print_blanks(2); -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/system.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/***************************************************************************** -The init_ardupilot function processes everything we need for an in - air restart - We will determine later if we are actually on the ground and process a - ground start in that case. - -*****************************************************************************/ - -#if CLI_ENABLED == ENABLED -// Functions called from the top-level menu -static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp -static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Commands:\n" - " logs\n" - " setup\n" - " test\n" - " planner\n" - "\n" - "Move the slide switch and reset to FLY.\n" - "\n")); - return(0); -} - -// Command/function table for the top-level menu. -const struct Menu::command main_menu_commands[] PROGMEM = { -// command function called -// ======= =============== - {"logs", process_logs}, - {"setup", setup_mode}, - {"test", test_mode}, - {"help", main_menu_help}, - {"planner", planner_mode} -}; - -// Create the top-level menu object. -MENU(main_menu, THISFIRMWARE, main_menu_commands); - -// the user wants the CLI. It never exits -static void run_cli(void) -{ - while (1) { - main_menu.run(); - } -} - -#endif // CLI_ENABLED - -static void init_ardupilot() -{ - // Console serial port - // - // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. - // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - Serial.begin(SERIAL0_BAUD, 128, 128); - - // GPS serial port. - // - // Not used if the IMU/X-Plane GPS is in use. - // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // - #if GPS_PROTOCOL != GPS_PROTOCOL_IMU - Serial1.begin(38400, 128, 16); - #endif - - Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE - "\n\nFree RAM: %u\n"), - memcheck_available_memory()); - - - // - // Check the EEPROM format version before loading any parameters from EEPROM. - // - report_version(); - - // setup IO pins - pinMode(C_LED_PIN, OUTPUT); // GPS status LED - pinMode(A_LED_PIN, OUTPUT); // GPS status LED - pinMode(B_LED_PIN, OUTPUT); // GPS status LED - pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode - pinMode(PUSHBUTTON_PIN, INPUT); // unused - DDRL |= B00000100; // Set Port L, pin 2 to output for the relay - - // XXX set Analog out 14 to output - // 76543210 - //DDRK |= B01010000; - - #if MOTOR_LEDS == 1 - pinMode(FR_LED, OUTPUT); // GPS status LED - pinMode(RE_LED, OUTPUT); // GPS status LED - pinMode(RI_LED, OUTPUT); // GPS status LED - pinMode(LE_LED, OUTPUT); // GPS status LED - #endif - - #if PIEZO == 1 - pinMode(PIEZO_PIN,OUTPUT); - piezo_beep(); - #endif - - - if (!g.format_version.load() || - g.format_version != Parameters::k_format_version) { - //Serial.printf_P(PSTR("\n\nForcing complete parameter reset...")); - - /*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" - "\n\nForcing complete parameter reset..."), - g.format_version.get(), - Parameters::k_format_version); - */ - - // erase all parameters - Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n")); - delay(100); // wait for serial send - AP_Var::erase_all(); - - // save the new format version - g.format_version.set_and_save(Parameters::k_format_version); - - // save default radio values - default_dead_zones(); - - Serial.printf_P(PSTR("Please Run Setup...\n")); - while (true) { - delay(1000); - if(motor_light){ - digitalWrite(A_LED_PIN, HIGH); - digitalWrite(B_LED_PIN, HIGH); - digitalWrite(C_LED_PIN, HIGH); - }else{ - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, LOW); - digitalWrite(C_LED_PIN, LOW); - } - motor_light = !motor_light; - } - - }else{ - // save default radio values - //default_dead_zones(); - - // Load all auto-loaded EEPROM variables - AP_Var::load_all(); - } - - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // - Serial3.begin(map_baudrate(g.serial3_baud,SERIAL3_BAUD), 128, 128); - - #ifdef RADIO_OVERRIDE_DEFAULTS - { - int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; - APM_RC.setHIL(rc_override); - } - #endif - - #if FRAME_CONFIG == HELI_FRAME - heli_init_swash(); // heli initialisation - #endif - - init_rc_in(); // sets up rc channels from radio - init_rc_out(); // sets up the timer libs - init_camera(); - - #if HIL_MODE != HIL_MODE_ATTITUDE - // begin filtering the ADC Gyros - adc.filter_result = true; - - adc.Init(); // APM ADC library initialization - barometer.Init(); // APM Abs Pressure sensor initialization - #endif - - // Do GPS init - g_gps = &g_gps_driver; - g_gps->init(); // GPS Initialization - g_gps->callback = mavlink_delay; - - // init the GCS - gcs0.init(&Serial); - gcs3.init(&Serial3); - - if(g.compass_enabled) - init_compass(); - - #ifdef OPTFLOW_ENABLED - // init the optical flow sensor - if(g.optflow_enabled) { - init_optflow(); - } - #endif - -// agmatthews USERHOOKS -#ifdef USERHOOK_INIT - USERHOOK_INIT -#endif - // Logging: - // -------- - // DataFlash log initialization -#if LOGGING_ENABLED == ENABLED - DataFlash.Init(); -#endif - -#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED - // If the switch is in 'menu' mode, run the main menu. - // - // Since we can't be sure that the setup or test mode won't leave - // the system in an odd state, we don't let the user exit the top - // menu; they must reset in order to fly. - // - if (check_startup_for_CLI()) { - digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED - Serial.printf_P(PSTR("\nCLI:\n\n")); - run_cli(); - } -#else - Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n")); -#endif // CLI_ENABLED - - if(g.esc_calibrate == 1){ - init_esc(); - } - - // Logging: - // -------- - if(g.log_bitmask != 0){ - // TODO - Here we will check on the length of the last log - // We don't want to create a bunch of little logs due to powering on and off - start_new_log(); - } - - GPS_enabled = false; - - // Read in the GPS - for (byte counter = 0; ; counter++) { - g_gps->update(); - if (g_gps->status() != 0){ - GPS_enabled = true; - break; - } - - if (counter >= 2) { - GPS_enabled = false; - break; - } - } - - // lengthen the idle timeout for gps Auto_detect - // --------------------------------------------- - g_gps->idleTimeout = 20000; - - // print the GPS status - // -------------------- - report_gps(); - - #if HIL_MODE != HIL_MODE_ATTITUDE - // read Baro pressure at ground - //----------------------------- - init_barometer(); - #endif - - // initialize commands - // ------------------- - init_commands(); - - // set the correct flight mode - // --------------------------- - reset_control_switch(); - - startup_ground(); - - Log_Write_Startup(); - - SendDebug("\nReady to FLY "); -} - -//******************************************************************************** -//This function does all the calibrations, etc. that we need during a ground start -//******************************************************************************** -static void startup_ground(void) -{ - gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START")); - - #if HIL_MODE != HIL_MODE_ATTITUDE - // Warm up and read Gyro offsets - // ----------------------------- - imu.init_gyro(mavlink_delay); - #if CLI_ENABLED == ENABLED - report_imu(); - #endif - #endif - - // reset the leds - // --------------------------- - clear_leds(); -} - -/* -#define YAW_HOLD 0 -#define YAW_ACRO 1 -#define YAW_AUTO 2 -#define YAW_LOOK_AT_HOME 3 - -#define ROLL_PITCH_STABLE 0 -#define ROLL_PITCH_ACRO 1 -#define ROLL_PITCH_AUTO 2 - -#define THROTTLE_MANUAL 0 -#define THROTTLE_HOLD 1 -#define THROTTLE_AUTO 2 - -*/ - -static void set_mode(byte mode) -{ - if(control_mode == mode){ - // don't switch modes if we are already in the correct mode. - return; - } - - old_control_mode = control_mode; - - control_mode = mode; - control_mode = constrain(control_mode, 0, NUM_MODES - 1); - - // used to stop fly_aways - motor_auto_armed = (g.rc_3.control_in > 0); - - // clearing value used in interactive alt hold - manual_boost = 0; - - // clearing value used to set WP's dynamically. - CH7_wp_index = 0; - - Serial.println(flight_mode_strings[control_mode]); - - // report the GPS and Motor arming status - led_mode = NORMAL_LEDS; - - reset_nav(); - - switch(control_mode) - { - case ACRO: - yaw_mode = YAW_ACRO; - roll_pitch_mode = ROLL_PITCH_ACRO; - throttle_mode = THROTTLE_MANUAL; - reset_hold_I(); - break; - - case STABILIZE: - yaw_mode = YAW_HOLD; - roll_pitch_mode = ROLL_PITCH_STABLE; - throttle_mode = THROTTLE_MANUAL; - reset_hold_I(); - break; - - case ALT_HOLD: - yaw_mode = ALT_HOLD_YAW; - roll_pitch_mode = ALT_HOLD_RP; - throttle_mode = ALT_HOLD_THR; - reset_hold_I(); - - init_throttle_cruise(); - next_WP = current_loc; - break; - - case AUTO: - reset_hold_I(); - yaw_mode = AUTO_YAW; - roll_pitch_mode = AUTO_RP; - throttle_mode = AUTO_THR; - - init_throttle_cruise(); - - // loads the commands from where we left off - init_commands(); - break; - - case CIRCLE: - yaw_mode = CIRCLE_YAW; - roll_pitch_mode = CIRCLE_RP; - throttle_mode = CIRCLE_THR; - - init_throttle_cruise(); - next_WP = current_loc; - break; - - case LOITER: - yaw_mode = LOITER_YAW; - roll_pitch_mode = LOITER_RP; - throttle_mode = LOITER_THR; - - init_throttle_cruise(); - next_WP = current_loc; - break; - - case POSITION: - yaw_mode = YAW_HOLD; - roll_pitch_mode = ROLL_PITCH_AUTO; - throttle_mode = THROTTLE_MANUAL; - - next_WP = current_loc; - break; - - case GUIDED: - yaw_mode = YAW_AUTO; - roll_pitch_mode = ROLL_PITCH_AUTO; - throttle_mode = THROTTLE_AUTO; - - //xtrack_enabled = true; - init_throttle_cruise(); - next_WP = current_loc; - set_next_WP(&guided_WP); - break; - - case RTL: - yaw_mode = RTL_YAW; - roll_pitch_mode = RTL_RP; - throttle_mode = RTL_THR; - - //xtrack_enabled = true; - init_throttle_cruise(); - do_RTL(); - break; - - default: - break; - } - - Log_Write_Mode(control_mode); -} - -static void set_failsafe(boolean mode) -{ - // only act on changes - // ------------------- - if(failsafe != mode){ - - // store the value so we don't trip the gate twice - // ----------------------------------------------- - failsafe = mode; - - if (failsafe == false){ - // We've regained radio contact - // ---------------------------- - failsafe_off_event(); - - }else{ - // We've lost radio contact - // ------------------------ - failsafe_on_event(); - - } - } -} - - -static void -init_compass() -{ - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - dcm.set_compass(&compass); - compass.init(); - compass.get_offsets(); // load offsets to account for airframe magnetic interference -} - -#ifdef OPTFLOW_ENABLED -static void -init_optflow() -{ - if( optflow.init() == false ) { - g.optflow_enabled = false; - //SendDebug("\nFailed to Init OptFlow "); - } - optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft - optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view -} -#endif - -static void -init_simple_bearing() -{ - initial_simple_bearing = dcm.yaw_sensor; -} - -static void -init_throttle_cruise() -{ - // are we moving from manual throttle to auto_throttle? - if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ - g.pi_throttle.reset_I(); - #if FRAME_CONFIG == HELI_FRAME - g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in)); - #else - g.throttle_cruise.set_and_save(g.rc_3.control_in); - #endif - } -} - -#if CLI_ENABLED == ENABLED -#if BROKEN_SLIDER == 1 - -static boolean -check_startup_for_CLI() -{ - //return true; - if((g.rc_4.radio_max) < 1600){ - // CLI mode - return true; - - }else if(abs(g.rc_4.control_in) > 3000){ - // CLI mode - return true; - - }else{ - // startup to fly - return false; - } -} - -#else - -static boolean -check_startup_for_CLI() -{ - return (digitalRead(SLIDE_SWITCH_PIN) == 0); -} - -#endif // BROKEN_SLIDER -#endif // CLI_ENABLED - -/* - map from a 8 bit EEPROM baud rate to a real baud rate - */ -static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) -{ - switch (rate) { - case 9: return 9600; - case 19: return 19200; - case 38: return 38400; - case 57: return 57600; - case 111: return 111100; - case 115: return 115200; - } - //Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); - return default_baud; -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduCopter/test.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CLI_ENABLED == ENABLED - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); -static int8_t test_radio(uint8_t argc, const Menu::arg *argv); -static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); -//static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); -static int8_t test_gps(uint8_t argc, const Menu::arg *argv); -static int8_t test_tri(uint8_t argc, const Menu::arg *argv); -static int8_t test_adc(uint8_t argc, const Menu::arg *argv); -static int8_t test_imu(uint8_t argc, const Menu::arg *argv); -//static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); -//static int8_t test_omega(uint8_t argc, const Menu::arg *argv); -static int8_t test_battery(uint8_t argc, const Menu::arg *argv); -//static int8_t test_nav(uint8_t argc, const Menu::arg *argv); - -//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv); -//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv); -static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); -static int8_t test_current(uint8_t argc, const Menu::arg *argv); -static int8_t test_relay(uint8_t argc, const Menu::arg *argv); -static int8_t test_wp(uint8_t argc, const Menu::arg *argv); -static int8_t test_baro(uint8_t argc, const Menu::arg *argv); -//static int8_t test_mag(uint8_t argc, const Menu::arg *argv); -static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); -#ifdef OPTFLOW_ENABLED -static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); -#endif -//static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); -static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); -//static int8_t test_mission(uint8_t argc, const Menu::arg *argv); - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of printf that reads from flash memory -/*static int8_t help_test(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("\n" - "Commands:\n" - " radio\n" - " servos\n" - " g_gps\n" - " imu\n" - " battery\n" - "\n")); -}*/ - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -const struct Menu::command test_menu_commands[] PROGMEM = { - {"pwm", test_radio_pwm}, - {"radio", test_radio}, - {"failsafe", test_failsafe}, -// {"stabilize", test_stabilize}, - {"gps", test_gps}, -#if HIL_MODE != HIL_MODE_ATTITUDE - {"adc", test_adc}, -#endif - {"imu", test_imu}, - //{"dcm", test_dcm}, - //{"omega", test_omega}, - {"battery", test_battery}, - {"tune", test_tuning}, - {"tri", test_tri}, - {"current", test_current}, - {"relay", test_relay}, - {"waypoints", test_wp}, - //{"nav", test_nav}, -#if HIL_MODE != HIL_MODE_ATTITUDE - {"altitude", test_baro}, -#endif - {"sonar", test_sonar}, - //{"compass", test_mag}, -#ifdef OPTFLOW_ENABLED - {"optflow", test_optflow}, -#endif - //{"xbee", test_xbee}, - {"eedump", test_eedump}, - {"rawgps", test_rawgps}, -// {"mission", test_mission}, - //{"reverse", test_reverse}, - //{"wp", test_wp_nav}, -}; - -// A Macro to create the Menu -MENU(test_menu, "test", test_menu_commands); - -static int8_t -test_mode(uint8_t argc, const Menu::arg *argv) -{ - //Serial.printf_P(PSTR("Test Mode\n\n")); - test_menu.run(); - return 0; -} - -static int8_t -test_eedump(uint8_t argc, const Menu::arg *argv) -{ - int i, j; - - // hexdump the EEPROM - for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { - Serial.printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); - Serial.println(); - } - return(0); -} - -static int8_t -test_radio_pwm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - // servo Yaw - //APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - - Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - g.rc_1.radio_in, - g.rc_2.radio_in, - g.rc_3.radio_in, - g.rc_4.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_tri(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - g.rc_4.servo_out = g.rc_4.control_in; - g.rc_4.calc_pwm(); - - Serial.printf_P(PSTR("input: %d\toutput%d\n"), - g.rc_4.control_in, - g.rc_4.radio_out); - - APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - - if(Serial.available() > 0){ - return (0); - } - } -} - -/* -static int8_t -test_nav(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(1000); - g_gps->ground_course = 19500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); - - g_gps->ground_course = 28500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); - - g_gps->ground_course = 1500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); - - g_gps->ground_course = 10500; - calc_nav_rate2(g.waypoint_speed_max); - calc_nav_pitch_roll2(); - - - //if(Serial.available() > 0){ - return (0); - //} - } -} -*/ - -static int8_t -test_radio(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - read_radio(); - - - Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), - g.rc_1.control_in, - g.rc_2.control_in, - g.rc_3.control_in, - g.rc_4.control_in, - g.rc_5.control_in, - g.rc_6.control_in, - g.rc_7.control_in); - - //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100)); - - /*Serial.printf_P(PSTR( "min: %d" - "\t in: %d" - "\t pwm_in: %d" - "\t sout: %d" - "\t pwm_out %d\n"), - g.rc_3.radio_min, - g.rc_3.control_in, - g.rc_3.radio_in, - g.rc_3.servo_out, - g.rc_3.pwm_out - ); - */ - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_failsafe(uint8_t argc, const Menu::arg *argv) -{ - - #if THROTTLE_FAILSAFE - byte fail_test; - print_hit_enter(); - for(int i = 0; i < 50; i++){ - delay(20); - read_radio(); - } - - oldSwitchPosition = readSwitch(); - - Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); - while(g.rc_3.control_in > 0){ - delay(20); - read_radio(); - } - - while(1){ - delay(20); - read_radio(); - - if(g.rc_3.control_in > 0){ - Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in); - fail_test++; - } - - if(oldSwitchPosition != readSwitch()){ - Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); - Serial.println(flight_mode_strings[readSwitch()]); - fail_test++; - } - - if(g.throttle_fs_enabled && g.rc_3.get_failsafe()){ - Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in); - Serial.println(flight_mode_strings[readSwitch()]); - fail_test++; - } - - if(fail_test > 0){ - return (0); - } - if(Serial.available() > 0){ - Serial.printf_P(PSTR("LOS caused no change in ACM.\n")); - return (0); - } - } - #else - return (0); - #endif -} - -/*static int8_t -test_stabilize(uint8_t argc, const Menu::arg *argv) -{ - static byte ts_num; - - - print_hit_enter(); - delay(1000); - - // setup the radio - // --------------- - init_rc_in(); - - control_mode = STABILIZE; - Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP()); - Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); - - motor_auto_armed = false; - motor_armed = true; - - while(1){ - // 50 hz - if (millis() - fast_loopTimer > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer; - fast_loopTimer = millis(); - G_Dt = (float)delta_ms_fast_loop / 1000.f; - - if(g.compass_enabled){ - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.roll, dcm.pitch); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - medium_loopCounter = 0; - } - } - - // for trim features - read_trim_switch(); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - // IMU - // --- - read_AHRS(); - - // allow us to zero out sensors with control switches - if(g.rc_5.control_in < 600){ - dcm.roll_sensor = dcm.pitch_sensor = 0; - } - - // custom code/exceptions for flight modes - // --------------------------------------- - update_current_flight_mode(); - - // write out the servo PWM values - // ------------------------------ - set_servos_4(); - - ts_num++; - if (ts_num > 10){ - ts_num = 0; - Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "), - (int)(dcm.roll_sensor/100), - (int)(dcm.pitch_sensor/100), - g.rc_1.pwm_out); - - print_motor_out(); - } - // R: 1417, L: 1453 F: 1453 B: 1417 - - //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); - //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); - - if(Serial.available() > 0){ - if(g.compass_enabled){ - compass.save_offsets(); - report_compass(); - } - return (0); - } - - } - } -} -*/ -#if HIL_MODE != HIL_MODE_ATTITUDE -static int8_t -test_adc(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - Serial.printf_P(PSTR("ADC\n")); - delay(1000); - - while(1){ - for(int i = 0; i < 9; i++){ - Serial.printf_P(PSTR("%u,"),adc.Ch(i)); - } - Serial.println(); - delay(20); - if(Serial.available() > 0){ - return (0); - } - } -} -#endif - -static int8_t -test_imu(uint8_t argc, const Menu::arg *argv) -{ - //Serial.printf_P(PSTR("Calibrating.")); - - //dcm.kp_yaw(0.02); - //dcm.ki_yaw(0); - - report_imu(); - imu.init_gyro(); - report_imu(); - - print_hit_enter(); - delay(1000); - - //float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw; - fast_loopTimer = millis(); - - while(1){ - //delay(20); - if (millis() - fast_loopTimer >= 5) { - - // IMU - // --- - read_AHRS(); - - //Vector3f accels = imu.get_accel(); - //Vector3f gyros = imu.get_gyro(); - //Vector3f accel_filt = imu.get_accel_filtered(); - //accels_rot = dcm.get_dcm_matrix() * accel_filt; - - - medium_loopCounter++; - - if(medium_loopCounter == 4){ - update_trig(); - } - - if(medium_loopCounter == 20){ - //read_radio(); - medium_loopCounter = 0; - //tuning(); - //dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0); - - /* - Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld, kp:%1.4f, kp:%1.4f \n"), - dcm.roll_sensor, - dcm.pitch_sensor, - dcm.yaw_sensor, - dcm.kp_roll_pitch(), - (float)g.rc_6.control_in / 2000.0); - */ - Serial.printf_P(PSTR("%ld, %ld, %ld\n"), - dcm.roll_sensor, - dcm.pitch_sensor, - dcm.yaw_sensor); - - if(g.compass_enabled){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); - } - } - - // We are using the IMU - // --------------------- - /* - Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t" - "G: %4.4f, %4.4f, %4.4f\t"), - accels.x, accels.y, accels.z, - gyros.x, gyros.y, gyros.z); - */ - /* - Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"), - cos_pitch_x, - sin_pitch_y, - cos_roll_x, - sin_roll_y, - cos_yaw_x, // x - sin_yaw_y); // y - //*/ - //Log_Write_Raw(); - } - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_gps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(333); - - // Blink GPS LED if we don't have a fix - // ------------------------------------ - update_GPS_light(); - - g_gps->update(); - - if (g_gps->new_data){ - Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), - g_gps->latitude, - g_gps->longitude, - g_gps->altitude/100, - g_gps->num_sats); - g_gps->new_data = false; - }else{ - Serial.print("."); - } - if(Serial.available() > 0){ - return (0); - } - } -} - -/* -static int8_t -test_dcm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - Vector3f _cam_vector; - Vector3f _out_vector; - - G_Dt = .02; - - while(1){ - for(byte i = 0; i <= 50; i++){ - delay(20); - // IMU - // --- - read_AHRS(); - } - - Matrix3f temp = dcm.get_dcm_matrix(); - Matrix3f temp_t = dcm.get_dcm_transposed(); - - Serial.printf_P(PSTR("dcm\n" - "%4.4f \t %4.4f \t %4.4f \n" - "%4.4f \t %4.4f \t %4.4f \n" - "%4.4f \t %4.4f \t %4.4f \n\n"), - temp.a.x, temp.a.y, temp.a.z, - temp.b.x, temp.b.y, temp.b.z, - temp.c.x, temp.c.y, temp.c.z); - - int _pitch = degrees(-asin(temp.c.x)); - int _roll = degrees(atan2(temp.c.y, temp.c.z)); - int _yaw = degrees(atan2(temp.b.x, temp.a.x)); - Serial.printf_P(PSTR( "angles\n" - "%d \t %d \t %d\n\n"), - _pitch, - _roll, - _yaw); - - //_out_vector = _cam_vector * temp; - //Serial.printf_P(PSTR( "cam\n" - // "%d \t %d \t %d\n\n"), - // (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100); - - if(Serial.available() > 0){ - return (0); - } - } -} -*/ -/* -static int8_t -test_dcm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - delay(1000); - - while(1){ - Vector3f accels = dcm.get_accel(); - Serial.print("accels.z:"); - Serial.print(accels.z); - Serial.print("omega.z:"); - Serial.print(omega.z); - delay(100); - - if(Serial.available() > 0){ - return (0); - } - } -} -*/ - -/*static int8_t -test_omega(uint8_t argc, const Menu::arg *argv) -{ - static byte ts_num; - float old_yaw; - - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Omega")); - delay(1000); - - G_Dt = .02; - - while(1){ - delay(20); - // IMU - // --- - read_AHRS(); - - float my_oz = (dcm.yaw - old_yaw) * 50; - - old_yaw = dcm.yaw; - - ts_num++; - if (ts_num > 2){ - ts_num = 0; - //Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz); - Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz); - } - - if(Serial.available() > 0){ - return (0); - } - } - return (0); -} -//*/ - -static int8_t -test_battery(uint8_t argc, const Menu::arg *argv) -{ -#if BATTERY_EVENT == 1 - for (int i = 0; i < 20; i++){ - delay(20); - read_battery(); - } - Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"), - battery_voltage1, - battery_voltage2, - battery_voltage3, - battery_voltage4); -#else - Serial.printf_P(PSTR("Not enabled\n")); - -#endif - return (0); -} - -static int8_t -test_tuning(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - - while(1){ - delay(200); - read_radio(); - tuning(); - Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_current(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - //delta_ms_medium_loop = 100; - - while(1){ - delay(100); - read_radio(); - read_battery(); - Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), - battery_voltage, - current_amps, - current_total); - - APM_RC.OutputCh(CH_1, g.rc_3.radio_in); - APM_RC.OutputCh(CH_2, g.rc_3.radio_in); - APM_RC.OutputCh(CH_3, g.rc_3.radio_in); - APM_RC.OutputCh(CH_4, g.rc_3.radio_in); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - Serial.printf_P(PSTR("Relay on\n")); - relay.on(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } - - Serial.printf_P(PSTR("Relay off\n")); - relay.off(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_wp(uint8_t argc, const Menu::arg *argv) -{ - delay(1000); - - // save the alitude above home option - Serial.printf_P(PSTR("Hold alt ")); - if(g.RTL_altitude < 0){ - Serial.printf_P(PSTR("\n")); - }else{ - Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100); - } - - Serial.printf_P(PSTR("%d wp\n"), (int)g.waypoint_total); - Serial.printf_P(PSTR("Hit rad: %d\n"), (int)g.waypoint_radius); - //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - - report_wp(); - - return (0); -} - -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); - while(1){ - if (Serial3.available()){ - digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS - Serial1.write(Serial3.read()); - digitalWrite(B_LED_PIN, LOW); - } - if (Serial1.available()){ - digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS - Serial3.write(Serial1.read()); - digitalWrite(C_LED_PIN, LOW); - } - if(Serial.available() > 0){ - return (0); - } - } - } - -/*static int8_t -test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); - - while(1){ - if (Serial3.available()) - Serial3.write(Serial3.read()); - - if(Serial.available() > 0){ - return (0); - } - } -} -*/ - -#if HIL_MODE != HIL_MODE_ATTITUDE -static int8_t -test_baro(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - init_barometer(); - - while(1){ - delay(100); - baro_alt = read_barometer(); - Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt); - //Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2); - if(Serial.available() > 0){ - return (0); - } - } -} -#endif - -/* -static int8_t -test_mag(uint8_t argc, const Menu::arg *argv) -{ - if(g.compass_enabled) { - //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); - - print_hit_enter(); - - while(1){ - delay(100); - compass.read(); - compass.calculate(dcm.get_dcm_matrix()); - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z); - - if(Serial.available() > 0){ - return (0); - } - } - } else { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); - } -} -*/ -/* -static int8_t -test_reverse(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - g.rc_4.set_reverse(0); - g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_4.servo_out = g.rc_4.control_in; - g.rc_4.calc_pwm(); - Serial.printf_P(PSTR("PWM:%d input: %d\toutput%d "), - APM_RC.InputCh(CH_4), - g.rc_4.control_in, - g.rc_4.radio_out); - APM_RC.OutputCh(CH_6, g.rc_4.radio_out); - - - g.rc_4.set_reverse(1); - g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_4.servo_out = g.rc_4.control_in; - g.rc_4.calc_pwm(); - Serial.printf_P(PSTR("\trev input: %d\toutput%d\n"), - g.rc_4.control_in, - g.rc_4.radio_out); - - APM_RC.OutputCh(CH_7, g.rc_4.radio_out); - - if(Serial.available() > 0){ - g.rc_4.set_reverse(0); - return (0); - } - } -}*/ - -/* - test the sonar - */ -static int8_t -test_sonar(uint8_t argc, const Menu::arg *argv) -{ - if(g.sonar_enabled == false){ - Serial.printf_P(PSTR("Sonar disabled\n")); - return (0); - } - - print_hit_enter(); - while(1) { - delay(100); - - Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read()); - //Serial.printf_P(PSTR("Sonar, %d, %d\n"), sonar.read(), sonar.raw_value); - - if(Serial.available() > 0){ - return (0); - } - } - - return (0); -} - -#ifdef OPTFLOW_ENABLED -static int8_t -test_optflow(uint8_t argc, const Menu::arg *argv) -{ - ///* - if(g.optflow_enabled) { - Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); - print_hit_enter(); - - while(1){ - delay(200); - optflow.read(); - Log_Write_Optflow(); - Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"), - optflow.x, - optflow.dx, - optflow.y, - optflow.dy, - optflow.surface_quality); - - if(Serial.available() > 0){ - return (0); - } - } - } else { - Serial.printf_P(PSTR("OptFlow: ")); - print_enabled(false); - return (0); - } -} -#endif - -/* -static int8_t -test_mission(uint8_t argc, const Menu::arg *argv) -{ - //write out a basic mission to the EEPROM - -//{ -// uint8_t id; ///< command id -// uint8_t options; ///< options bitmask (1<<0 = relative altitude) -// uint8_t p1; ///< param 1 -// int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100) -// int32_t lat; ///< param 3 - Lattitude * 10**7 -// int32_t lng; ///< param 4 - Longitude * 10**7 -//} - - // clear home - {Location t = {0, 0, 0, 0, 0, 0}; - set_command_with_index(t,0);} - - // CMD opt pitch alt/cm - {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; - set_command_with_index(t,1);} - - if (!strcmp_P(argv[1].str, PSTR("wp"))) { - - // CMD opt - {Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0}; - set_command_with_index(t,2);} - // CMD opt - {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; - set_command_with_index(t,3);} - - // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; - set_command_with_index(t,4);} - - } else { - //2250 = 25 meteres - // CMD opt p1 //alt //NS //WE - {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 - set_command_with_index(t,2);} - - // CMD opt dir angle/deg deg/s relative - {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; - set_command_with_index(t,3);} - - // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; - set_command_with_index(t,4);} - - } - - g.RTL_altitude.set_and_save(300); - g.waypoint_total.set_and_save(4); - g.waypoint_radius.set_and_save(3); - - test_wp(NULL, NULL); - return (0); -} -*/ - -static void print_hit_enter() -{ - Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); -} - -/* -static void fake_out_gps() -{ - static float rads; - g_gps->new_data = true; - g_gps->fix = true; - - //int length = g.rc_6.control_in; - rads += .05; - - if (rads > 6.28){ - rads = 0; - } - - g_gps->latitude = 377696000; // Y - g_gps->longitude = -1224319000; // X - g_gps->altitude = 9000; // meters * 100 - - //next_WP.lng = home.lng - length * sin(rads); // X - //next_WP.lat = home.lat + length * cos(rads); // Y -} - -*/ - -static void print_motor_out(){ - Serial.printf("out: R: %d, L: %d F: %d B: %d\n", - (motor_out[CH_1] - g.rc_3.radio_min), - (motor_out[CH_2] - g.rc_3.radio_min), - (motor_out[CH_3] - g.rc_3.radio_min), - (motor_out[CH_4] - g.rc_3.radio_min)); -} - -#endif // CLI_ENABLED diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp deleted file mode 100644 index e51050aa6c..0000000000 --- a/ArduPlane/ArduPlane.cpp +++ /dev/null @@ -1,7973 +0,0 @@ -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/ArduPlane.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#define THISFIRMWARE "ArduPlane V2.24" -/* -Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short -Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi -Please contribute your ideas! - - -This firmware is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. -*/ - -//////////////////////////////////////////////////////////////////////////////// -// Header includes -//////////////////////////////////////////////////////////////////////////////// - -// AVR runtime -#include -#include -#include -#include - -// Libraries -#include -#include -#include // ArduPilot Mega RC Library -#include // ArduPilot GPS library -#include // Arduino I2C lib -#include // Arduino SPI lib -#include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include // ArduPilot Mega BMP085 Library -#include // ArduPilot Mega Magnetometer Library -#include // ArduPilot Mega Vector/Matrix math Library -#include // ArduPilot Mega IMU Library -#include // ArduPilot Mega DCM Library -#include // PID library -#include // RC Channel Library -#include // Range finder library -#include -#include // APM relay -#include // MAVLink GCS definitions -#include - -// Configuration -#include "config.h" - -// Local modules -#include "defines.h" -#include "Parameters.h" -#include "GCS.h" - -//////////////////////////////////////////////////////////////////////////////// -// Serial ports -//////////////////////////////////////////////////////////////////////////////// -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -#line 1 "autogenerated" -#include "WProgram.h" - void setup() ; - void loop() ; - static void fast_loop() ; - static void medium_loop() ; - static void slow_loop() ; - static void one_second_loop() ; - static void update_GPS(void) ; - static void update_current_flight_mode(void) ; - static void update_navigation() ; - static void update_alt() ; - static void stabilize() ; - static void crash_checker() ; - static void calc_throttle() ; - static void calc_nav_yaw(float speed_scaler) ; - static void calc_nav_pitch() ; - static void calc_nav_roll() ; - float roll_slew_limit(float servo) ; - static void throttle_slew_limit() ; - static void reset_I(void) ; - static void set_servos(void) ; - static void demo_servos(byte i) ; - static NOINLINE void send_heartbeat(mavlink_channel_t chan) ; - static NOINLINE void send_attitude(mavlink_channel_t chan) ; - static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) ; - static void NOINLINE send_meminfo(mavlink_channel_t chan) ; - static void NOINLINE send_location(mavlink_channel_t chan) ; - static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) ; - static void NOINLINE send_gps_raw(mavlink_channel_t chan) ; - static void NOINLINE send_servo_out(mavlink_channel_t chan) ; - static void NOINLINE send_radio_in(mavlink_channel_t chan) ; - static void NOINLINE send_radio_out(mavlink_channel_t chan) ; - static void NOINLINE send_vfr_hud(mavlink_channel_t chan) ; - static void NOINLINE send_raw_imu1(mavlink_channel_t chan) ; - static void NOINLINE send_raw_imu2(mavlink_channel_t chan) ; - static void NOINLINE send_raw_imu3(mavlink_channel_t chan) ; - static void NOINLINE send_gps_status(mavlink_channel_t chan) ; - static void NOINLINE send_current_waypoint(mavlink_channel_t chan) ; - static void NOINLINE send_statustext(mavlink_channel_t chan) ; - static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; - static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) ; - void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) ; - static void mavlink_delay(unsigned long t) ; - static void gcs_send_message(enum ap_message id) ; - static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) ; - static void gcs_update(void) ; - static void gcs_send_text(gcs_severity severity, const char *str) ; - static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) ; - static bool print_log_menu(void) ; - static byte get_num_logs(void) ; - static void start_new_log(byte num_existing_logs) ; - static void get_log_boundaries(byte log_num, int & start_page, int & end_page) ; - static int find_last_log_page(int bottom_page) ; - static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) ; - static void Log_Write_Performance() ; - static void Log_Write_Cmd(byte num, struct Location *wp) ; - static void Log_Write_Startup(byte type) ; - static void Log_Write_Control_Tuning() ; - static void Log_Write_Nav_Tuning() ; - static void Log_Write_Mode(byte mode) ; - static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) ; - static void Log_Write_Raw() ; - static void Log_Write_Current() ; - static void Log_Read_Current() ; - static void Log_Read_Control_Tuning() ; - static void Log_Read_Nav_Tuning() ; - static void Log_Read_Performance() ; - static void Log_Read_Cmd() ; - static void Log_Read_Startup() ; - static void Log_Read_Attitude() ; - static void Log_Read_Mode() ; - static void Log_Read_GPS() ; - static void Log_Read_Raw() ; - static void Log_Read(int start_page, int end_page) ; - static void Log_Write_Mode(byte mode) ; - static void Log_Write_Startup(byte type) ; - static void Log_Write_Cmd(byte num, struct Location *wp) ; - static void Log_Write_Current() ; - static void Log_Write_Nav_Tuning() ; - static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) ; - static void Log_Write_Performance() ; - static byte get_num_logs(void) ; - static void start_new_log(byte num_existing_logs) ; - static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) ; - static void Log_Write_Control_Tuning() ; - static void Log_Write_Raw() ; - static void add_altitude_data(unsigned long xl, long y) ; - static void init_commands() ; - static void update_auto() ; - static void reload_commands_airstart() ; - static struct Location get_cmd_with_index(int i) ; - static void set_cmd_with_index(struct Location temp, int i) ; - static void increment_cmd_index() ; - static void decrement_cmd_index() ; - static long read_alt_to_hold() ; - static void set_next_WP(struct Location *wp) ; - static void set_guided_WP(void) ; - void init_home() ; - static void handle_process_nav_cmd() ; - static void handle_process_condition_command() ; - static void handle_process_do_command() ; - static void handle_no_commands() ; - static void do_RTL(void) ; - static void do_takeoff() ; - static void do_nav_wp() ; - static void do_land() ; - static void do_loiter_unlimited() ; - static void do_loiter_turns() ; - static void do_loiter_time() ; - static bool verify_takeoff() ; - static bool verify_land() ; - static bool verify_nav_wp() ; - static bool verify_loiter_unlim() ; - static bool verify_loiter_time() ; - static bool verify_loiter_turns() ; - static bool verify_RTL() ; - static void do_wait_delay() ; - static void do_change_alt() ; - static void do_within_distance() ; - static bool verify_wait_delay() ; - static bool verify_change_alt() ; - static bool verify_within_distance() ; - static void do_loiter_at_location() ; - static void do_jump() ; - static void do_change_speed() ; - static void do_set_home() ; - static void do_set_servo() ; - static void do_set_relay() ; - static void do_repeat_servo() ; - static void do_repeat_relay() ; - static void change_command(uint8_t cmd_index) ; - static void update_commands(void) ; - static void verify_commands(void) ; - static void process_next_command() ; - static void process_nav_cmd() ; - static void process_non_nav_command() ; - static void read_control_switch() ; - static byte readSwitch(void); - static void reset_control_switch() ; - static void update_servo_switches() ; - static void failsafe_short_on_event() ; - static void failsafe_long_on_event() ; - static void failsafe_short_off_event() ; - static void low_battery_event(void) ; - static void navigate() ; - void calc_distance_error() ; - static void calc_airspeed_errors() ; - static void calc_bearing_error() ; - static void calc_altitude_error() ; - static long wrap_360(long error) ; - static long wrap_180(long error) ; - static void update_loiter() ; - static void update_crosstrack(void) ; - static void reset_crosstrack() ; - static long get_distance(struct Location *loc1, struct Location *loc2) ; - static long get_bearing(struct Location *loc1, struct Location *loc2) ; - static void init_rc_in() ; - static void init_rc_out() ; - static void read_radio() ; - static void control_failsafe(uint16_t pwm) ; - static void trim_control_surfaces() ; - static void trim_radio() ; - void ReadSCP1000(void) ; - static void init_barometer(void) ; - static long read_barometer(void) ; - static void read_airspeed(void) ; - static void zero_airspeed(void) ; - static void read_battery(void) ; - static void report_batt_monitor() ; - static void report_radio() ; - static void report_gains() ; - static void report_xtrack() ; - static void report_throttle() ; - static void report_imu() ; - static void report_compass() ; - static void report_flight_modes() ; - static void print_PID(PID * pid) ; - static void print_radio_values() ; - static void print_switch(byte p, byte m) ; - static void print_done() ; - static void print_blanks(int num) ; - static void print_divider(void) ; - static int8_t radio_input_switch(void) ; - static void zero_eeprom(void) ; - static void print_enabled(bool b) ; - static void print_accel_offsets(void) ; - static void print_gyro_offsets(void) ; - static void run_cli(void) ; - static void init_ardupilot() ; - static void startup_ground(void) ; - static void set_mode(byte mode) ; - static void check_long_failsafe() ; - static void check_short_failsafe() ; - static void startup_IMU_ground(void) ; - static void update_GPS_light(void) ; - static void resetPerfData(void) ; - static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) ; - static void print_hit_enter() ; - static void test_wp_print(struct Location *cmd, byte wp_index) ; -#line 64 "/home/jgoppert/Projects/ardupilotone/ArduPlane/ArduPlane.pde" -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port -FastSerialPort3(Serial3); // Telemetry port - -//////////////////////////////////////////////////////////////////////////////// -// Parameters -//////////////////////////////////////////////////////////////////////////////// -// -// Global parameters are all contained within the 'g' class. -// -static Parameters g; - - -//////////////////////////////////////////////////////////////////////////////// -// prototypes -static void update_events(void); - - -//////////////////////////////////////////////////////////////////////////////// -// Sensors -//////////////////////////////////////////////////////////////////////////////// -// -// There are three basic options related to flight sensor selection. -// -// - Normal flight mode. Real sensors are used. -// - HIL Attitude mode. Most sensors are disabled, as the HIL -// protocol supplies attitude information directly. -// - HIL Sensors mode. Synthetic sensors are configured that -// supply data from the simulation. -// - -// All GPS access should be through this pointer. -static GPS *g_gps; - -// flight modes convenience array -static AP_Int8 *flight_modes = &g.flight_mode1; - -#if HIL_MODE == HIL_MODE_DISABLED - -// real sensors -static AP_ADC_ADS7844 adc; -static APM_BMP085_Class barometer; -static AP_Compass_HMC5843 compass(Parameters::k_param_compass); - -// real GPS selection -#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO -AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); - -#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA -AP_GPS_NMEA g_gps_driver(&Serial1); - -#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF -AP_GPS_SIRF g_gps_driver(&Serial1); - -#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX -AP_GPS_UBLOX g_gps_driver(&Serial1); - -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK -AP_GPS_MTK g_gps_driver(&Serial1); - -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 -AP_GPS_MTK16 g_gps_driver(&Serial1); - -#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE -AP_GPS_None g_gps_driver(NULL); - -#else - #error Unrecognised GPS_PROTOCOL setting. -#endif // GPS PROTOCOL - -#elif HIL_MODE == HIL_MODE_SENSORS -// sensor emulators -AP_ADC_HIL adc; -APM_BMP085_HIL_Class barometer; -AP_Compass_HIL compass; -AP_GPS_HIL g_gps_driver(NULL); - -#elif HIL_MODE == HIL_MODE_ATTITUDE -AP_ADC_HIL adc; -AP_DCM_HIL dcm; -AP_GPS_HIL g_gps_driver(NULL); -AP_Compass_HIL compass; // never used -AP_IMU_Shim imu; // never used - -#else - #error Unrecognised HIL_MODE setting. -#endif // HIL MODE - -#if HIL_MODE != HIL_MODE_ATTITUDE - #if HIL_MODE != HIL_MODE_SENSORS - // Normal - AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); - #else - // hil imu - AP_IMU_Shim imu; - #endif - // normal dcm - AP_DCM dcm(&imu, g_gps); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// GCS selection -//////////////////////////////////////////////////////////////////////////////// -// -GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0); -GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3); - -//////////////////////////////////////////////////////////////////////////////// -// SONAR selection -//////////////////////////////////////////////////////////////////////////////// -// -ModeFilter sonar_mode_filter; - -#if SONAR_TYPE == MAX_SONAR_XL - AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); -#elif SONAR_TYPE == MAX_SONAR_LV - // XXX honestly I think these output the same values - // If someone knows, can they confirm it? - AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Global variables -//////////////////////////////////////////////////////////////////////////////// - -byte control_mode = INITIALISING; -byte oldSwitchPosition; // for remembering the control mode switch -bool inverted_flight = false; - -static const char *comma = ","; - -static const char* flight_mode_strings[] = { - "Manual", - "Circle", - "Stabilize", - "", - "", - "FBW_A", - "FBW_B", - "", - "", - "", - "Auto", - "RTL", - "Loiter", - "Takeoff", - "Land"}; - - -/* Radio values - Channel assignments - 1 Ailerons (rudder if no ailerons) - 2 Elevator - 3 Throttle - 4 Rudder (if we have ailerons) - 5 Aux5 - 6 Aux6 - 7 Aux7 - 8 Aux8/Mode - Each Aux channel can be configured to have any of the available auxiliary functions assigned to it. - See libraries/RC_Channel/RC_Channel_aux.h for more information -*/ - -// Failsafe -// -------- -static int failsafe; // track which type of failsafe is being processed -static bool ch3_failsafe; -static byte crash_timer; - -// Radio -// ----- -static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm -static uint16_t elevon2_trim = 1500; -static uint16_t ch1_temp = 1500; // Used for elevon mixing -static uint16_t ch2_temp = 1500; -static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; -static bool rc_override_active = false; -static uint32_t rc_override_fs_timer = 0; -static uint32_t ch3_failsafe_timer = 0; - -// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon - -// LED output -// ---------- -static bool GPS_light; // status of the GPS light - -// GPS variables -// ------------- -static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage -static float scaleLongUp = 1; // used to reverse longitude scaling -static float scaleLongDown = 1; // used to reverse longitude scaling -static byte ground_start_count = 5; // have we achieved first lock and set Home? -static int ground_start_avg; // 5 samples to avg speed for ground start -static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present - -// Location & Navigation -// --------------------- -const float radius_of_earth = 6378100; // meters -const float gravity = 9.81; // meters/ sec^2 -static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate -static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target -static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? -static long hold_course = -1; // deg * 100 dir of plane - -static byte command_index; // current command memory location -static byte nav_command_index; // active nav command memory location -static byte non_nav_command_index; // active non-nav command memory location -static byte nav_command_ID = NO_COMMAND; // active nav command ID -static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID - -// Airspeed -// -------- -static int airspeed; // m/s * 100 -static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -static float airspeed_error; // m/s * 100 -static float airspeed_fbwB; // m/s * 100 -static long energy_error; // energy state error (kinetic + potential) for altitude hold -static long airspeed_energy_error; // kinetic portion of energy error - -// Location Errors -// --------------- -static long bearing_error; // deg * 100 : 0 to 36000 -static long altitude_error; // meters * 100 we are off in altitude -static float crosstrack_error; // meters we are off trackline - -// Battery Sensors -// --------------- -static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter -static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter -static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter - -static float current_amps; -static float current_total; - -// Airspeed Sensors -// ---------------- -static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering -static int airspeed_pressure; // airspeed as a pressure value - -// Barometer Sensor variables -// -------------------------- -static unsigned long abs_pressure; - -// Altitude Sensor variables -// ---------------------- -static int sonar_alt; - -// flight mode specific -// -------------------- -static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. -static bool land_complete; -static long takeoff_altitude; -// static int landing_distance; // meters; -static int landing_pitch; // pitch for landing set by commands -static int takeoff_pitch; - -// Loiter management -// ----------------- -static long old_target_bearing; // deg * 100 -static int loiter_total; // deg : how many times to loiter * 360 -static int loiter_delta; // deg : how far we just turned -static int loiter_sum; // deg : how far we have turned around a waypoint -static long loiter_time; // millis : when we started LOITER mode -static int loiter_time_max; // millis : how long to stay in LOITER mode - -// these are the values for navigation control functions -// ---------------------------------------------------- -static long nav_roll; // deg * 100 : target roll angle -static long nav_pitch; // deg * 100 : target pitch angle -static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel - -// Waypoints -// --------- -static long wp_distance; // meters - distance between plane and next waypoint -static long wp_totalDistance; // meters - distance between old and next waypoint - -// repeating event control -// ----------------------- -static byte event_id; // what to do - see defines -static long event_timer; // when the event was asked for in ms -static uint16_t event_delay; // how long to delay the next firing of event in millis -static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles -static int event_value; // per command value, such as PWM for servos -static int event_undo_value; // the value used to cycle events (alternate value to event_value) - -// delay command -// -------------- -static long condition_value; // used in condition commands (eg delay, change alt, etc.) -static long condition_start; -static int condition_rate; - -// 3D Location vectors -// ------------------- -static struct Location home; // home location -static struct Location prev_WP; // last waypoint -static struct Location current_loc; // current location -static struct Location next_WP; // next waypoint -static struct Location guided_WP; // guided mode waypoint -static struct Location next_nav_command; // command preloaded -static struct Location next_nonnav_command; // command preloaded -static long target_altitude; // used for altitude management between waypoints -static long offset_altitude; // used for altitude management between waypoints -static bool home_is_set; // Flag for if we have g_gps lock and have set the home location - - -// IMU variables -// ------------- -static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) - - -// Performance monitoring -// ---------------------- -static long perf_mon_timer; // Metric based on accel gain deweighting -static int G_Dt_max = 0; // Max main loop cycle time in milliseconds -static int gps_fix_count = 0; -static int pmTest1 = 0; - - -// System Timers -// -------------- -static unsigned long fast_loopTimer; // Time in miliseconds of main control loop -static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete -static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -static int mainLoop_count; - -static unsigned long medium_loopTimer; // Time in miliseconds of medium loop -static byte medium_loopCounter; // Counters for branching from main control loop to slower loops -static uint8_t delta_ms_medium_loop; - -static byte slow_loopCounter; -static byte superslow_loopCounter; -static byte counter_one_herz; - -static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav - -static unsigned long dTnav; // Delta Time in milliseconds for navigation computations -static float load; // % MCU cycles used - -AP_Relay relay; - -//////////////////////////////////////////////////////////////////////////////// -// Top-level logic -//////////////////////////////////////////////////////////////////////////////// - -void setup() { - memcheck_init(); - init_ardupilot(); -} - -void loop() -{ - // We want this to execute at 50Hz if possible - // ------------------------------------------- - if (millis()-fast_loopTimer > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer; - load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop; - G_Dt = (float)delta_ms_fast_loop / 1000.f; - fast_loopTimer = millis(); - - mainLoop_count++; - - // Execute the fast loop - // --------------------- - fast_loop(); - - // Execute the medium loop - // ----------------------- - medium_loop(); - - counter_one_herz++; - if(counter_one_herz == 50){ - one_second_loop(); - counter_one_herz = 0; - } - - if (millis() - perf_mon_timer > 20000) { - if (mainLoop_count != 0) { - if (g.log_bitmask & MASK_LOG_PM) - Log_Write_Performance(); - - resetPerfData(); - } - } - - fast_loopTimeStamp = millis(); - } -} - -// Main loop 50Hz -static void fast_loop() -{ - // This is the fast loop - we want it to execute at 50Hz if possible - // ----------------------------------------------------------------- - if (delta_ms_fast_loop > G_Dt_max) - G_Dt_max = delta_ms_fast_loop; - - // Read radio - // ---------- - read_radio(); - - // try to send any deferred messages if the serial port now has - // some space available - gcs_send_message(MSG_RETRY_DEFERRED); - - // check for loss of control signal failsafe condition - // ------------------------------------ - check_short_failsafe(); - - // Read Airspeed - // ------------- - if (g.airspeed_enabled == true) { -#if HIL_MODE != HIL_MODE_ATTITUDE - read_airspeed(); -#endif - } else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { - calc_airspeed_errors(); - } - - #if HIL_MODE == HIL_MODE_SENSORS - // update hil before dcm update - gcs_update(); - #endif - - dcm.update_DCM(); - - // uses the yaw from the DCM to give more accurate turns - calc_bearing_error(); - - # if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) - Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); - - if (g.log_bitmask & MASK_LOG_RAW) - Log_Write_Raw(); - #endif - - // inertial navigation - // ------------------ - #if INERTIAL_NAVIGATION == ENABLED - // TODO: implement inertial nav function - inertialNavigation(); - #endif - - // custom code/exceptions for flight modes - // --------------------------------------- - update_current_flight_mode(); - - // apply desired roll, pitch and yaw to the plane - // ---------------------------------------------- - if (control_mode > MANUAL) - stabilize(); - - // write out the servo PWM values - // ------------------------------ - set_servos(); - - - // XXX is it appropriate to be doing the comms below on the fast loop? - - gcs_update(); - gcs_data_stream_send(45,1000); -} - -static void medium_loop() -{ - // This is the start of the medium (10 Hz) loop pieces - // ----------------------------------------- - switch(medium_loopCounter) { - - // This case deals with the GPS - //------------------------------- - case 0: - medium_loopCounter++; - if(GPS_enabled) update_GPS(); - - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.compass_enabled){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - } - #endif -/*{ -Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); -Serial.print(dcm.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); -Serial.print(dcm.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); -Vector3f tempaccel = imu.get_accel(); -Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); -Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); -Serial.println(tempaccel.z, DEC); -}*/ - - break; - - // This case performs some navigation computations - //------------------------------------------------ - case 1: - medium_loopCounter++; - - - if(g_gps->new_data){ - g_gps->new_data = false; - dTnav = millis() - nav_loopTimer; - nav_loopTimer = millis(); - - // calculate the plane's desired bearing - // ------------------------------------- - navigate(); - } - - break; - - // command processing - //------------------------------ - case 2: - medium_loopCounter++; - - // Read altitude from sensors - // ------------------ - update_alt(); - if(g.sonar_enabled) sonar_alt = sonar.read(); - - // altitude smoothing - // ------------------ - if (control_mode != FLY_BY_WIRE_B) - calc_altitude_error(); - - // perform next command - // -------------------- - update_commands(); - break; - - // This case deals with sending high rate telemetry - //------------------------------------------------- - case 3: - medium_loopCounter++; - - #if HIL_MODE != HIL_MODE_ATTITUDE - if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) - Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); - - if (g.log_bitmask & MASK_LOG_CTUN) - Log_Write_Control_Tuning(); - #endif - - if (g.log_bitmask & MASK_LOG_NTUN) - Log_Write_Nav_Tuning(); - - if (g.log_bitmask & MASK_LOG_GPS) - Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); - - // send all requested output streams with rates requested - // between 5 and 45 Hz - gcs_data_stream_send(5,45); - break; - - // This case controls the slow loop - //--------------------------------- - case 4: - medium_loopCounter = 0; - delta_ms_medium_loop = millis() - medium_loopTimer; - medium_loopTimer = millis(); - - if (g.battery_monitoring != 0){ - read_battery(); - } - - slow_loop(); - break; - } -} - -static void slow_loop() -{ - // This is the slow (3 1/3 Hz) loop pieces - //---------------------------------------- - switch (slow_loopCounter){ - case 0: - slow_loopCounter++; - check_long_failsafe(); - superslow_loopCounter++; - if(superslow_loopCounter >=200) { // 200 = Execute every minute - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.compass_enabled) { - compass.save_offsets(); - } - #endif - - superslow_loopCounter = 0; - } - break; - - case 1: - slow_loopCounter++; - - // Read 3-position switch on radio - // ------------------------------- - read_control_switch(); - - // Read Control Surfaces/Mix switches - // ---------------------------------- - update_servo_switches(); - - update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); - - break; - - case 2: - slow_loopCounter = 0; - update_events(); - - mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter - gcs_data_stream_send(3,5); - break; - } -} - -static void one_second_loop() -{ - if (g.log_bitmask & MASK_LOG_CUR) - Log_Write_Current(); - - // send a heartbeat - gcs_send_message(MSG_HEARTBEAT); - gcs_data_stream_send(1,3); -} - -static void update_GPS(void) -{ - g_gps->update(); - update_GPS_light(); - - if (g_gps->new_data && g_gps->fix) { - // for performance - // --------------- - gps_fix_count++; - - if(ground_start_count > 1){ - ground_start_count--; - ground_start_avg += g_gps->ground_speed; - - } else if (ground_start_count == 1) { - // We countdown N number of good GPS fixes - // so that the altitude is more accurate - // ------------------------------------- - if (current_loc.lat == 0) { - ground_start_count = 5; - - } else { - if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){ - startup_ground(); - - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - - init_home(); - } else if (ENABLE_AIR_START == 0) { - init_home(); - } - - ground_start_count = 0; - } - } - - - current_loc.lng = g_gps->longitude; // Lon * 10**7 - current_loc.lat = g_gps->latitude; // Lat * 10**7 - - } -} - -static void update_current_flight_mode(void) -{ - if(control_mode == AUTO){ - crash_checker(); - - switch(nav_command_ID){ - case MAV_CMD_NAV_TAKEOFF: - if (hold_course > -1) { - calc_nav_roll(); - } else { - nav_roll = 0; - } - - if (g.airspeed_enabled == true) - { - calc_nav_pitch(); - if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; - } else { - nav_pitch = (long)((float)g_gps->ground_speed / (float)g.airspeed_cruise * (float)takeoff_pitch * 0.5); - nav_pitch = constrain(nav_pitch, 500l, (long)takeoff_pitch); - } - - g.channel_throttle.servo_out = g.throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle - // What is the case for doing something else? Why wouldn't you want max throttle for TO? - // ****************************** - - break; - - case MAV_CMD_NAV_LAND: - calc_nav_roll(); - - if (g.airspeed_enabled == true){ - calc_nav_pitch(); - calc_throttle(); - }else{ - calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle - calc_throttle(); // throttle based on altitude error - nav_pitch = landing_pitch; // pitch held constant - } - - if (land_complete){ - g.channel_throttle.servo_out = 0; - } - break; - - default: - hold_course = -1; - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - } - }else{ - switch(control_mode){ - case RTL: - case LOITER: - case GUIDED: - hold_course = -1; - crash_checker(); - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - - case FLY_BY_WIRE_A: - // set nav_roll and nav_pitch using sticks - nav_roll = g.channel_roll.norm_input() * g.roll_limit; - nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min; - // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. - nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority - if (inverted_flight) nav_pitch = -nav_pitch; - break; - - case FLY_BY_WIRE_B: - // Substitute stick inputs for Navigation control output - // We use g.pitch_limit_min because its magnitude is - // normally greater than g.pitch_limit_max - nav_roll = g.channel_roll.norm_input() * g.roll_limit; - altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; - - if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) { - altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; - } else { - if (g.channel_pitch.norm_input()<0) - altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ; - else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ; - } - - if (g.airspeed_enabled == true) - { - airspeed_fbwB = ((int)(g.flybywire_airspeed_max - - g.flybywire_airspeed_min) * - g.channel_throttle.servo_out) + - ((int)g.flybywire_airspeed_min * 100); - airspeed_energy_error = (long)(((long)airspeed_fbwB * - (long)airspeed_fbwB) - - ((long)airspeed * (long)airspeed))/20000; - airspeed_error = (airspeed_error - airspeed); - } - - calc_throttle(); - calc_nav_pitch(); - break; - - case STABILIZE: - nav_roll = 0; - nav_pitch = 0; - // throttle is passthrough - break; - - case CIRCLE: - // we have no GPS installed and have lost radio contact - // or we just want to fly around in a gentle circle w/o GPS - // ---------------------------------------------------- - nav_roll = g.roll_limit / 3; - nav_pitch = 0; - - if (failsafe != FAILSAFE_NONE){ - g.channel_throttle.servo_out = g.throttle_cruise; - } - break; - - case MANUAL: - // servo_out is for Sim control only - // --------------------------------- - g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); - g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); - g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); - break; - //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - - } - } -} - -static void update_navigation() -{ - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ - - // distance and bearing calcs only - if(control_mode == AUTO){ - verify_commands(); - }else{ - - switch(control_mode){ - case LOITER: - case RTL: - case GUIDED: - update_loiter(); - calc_bearing_error(); - break; - - } - } -} - - -static void update_alt() -{ - #if HIL_MODE == HIL_MODE_ATTITUDE - current_loc.alt = g_gps->altitude; - #else - // this function is in place to potentially add a sonar sensor in the future - //altitude_sensor = BARO; - - current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100) - current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); - #endif - - // Calculate new climb rate - //if(medium_loopCounter == 0 && slow_loopCounter == 0) - // add_altitude_data(millis() / 100, g_gps->altitude / 10); -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/Attitude.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -//**************************************************************** -// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. -//**************************************************************** - -static void stabilize() -{ - float ch1_inf = 1.0; - float ch2_inf = 1.0; - float ch4_inf = 1.0; - float speed_scaler; - - if (g.airspeed_enabled == true){ - if(airspeed > 0) - speed_scaler = (STANDARD_SPEED * 100) / airspeed; - else - speed_scaler = 2.0; - speed_scaler = constrain(speed_scaler, 0.5, 2.0); - } else { - if (g.channel_throttle.servo_out > 0){ - speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root - // Should maybe be to the 2/7 power, but we aren't goint to implement that... - }else{ - speed_scaler = 1.67; - } - speed_scaler = constrain(speed_scaler, 0.6, 1.67); // This case is constrained tighter as we don't have real speed info - } - - if(crash_timer > 0){ - nav_roll = 0; - } - - if (inverted_flight) { - // we want to fly upside down. We need to cope with wrap of - // the roll_sensor interfering with wrap of nav_roll, which - // would really confuse the PID code. The easiest way to - // handle this is to ensure both go in the same direction from - // zero - nav_roll += 18000; - if (dcm.roll_sensor < 0) nav_roll -= 36000; - } - - // For Testing Only - // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; - // Serial.printf_P(PSTR(" roll_sensor ")); - // Serial.print(roll_sensor,DEC); - - // Calculate dersired servo output for the roll - // --------------------------------------------- - g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler); - long tempcalc = nav_pitch + - fabs(dcm.roll_sensor * g.kff_pitch_compensation) + - (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - - (dcm.pitch_sensor - g.pitch_trim); - if (inverted_flight) { - // when flying upside down the elevator control is inverted - tempcalc = -tempcalc; - } - g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); - - // Mix Stick input to allow users to override control surfaces - // ----------------------------------------------------------- - if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) { - - - // TODO: use RC_Channel control_mix function? - ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; - ch1_inf = fabs(ch1_inf); - ch1_inf = min(ch1_inf, 400.0); - ch1_inf = ((400.0 - ch1_inf) /400.0); - - ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim; - ch2_inf = fabs(ch2_inf); - ch2_inf = min(ch2_inf, 400.0); - ch2_inf = ((400.0 - ch2_inf) /400.0); - - // scale the sensor input based on the stick input - // ----------------------------------------------- - g.channel_roll.servo_out *= ch1_inf; - g.channel_pitch.servo_out *= ch2_inf; - - // Mix in stick inputs - // ------------------- - g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); - g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle(); - - //Serial.printf_P(PSTR(" servo_out[CH_ROLL] ")); - //Serial.println(servo_out[CH_ROLL],DEC); - } - - // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes - // important for steering on the ground during landing - // ----------------------------------------------- - if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) { - ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; - ch4_inf = fabs(ch4_inf); - ch4_inf = min(ch4_inf, 400.0); - ch4_inf = ((400.0 - ch4_inf) /400.0); - } - - // Apply output to Rudder - // ---------------------- - calc_nav_yaw(speed_scaler); - g.channel_rudder.servo_out *= ch4_inf; - g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle(); - - // Call slew rate limiter if used - // ------------------------------ - //#if(ROLL_SLEW_LIMIT != 0) - // g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out); - //#endif -} - -static void crash_checker() -{ - if(dcm.pitch_sensor < -4500){ - crash_timer = 255; - } - if(crash_timer > 0) - crash_timer--; -} - - -static void calc_throttle() -{ - if (g.airspeed_enabled == false) { - int throttle_target = g.throttle_cruise + throttle_nudge; - - // no airspeed sensor, we use nav pitch to determine the proper throttle output - // AUTO, RTL, etc - // --------------------------------------------------------------------------- - if (nav_pitch >= 0) { - g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch / g.pitch_limit_max; - } else { - g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch / g.pitch_limit_min; - } - - g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); - } else { - // throttle control with airspeed compensation - // ------------------------------------------- - energy_error = airspeed_energy_error + (float)altitude_error * 0.098f; - - // positive energy errors make the throttle go higher - g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav); - g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle); - - g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, - g.throttle_min.get(), g.throttle_max.get()); // TODO - resolve why "saved" is used here versus "current" - } - -} - -/***************************************** - * Calculate desired roll/pitch/yaw angles (in medium freq loop) - *****************************************/ - -// Yaw is separated into a function for future implementation of heading hold on rolling take-off -// ---------------------------------------------------------------------------------------- -static void calc_nav_yaw(float speed_scaler) -{ -#if HIL_MODE != HIL_MODE_ATTITUDE - Vector3f temp = imu.get_accel(); - long error = -temp.y; - - // Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero) - g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler); -#else - g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; - // XXX probably need something here based on heading -#endif -} - - -static void calc_nav_pitch() -{ - // Calculate the Pitch of the plane - // -------------------------------- - if (g.airspeed_enabled == true) { - nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); - } else { - nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); - } - nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get()); -} - - -#define YAW_DAMPENER 0 - -static void calc_nav_roll() -{ - - // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. - // This does not make provisions for wind speed in excess of airframe speed - nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0); - nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4); - - // negative error = left turn - // positive error = right turn - // Calculate the required roll of the plane - // ---------------------------------------- - nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100 - nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); - - Vector3f omega; - omega = dcm.get_gyro(); - - // rate limiter - long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 - rate = constrain(rate, -6000, 6000); // limit input - int dampener = rate * YAW_DAMPENER; // 34377 * .175 = 6000 - - // add in yaw dampener - nav_roll -= dampener; - nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); -} - - -/***************************************** - * Roll servo slew limit - *****************************************/ -/* -float roll_slew_limit(float servo) -{ - static float last; - float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f); - last = servo; - return temp; -}*/ - -/***************************************** - * Throttle slew limit - *****************************************/ -static void throttle_slew_limit() -{ - static int last = 1000; - if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit - - float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000 -Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last); - g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp); - last = g.channel_throttle.radio_out; - } -} - - -// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. -// Keeps outdated data out of our calculations -static void reset_I(void) -{ - g.pidNavRoll.reset_I(); - g.pidNavPitchAirspeed.reset_I(); - g.pidNavPitchAltitude.reset_I(); - g.pidTeThrottle.reset_I(); -// g.pidAltitudeThrottle.reset_I(); -} - -/***************************************** -* Set the flight control servos based on the current calculated values -*****************************************/ -static void set_servos(void) -{ - int flapSpeedSource = 0; - - // vectorize the rc channels - RC_Channel_aux* rc_array[NUM_CHANNELS]; - rc_array[CH_1] = NULL; - rc_array[CH_2] = NULL; - rc_array[CH_3] = NULL; - rc_array[CH_4] = NULL; - rc_array[CH_5] = &g.rc_5; - rc_array[CH_6] = &g.rc_6; - rc_array[CH_7] = &g.rc_7; - rc_array[CH_8] = &g.rc_8; - - if(control_mode == MANUAL){ - // do a direct pass through of radio values - if (g.mix_mode == 0){ - g.channel_roll.radio_out = g.channel_roll.radio_in; - g.channel_pitch.radio_out = g.channel_pitch.radio_in; - } else { - g.channel_roll.radio_out = APM_RC.InputCh(CH_ROLL); - g.channel_pitch.radio_out = APM_RC.InputCh(CH_PITCH); - } - g.channel_throttle.radio_out = g.channel_throttle.radio_in; - g.channel_rudder.radio_out = g.channel_rudder.radio_in; - // FIXME To me it does not make sense to control the aileron using radio_in in manual mode - // Doug could you please take a look at this ? - if (g_rc_function[RC_Channel_aux::k_aileron]) { - if (g_rc_function[RC_Channel_aux::k_aileron] != rc_array[g.flight_mode_channel-1]) { - g_rc_function[RC_Channel_aux::k_aileron]->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; - } - } - // only use radio_in if the channel is not used as flight_mode_channel - if (g_rc_function[RC_Channel_aux::k_flap_auto]) { - if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) { - g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; - } else { - g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim; - } - } - } else { - if (g.mix_mode == 0) { - g.channel_roll.calc_pwm(); - g.channel_pitch.calc_pwm(); - g.channel_rudder.calc_pwm(); - if (g_rc_function[RC_Channel_aux::k_aileron]) { - g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out; - g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); - } - - }else{ - /*Elevon mode*/ - float ch1; - float ch2; - ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out); - ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out; - g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); - g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); - } - - #if THROTTLE_OUT == 0 - g.channel_throttle.servo_out = 0; - #else - // convert 0 to 100% into PWM - g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); - - // We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. - /* Disable throttle if following conditions are met: - 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher - AND - 2 - Our reported altitude is within 10 meters of the home altitude. - 3 - Our reported speed is under 5 meters per second. - 4 - We are not performing a takeoff in Auto mode - OR - 5 - Home location is not set - */ - if ( - (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) && - (abs(home.alt - current_loc.alt) < 1000) && - ((g.airspeed_enabled ? airspeed : g_gps->ground_speed) < 500 ) && - !(control_mode==AUTO && takeoff_complete == false) - ) { - g.channel_throttle.servo_out = 0; - g.channel_throttle.calc_pwm(); - } - - #endif - - g.channel_throttle.calc_pwm(); - - /* TO DO - fix this for RC_Channel library - #if THROTTLE_REVERSE == 1 - radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE]; - #endif - */ - - throttle_slew_limit(); - } - - // Auto flap deployment - if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) { - if(control_mode < FLY_BY_WIRE_B) { - // only use radio_in if the channel is not used as flight_mode_channel - if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) { - g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; - } else { - g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim; - } - } else if (control_mode >= FLY_BY_WIRE_B) { - if (control_mode == FLY_BY_WIRE_B) { - flapSpeedSource = airspeed_fbwB/100; - } else if (g.airspeed_enabled == true) { - flapSpeedSource = g.airspeed_cruise/100; - } else { - flapSpeedSource = g.throttle_cruise; - } - if ( flapSpeedSource > g.flap_1_speed) { - g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = 0; - } else if (flapSpeedSource > g.flap_2_speed) { - g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_1_percent; - } else { - g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_2_percent; - } - g_rc_function[RC_Channel_aux::k_flap_auto]->calc_pwm(); - } - } - -#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS - // send values to the PWM timers for output - // ---------------------------------------- - APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos - // Route configurable aux. functions to their respective servos - g.rc_5.output_ch(CH_5); - g.rc_6.output_ch(CH_6); - g.rc_7.output_ch(CH_7); - g.rc_8.output_ch(CH_8); -#endif -} - -static void demo_servos(byte i) { - - while(i > 0){ - gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); -#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS - APM_RC.OutputCh(1, 1400); - mavlink_delay(400); - APM_RC.OutputCh(1, 1600); - mavlink_delay(200); - APM_RC.OutputCh(1, 1500); -#endif - mavlink_delay(400); - i--; - } -} - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/GCS_Mavlink.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Mavlink_compat.h" - -// use this to prevent recursion during sensor init -static bool in_mavlink_delay; - -// this costs us 51 bytes, but means that low priority -// messages don't block the CPU -static mavlink_statustext_t pending_status; - -// true when we have received at least 1 MAVLink packet -static bool mavlink_active; - -// check if a message will fit in the payload space available -#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false - -/* - !!NOTE!! - - the use of NOINLINE separate functions for each message type avoids - a compiler bug in gcc that would cause it to use far more stack - space than is needed. Without the NOINLINE we use the sum of the - stack needed for each message type. Please be careful to follow the - pattern below when adding any new messages - */ - -static NOINLINE void send_heartbeat(mavlink_channel_t chan) -{ -#ifdef MAVLINK10 - uint8_t base_mode = 0; - uint8_t system_status = MAV_STATE_ACTIVE; - - // we map the custom_mode to our internal mode plus 16, to lower - // the chance that a ground station will give us 0 and we - // interpret it as manual. This is necessary as the SET_MODE - // command has no way to indicate that the custom_mode is filled in - uint32_t custom_mode = control_mode + 16; - - // work out the base_mode. This value is almost completely useless - // for APM, but we calculate it as best we can so a generic - // MAVLink enabled ground station can work out something about - // what the MAV is up to. The actual bit values are highly - // ambiguous for most of the APM flight modes. In practice, you - // only get useful information from the custom_mode, which maps to - // the APM flight mode and has a well defined meaning in the - // ArduPlane documentation - switch (control_mode) { - case MANUAL: - base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - case STABILIZE: - case FLY_BY_WIRE_A: - case FLY_BY_WIRE_B: - case FLY_BY_WIRE_C: - base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; - break; - case AUTO: - case RTL: - case LOITER: - case GUIDED: - case CIRCLE: - base_mode = MAV_MODE_FLAG_GUIDED_ENABLED | - MAV_MODE_FLAG_STABILIZE_ENABLED; - // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what - // APM does in any mode, as that is defined as "system finds its own goal - // positions", which APM does not currently do - break; - case INITIALISING: - system_status = MAV_STATE_CALIBRATING; - break; - } - - if (control_mode != MANUAL && control_mode != INITIALISING) { - // stabiliser of some form is enabled - base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - -#if ENABLE_STICK_MIXING==ENABLED - if (control_mode != INITIALISING) { - // all modes except INITIALISING have some form of manual - // override if stick mixing is enabled - base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } -#endif - -#if HIL_MODE != HIL_MODE_DISABLED - base_mode |= MAV_MODE_FLAG_HIL_ENABLED; -#endif - - // we are armed if we are not initialising - if (control_mode != INITIALISING) { - base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } - - mavlink_msg_heartbeat_send( - chan, - MAV_TYPE_FIXED_WING, - MAV_AUTOPILOT_ARDUPILOTMEGA, - base_mode, - custom_mode, - system_status); -#else // MAVLINK10 - mavlink_msg_heartbeat_send( - chan, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); -#endif // MAVLINK10 -} - -static NOINLINE void send_attitude(mavlink_channel_t chan) -{ - Vector3f omega = dcm.get_gyro(); - mavlink_msg_attitude_send( - chan, - micros(), - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); -} - -static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) -{ -#ifdef MAVLINK10 - uint32_t control_sensors_present = 0; - uint32_t control_sensors_enabled; - uint32_t control_sensors_health; - - // first what sensors/controllers we have - control_sensors_present |= (1<<0); // 3D gyro present - control_sensors_present |= (1<<1); // 3D accelerometer present - if (g.compass_enabled) { - control_sensors_present |= (1<<2); // compass present - } - control_sensors_present |= (1<<3); // absolute pressure sensor present - if (g_gps->fix) { - control_sensors_present |= (1<<5); // GPS present - } - control_sensors_present |= (1<<10); // 3D angular rate control - control_sensors_present |= (1<<11); // attitude stabilisation - control_sensors_present |= (1<<12); // yaw position - control_sensors_present |= (1<<13); // altitude control - control_sensors_present |= (1<<14); // X/Y position control - control_sensors_present |= (1<<15); // motor control - - // now what sensors/controllers are enabled - - // first the sensors - control_sensors_enabled = control_sensors_present & 0x1FF; - - // now the controllers - control_sensors_enabled = control_sensors_present & 0x1FF; - - switch (control_mode) { - case MANUAL: - break; - - case STABILIZE: - case FLY_BY_WIRE_A: - control_sensors_enabled |= (1<<10); // 3D angular rate control - control_sensors_enabled |= (1<<11); // attitude stabilisation - break; - - case FLY_BY_WIRE_B: - control_sensors_enabled |= (1<<10); // 3D angular rate control - control_sensors_enabled |= (1<<11); // attitude stabilisation - control_sensors_enabled |= (1<<15); // motor control - break; - - case FLY_BY_WIRE_C: - control_sensors_enabled |= (1<<10); // 3D angular rate control - control_sensors_enabled |= (1<<11); // attitude stabilisation - control_sensors_enabled |= (1<<13); // altitude control - control_sensors_enabled |= (1<<15); // motor control - break; - - case AUTO: - case RTL: - case LOITER: - case GUIDED: - case CIRCLE: - control_sensors_enabled |= (1<<10); // 3D angular rate control - control_sensors_enabled |= (1<<11); // attitude stabilisation - control_sensors_enabled |= (1<<12); // yaw position - control_sensors_enabled |= (1<<13); // altitude control - control_sensors_enabled |= (1<<14); // X/Y position control - control_sensors_enabled |= (1<<15); // motor control - break; - - case INITIALISING: - break; - } - - // at the moment all sensors/controllers are assumed healthy - control_sensors_health = control_sensors_present; - - uint16_t battery_current = -1; - uint8_t battery_remaining = -1; - - if (current_total != 0 && g.pack_capacity != 0) { - battery_remaining = (100.0 * (g.pack_capacity - current_total) / g.pack_capacity); - } - if (current_total != 0) { - battery_current = current_amps * 100; - } - - mavlink_msg_sys_status_send( - chan, - control_sensors_present, - control_sensors_enabled, - control_sensors_health, - (uint16_t)(load * 1000), - battery_voltage * 1000, // mV - battery_current, // in 10mA units - battery_remaining, // in % - 0, // comm drops %, - 0, // comm drops in pkts, - 0, 0, 0, 0); - -#else // MAVLINK10 - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; - - switch(control_mode) { - case MANUAL: - mode = MAV_MODE_MANUAL; - break; - case STABILIZE: - mode = MAV_MODE_TEST1; - break; - case FLY_BY_WIRE_A: - mode = MAV_MODE_TEST2; - nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST2; - nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case GUIDED: - mode = MAV_MODE_GUIDED; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LOITER; - break; - case INITIALISING: - mode = MAV_MODE_UNINIT; - nav_mode = MAV_NAV_GROUNDED; - break; - } - - uint8_t status = MAV_STATE_ACTIVE; - uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 - - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - load * 1000, - battery_voltage * 1000, - battery_remaining, - packet_drops); -#endif // MAVLINK10 -} - -static void NOINLINE send_meminfo(mavlink_channel_t chan) -{ - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); -} - -static void NOINLINE send_location(mavlink_channel_t chan) -{ - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now -#ifdef MAVLINK10 - mavlink_msg_global_position_int_send( - chan, - millis(), - current_loc.lat, // in 1E7 degrees - current_loc.lng, // in 1E7 degrees - g_gps->altitude*10, // millimeters above sea level - current_loc.alt * 10, // millimeters above ground - g_gps->ground_speed * rot.a.x, // X speed cm/s - g_gps->ground_speed * rot.b.x, // Y speed cm/s - g_gps->ground_speed * rot.c.x, - g_gps->ground_course); // course in 1/100 degree -#else // MAVLINK10 - mavlink_msg_global_position_int_send( - chan, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, - g_gps->ground_speed * rot.a.x, - g_gps->ground_speed * rot.b.x, - g_gps->ground_speed * rot.c.x); -#endif // MAVLINK10 -} - -static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) -{ - mavlink_msg_nav_controller_output_send( - chan, - nav_roll / 1.0e2, - nav_pitch / 1.0e2, - nav_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - airspeed_error, - crosstrack_error); -} - -static void NOINLINE send_gps_raw(mavlink_channel_t chan) -{ -#ifdef MAVLINK10 - uint8_t fix; - if (g_gps->status() == 2) { - fix = 3; - } else { - fix = 0; - } - - mavlink_msg_gps_raw_int_send( - chan, - micros(), - fix, - g_gps->latitude, // in 1E7 degrees - g_gps->longitude, // in 1E7 degrees - g_gps->altitude * 10, // in mm - g_gps->hdop, - 65535, - g_gps->ground_speed, // cm/s - g_gps->ground_course, // 1/100 degrees, - g_gps->num_sats); - -#else // MAVLINK10 - mavlink_msg_gps_raw_send( - chan, - micros(), - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); -#endif // MAVLINK10 -} - -static void NOINLINE send_servo_out(mavlink_channel_t chan) -{ - const uint8_t rssi = 1; - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with - // HIL maintainers -#ifdef MAVLINK10 - mavlink_msg_rc_channels_scaled_send( - chan, - millis(), - 0, // port 0 - 10000 * g.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.norm_output(), - 0, - 0, - 0, - 0, - rssi); - -#else // MAVLINK10 - mavlink_msg_rc_channels_scaled_send( - chan, - 10000 * g.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.norm_output(), - 0, - 0, - 0, - 0, - rssi); -#endif // MAVLINK10 -} - -static void NOINLINE send_radio_in(mavlink_channel_t chan) -{ - uint8_t rssi = 1; -#ifdef MAVLINK10 - mavlink_msg_rc_channels_raw_send( - chan, - millis(), - 0, // port - g.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, // XXX currently only 4 RC channels defined - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); - -#else // MAVLINK10 - mavlink_msg_rc_channels_raw_send( - chan, - g.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, // XXX currently only 4 RC channels defined - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); -#endif // MAVLINK10 -} - -static void NOINLINE send_radio_out(mavlink_channel_t chan) -{ -#ifdef MAVLINK10 - mavlink_msg_servo_output_raw_send( - chan, - micros(), - 0, // port - g.channel_roll.radio_out, - g.channel_pitch.radio_out, - g.channel_throttle.radio_out, - g.channel_rudder.radio_out, - g.rc_5.radio_out, // XXX currently only 4 RC channels defined - g.rc_6.radio_out, - g.rc_7.radio_out, - g.rc_8.radio_out); -#else // MAVLINK10 - mavlink_msg_servo_output_raw_send( - chan, - g.channel_roll.radio_out, - g.channel_pitch.radio_out, - g.channel_throttle.radio_out, - g.channel_rudder.radio_out, - g.rc_5.radio_out, // XXX currently only 4 RC channels defined - g.rc_6.radio_out, - g.rc_7.radio_out, - g.rc_8.radio_out); -#endif // MAVLINK10 -} - -static void NOINLINE send_vfr_hud(mavlink_channel_t chan) -{ - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, - (int)g.channel_throttle.servo_out, - current_loc.alt / 100.0, - 0); -} - -#if HIL_MODE != HIL_MODE_ATTITUDE -static void NOINLINE send_raw_imu1(mavlink_channel_t chan) -{ - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); - - mavlink_msg_raw_imu_send( - chan, - micros(), - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); -} - -static void NOINLINE send_raw_imu2(mavlink_channel_t chan) -{ - mavlink_msg_scaled_pressure_send( - chan, - micros(), - (float)barometer.Press/100.0, - (float)(barometer.Press-g.ground_pressure)/100.0, - (int)(barometer.Temp*10)); -} - -static void NOINLINE send_raw_imu3(mavlink_channel_t chan) -{ - Vector3f mag_offsets = compass.get_offsets(); - - mavlink_msg_sensor_offsets_send(chan, - mag_offsets.x, - mag_offsets.y, - mag_offsets.z, - compass.get_declination(), - barometer.RawPress, - barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), - imu.ax(), imu.ay(), imu.az()); -} -#endif // HIL_MODE != HIL_MODE_ATTITUDE - -static void NOINLINE send_gps_status(mavlink_channel_t chan) -{ - mavlink_msg_gps_status_send( - chan, - g_gps->num_sats, - NULL, - NULL, - NULL, - NULL, - NULL); -} - -static void NOINLINE send_current_waypoint(mavlink_channel_t chan) -{ - mavlink_msg_waypoint_current_send( - chan, - g.command_index); -} - -static void NOINLINE send_statustext(mavlink_channel_t chan) -{ - mavlink_msg_statustext_send( - chan, - pending_status.severity, - pending_status.text); -} - - -// try to send a message, return false if it won't fit in the serial tx buffer -static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) -{ - int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; - - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // defer any messages on the telemetry port for 1 second after - // bootup, to try to prevent bricking of Xbees - return false; - } - - switch (id) { - case MSG_HEARTBEAT: - CHECK_PAYLOAD_SIZE(HEARTBEAT); - send_heartbeat(chan); - return true; - - case MSG_EXTENDED_STATUS1: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - send_extended_status1(chan, packet_drops); - break; - - case MSG_EXTENDED_STATUS2: - CHECK_PAYLOAD_SIZE(MEMINFO); - send_meminfo(chan); - break; - - case MSG_ATTITUDE: - CHECK_PAYLOAD_SIZE(ATTITUDE); - send_attitude(chan); - break; - - case MSG_LOCATION: - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - send_location(chan); - break; - - case MSG_NAV_CONTROLLER_OUTPUT: - if (control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - send_nav_controller_output(chan); - } - break; - - case MSG_GPS_RAW: -#ifdef MAVLINK10 - CHECK_PAYLOAD_SIZE(GPS_RAW_INT); -#else - CHECK_PAYLOAD_SIZE(GPS_RAW); -#endif - send_gps_raw(chan); - break; - - case MSG_SERVO_OUT: - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - send_servo_out(chan); - break; - - case MSG_RADIO_IN: - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - send_radio_in(chan); - break; - - case MSG_RADIO_OUT: - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - send_radio_out(chan); - break; - - case MSG_VFR_HUD: - CHECK_PAYLOAD_SIZE(VFR_HUD); - send_vfr_hud(chan); - break; - -#if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU1: - CHECK_PAYLOAD_SIZE(RAW_IMU); - send_raw_imu1(chan); - break; - - case MSG_RAW_IMU2: - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - send_raw_imu2(chan); - break; - - case MSG_RAW_IMU3: - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - send_raw_imu3(chan); - break; -#endif // HIL_MODE != HIL_MODE_ATTITUDE - - case MSG_GPS_STATUS: - CHECK_PAYLOAD_SIZE(GPS_STATUS); - send_gps_status(chan); - break; - - case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); - send_current_waypoint(chan); - break; - - case MSG_NEXT_PARAM: - CHECK_PAYLOAD_SIZE(PARAM_VALUE); - if (chan == MAVLINK_COMM_0) { - gcs0.queued_param_send(); - } else { - gcs3.queued_param_send(); - } - break; - - case MSG_NEXT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); - if (chan == MAVLINK_COMM_0) { - gcs0.queued_waypoint_send(); - } else { - gcs3.queued_waypoint_send(); - } - break; - - case MSG_STATUSTEXT: - CHECK_PAYLOAD_SIZE(STATUSTEXT); - send_statustext(chan); - break; - - case MSG_RETRY_DEFERRED: - break; // just here to prevent a warning - } - return true; -} - - -#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED -static struct mavlink_queue { - enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES]; - uint8_t next_deferred_message; - uint8_t num_deferred_messages; -} mavlink_queue[2]; - -// send a message using mavlink -static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) -{ - uint8_t i, nextid; - struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; - - // see if we can send the deferred messages, if any - while (q->num_deferred_messages != 0) { - if (!mavlink_try_send_message(chan, - q->deferred_messages[q->next_deferred_message], - packet_drops)) { - break; - } - q->next_deferred_message++; - if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { - q->next_deferred_message = 0; - } - q->num_deferred_messages--; - } - - if (id == MSG_RETRY_DEFERRED) { - return; - } - - // this message id might already be deferred - for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { - if (q->deferred_messages[nextid] == id) { - // its already deferred, discard - return; - } - nextid++; - if (nextid == MAX_DEFERRED_MESSAGES) { - nextid = 0; - } - } - - if (q->num_deferred_messages != 0 || - !mavlink_try_send_message(chan, id, packet_drops)) { - // can't send it now, so defer it - if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { - // the defer buffer is full, discard - return; - } - nextid = q->next_deferred_message + q->num_deferred_messages; - if (nextid >= MAX_DEFERRED_MESSAGES) { - nextid -= MAX_DEFERRED_MESSAGES; - } - q->deferred_messages[nextid] = id; - q->num_deferred_messages++; - } -} - -void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) -{ - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // don't send status MAVLink messages for 2 seconds after - // bootup, to try to prevent Xbee bricking - return; - } - - if (severity == SEVERITY_LOW) { - // send via the deferred queuing system - pending_status.severity = (uint8_t)severity; - strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); - mavlink_send_message(chan, MSG_STATUSTEXT, 0); - } else { - // send immediately -#ifdef MAVLINK10 - mavlink_msg_statustext_send(chan, severity, str); -#else - mavlink_msg_statustext_send(chan, severity, (const int8_t*) str); -#endif - } -} - - -GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : -packet_drops(0), - -// parameters -// note, all values not explicitly initialised here are zeroed -waypoint_send_timeout(1000), // 1 second -waypoint_receive_timeout(1000), // 1 second - -// stream rates -_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), - // AP_VAR //ref //index, default, name - streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), - streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), - streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), - streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), - streamRatePosition (&_group, 4, 0, PSTR("POSITION")), - streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), - streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), - streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) -{ - -} - -void -GCS_MAVLINK::init(FastSerial * port) -{ - GCS_Class::init(port); - if (port == &Serial) { - mavlink_comm_0_port = port; - chan = MAVLINK_COMM_0; - }else{ - mavlink_comm_1_port = port; - chan = MAVLINK_COMM_1; - } - _queued_parameter = NULL; -} - -void -GCS_MAVLINK::update(void) -{ - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; - status.packet_rx_drop_count = 0; - - // process received bytes - while (comm_get_available(chan)) - { - uint8_t c = comm_receive_ch(chan); - -#if CLI_ENABLED == ENABLED - /* allow CLI to be started by hitting enter 3 times, if no - heartbeat packets have been received */ - if (mavlink_active == 0) { - if (c == '\n' || c == '\r') { - crlf_count++; - } else { - crlf_count = 0; - } - if (crlf_count == 3) { - run_cli(); - } - } -#endif - - // Try to get a new message - if (mavlink_parse_char(chan, c, &msg, &status)) { - mavlink_active = 1; - handleMessage(&msg); - } - } - - // Update packet drops counter - packet_drops += status.packet_rx_drop_count; - - // send out queued params/ waypoints - if (NULL != _queued_parameter) { - send_message(MSG_NEXT_PARAM); - } - - if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.command_total) { - send_message(MSG_NEXT_WAYPOINT); - } - - // stop waypoint sending if timeout - if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ - waypoint_sending = false; - } - - // stop waypoint receiving if timeout - if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ - waypoint_receiving = false; - } -} - -void -GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) -{ - if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { - - if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ - send_message(MSG_RAW_IMU1); - send_message(MSG_RAW_IMU2); - send_message(MSG_RAW_IMU3); - } - - if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { - send_message(MSG_EXTENDED_STATUS1); - send_message(MSG_EXTENDED_STATUS2); - send_message(MSG_GPS_STATUS); - send_message(MSG_CURRENT_WAYPOINT); - send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working - send_message(MSG_NAV_CONTROLLER_OUTPUT); - } - - if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { - // sent with GPS read - send_message(MSG_LOCATION); - } - - if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { - // This is used for HIL. Do not change without discussing with HIL maintainers - send_message(MSG_SERVO_OUT); - } - - if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { - send_message(MSG_RADIO_OUT); - send_message(MSG_RADIO_IN); - } - - if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info - send_message(MSG_ATTITUDE); - } - - if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info - send_message(MSG_VFR_HUD); - } - - if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - // Available datastream - } - } -} - - - -void -GCS_MAVLINK::send_message(enum ap_message id) -{ - mavlink_send_message(chan,id, packet_drops); -} - -void -GCS_MAVLINK::send_text(gcs_severity severity, const char *str) -{ - mavlink_send_text(chan,severity,str); -} - -void -GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) -{ - mavlink_statustext_t m; - uint8_t i; - for (i=0; imsgid) { - - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - { - // decode - mavlink_request_data_stream_t packet; - mavlink_msg_request_data_stream_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - int freq = 0; // packet frequency - - if (packet.start_stop == 0) - freq = 0; // stop sending - else if (packet.start_stop == 1) - freq = packet.req_message_rate; // start sending - else - break; - - switch(packet.req_stream_id){ - - case MAV_DATA_STREAM_ALL: - streamRateRawSensors = freq; - streamRateExtendedStatus = freq; - streamRateRCChannels = freq; - streamRateRawController = freq; - streamRatePosition = freq; - streamRateExtra1 = freq; - streamRateExtra2 = freq; - streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. - break; - - case MAV_DATA_STREAM_RAW_SENSORS: - streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly - // we will not continue to broadcast raw sensor data at 50Hz. - break; - case MAV_DATA_STREAM_EXTENDED_STATUS: - streamRateExtendedStatus.set_and_save(freq); - break; - - case MAV_DATA_STREAM_RC_CHANNELS: - streamRateRCChannels.set_and_save(freq); - break; - - case MAV_DATA_STREAM_RAW_CONTROLLER: - streamRateRawController.set_and_save(freq); - break; - - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; - - case MAV_DATA_STREAM_POSITION: - streamRatePosition.set_and_save(freq); - break; - - case MAV_DATA_STREAM_EXTRA1: - streamRateExtra1.set_and_save(freq); - break; - - case MAV_DATA_STREAM_EXTRA2: - streamRateExtra2.set_and_save(freq); - break; - - case MAV_DATA_STREAM_EXTRA3: - streamRateExtra3.set_and_save(freq); - break; - - default: - break; - } - break; - } - -#ifdef MAVLINK10 - case MAVLINK_MSG_ID_COMMAND_LONG: - { - // decode - mavlink_command_long_t packet; - mavlink_msg_command_long_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; - - uint8_t result; - - // do command - send_text(SEVERITY_LOW,PSTR("command received: ")); - - switch(packet.command) { - - case MAV_CMD_NAV_LOITER_UNLIM: - set_mode(LOITER); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - set_mode(RTL); - result = MAV_RESULT_ACCEPTED; - break; - -#if 0 - // not implemented yet, but could implement some of them - case MAV_CMD_NAV_LAND: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_NAV_ROI: - case MAV_CMD_NAV_PATHPLANNING: - break; -#endif - - - case MAV_CMD_PREFLIGHT_CALIBRATION: - if (packet.param1 == 1 || - packet.param2 == 1 || - packet.param3 == 1) { - startup_IMU_ground(); - } - if (packet.param4 == 1) { - trim_radio(); - } - result = MAV_RESULT_ACCEPTED; - break; - - - default: - result = MAV_RESULT_UNSUPPORTED; - break; - } - - mavlink_msg_command_ack_send( - chan, - packet.command, - result); - - break; - } - -#else // MAVLINK10 - case MAVLINK_MSG_ID_ACTION: - { - // decode - mavlink_action_t packet; - mavlink_msg_action_decode(msg, &packet); - if (mavlink_check_target(packet.target,packet.target_component)) break; - - uint8_t result = 0; - - // do action - send_text(SEVERITY_LOW,PSTR("action received: ")); -//Serial.println(packet.action); - switch(packet.action){ - - case MAV_ACTION_LAUNCH: - //set_mode(TAKEOFF); - break; - - case MAV_ACTION_RETURN: - set_mode(RTL); - result=1; - break; - - case MAV_ACTION_EMCY_LAND: - //set_mode(LAND); - break; - - case MAV_ACTION_HALT: - do_loiter_at_location(); - result=1; - break; - - /* No mappable implementation in APM 2.0 - case MAV_ACTION_MOTORS_START: - case MAV_ACTION_CONFIRM_KILL: - case MAV_ACTION_EMCY_KILL: - case MAV_ACTION_MOTORS_STOP: - case MAV_ACTION_SHUTDOWN: - break; - */ - - case MAV_ACTION_CONTINUE: - process_next_command(); - result=1; - break; - - case MAV_ACTION_SET_MANUAL: - set_mode(MANUAL); - result=1; - break; - - case MAV_ACTION_SET_AUTO: - set_mode(AUTO); - result=1; - break; - - case MAV_ACTION_STORAGE_READ: - AP_Var::load_all(); - result=1; - break; - - case MAV_ACTION_STORAGE_WRITE: - AP_Var::save_all(); - result=1; - break; - - case MAV_ACTION_CALIBRATE_RC: break; - trim_radio(); - result=1; - break; - - case MAV_ACTION_CALIBRATE_GYRO: - case MAV_ACTION_CALIBRATE_MAG: - case MAV_ACTION_CALIBRATE_ACC: - case MAV_ACTION_CALIBRATE_PRESSURE: - case MAV_ACTION_REBOOT: // this is a rough interpretation - startup_IMU_ground(); - result=1; - break; - - /* For future implemtation - case MAV_ACTION_REC_START: break; - case MAV_ACTION_REC_PAUSE: break; - case MAV_ACTION_REC_STOP: break; - */ - - /* Takeoff is not an implemented flight mode in APM 2.0 - case MAV_ACTION_TAKEOFF: - set_mode(TAKEOFF); - break; - */ - - case MAV_ACTION_NAVIGATE: - set_mode(AUTO); - result=1; - break; - - /* Land is not an implemented flight mode in APM 2.0 - case MAV_ACTION_LAND: - set_mode(LAND); - break; - */ - - case MAV_ACTION_LOITER: - set_mode(LOITER); - result=1; - break; - - default: break; - } - - mavlink_msg_action_ack_send( - chan, - packet.action, - result - ); - - break; - } -#endif - - case MAVLINK_MSG_ID_SET_MODE: - { - // decode - mavlink_set_mode_t packet; - mavlink_msg_set_mode_decode(msg, &packet); - -#ifdef MAVLINK10 - // we ignore base_mode as there is no sane way to map - // from that bitmap to a APM flight mode. We rely on - // custom_mode instead. - // see comment on custom_mode above - int16_t adjusted_mode = packet.custom_mode - 16; - - switch (adjusted_mode) { - case MANUAL: - case CIRCLE: - case STABILIZE: - case FLY_BY_WIRE_A: - case FLY_BY_WIRE_B: - case FLY_BY_WIRE_C: - case AUTO: - case RTL: - case LOITER: - set_mode(adjusted_mode); - break; - } - -#else // MAVLINK10 - - switch(packet.mode){ - - case MAV_MODE_MANUAL: - set_mode(MANUAL); - break; - - case MAV_MODE_GUIDED: - set_mode(GUIDED); - break; - - case MAV_MODE_AUTO: - if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); - if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); - if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); - mav_nav = 255; - break; - - case MAV_MODE_TEST1: - set_mode(STABILIZE); - break; - - case MAV_MODE_TEST2: - if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A); - if(mav_nav == 2) set_mode(FLY_BY_WIRE_B); - //if(mav_nav == 3) set_mode(FLY_BY_WIRE_C); - mav_nav = 255; - break; - - } -#endif - break; - } - -#ifndef MAVLINK10 - case MAVLINK_MSG_ID_SET_NAV_MODE: - { - // decode - mavlink_set_nav_mode_t packet; - mavlink_msg_set_nav_mode_decode(msg, &packet); - // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message - mav_nav = packet.nav_mode; - break; - } -#endif // MAVLINK10 - - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - // Start sending waypoints - mavlink_msg_waypoint_count_send( - chan,msg->sysid, - msg->compid, - g.command_total + 1); // + home - - waypoint_timelast_send = millis(); - waypoint_sending = true; - waypoint_receiving = false; - waypoint_dest_sysid = msg->sysid; - waypoint_dest_compid = msg->compid; - break; - } - - - // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - // Check if sending waypiont - //if (!waypoint_sending) break; - // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW - - // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - // send waypoint - tell_command = get_cmd_with_index(packet.seq); - - // set frame of waypoint - uint8_t frame; - - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { - frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame - } else { - frame = MAV_FRAME_GLOBAL; // reference frame - } - - float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; - - // time that the mav should loiter in milliseconds - uint8_t current = 0; // 1 (true), 0 (false) - - if (packet.seq == (uint16_t)g.command_index) - current = 1; - - uint8_t autocontinue = 1; // 1 (true), 0 (false) - - float x = 0, y = 0, z = 0; - - if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { - // command needs scaling - x = tell_command.lat/1.0e7; // local (x), global (latitude) - y = tell_command.lng/1.0e7; // local (y), global (longitude) - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { - z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt - } else { - z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) - } - } - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - param1 = tell_command.p1; - break; - - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1*10; // APM loiter time is in ten second increments - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - x=0; // Clear fields loaded above that we don't want sent for this command - y=0; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; - - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - param1 = tell_command.p1; - break; - - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - param3 = tell_command.lat; - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - } - - mavlink_msg_waypoint_send(chan,msg->sysid, - msg->compid, - packet.seq, - frame, - tell_command.id, - current, - autocontinue, - param1, - param2, - param3, - param4, - x, - y, - z); - - // update last waypoint comm stamp - waypoint_timelast_send = millis(); - break; - } - - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // turn off waypoint send - waypoint_sending = false; - break; - } - - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - { - // decode - mavlink_param_request_list_t packet; - mavlink_msg_param_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // Start sending parameters - next call to ::update will kick the first one out - - _queued_parameter = AP_Var::first(); - _queued_parameter_index = 0; - _queued_parameter_count = _count_parameters(); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; - - // clear all commands - g.command_total.set_and_save(0); - - // note that we don't send multiple acks, as otherwise a - // GCS that is doing a clear followed by a set may see - // the additional ACKs as ACKs of the set operations - mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // set current command - change_command(packet.seq); - - mavlink_msg_waypoint_current_send(chan, g.command_index); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // start waypoint receiving - if (packet.count > MAX_WAYPOINTS) { - packet.count = MAX_WAYPOINTS; - } - g.command_total.set_and_save(packet.count - 1); - - waypoint_timelast_receive = millis(); - waypoint_receiving = true; - waypoint_sending = false; - waypoint_request_i = 0; - break; - } - -#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS - case MAVLINK_MSG_ID_SET_MAG_OFFSETS: - { - mavlink_set_mag_offsets_t packet; - mavlink_msg_set_mag_offsets_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); - break; - } -#endif - - // XXX receive a WP from GCS and store in EEPROM - case MAVLINK_MSG_ID_WAYPOINT: - { - // decode - mavlink_waypoint_t packet; - uint8_t result = MAV_MISSION_ACCEPTED; - - mavlink_msg_waypoint_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // defaults - tell_command.id = packet.command; - - switch (packet.frame) - { - case MAV_FRAME_MISSION: - case MAV_FRAME_GLOBAL: - { - tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm - tell_command.options = 0; // absolute altitude - break; - } - -#ifdef MAV_FRAME_LOCAL_NED - case MAV_FRAME_LOCAL_NED: // local (relative to home position) - { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = -packet.z*1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; - break; - } -#endif - -#ifdef MAV_FRAME_LOCAL - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; - break; - } -#endif - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - { - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! - break; - } - - default: - result = MAV_MISSION_UNSUPPORTED_FRAME; - break; - } - - - if (result != MAV_MISSION_ACCEPTED) goto mission_failed; - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - case MAV_CMD_NAV_WAYPOINT: - case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - case MAV_CMD_NAV_LAND: - break; - - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.lat = packet.param1; - break; - - case MAV_CMD_NAV_LOITER_TIME: - tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments - break; - - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - tell_command.lat = packet.param1; - break; - - case MAV_CMD_DO_JUMP: - tell_command.lat = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_REPEAT_SERVO: - tell_command.lng = packet.param4; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - tell_command.lat = packet.param3; - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - - default: -#ifdef MAVLINK10 - result = MAV_MISSION_UNSUPPORTED; -#endif - break; - } - - if (result != MAV_MISSION_ACCEPTED) goto mission_failed; - - if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission - guided_WP = tell_command; - - // add home alt if needed - if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ - guided_WP.alt += home.alt; - } - - set_mode(GUIDED); - - // make any new wp uploaded instant (in case we are already in Guided mode) - set_guided_WP(); - - // verify we recevied the command - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - 0); - - } else { - // Check if receiving waypoints (mission upload expected) - if (!waypoint_receiving) { - result = MAV_MISSION_ERROR; - goto mission_failed; - } - - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) { - result = MAV_MISSION_INVALID_SEQUENCE; - goto mission_failed; - } - - set_cmd_with_index(tell_command, packet.seq); - - // update waypoint receiving state machine - waypoint_timelast_receive = millis(); - waypoint_request_i++; - - if (waypoint_request_i > (uint16_t)g.command_total){ - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - result); - - send_text(SEVERITY_LOW,PSTR("flight plan received")); - waypoint_receiving = false; - // XXX ignores waypoint radius for individual waypoints, can - // only set WP_RADIUS parameter - } - } - break; - - mission_failed: - // we are rejecting the mission/waypoint - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - result); - break; - } - - case MAVLINK_MSG_ID_PARAM_SET: - { - AP_Var *vp; - AP_Meta_class::Type_id var_type; - - // decode - mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - // set parameter - - char key[ONBOARD_PARAM_NAME_LENGTH+1]; - strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); - key[ONBOARD_PARAM_NAME_LENGTH] = 0; - - // find the requested parameter - vp = AP_Var::find(key); - if ((NULL != vp) && // exists - !isnan(packet.param_value) && // not nan - !isinf(packet.param_value)) { // not inf - - // add a small amount before casting parameter values - // from float to integer to avoid truncating to the - // next lower integer value. - float rounding_addition = 0.01; - - // fetch the variable type ID - var_type = vp->meta_type_id(); - - // handle variables with standard type IDs - if (var_type == AP_Var::k_typeid_float) { - ((AP_Float *)vp)->set_and_save(packet.param_value); - } else if (var_type == AP_Var::k_typeid_float16) { - ((AP_Float16 *)vp)->set_and_save(packet.param_value); - } else if (var_type == AP_Var::k_typeid_int32) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else if (var_type == AP_Var::k_typeid_int16) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else if (var_type == AP_Var::k_typeid_int8) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else { - // we don't support mavlink set on this parameter - break; - } - - // Report back the new value if we accepted the change - // we send the value we actually set, which could be - // different from the value sent, in case someone sent - // a fractional value to an integer type -#ifdef MAVLINK10 - mavlink_msg_param_value_send( - chan, - key, - vp->cast_to_float(), - mav_var_type(vp->meta_type_id()), - _count_parameters(), - -1); // XXX we don't actually know what its index is... -#else // MAVLINK10 - mavlink_msg_param_value_send( - chan, - (int8_t *)key, - vp->cast_to_float(), - _count_parameters(), - -1); // XXX we don't actually know what its index is... -#endif // MAVLINK10 - } - - break; - } // end case - - case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: - { - // allow override of RC channel values for HIL - // or for complete GCS control of switch position - // and RC PWM values. - if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs - mavlink_rc_channels_override_t packet; - int16_t v[8]; - mavlink_msg_rc_channels_override_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system,packet.target_component)) - break; - - v[0] = packet.chan1_raw; - v[1] = packet.chan2_raw; - v[2] = packet.chan3_raw; - v[3] = packet.chan4_raw; - v[4] = packet.chan5_raw; - v[5] = packet.chan6_raw; - v[6] = packet.chan7_raw; - v[7] = packet.chan8_raw; - rc_override_active = APM_RC.setHIL(v); - rc_override_fs_timer = millis(); - break; - } - - case MAVLINK_MSG_ID_HEARTBEAT: - { - // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes - if(msg->sysid != g.sysid_my_gcs) break; - rc_override_fs_timer = millis(); - pmTest1++; - break; - } - - #if HIL_MODE != HIL_MODE_DISABLED - // This is used both as a sensor and to pass the location - // in HIL_ATTITUDE mode. -#ifdef MAVLINK10 - case MAVLINK_MSG_ID_GPS_RAW_INT: - { - // decode - mavlink_gps_raw_int_t packet; - mavlink_msg_gps_raw_int_decode(msg, &packet); - - // set gps hil sensor - g_gps->setHIL(packet.time_usec/1000.0, - packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, - packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 0); - break; - } -#else // MAVLINK10 - case MAVLINK_MSG_ID_GPS_RAW: - { - // decode - mavlink_gps_raw_t packet; - mavlink_msg_gps_raw_decode(msg, &packet); - - // set gps hil sensor - g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, - packet.v,packet.hdg,0,0); - break; - } -#endif // MAVLINK10 - - // Is this resolved? - MAVLink protocol change..... - case MAVLINK_MSG_ID_VFR_HUD: - { - // decode - mavlink_vfr_hud_t packet; - mavlink_msg_vfr_hud_decode(msg, &packet); - - // set airspeed - airspeed = 100*packet.airspeed; - break; - } - -#endif -#if HIL_MODE == HIL_MODE_ATTITUDE - case MAVLINK_MSG_ID_ATTITUDE: - { - // decode - mavlink_attitude_t packet; - mavlink_msg_attitude_decode(msg, &packet); - - // set dcm hil sensor - dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); - break; - } -#endif -#if HIL_MODE == HIL_MODE_SENSORS - - case MAVLINK_MSG_ID_RAW_IMU: - { - // decode - mavlink_raw_imu_t packet; - mavlink_msg_raw_imu_decode(msg, &packet); - - // set imu hil sensors - // TODO: check scaling for temp/absPress - float temp = 70; - float absPress = 1; - //Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc); - //Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro); - - // rad/sec - Vector3f gyros; - gyros.x = (float)packet.xgyro / 1000.0; - gyros.y = (float)packet.ygyro / 1000.0; - gyros.z = (float)packet.zgyro / 1000.0; - // m/s/s - Vector3f accels; - accels.x = (float)packet.xacc / 1000.0; - accels.y = (float)packet.yacc / 1000.0; - accels.z = (float)packet.zacc / 1000.0; - - imu.set_gyro(gyros); - - imu.set_accel(accels); - - compass.setHIL(packet.xmag,packet.ymag,packet.zmag); - break; - } - - case MAVLINK_MSG_ID_RAW_PRESSURE: - { - // decode - mavlink_raw_pressure_t packet; - mavlink_msg_raw_pressure_decode(msg, &packet); - - // set pressure hil sensor - // TODO: check scaling - float temp = 70; - barometer.setHIL(temp,packet.press_diff1 + 101325); - break; - } -#endif // HIL_MODE - } // end switch -} // end handle mavlink - -uint16_t -GCS_MAVLINK::_count_parameters() -{ - // if we haven't cached the parameter count yet... - if (0 == _parameter_count) { - AP_Var *vp; - - vp = AP_Var::first(); - do { - // if a parameter responds to cast_to_float then we are going to be able to report it - if (!isnan(vp->cast_to_float())) { - _parameter_count++; - } - } while (NULL != (vp = vp->next())); - } - return _parameter_count; -} - -AP_Var * -GCS_MAVLINK::_find_parameter(uint16_t index) -{ - AP_Var *vp; - - vp = AP_Var::first(); - while (NULL != vp) { - - // if the parameter is reportable - if (!(isnan(vp->cast_to_float()))) { - // if we have counted down to the index we want - if (0 == index) { - // return the parameter - return vp; - } - // count off this parameter, as it is reportable but not - // the one we want - index--; - } - // and move to the next parameter - vp = vp->next(); - } - return NULL; -} - -/** -* @brief Send the next pending parameter, called from deferred message -* handling code -*/ -void -GCS_MAVLINK::queued_param_send() -{ - // Check to see if we are sending parameters - if (NULL == _queued_parameter) return; - - AP_Var *vp; - float value; - - // copy the current parameter and prepare to move to the next - vp = _queued_parameter; - _queued_parameter = _queued_parameter->next(); - - // if the parameter can be cast to float, report it here and break out of the loop - value = vp->cast_to_float(); - if (!isnan(value)) { - char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK - vp->copy_name(param_name, sizeof(param_name)); - -#ifdef MAVLINK10 - mavlink_msg_param_value_send( - chan, - param_name, - value, - mav_var_type(vp->meta_type_id()), - _queued_parameter_count, - _queued_parameter_index); -#else // MAVLINK10 - mavlink_msg_param_value_send( - chan, - (int8_t*)param_name, - value, - _queued_parameter_count, - _queued_parameter_index); -#endif // MAVLINK10 - - _queued_parameter_index++; - } -} - -/** -* @brief Send the next pending waypoint, called from deferred message -* handling code -*/ -void -GCS_MAVLINK::queued_waypoint_send() -{ - if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.command_total) { - mavlink_msg_waypoint_request_send( - chan, - waypoint_dest_sysid, - waypoint_dest_compid, - waypoint_request_i); - } -} - -/* - a delay() callback that processes MAVLink packets. We set this as the - callback in long running library initialisation routines to allow - MAVLink to process packets while waiting for the initialisation to - complete -*/ -static void mavlink_delay(unsigned long t) -{ - unsigned long tstart; - static unsigned long last_1hz, last_50hz; - - if (in_mavlink_delay) { - // this should never happen, but let's not tempt fate by - // letting the stack grow too much - delay(t); - return; - } - - in_mavlink_delay = true; - - tstart = millis(); - do { - unsigned long tnow = millis(); - if (tnow - last_1hz > 1000) { - last_1hz = tnow; - gcs_send_message(MSG_HEARTBEAT); - gcs_send_message(MSG_EXTENDED_STATUS1); - } - if (tnow - last_50hz > 20) { - last_50hz = tnow; - gcs_update(); - } - delay(1); - } while (millis() - tstart < t); - - in_mavlink_delay = false; -} - -/* - send a message on both GCS links - */ -static void gcs_send_message(enum ap_message id) -{ - gcs0.send_message(id); - gcs3.send_message(id); -} - -/* - send data streams in the given rate range on both links - */ -static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) -{ - gcs0.data_stream_send(freqMin, freqMax); - gcs3.data_stream_send(freqMin, freqMax); -} - -/* - look for incoming commands on the GCS links - */ -static void gcs_update(void) -{ - gcs0.update(); - gcs3.update(); -} - -static void gcs_send_text(gcs_severity severity, const char *str) -{ - gcs0.send_text(severity, str); - gcs3.send_text(severity, str); -} - -static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) -{ - gcs0.send_text(severity, str); - gcs3.send_text(severity, str); -} - -/* - send a low priority formatted message to the GCS - only one fits in the queue, so if you send more than one before the - last one gets into the serial buffer then the old one will be lost - */ -static void gcs_send_text_fmt(const prog_char_t *fmt, ...) -{ - char fmtstr[40]; - va_list ap; - uint8_t i; - for (i=0; i dump log \n" - " erase erase all logs\n" - " enable |all enable logging or everything\n" - " disable |all disable logging or everything\n" - "\n")); - return 0; -} - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -static const struct Menu::command log_menu_commands[] PROGMEM = { - {"dump", dump_log}, - {"erase", erase_logs}, - {"enable", select_logs}, - {"disable", select_logs}, - {"help", help_log} -}; - -// A Macro to create the Menu -MENU2(log_menu, "Log", log_menu_commands, print_log_menu); - -static void get_log_boundaries(byte log_num, int & start_page, int & end_page); - -static bool -print_log_menu(void) -{ - int log_start; - int log_end; - byte last_log_num = get_num_logs(); - - Serial.printf_P(PSTR("logs enabled: ")); - if (0 == g.log_bitmask) { - Serial.printf_P(PSTR("none")); - }else{ - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // the bit being set and print the name of the log option to suit. - #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) - PLOG(ATTITUDE_FAST); - PLOG(ATTITUDE_MED); - PLOG(GPS); - PLOG(PM); - PLOG(CTUN); - PLOG(NTUN); - PLOG(MODE); - PLOG(RAW); - PLOG(CMD); - PLOG(CUR); - #undef PLOG - } - Serial.println(); - - if (last_log_num == 0) { - Serial.printf_P(PSTR("\nNo logs available for download\n")); - }else{ - - Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); - for(int i=1;i last_log_num)) { - Serial.printf_P(PSTR("bad log number\n")); - return(-1); - } - - get_log_boundaries(dump_log, dump_log_start, dump_log_end); - Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), - dump_log, - dump_log_start, - dump_log_end); - - Log_Read(dump_log_start, dump_log_end); - Serial.printf_P(PSTR("Log read complete\n")); - return 0; -} - -static int8_t -erase_logs(uint8_t argc, const Menu::arg *argv) -{ - for(int i = 10 ; i > 0; i--) { - Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); - delay(1000); - } - Serial.printf_P(PSTR("\nErasing log...\n")); - for(int j = 1; j < 4096; j++) - DataFlash.PageErase(j); - DataFlash.StartWrite(1); - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_INDEX_MSG); - DataFlash.WriteByte(0); - DataFlash.WriteByte(END_BYTE); - DataFlash.FinishWrite(); - Serial.printf_P(PSTR("\nLog erased.\n")); - return 0; -} - -static int8_t -select_logs(uint8_t argc, const Menu::arg *argv) -{ - uint16_t bits; - - if (argc != 2) { - Serial.printf_P(PSTR("missing log type\n")); - return(-1); - } - - bits = 0; - - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // that name as the argument to the command, and set the bit in - // bits accordingly. - // - if (!strcasecmp_P(argv[1].str, PSTR("all"))) { - bits = ~0; - } else { - #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s - TARG(ATTITUDE_FAST); - TARG(ATTITUDE_MED); - TARG(GPS); - TARG(PM); - TARG(CTUN); - TARG(NTUN); - TARG(MODE); - TARG(RAW); - TARG(CMD); - TARG(CUR); - #undef TARG - } - - if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { - g.log_bitmask.set_and_save(g.log_bitmask | bits); - }else{ - g.log_bitmask.set_and_save(g.log_bitmask & ~bits); - } - return(0); -} - -static int8_t -process_logs(uint8_t argc, const Menu::arg *argv) -{ - log_menu.run(); - return 0; -} - - -static byte get_num_logs(void) -{ - int page = 1; - byte data; - byte log_step = 0; - - DataFlash.StartRead(1); - - while (page == 1) { - data = DataFlash.ReadByte(); - - switch(log_step){ //This is a state machine to read the packets - case 0: - if(data==HEAD_BYTE1) // Head byte 1 - log_step++; - break; - - case 1: - if(data==HEAD_BYTE2) // Head byte 2 - log_step++; - else - log_step = 0; - break; - - case 2: - if(data==LOG_INDEX_MSG){ - byte num_logs = DataFlash.ReadByte(); - return num_logs; - }else{ - log_step=0; // Restart, we have a problem... - } - break; - } - page = DataFlash.GetPage(); - } - return 0; -} - -// send the number of the last log? -static void start_new_log(byte num_existing_logs) -{ - int start_pages[50] = {0,0,0}; - int end_pages[50] = {0,0,0}; - - if(num_existing_logs > 0) { - for(int i=0;i 0) - start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1; - else - start_pages[0] = 2; - num_existing_logs++; - DataFlash.StartWrite(1); - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_INDEX_MSG); - DataFlash.WriteByte(num_existing_logs); - for(int i=0;i Logs full - logging discontinued")); - } -} - -static void get_log_boundaries(byte log_num, int & start_page, int & end_page) -{ - int page = 1; - byte data; - byte log_step = 0; - - DataFlash.StartRead(1); - while (page == 1) { - data = DataFlash.ReadByte(); - switch(log_step) //This is a state machine to read the packets - { - case 0: - if(data==HEAD_BYTE1) // Head byte 1 - log_step++; - break; - case 1: - if(data==HEAD_BYTE2) // Head byte 2 - log_step++; - else - log_step = 0; - break; - case 2: - if(data==LOG_INDEX_MSG){ - byte num_logs = DataFlash.ReadByte(); - for(int i=0;i 1) { - look_page = (top_page + bottom_page) / 2; - DataFlash.StartRead(look_page); - check = DataFlash.ReadLong(); - if(check == (long)0xFFFFFFFF) - top_page = look_page; - else - bottom_page = look_page; - } - return top_page; -} - - -// Write an attitude packet. Total length : 10 bytes -static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_ATTITUDE_MSG); - DataFlash.WriteInt(log_roll); - DataFlash.WriteInt(log_pitch); - DataFlash.WriteInt(log_yaw); - DataFlash.WriteByte(END_BYTE); -} - -// Write a performance monitoring packet. Total length : 19 bytes -static void Log_Write_Performance() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - DataFlash.WriteLong(millis()- perf_mon_timer); - DataFlash.WriteInt(mainLoop_count); - DataFlash.WriteInt(G_Dt_max); - DataFlash.WriteByte(dcm.gyro_sat_count); - DataFlash.WriteByte(imu.adc_constraints); - DataFlash.WriteByte(dcm.renorm_sqrt_count); - DataFlash.WriteByte(dcm.renorm_blowup_count); - DataFlash.WriteByte(gps_fix_count); - DataFlash.WriteInt((int)(dcm.get_health() * 1000)); - DataFlash.WriteInt(pmTest1); - DataFlash.WriteByte(END_BYTE); -} - -// Write a command processing packet. Total length : 19 bytes -//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng) -static void Log_Write_Cmd(byte num, struct Location *wp) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CMD_MSG); - DataFlash.WriteByte(num); - DataFlash.WriteByte(wp->id); - DataFlash.WriteByte(wp->p1); - DataFlash.WriteLong(wp->alt); - DataFlash.WriteLong(wp->lat); - DataFlash.WriteLong(wp->lng); - DataFlash.WriteByte(END_BYTE); -} - -static void Log_Write_Startup(byte type) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_STARTUP_MSG); - DataFlash.WriteByte(type); - DataFlash.WriteByte(g.command_total); - DataFlash.WriteByte(END_BYTE); - - // create a location struct to hold the temp Waypoints for printing - struct Location cmd = get_cmd_with_index(0); - Log_Write_Cmd(0, &cmd); - - for (int i = 1; i <= g.command_total; i++){ - cmd = get_cmd_with_index(i); - Log_Write_Cmd(i, &cmd); - } -} - - -// Write a control tuning packet. Total length : 22 bytes -#if HIL_MODE != HIL_MODE_ATTITUDE -static void Log_Write_Control_Tuning() -{ - Vector3f accel = imu.get_accel(); - - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); - DataFlash.WriteInt((int)(g.channel_roll.servo_out)); - DataFlash.WriteInt((int)nav_roll); - DataFlash.WriteInt((int)dcm.roll_sensor); - DataFlash.WriteInt((int)(g.channel_pitch.servo_out)); - DataFlash.WriteInt((int)nav_pitch); - DataFlash.WriteInt((int)dcm.pitch_sensor); - DataFlash.WriteInt((int)(g.channel_throttle.servo_out)); - DataFlash.WriteInt((int)(g.channel_rudder.servo_out)); - DataFlash.WriteInt((int)(accel.y * 10000)); - DataFlash.WriteByte(END_BYTE); -} -#endif - -// Write a navigation tuning packet. Total length : 18 bytes -static void Log_Write_Nav_Tuning() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_NAV_TUNING_MSG); - DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); - DataFlash.WriteInt((int)wp_distance); - DataFlash.WriteInt((uint16_t)target_bearing); - DataFlash.WriteInt((uint16_t)nav_bearing); - DataFlash.WriteInt(altitude_error); - DataFlash.WriteInt((int)airspeed); - DataFlash.WriteInt((int)(nav_gain_scaler*1000)); - DataFlash.WriteByte(END_BYTE); -} - -// Write a mode packet. Total length : 5 bytes -static void Log_Write_Mode(byte mode) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_MODE_MSG); - DataFlash.WriteByte(mode); - DataFlash.WriteByte(END_BYTE); -} - -// Write an GPS packet. Total length : 30 bytes -static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, - long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_GPS_MSG); - DataFlash.WriteLong(log_Time); - DataFlash.WriteByte(log_Fix); - DataFlash.WriteByte(log_NumSats); - DataFlash.WriteLong(log_Lattitude); - DataFlash.WriteLong(log_Longitude); - DataFlash.WriteInt(sonar_alt); // This one is just temporary for testing out sonar in fixed wing - DataFlash.WriteLong(log_mix_alt); - DataFlash.WriteLong(log_gps_alt); - DataFlash.WriteLong(log_Ground_Speed); - DataFlash.WriteLong(log_Ground_Course); - DataFlash.WriteByte(END_BYTE); -} - -// Write an raw accel/gyro data packet. Total length : 28 bytes -#if HIL_MODE != HIL_MODE_ATTITUDE -static void Log_Write_Raw() -{ - Vector3f gyro = imu.get_gyro(); - Vector3f accel = imu.get_accel(); - gyro *= t7; // Scale up for storage as long integers - accel *= t7; - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_RAW_MSG); - - DataFlash.WriteLong((long)gyro.x); - DataFlash.WriteLong((long)gyro.y); - DataFlash.WriteLong((long)gyro.z); - DataFlash.WriteLong((long)accel.x); - DataFlash.WriteLong((long)accel.y); - DataFlash.WriteLong((long)accel.z); - - DataFlash.WriteByte(END_BYTE); -} -#endif - -static void Log_Write_Current() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CURRENT_MSG); - DataFlash.WriteInt(g.channel_throttle.control_in); - DataFlash.WriteInt((int)(battery_voltage * 100.0)); - DataFlash.WriteInt((int)(current_amps * 100.0)); - DataFlash.WriteInt((int)current_total); - DataFlash.WriteByte(END_BYTE); -} - -// Read a Current packet -static void Log_Read_Current() -{ - Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"), - DataFlash.ReadInt(), - ((float)DataFlash.ReadInt() / 100.f), - ((float)DataFlash.ReadInt() / 100.f), - DataFlash.ReadInt()); -} - -// Read an control tuning packet -static void Log_Read_Control_Tuning() -{ - float logvar; - - Serial.printf_P(PSTR("CTUN:")); - for (int y = 1; y < 10; y++) { - logvar = DataFlash.ReadInt(); - if(y < 8) logvar = logvar/100.f; - if(y == 9) logvar = logvar/10000.f; - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -// Read a nav tuning packet -static void Log_Read_Nav_Tuning() -{ - Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n - (float)((uint16_t)DataFlash.ReadInt())/100.0, - DataFlash.ReadInt(), - (float)((uint16_t)DataFlash.ReadInt())/100.0, - (float)((uint16_t)DataFlash.ReadInt())/100.0, - (float)DataFlash.ReadInt()/100.0, - (float)DataFlash.ReadInt()/100.0, - (float)DataFlash.ReadInt()/1000.0); -} - -// Read a performance packet -static void Log_Read_Performance() -{ - long pm_time; - int logvar; - - Serial.printf_P(PSTR("PM:")); - pm_time = DataFlash.ReadLong(); - Serial.print(pm_time); - Serial.print(comma); - for (int y = 1; y <= 9; y++) { - if(y < 3 || y > 7){ - logvar = DataFlash.ReadInt(); - }else{ - logvar = DataFlash.ReadByte(); - } - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -// Read a command processing packet -static void Log_Read_Cmd() -{ - byte logvarb; - long logvarl; - - Serial.printf_P(PSTR("CMD:")); - for(int i = 1; i < 4; i++) { - logvarb = DataFlash.ReadByte(); - Serial.print(logvarb, DEC); - Serial.print(comma); - } - for(int i = 1; i < 4; i++) { - logvarl = DataFlash.ReadLong(); - Serial.print(logvarl, DEC); - Serial.print(comma); - } - Serial.println(" "); -} - -static void Log_Read_Startup() -{ - byte logbyte = DataFlash.ReadByte(); - - if (logbyte == TYPE_AIRSTART_MSG) - Serial.printf_P(PSTR("AIR START - ")); - else if (logbyte == TYPE_GROUNDSTART_MSG) - Serial.printf_P(PSTR("GROUND START - ")); - else - Serial.printf_P(PSTR("UNKNOWN STARTUP - ")); - - Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte()); -} - -// Read an attitude packet -static void Log_Read_Attitude() -{ - Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), - DataFlash.ReadInt(), - DataFlash.ReadInt(), - (uint16_t)DataFlash.ReadInt()); -} - -// Read a mode packet -static void Log_Read_Mode() -{ - Serial.printf_P(PSTR("MOD:")); - Serial.println(flight_mode_strings[DataFlash.ReadByte()]); -} - -// Read a GPS packet -static void Log_Read_GPS() -{ - Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"), - DataFlash.ReadLong(), - (int)DataFlash.ReadByte(), - (int)DataFlash.ReadByte(), - (float)DataFlash.ReadLong() / t7, - (float)DataFlash.ReadLong() / t7, - (float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing - (float)DataFlash.ReadLong() / 100.0, - (float)DataFlash.ReadLong() / 100.0, - (float)DataFlash.ReadLong() / 100.0, - (float)DataFlash.ReadLong() / 100.0); - -} - -// Read a raw accel/gyro packet -static void Log_Read_Raw() -{ - float logvar; - Serial.printf_P(PSTR("RAW:")); - for (int y = 0; y < 6; y++) { - logvar = (float)DataFlash.ReadLong() / t7; - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -// Read the DataFlash log memory : Packet Parser -static void Log_Read(int start_page, int end_page) -{ - byte data; - byte log_step = 0; - int packet_count = 0; - int page = start_page; - - #ifdef AIRFRAME_NAME - Serial.printf_P(PSTR((AIRFRAME_NAME) - #endif - Serial.printf_P(PSTR("\n" THISFIRMWARE - "\nFree RAM: %u\n"), - memcheck_available_memory()); - - DataFlash.StartRead(start_page); - while (page < end_page && page != -1){ - data = DataFlash.ReadByte(); - switch(log_step) // This is a state machine to read the packets - { - case 0: - if(data == HEAD_BYTE1) // Head byte 1 - log_step++; - break; - case 1: - if(data == HEAD_BYTE2) // Head byte 2 - log_step++; - else - log_step = 0; - break; - case 2: - if(data == LOG_ATTITUDE_MSG){ - Log_Read_Attitude(); - log_step++; - - }else if(data == LOG_MODE_MSG){ - Log_Read_Mode(); - log_step++; - - }else if(data == LOG_CONTROL_TUNING_MSG){ - Log_Read_Control_Tuning(); - log_step++; - - }else if(data == LOG_NAV_TUNING_MSG){ - Log_Read_Nav_Tuning(); - log_step++; - - }else if(data == LOG_PERFORMANCE_MSG){ - Log_Read_Performance(); - log_step++; - - }else if(data == LOG_RAW_MSG){ - Log_Read_Raw(); - log_step++; - - }else if(data == LOG_CMD_MSG){ - Log_Read_Cmd(); - log_step++; - - }else if(data == LOG_CURRENT_MSG){ - Log_Read_Current(); - log_step++; - - }else if(data == LOG_STARTUP_MSG){ - Log_Read_Startup(); - log_step++; - }else { - if(data == LOG_GPS_MSG){ - Log_Read_GPS(); - log_step++; - }else{ - Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count); - log_step = 0; // Restart, we have a problem... - } - } - break; - case 3: - if(data == END_BYTE){ - packet_count++; - }else{ - Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data); - } - log_step = 0; // Restart sequence: new packet... - break; - } - page = DataFlash.GetPage(); - } - Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); -} - -#else // LOGGING_ENABLED - -// dummy functions -static void Log_Write_Mode(byte mode) {} -static void Log_Write_Startup(byte type) {} -static void Log_Write_Cmd(byte num, struct Location *wp) {} -static void Log_Write_Current() {} -static void Log_Write_Nav_Tuning() {} -static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, - long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {} -static void Log_Write_Performance() {} -static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -static byte get_num_logs(void) { return 0; } -static void start_new_log(byte num_existing_logs) {} -static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {} -static void Log_Write_Control_Tuning() {} -static void Log_Write_Raw() {} - - -#endif // LOGGING_ENABLED -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/climb_rate.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -struct DataPoint { - unsigned long x; - long y; -}; - -DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from -unsigned char hindex; // Index in history for the current data point - -unsigned long xoffset; -unsigned char n; - -// Intermediate variables for regression calculation -long xi; -long yi; -long xiyi; -unsigned long xi2; - -#if 0 // currently unused -static void add_altitude_data(unsigned long xl, long y) -{ - //Reset the regression if our X variable overflowed - if (xl < xoffset) - n = 0; - - //To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length - if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH) - n = 0; - - if (n == ALTITUDE_HISTORY_LENGTH) { - xi -= history[hindex].x; - yi -= history[hindex].y; - xiyi -= (long)history[hindex].x * history[hindex].y; - xi2 -= history[hindex].x * history[hindex].x; - } else { - if (n == 0) { - xoffset = xl; - xi = 0; - yi = 0; - xiyi = 0; - xi2 = 0; - } - n++; - } - - history[hindex].x = xl - xoffset; - history[hindex].y = y; - - xi += history[hindex].x; - yi += history[hindex].y; - xiyi += (long)history[hindex].x * history[hindex].y; - xi2 += history[hindex].x * history[hindex].x; - - if (++hindex >= ALTITUDE_HISTORY_LENGTH) - hindex = 0; -} -#endif - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* Functions in this file: - void init_commands() - void update_auto() - void reload_commands_airstart() - struct Location get_cmd_with_index(int i) - void set_cmd_with_index(struct Location temp, int i) - void increment_cmd_index() - void decrement_cmd_index() - long read_alt_to_hold() - void set_next_WP(struct Location *wp) - void set_guided_WP(void) - void init_home() -************************************************************ -*/ - -static void init_commands() -{ - g.command_index.set_and_save(0); - nav_command_ID = NO_COMMAND; - non_nav_command_ID = NO_COMMAND; - next_nav_command.id = CMD_BLANK; -} - -static void update_auto() -{ - if (g.command_index >= g.command_total) { - handle_no_commands(); - if(g.command_total == 0) { - next_WP.lat = home.lat + 1000; // so we don't have bad calcs - next_WP.lng = home.lng + 1000; // so we don't have bad calcs - } - } else { - if(g.command_index != 0) { - g.command_index = nav_command_index; - nav_command_index--; - } - nav_command_ID = NO_COMMAND; - non_nav_command_ID = NO_COMMAND; - next_nav_command.id = CMD_BLANK; - process_next_command(); - } -} - -// this is only used by an air-start -static void reload_commands_airstart() -{ - init_commands(); - g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all? - decrement_cmd_index(); -} - -// Getters -// ------- -static struct Location get_cmd_with_index(int i) -{ - struct Location temp; - long mem; - - // Find out proper location in memory by using the start_byte position + the index - // -------------------------------------------------------------------------------- - if (i > g.command_total) { - temp.id = CMD_BLANK; - }else{ - // read WP position - mem = (WP_START_BYTE) + (i * WP_SIZE); - temp.id = eeprom_read_byte((uint8_t*)mem); - - mem++; - temp.options = eeprom_read_byte((uint8_t*)mem); - - mem++; - temp.p1 = eeprom_read_byte((uint8_t*)mem); - - mem++; - temp.alt = (long)eeprom_read_dword((uint32_t*)mem); - - mem += 4; - temp.lat = (long)eeprom_read_dword((uint32_t*)mem); - - mem += 4; - temp.lng = (long)eeprom_read_dword((uint32_t*)mem); - } - - // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative - if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){ - temp.alt += home.alt; - } - - return temp; -} - -// Setters -// ------- -static void set_cmd_with_index(struct Location temp, int i) -{ - i = constrain(i, 0, g.command_total.get()); - uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - - // Set altitude options bitmask - // XXX What is this trying to do? - if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){ - temp.options = MASK_OPTIONS_RELATIVE_ALT; - } else { - temp.options = 0; - } - - eeprom_write_byte((uint8_t *) mem, temp.id); - - mem++; - eeprom_write_byte((uint8_t *) mem, temp.options); - - mem++; - eeprom_write_byte((uint8_t *) mem, temp.p1); - - mem++; - eeprom_write_dword((uint32_t *) mem, temp.alt); - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lat); - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lng); -} - -static void increment_cmd_index() -{ - if (g.command_index <= g.command_total) { - g.command_index.set_and_save(g.command_index + 1); - } -} - -static void decrement_cmd_index() -{ - if (g.command_index > 0) { - g.command_index.set_and_save(g.command_index - 1); - } -} - -static long read_alt_to_hold() -{ - if(g.RTL_altitude < 0) - return current_loc.alt; - else - return g.RTL_altitude + home.alt; -} - - -/* -This function stores waypoint commands -It looks to see what the next command type is and finds the last command. -*/ -static void set_next_WP(struct Location *wp) -{ - // copy the current WP into the OldWP slot - // --------------------------------------- - prev_WP = next_WP; - - // Load the next_WP slot - // --------------------- - next_WP = *wp; - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - target_altitude = current_loc.alt; - - if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) - offset_altitude = next_WP.alt - prev_WP.alt; - else - offset_altitude = 0; - - // zero out our loiter vals to watch for missed waypoints - loiter_delta = 0; - loiter_sum = 0; - loiter_total = 0; - - // this is used to offset the shrinking longitude as we go towards the poles - float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); - // this is handy for the groundstation - wp_totalDistance = get_distance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - target_bearing = get_bearing(¤t_loc, &next_WP); - nav_bearing = target_bearing; - - // to check if we have missed the WP - // ---------------------------- - old_target_bearing = target_bearing; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); -} - -static void set_guided_WP(void) -{ - // copy the current location into the OldWP slot - // --------------------------------------- - prev_WP = current_loc; - - // Load the next_WP slot - // --------------------- - next_WP = guided_WP; - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - target_altitude = current_loc.alt; - offset_altitude = next_WP.alt - prev_WP.alt; - - // this is used to offset the shrinking longitude as we go towards the poles - float rads = (abs(next_WP.lat)/t7) * 0.0174532925; - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); - - // this is handy for the groundstation - wp_totalDistance = get_distance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - target_bearing = get_bearing(¤t_loc, &next_WP); - - // to check if we have missed the WP - // ---------------------------- - old_target_bearing = target_bearing; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); -} - -// run this at setup on the ground -// ------------------------------- -void init_home() -{ - gcs_send_text_P(SEVERITY_LOW, PSTR("init home")); - - // block until we get a good fix - // ----------------------------- - while (!g_gps->new_data || !g_gps->fix) { - g_gps->update(); - } - - home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = g_gps->longitude; // Lon * 10**7 - home.lat = g_gps->latitude; // Lat * 10**7 - home.alt = max(g_gps->altitude, 0); - home_is_set = true; - - gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); - - // Save Home to EEPROM - Command 0 - // ------------------- - set_cmd_with_index(home, 0); - - // Save prev loc - // ------------- - next_WP = prev_WP = home; - - // Load home for a default guided_WP - // ------------- - guided_WP = home; - guided_WP.alt += g.RTL_altitude; - -} - - - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands_logic.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/********************************************************************************/ -// Command Event Handlers -/********************************************************************************/ -static void -handle_process_nav_cmd() -{ - // reset navigation integrators - // ------------------------- - reset_I(); - - gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); - switch(next_nav_command.id){ - - case MAV_CMD_NAV_TAKEOFF: - do_takeoff(); - break; - - case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint - do_nav_wp(); - break; - - case MAV_CMD_NAV_LAND: // LAND to Waypoint - do_land(); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely - do_loiter_unlimited(); - break; - - case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times - do_loiter_turns(); - break; - - case MAV_CMD_NAV_LOITER_TIME: - do_loiter_time(); - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - do_RTL(); - break; - - default: - break; - } -} - -static void -handle_process_condition_command() -{ - gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); - switch(next_nonnav_command.id){ - - case MAV_CMD_CONDITION_DELAY: - do_wait_delay(); - break; - - case MAV_CMD_CONDITION_DISTANCE: - do_within_distance(); - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - do_change_alt(); - break; - - /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also) - gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set")); - - // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - landing_pitch = next_nav_command.lng * 100; - g.airspeed_cruise = next_nav_command.alt * 100; - g.throttle_cruise = next_nav_command.lat; - landing_distance = next_nav_command.p1; - - SendDebug_P("MSG: throttle_cruise = "); - SendDebugln(g.throttle_cruise,DEC); - break; - */ - - default: - break; - } -} - -static void handle_process_do_command() -{ - gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); - switch(next_nonnav_command.id){ - - case MAV_CMD_DO_JUMP: - do_jump(); - break; - - case MAV_CMD_DO_CHANGE_SPEED: - do_change_speed(); - break; - - case MAV_CMD_DO_SET_HOME: - do_set_home(); - break; - - case MAV_CMD_DO_SET_SERVO: - do_set_servo(); - break; - - case MAV_CMD_DO_SET_RELAY: - do_set_relay(); - break; - - case MAV_CMD_DO_REPEAT_SERVO: - do_repeat_servo(); - break; - - case MAV_CMD_DO_REPEAT_RELAY: - do_repeat_relay(); - break; - } -} - -static void handle_no_commands() -{ - gcs_send_text_fmt(PSTR("Returning to Home")); - next_nav_command = home; - next_nav_command.alt = read_alt_to_hold(); - next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; - nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM; - non_nav_command_ID = WAIT_COMMAND; - handle_process_nav_cmd(); - -} - -/********************************************************************************/ -// Verify command Handlers -/********************************************************************************/ - -static bool verify_nav_command() // Returns true if command complete -{ - switch(nav_command_ID) { - - case MAV_CMD_NAV_TAKEOFF: - return verify_takeoff(); - break; - - case MAV_CMD_NAV_LAND: - return verify_land(); - break; - - case MAV_CMD_NAV_WAYPOINT: - return verify_nav_wp(); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: - return verify_loiter_unlim(); - break; - - case MAV_CMD_NAV_LOITER_TURNS: - return verify_loiter_turns(); - break; - - case MAV_CMD_NAV_LOITER_TIME: - return verify_loiter_time(); - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - return verify_RTL(); - break; - - default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd")); - return false; - break; - } -} - -static bool verify_condition_command() // Returns true if command complete -{ - switch(non_nav_command_ID) { - case NO_COMMAND: - break; - - case MAV_CMD_CONDITION_DELAY: - return verify_wait_delay(); - break; - - case MAV_CMD_CONDITION_DISTANCE: - return verify_within_distance(); - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - return verify_change_alt(); - break; - - case WAIT_COMMAND: - return 0; - break; - - - default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); - break; - } - return false; -} - -/********************************************************************************/ -// Nav (Must) commands -/********************************************************************************/ - -static void do_RTL(void) -{ - control_mode = RTL; - crash_timer = 0; - next_WP = home; - - // Altitude to hold over home - // Set by configuration tool - // ------------------------- - next_WP.alt = read_alt_to_hold(); - - if (g.log_bitmask & MASK_LOG_MODE) - Log_Write_Mode(control_mode); -} - -static void do_takeoff() -{ - set_next_WP(&next_nav_command); - // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - takeoff_pitch = (int)next_nav_command.p1 * 100; - //Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch); - //Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt); - takeoff_altitude = next_nav_command.alt; - //Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude); - next_WP.lat = home.lat + 1000; // so we don't have bad calcs - next_WP.lng = home.lng + 1000; // so we don't have bad calcs - takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction - // Flag also used to override "on the ground" throttle disable -} - -static void do_nav_wp() -{ - set_next_WP(&next_nav_command); -} - -static void do_land() -{ - set_next_WP(&next_nav_command); -} - -static void do_loiter_unlimited() -{ - set_next_WP(&next_nav_command); -} - -static void do_loiter_turns() -{ - set_next_WP(&next_nav_command); - loiter_total = next_nav_command.p1 * 360; -} - -static void do_loiter_time() -{ - set_next_WP(&next_nav_command); - loiter_time = millis(); - loiter_time_max = next_nav_command.p1; // units are (seconds * 10) -} - -/********************************************************************************/ -// Verify Nav (Must) commands -/********************************************************************************/ -static bool verify_takeoff() -{ - if (g_gps->ground_speed > 300){ - if(hold_course == -1){ - // save our current course to take off - if(g.compass_enabled) { - hold_course = dcm.yaw_sensor; - } else { - hold_course = g_gps->ground_course; - } - } - } - - if(hold_course > -1){ - // recalc bearing error with hold_course; - nav_bearing = hold_course; - // recalc bearing error - calc_bearing_error(); - } - - if (current_loc.alt > takeoff_altitude) { - hold_course = -1; - takeoff_complete = true; - return true; - } else { - return false; - } -} - -static bool verify_land() -{ - // we don't verify landing - we never go to a new Nav command after Land - if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) - || (current_loc.alt <= next_WP.alt + 300)){ - - land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude - - if(hold_course == -1){ - // save our current course to land - //hold_course = yaw_sensor; - // save the course line of the runway to land - hold_course = crosstrack_bearing; - } - } - - if(hold_course > -1){ - // recalc bearing error with hold_course; - nav_bearing = hold_course; - // recalc bearing error - calc_bearing_error(); - } - - update_crosstrack(); - return false; -} - -static bool verify_nav_wp() -{ - hold_course = -1; - update_crosstrack(); - if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { - gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index); - return true; - } - // add in a more complex case - // Doug to do - if(loiter_sum > 300){ - gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP")); - return true; - } - return false; -} - -static bool verify_loiter_unlim() -{ - update_loiter(); - calc_bearing_error(); - return false; -} - -static bool verify_loiter_time() -{ - update_loiter(); - calc_bearing_error(); - if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete")); - return true; - } - return false; -} - -static bool verify_loiter_turns() -{ - update_loiter(); - calc_bearing_error(); - if(loiter_sum > loiter_total) { - loiter_total = 0; - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete")); - // clear the command queue; - return true; - } - return false; -} - -static bool verify_RTL() -{ - if (wp_distance <= g.waypoint_radius) { - gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); - return true; - }else{ - return false; - } -} - -/********************************************************************************/ -// Condition (May) commands -/********************************************************************************/ - -static void do_wait_delay() -{ - condition_start = millis(); - condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds -} - -static void do_change_alt() -{ - condition_rate = next_nonnav_command.lat; - condition_value = next_nonnav_command.alt; - target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update - next_WP.alt = condition_value; // For future nav calculations - offset_altitude = 0; // For future nav calculations -} - -static void do_within_distance() -{ - condition_value = next_nonnav_command.lat; -} - -/********************************************************************************/ -// Verify Condition (May) commands -/********************************************************************************/ - -static bool verify_wait_delay() -{ - if ((unsigned)(millis() - condition_start) > condition_value){ - condition_value = 0; - return true; - } - return false; -} - -static bool verify_change_alt() -{ - if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) { - condition_value = 0; - return true; - } - target_altitude += condition_rate / 10; - return false; -} - -static bool verify_within_distance() -{ - if (wp_distance < condition_value){ - condition_value = 0; - return true; - } - return false; -} - -/********************************************************************************/ -// Do (Now) commands -/********************************************************************************/ - -static void do_loiter_at_location() -{ - next_WP = current_loc; -} - -static void do_jump() -{ - struct Location temp; - if(next_nonnav_command.lat > 0) { - - nav_command_ID = NO_COMMAND; - non_nav_command_ID = NO_COMMAND; - temp = get_cmd_with_index(g.command_index); - temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter - - set_cmd_with_index(temp, g.command_index); - g.command_index.set_and_save(next_nonnav_command.p1 - 1); - } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever - nav_command_ID = NO_COMMAND; - non_nav_command_ID = NO_COMMAND; - g.command_index.set_and_save(next_nonnav_command.p1 - 1); - } -} - -static void do_change_speed() -{ - // Note: we have no implementation for commanded ground speed, only air speed and throttle - if(next_nonnav_command.alt > 0) - g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100); - - if(next_nonnav_command.lat > 0) - g.throttle_cruise.set_and_save(next_nonnav_command.lat); -} - -static void do_set_home() -{ - if(next_nonnav_command.p1 == 1 && GPS_enabled) { - init_home(); - } else { - home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = next_nonnav_command.lng; // Lon * 10**7 - home.lat = next_nonnav_command.lat; // Lat * 10**7 - home.alt = max(next_nonnav_command.alt, 0); - home_is_set = true; - } -} - -static void do_set_servo() -{ - APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt); -} - -static void do_set_relay() -{ - if (next_nonnav_command.p1 == 1) { - relay.on(); - } else if (next_nonnav_command.p1 == 0) { - relay.off(); - }else{ - relay.toggle(); - } -} - -static void do_repeat_servo() -{ - event_id = next_nonnav_command.p1 - 1; - - if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) { - - event_timer = 0; - event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_nonnav_command.lat * 2; - event_value = next_nonnav_command.alt; - - switch(next_nonnav_command.p1) { - case CH_5: - event_undo_value = g.rc_5.radio_trim; - break; - case CH_6: - event_undo_value = g.rc_6.radio_trim; - break; - case CH_7: - event_undo_value = g.rc_7.radio_trim; - break; - case CH_8: - event_undo_value = g.rc_8.radio_trim; - break; - } - update_events(); - } -} - -static void do_repeat_relay() -{ - event_id = RELAY_TOGGLE; - event_timer = 0; - event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_nonnav_command.alt * 2; - update_events(); -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/commands_process.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// For changing active command mid-mission -//---------------------------------------- -static void change_command(uint8_t cmd_index) -{ - struct Location temp = get_cmd_with_index(cmd_index); - if (temp.id > MAV_CMD_NAV_LAST ){ - gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); - } else { - gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index); - nav_command_ID = NO_COMMAND; - next_nav_command.id = NO_COMMAND; - non_nav_command_ID = NO_COMMAND; - nav_command_index = cmd_index - 1; - g.command_index.set_and_save(cmd_index - 1); - process_next_command(); - } -} - - -// called by 10 Hz loop -// -------------------- -static void update_commands(void) -{ - if(home_is_set == false){ - return; // don't do commands - } - - if(control_mode == AUTO){ - process_next_command(); - } // Other (eg GCS_Auto) modes may be implemented here -} - -static void verify_commands(void) -{ - if(verify_nav_command()){ - nav_command_ID = NO_COMMAND; - } - - if(verify_condition_command()){ - non_nav_command_ID = NO_COMMAND; - } -} - - -static void process_next_command() -{ - // This function makes sure that we always have a current navigation command - // and loads conditional or immediate commands if applicable - - struct Location temp; - byte old_index; - - // these are Navigation/Must commands - // --------------------------------- - if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded - old_index = nav_command_index; - temp.id = MAV_CMD_NAV_LAST; - while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { - nav_command_index++; - temp = get_cmd_with_index(nav_command_index); - } - gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index); - if(nav_command_index > g.command_total){ - // we are out of commands! - gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); - handle_no_commands(); - } else { - next_nav_command = temp; - nav_command_ID = next_nav_command.id; - non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded - non_nav_command_ID = NO_COMMAND; - - if (g.log_bitmask & MASK_LOG_CMD) { - Log_Write_Cmd(g.command_index, &next_nav_command); - } - process_nav_cmd(); - } - } - - // these are Condition/May and Do/Now commands - // ------------------------------------------- - if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command - non_nav_command_index = old_index + 1; - //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); - } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command - non_nav_command_index++; - } - - //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index); - //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); - //gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID); - if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) { - temp = get_cmd_with_index(non_nav_command_index); - if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do - g.command_index.set_and_save(nav_command_index); - non_nav_command_index = nav_command_index; - non_nav_command_ID = WAIT_COMMAND; - gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); - } else { // The next command is a non-nav command. Prepare to execute it. - g.command_index.set_and_save(non_nav_command_index); - next_nonnav_command = temp; - non_nav_command_ID = next_nonnav_command.id; - gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); - if (g.log_bitmask & MASK_LOG_CMD) { - Log_Write_Cmd(g.command_index, &next_nonnav_command); - } - - process_non_nav_command(); - } - - } -} - -/**************************************************/ -// These functions implement the commands. -/**************************************************/ -static void process_nav_cmd() -{ - //gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); - - // clear non-nav command ID and index - non_nav_command_index = NO_COMMAND; // Redundant - remove? - non_nav_command_ID = NO_COMMAND; // Redundant - remove? - - handle_process_nav_cmd(); - -} - -static void process_non_nav_command() -{ - //gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); - - if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { - handle_process_condition_command(); - } else { - handle_process_do_command(); - // flag command ID so a new one is loaded - // ----------------------------------------- - non_nav_command_ID = NO_COMMAND; - } -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/control_modes.pde" -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -static void read_control_switch() -{ - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - - set_mode(flight_modes[switchPosition]); - - oldSwitchPosition = switchPosition; - prev_WP = current_loc; - - // reset navigation integrators - // ------------------------- - reset_I(); - } - - if (g.inverted_flight_ch != 0) { - // if the user has configured an inverted flight channel, then - // fly upside down when that channel goes above INVERTED_FLIGHT_PWM - inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); - } -} - -static byte readSwitch(void){ - uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); - if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; - if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; - if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; - if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual - if (pulsewidth >= 1750) return 5; // Hardware Manual - return 0; -} - -static void reset_control_switch() -{ - oldSwitchPosition = 0; - read_control_switch(); -} - -static void update_servo_switches() -{ - if (!g.switch_enable) { - // switches are disabled in EEPROM (see SWITCH_ENABLE option) - // this means the EEPROM control of all channel reversal is enabled - return; - } - // up is reverse - // up is elevon - g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode - if (g.mix_mode == 0) { - g.channel_roll.set_reverse((PINE & 128) ? true : false); - g.channel_pitch.set_reverse((PINE & 64) ? true : false); - g.channel_rudder.set_reverse((PINL & 64) ? true : false); - } else { - g.reverse_elevons = (PINE & 128) ? true : false; - g.reverse_ch1_elevon = (PINE & 64) ? true : false; - g.reverse_ch2_elevon = (PINL & 64) ? true : false; - } -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/events.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - - -static void failsafe_short_on_event() -{ - // This is how to handle a short loss of control signal failsafe. - failsafe = FAILSAFE_SHORT; - ch3_failsafe_timer = millis(); - gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on")); - switch(control_mode) - { - case MANUAL: - case STABILIZE: - case FLY_BY_WIRE_A: // middle position - case FLY_BY_WIRE_B: // middle position - set_mode(CIRCLE); - break; - - case AUTO: - case LOITER: - if(g.short_fs_action == 1) { - set_mode(RTL); - } - break; - - case CIRCLE: - case RTL: - default: - break; - } - gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode); -} - -static void failsafe_long_on_event() -{ - // This is how to handle a long loss of control signal failsafe. - gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on")); - APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC - switch(control_mode) - { - case MANUAL: - case STABILIZE: - case FLY_BY_WIRE_A: // middle position - case FLY_BY_WIRE_B: // middle position - case CIRCLE: - set_mode(RTL); - break; - - case AUTO: - case LOITER: - if(g.long_fs_action == 1) { - set_mode(RTL); - } - break; - - case RTL: - default: - break; - } -} - -static void failsafe_short_off_event() -{ - // We're back in radio contact - gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off")); - failsafe = FAILSAFE_NONE; - - // re-read the switch so we can return to our preferred mode - // -------------------------------------------------------- - reset_control_switch(); - - // Reset control integrators - // --------------------- - reset_I(); -} - -#if BATTERY_EVENT == ENABLED -static void low_battery_event(void) -{ - gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); - set_mode(RTL); - g.throttle_cruise = THROTTLE_CRUISE; -} -#endif - -static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY -{ - if(event_repeat == 0 || (millis() - event_timer) < event_delay) - return; - - if (event_repeat > 0){ - event_repeat --; - } - - if(event_repeat != 0) { // event_repeat = -1 means repeat forever - event_timer = millis(); - - if (event_id >= CH_5 && event_id <= CH_8) { - if(event_repeat%2) { - APM_RC.OutputCh(event_id, event_value); // send to Servos - } else { - APM_RC.OutputCh(event_id, event_undo_value); - } - } - - if (event_id == RELAY_TOGGLE) { - relay.toggle(); - } - } -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/navigation.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -//**************************************************************** -// Function that will calculate the desired direction to fly and distance -//**************************************************************** -static void navigate() -{ - // do not navigate with corrupt data - // --------------------------------- - if (g_gps->fix == 0) - { - g_gps->new_data = false; - return; - } - - if(next_WP.lat == 0){ - return; - } - - // waypoint distance from plane - // ---------------------------- - wp_distance = get_distance(¤t_loc, &next_WP); - - if (wp_distance < 0){ - gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); - //Serial.println(wp_distance,DEC); - return; - } - - // target_bearing is where we should be heading - // -------------------------------------------- - target_bearing = get_bearing(¤t_loc, &next_WP); - - // nav_bearing will includes xtrac correction - // ------------------------------------------ - nav_bearing = target_bearing; - - // check if we have missed the WP - loiter_delta = (target_bearing - old_target_bearing)/100; - - // reset the old value - old_target_bearing = target_bearing; - - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; - loiter_sum += abs(loiter_delta); - - // control mode specific updates to nav_bearing - // -------------------------------------------- - update_navigation(); -} - - -#if 0 -// Disabled for now -void calc_distance_error() -{ - distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01)); - distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); - wp_distance = max(distance_estimate,10); -} -#endif - -static void calc_airspeed_errors() -{ - // XXX excess casting here - if(control_mode>=AUTO && airspeed_nudge > 0) { - airspeed_error = g.airspeed_cruise + airspeed_nudge - airspeed; - airspeed_energy_error = (long)(((long)(g.airspeed_cruise + airspeed_nudge) * (long)(g.airspeed_cruise + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation - } else { - airspeed_error = g.airspeed_cruise - airspeed; - airspeed_energy_error = (long)(((long)g.airspeed_cruise * (long)g.airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation - } -} - -static void calc_bearing_error() -{ - if(takeoff_complete == true || g.compass_enabled == true) { - bearing_error = nav_bearing - dcm.yaw_sensor; - } else { - - // TODO: we need to use the Yaw gyro for in between GPS reads, - // maybe as an offset from a saved gryo value. - bearing_error = nav_bearing - g_gps->ground_course; - } - - bearing_error = wrap_180(bearing_error); -} - -static void calc_altitude_error() -{ - if(control_mode == AUTO && offset_altitude != 0) { - // limit climb rates - target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30)); - - // stay within a certain range - if(prev_WP.alt > next_WP.alt){ - target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt); - }else{ - target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); - } - } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { - target_altitude = next_WP.alt; - } - - /* - // Disabled for now - #if AIRSPEED_SENSOR == 1 - long altitude_estimate; // for smoothing GPS output - - // special thanks to Ryan Beall for this one - float pitch_angle = pitch_sensor - g.pitch_trim; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°) - pitch_angle = constrain(pitch_angle, -2000, 2000); - float scale = sin(radians(pitch_angle * .01)); - altitude_estimate += (float)airspeed * .0002 * scale; - altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt); - - // compute altitude error for throttle control - altitude_error = target_altitude - altitude_estimate; - #else - altitude_error = target_altitude - current_loc.alt; - #endif - */ - - altitude_error = target_altitude - current_loc.alt; -} - -static long wrap_360(long error) -{ - if (error > 36000) error -= 36000; - if (error < 0) error += 36000; - return error; -} - -static long wrap_180(long error) -{ - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; -} - -static void update_loiter() -{ - float power; - - if(wp_distance <= g.loiter_radius){ - power = float(wp_distance) / float(g.loiter_radius); - power = constrain(power, 0.5, 1); - nav_bearing += (int)(9000.0 * (2.0 + power)); - }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ - power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); - nav_bearing -= power * 9000; - - }else{ - update_crosstrack(); - loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit - - } -/* - if (wp_distance < g.loiter_radius){ - nav_bearing += 9000; - }else{ - nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance); - } - - update_crosstrack(); -*/ - nav_bearing = wrap_360(nav_bearing); -} - -static void update_crosstrack(void) -{ - // Crosstrack Error - // ---------------- - if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line - nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); - nav_bearing = wrap_360(nav_bearing); - } -} - -static void reset_crosstrack() -{ - crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following -} - -static long get_distance(struct Location *loc1, struct Location *loc2) -{ - if(loc1->lat == 0 || loc1->lng == 0) - return -1; - if(loc2->lat == 0 || loc2->lng == 0) - return -1; - float dlat = (float)(loc2->lat - loc1->lat); - float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; - return sqrt(sq(dlat) + sq(dlong)) * .01113195; -} - -static long get_bearing(struct Location *loc1, struct Location *loc2) -{ - long off_x = loc2->lng - loc1->lng; - long off_y = (loc2->lat - loc1->lat) * scaleLongUp; - long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; - if (bearing < 0) bearing += 36000; - return bearing; -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/planner.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Common for implementation details -static const struct Menu::command planner_menu_commands[] PROGMEM = { - {"gcs", planner_gcs}, -}; - -// A Macro to create the Menu -MENU(planner_menu, "planner", planner_menu_commands); - -static int8_t -planner_mode(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); - planner_menu.run(); - return 0; -} -static int8_t -planner_gcs(uint8_t argc, const Menu::arg *argv) -{ - gcs0.init(&Serial); - gcs3.init(&Serial3); - - int loopcount = 0; - while (1) { - if (millis()-fast_loopTimer > 19) { - fast_loopTimer = millis(); - - gcs_update(); - gcs_data_stream_send(45,1000); - if ((loopcount % 5) == 0) // 10 hz - gcs_data_stream_send(5,45); - if ((loopcount % 16) == 0) { // 3 hz - gcs_data_stream_send(1,5); - gcs_send_message(MSG_HEARTBEAT); - } - loopcount++; - } - } - return 0; -} - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/radio.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -//Function that will read the radio data, limit servos and trigger a failsafe -// ---------------------------------------------------------------------------- -static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling - - -static void init_rc_in() -{ - // set rc reversing - update_servo_switches(); - - // set rc channel ranges - g.channel_roll.set_angle(SERVO_MAX); - g.channel_pitch.set_angle(SERVO_MAX); - g.channel_rudder.set_angle(SERVO_MAX); - g.channel_throttle.set_range(0, 100); - - // set rc dead zones - g.channel_roll.set_dead_zone(60); - g.channel_pitch.set_dead_zone(60); - g.channel_rudder.set_dead_zone(60); - g.channel_throttle.set_dead_zone(6); - - //g.channel_roll.dead_zone = 60; - //g.channel_pitch.dead_zone = 60; - //g.channel_rudder.dead_zone = 60; - //g.channel_throttle.dead_zone = 6; - - //set auxiliary ranges - update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); -} - -static void init_rc_out() -{ - APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); - - APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); - APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); - APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); - - APM_RC.Init(); // APM Radio initialization - - APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); - - APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); - APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); - APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); -} - -static void read_radio() -{ - ch1_temp = APM_RC.InputCh(CH_ROLL); - ch2_temp = APM_RC.InputCh(CH_PITCH); - - if(g.mix_mode == 0){ - g.channel_roll.set_pwm(ch1_temp); - g.channel_pitch.set_pwm(ch2_temp); - }else{ - g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); - g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); - } - - g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); - g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); - g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); - g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); - g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); - - // TO DO - go through and patch throttle reverse for RC_Channel library compatibility - #if THROTTLE_REVERSE == 1 - g.channel_throttle.radio_in = g.channel_throttle.radio_max + g.channel_throttle.radio_min - g.channel_throttle.radio_in; - #endif - - control_failsafe(g.channel_throttle.radio_in); - - g.channel_throttle.servo_out = g.channel_throttle.control_in; - - if (g.channel_throttle.servo_out > 50) { - if(g.airspeed_enabled == true) { - airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); - } else { - throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); - } - } else { - airspeed_nudge = 0; - throttle_nudge = 0; - } - - /* - Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), - g.rc_1.control_in, - g.rc_2.control_in, - g.rc_3.control_in, - g.rc_4.control_in); - */ -} - -static void control_failsafe(uint16_t pwm) -{ - if(g.throttle_fs_enabled == 0) - return; - - // Check for failsafe condition based on loss of GCS control - if (rc_override_active) { - if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) { - ch3_failsafe = true; - } else { - ch3_failsafe = false; - } - - //Check for failsafe and debounce funky reads - } else if (g.throttle_fs_enabled) { - if (pwm < (unsigned)g.throttle_fs_value){ - // we detect a failsafe from radio - // throttle has dropped below the mark - failsafeCounter++; - if (failsafeCounter == 9){ - gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm); - }else if(failsafeCounter == 10) { - ch3_failsafe = true; - }else if (failsafeCounter > 10){ - failsafeCounter = 11; - } - - }else if(failsafeCounter > 0){ - // we are no longer in failsafe condition - // but we need to recover quickly - failsafeCounter--; - if (failsafeCounter > 3){ - failsafeCounter = 3; - } - if (failsafeCounter == 1){ - gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm); - }else if(failsafeCounter == 0) { - ch3_failsafe = false; - }else if (failsafeCounter <0){ - failsafeCounter = -1; - } - } - } -} - -static void trim_control_surfaces() -{ - read_radio(); - // Store control surface trim values - // --------------------------------- - if(g.mix_mode == 0){ - g.channel_roll.radio_trim = g.channel_roll.radio_in; - g.channel_pitch.radio_trim = g.channel_pitch.radio_in; - g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel - - }else{ - elevon1_trim = ch1_temp; - elevon2_trim = ch2_temp; - //Recompute values here using new values for elevon1_trim and elevon2_trim - //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed - uint16_t center = 1500; - g.channel_roll.radio_trim = center; - g.channel_pitch.radio_trim = center; - } - - // save to eeprom - g.channel_roll.save_eeprom(); - g.channel_pitch.save_eeprom(); - g.channel_throttle.save_eeprom(); - g.channel_rudder.save_eeprom(); - G_RC_AUX(k_aileron)->save_eeprom(); -} - -static void trim_radio() -{ - for (int y = 0; y < 30; y++) { - read_radio(); - } - - // Store the trim values - // --------------------- - if(g.mix_mode == 0){ - g.channel_roll.radio_trim = g.channel_roll.radio_in; - g.channel_pitch.radio_trim = g.channel_pitch.radio_in; - //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; - g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel - - } else { - elevon1_trim = ch1_temp; - elevon2_trim = ch2_temp; - uint16_t center = 1500; - g.channel_roll.radio_trim = center; - g.channel_pitch.radio_trim = center; - g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - } - - // save to eeprom - g.channel_roll.save_eeprom(); - g.channel_pitch.save_eeprom(); - //g.channel_throttle.save_eeprom(); - g.channel_rudder.save_eeprom(); - G_RC_AUX(k_aileron)->save_eeprom(); -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/sensors.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// Sensors are not available in HIL_MODE_ATTITUDE -#if HIL_MODE != HIL_MODE_ATTITUDE - -void ReadSCP1000(void) {} - -static void init_barometer(void) -{ - int flashcount = 0; - long ground_pressure = 0; - int ground_temperature; - - while(ground_pressure == 0){ - barometer.Read(); // Get initial data from absolute pressure sensor - ground_pressure = barometer.Press; - ground_temperature = barometer.Temp; - mavlink_delay(20); - //Serial.printf("barometer.Press %ld\n", barometer.Press); - } - - for(int i = 0; i < 30; i++){ // We take some readings... - - #if HIL_MODE == HIL_MODE_SENSORS - gcs_update(); // look for inbound hil packets - #endif - - barometer.Read(); // Get initial data from absolute pressure sensor - ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l; - ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; - - mavlink_delay(20); - if(flashcount == 5) { - digitalWrite(C_LED_PIN, LOW); - digitalWrite(A_LED_PIN, HIGH); - digitalWrite(B_LED_PIN, LOW); - } - - if(flashcount >= 10) { - flashcount = 0; - digitalWrite(C_LED_PIN, HIGH); - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, HIGH); - } - flashcount++; - } - - g.ground_pressure.set_and_save(ground_pressure); - g.ground_temperature.set_and_save(ground_temperature / 10.0f); - abs_pressure = ground_pressure; - - Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); - gcs_send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); -} - -static long read_barometer(void) -{ - float x, scaling, temp; - - barometer.Read(); // Get new data from absolute pressure sensor - - - //abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering - abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering - scaling = (float)g.ground_pressure / (float)abs_pressure; - temp = ((float)g.ground_temperature) + 273.15f; - x = log(scaling) * temp * 29271.267f; - return (x / 10); -} - -// in M/S * 100 -static void read_airspeed(void) -{ - #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed - if (g.airspeed_offset == 0) { - // runtime enabling of airspeed, we need to do instant - // calibration before we can use it. This isn't as - // accurate as the 50 point average in zero_airspeed(), - // but it is better than using it uncalibrated - airspeed_raw = (float)adc.Ch(AIRSPEED_CH); - g.airspeed_offset.set_and_save(airspeed_raw); - } - airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75); - airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0); - airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100; - #endif - - calc_airspeed_errors(); -} - -static void zero_airspeed(void) -{ - airspeed_raw = (float)adc.Ch(AIRSPEED_CH); - for(int c = 0; c < 10; c++){ - delay(20); - airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10); - } - g.airspeed_offset.set_and_save(airspeed_raw); -} - -#endif // HIL_MODE != HIL_MODE_ATTITUDE - -static void read_battery(void) -{ - battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; - battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; - battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9; - battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9; - - if(g.battery_monitoring == 1) - battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream - if(g.battery_monitoring == 2) - battery_voltage = battery_voltage4; - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) - battery_voltage = battery_voltage1; - if(g.battery_monitoring == 4) { - current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin - current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; - } - - #if BATTERY_EVENT == ENABLED - if(battery_voltage < LOW_VOLTAGE) low_battery_event(); - if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); - #endif -} - -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/setup.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CLI_ENABLED == ENABLED - -// Functions called from the setup menu -static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); -static int8_t setup_show (uint8_t argc, const Menu::arg *argv); -static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); -static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); -static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); -static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); -static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); -static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); - -// Command/function table for the setup menu -static const struct Menu::command setup_menu_commands[] PROGMEM = { - // command function called - // ======= =============== - {"reset", setup_factory}, - {"radio", setup_radio}, - {"modes", setup_flightmodes}, - {"compass", setup_compass}, - {"declination", setup_declination}, - {"battery", setup_batt_monitor}, - {"show", setup_show}, - {"erase", setup_erase}, -}; - -// Create the setup menu object. -MENU(setup_menu, "setup", setup_menu_commands); - -// Called from the top-level menu to run the setup menu. -static int8_t -setup_mode(uint8_t argc, const Menu::arg *argv) -{ - // Give the user some guidance - Serial.printf_P(PSTR("Setup Mode\n" - "\n" - "IMPORTANT: if you have not previously set this system up, use the\n" - "'reset' command to initialize the EEPROM to sensible default values\n" - "and then the 'radio' command to configure for your radio.\n" - "\n")); - - // Run the setup menu. When the menu exits, we will return to the main menu. - setup_menu.run(); - return 0; -} - -// Print the current configuration. -// Called by the setup menu 'show' command. -static int8_t -setup_show(uint8_t argc, const Menu::arg *argv) -{ - // clear the area - print_blanks(8); - - report_radio(); - report_batt_monitor(); - report_gains(); - report_xtrack(); - report_throttle(); - report_flight_modes(); - report_imu(); - report_compass(); - - Serial.printf_P(PSTR("Raw Values\n")); - print_divider(); - AP_Var_menu_show(argc, argv); - - return(0); -} - -// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). -// Called by the setup menu 'factoryreset' command. -static int8_t -setup_factory(uint8_t argc, const Menu::arg *argv) -{ - int c; - - Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); - - do { - c = Serial.read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) - return(-1); - AP_Var::erase_all(); - Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); - - //default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot() - - for (;;) { - } - // note, cannot actually return here - return(0); -} - - -// Perform radio setup. -// Called by the setup menu 'radio' command. -static int8_t -setup_radio(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("\n\nRadio Setup:\n")); - uint8_t i; - - for(i = 0; i < 100;i++){ - delay(20); - read_radio(); - } - - - if(g.channel_roll.radio_in < 500){ - while(1){ - Serial.printf_P(PSTR("\nNo radio; Check connectors.")); - delay(1000); - // stop here - } - } - - g.channel_roll.radio_min = g.channel_roll.radio_in; - g.channel_pitch.radio_min = g.channel_pitch.radio_in; - g.channel_throttle.radio_min = g.channel_throttle.radio_in; - g.channel_rudder.radio_min = g.channel_rudder.radio_in; - g.rc_5.radio_min = g.rc_5.radio_in; - g.rc_6.radio_min = g.rc_6.radio_in; - g.rc_7.radio_min = g.rc_7.radio_in; - g.rc_8.radio_min = g.rc_8.radio_in; - - g.channel_roll.radio_max = g.channel_roll.radio_in; - g.channel_pitch.radio_max = g.channel_pitch.radio_in; - g.channel_throttle.radio_max = g.channel_throttle.radio_in; - g.channel_rudder.radio_max = g.channel_rudder.radio_in; - g.rc_5.radio_max = g.rc_5.radio_in; - g.rc_6.radio_max = g.rc_6.radio_in; - g.rc_7.radio_max = g.rc_7.radio_in; - g.rc_8.radio_max = g.rc_8.radio_in; - - g.channel_roll.radio_trim = g.channel_roll.radio_in; - g.channel_pitch.radio_trim = g.channel_pitch.radio_in; - g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - g.rc_5.radio_trim = 1500; - g.rc_6.radio_trim = 1500; - g.rc_7.radio_trim = 1500; - g.rc_8.radio_trim = 1500; - - Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n")); - while(1){ - - delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - g.channel_roll.update_min_max(); - g.channel_pitch.update_min_max(); - g.channel_throttle.update_min_max(); - g.channel_rudder.update_min_max(); - g.rc_5.update_min_max(); - g.rc_6.update_min_max(); - g.rc_7.update_min_max(); - g.rc_8.update_min_max(); - - if(Serial.available() > 0){ - Serial.flush(); - g.channel_roll.save_eeprom(); - g.channel_pitch.save_eeprom(); - g.channel_throttle.save_eeprom(); - g.channel_rudder.save_eeprom(); - g.rc_5.save_eeprom(); - g.rc_6.save_eeprom(); - g.rc_7.save_eeprom(); - g.rc_8.save_eeprom(); - print_done(); - break; - } - } - trim_radio(); - report_radio(); - return(0); -} - - -static int8_t -setup_flightmodes(uint8_t argc, const Menu::arg *argv) -{ - byte switchPosition, mode = 0; - - Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); - print_hit_enter(); - trim_radio(); - - while(1){ - delay(20); - read_radio(); - switchPosition = readSwitch(); - - - // look for control switch change - if (oldSwitchPosition != switchPosition){ - // force position 5 to MANUAL - if (switchPosition > 4) { - flight_modes[switchPosition] = MANUAL; - } - // update our current mode - mode = flight_modes[switchPosition]; - - // update the user - print_switch(switchPosition, mode); - - // Remember switch position - oldSwitchPosition = switchPosition; - } - - // look for stick input - int radioInputSwitch = radio_input_switch(); - - if (radioInputSwitch != 0){ - - mode += radioInputSwitch; - - while ( - mode != MANUAL && - mode != CIRCLE && - mode != STABILIZE && - mode != FLY_BY_WIRE_A && - mode != FLY_BY_WIRE_B && - mode != AUTO && - mode != RTL && - mode != LOITER) - { - if (mode < MANUAL) - mode = LOITER; - else if (mode >LOITER) - mode = MANUAL; - else - mode += radioInputSwitch; - } - - // Override position 5 - if(switchPosition > 4) - mode = MANUAL; - - // save new mode - flight_modes[switchPosition] = mode; - - // print new mode - print_switch(switchPosition, mode); - } - - // escape hatch - if(Serial.available() > 0){ - // save changes - for (mode=0; mode<6; mode++) - flight_modes[mode].save(); - report_flight_modes(); - print_done(); - return (0); - } - } -} - -static int8_t -setup_declination(uint8_t argc, const Menu::arg *argv) -{ - compass.set_declination(radians(argv[1].f)); - report_compass(); - return 0; -} - - -static int8_t -setup_erase(uint8_t argc, const Menu::arg *argv) -{ - int c; - - Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ")); - - do { - c = Serial.read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) - return(-1); - zero_eeprom(); - return 0; -} - -static int8_t -setup_compass(uint8_t argc, const Menu::arg *argv) -{ - if (!strcmp_P(argv[1].str, PSTR("on"))) { - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - g.compass_enabled = false; - } else { - g.compass_enabled = true; - } - } else if (!strcmp_P(argv[1].str, PSTR("off"))) { - g.compass_enabled = false; - - } else { - Serial.printf_P(PSTR("\nOptions:[on,off]\n")); - report_compass(); - return 0; - } - - g.compass_enabled.save(); - report_compass(); - return 0; -} - -static int8_t -setup_batt_monitor(uint8_t argc, const Menu::arg *argv) -{ - if(argv[1].i >= 0 && argv[1].i <= 4){ - g.battery_monitoring.set_and_save(argv[1].i); - - } else { - Serial.printf_P(PSTR("\nOptions: 0-4")); - } - - report_batt_monitor(); - return 0; -} - -/***************************************************************************/ -// CLI reports -/***************************************************************************/ - -static void report_batt_monitor() -{ - //print_blanks(2); - Serial.printf_P(PSTR("Batt Mointor\n")); - print_divider(); - if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled")); - if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell")); - if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell")); - if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts")); - if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); - print_blanks(2); -} -static void report_radio() -{ - //print_blanks(2); - Serial.printf_P(PSTR("Radio\n")); - print_divider(); - // radio - print_radio_values(); - print_blanks(2); -} - -static void report_gains() -{ - //print_blanks(2); - Serial.printf_P(PSTR("Gains\n")); - print_divider(); - - Serial.printf_P(PSTR("servo roll:\n")); - print_PID(&g.pidServoRoll); - - Serial.printf_P(PSTR("servo pitch:\n")); - print_PID(&g.pidServoPitch); - - Serial.printf_P(PSTR("servo rudder:\n")); - print_PID(&g.pidServoRudder); - - Serial.printf_P(PSTR("nav roll:\n")); - print_PID(&g.pidNavRoll); - - Serial.printf_P(PSTR("nav pitch airspeed:\n")); - print_PID(&g.pidNavPitchAirspeed); - - Serial.printf_P(PSTR("energry throttle:\n")); - print_PID(&g.pidTeThrottle); - - Serial.printf_P(PSTR("nav pitch alt:\n")); - print_PID(&g.pidNavPitchAltitude); - - print_blanks(2); -} - -static void report_xtrack() -{ - //print_blanks(2); - Serial.printf_P(PSTR("Crosstrack\n")); - print_divider(); - // radio - Serial.printf_P(PSTR("XTRACK: %4.2f\n" - "XTRACK angle: %d\n"), - (float)g.crosstrack_gain, - (int)g.crosstrack_entry_angle); - print_blanks(2); -} - -static void report_throttle() -{ - //print_blanks(2); - Serial.printf_P(PSTR("Throttle\n")); - print_divider(); - - Serial.printf_P(PSTR("min: %d\n" - "max: %d\n" - "cruise: %d\n" - "failsafe_enabled: %d\n" - "failsafe_value: %d\n"), - (int)g.throttle_min, - (int)g.throttle_max, - (int)g.throttle_cruise, - (int)g.throttle_fs_enabled, - (int)g.throttle_fs_value); - print_blanks(2); -} - -static void report_imu() -{ - //print_blanks(2); - Serial.printf_P(PSTR("IMU\n")); - print_divider(); - - print_gyro_offsets(); - print_accel_offsets(); - print_blanks(2); -} - -static void report_compass() -{ - //print_blanks(2); - Serial.printf_P(PSTR("Compass: ")); - - switch (compass.product_id) { - case AP_COMPASS_TYPE_HMC5883L: - Serial.println_P(PSTR("HMC5883L")); - break; - case AP_COMPASS_TYPE_HMC5843: - Serial.println_P(PSTR("HMC5843")); - break; - case AP_COMPASS_TYPE_HIL: - Serial.println_P(PSTR("HIL")); - break; - default: - Serial.println_P(PSTR("??")); - break; - } - - print_divider(); - - print_enabled(g.compass_enabled); - - // mag declination - Serial.printf_P(PSTR("Mag Declination: %4.4f\n"), - degrees(compass.get_declination())); - - Vector3f offsets = compass.get_offsets(); - - // mag offsets - Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"), - offsets.x, - offsets.y, - offsets.z); - print_blanks(2); -} - -static void report_flight_modes() -{ - //print_blanks(2); - Serial.printf_P(PSTR("Flight modes\n")); - print_divider(); - - for(int i = 0; i < 6; i++ ){ - print_switch(i, flight_modes[i]); - } - print_blanks(2); -} - -/***************************************************************************/ -// CLI utilities -/***************************************************************************/ - -static void -print_PID(PID * pid) -{ - Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), - pid->kP(), - pid->kI(), - pid->kD(), - (long)pid->imax()); -} - -static void -print_radio_values() -{ - Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); - Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max); - Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max); - Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max); - Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max); - Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max); - Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max); - Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max); - -} - -static void -print_switch(byte p, byte m) -{ - Serial.printf_P(PSTR("Pos %d: "),p); - Serial.println(flight_mode_strings[m]); -} - -static void -print_done() -{ - Serial.printf_P(PSTR("\nSaved Settings\n\n")); -} - -static void -print_blanks(int num) -{ - while(num > 0){ - num--; - Serial.println(""); - } -} - -static void -print_divider(void) -{ - for (int i = 0; i < 40; i++) { - Serial.printf_P(PSTR("-")); - } - Serial.println(""); -} - -static int8_t -radio_input_switch(void) -{ - static int8_t bouncer = 0; - - - if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) { - bouncer = 10; - } - if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) { - bouncer = -10; - } - if (bouncer >0) { - bouncer --; - } - if (bouncer <0) { - bouncer ++; - } - - if (bouncer == 1 || bouncer == -1) { - return bouncer; - } else { - return 0; - } -} - - -static void zero_eeprom(void) -{ - byte b = 0; - Serial.printf_P(PSTR("\nErasing EEPROM\n")); - for (int i = 0; i < EEPROM_MAX_ADDR; i++) { - eeprom_write_byte((uint8_t *) i, b); - } - Serial.printf_P(PSTR("done\n")); -} - -static void print_enabled(bool b) -{ - if(b) - Serial.printf_P(PSTR("en")); - else - Serial.printf_P(PSTR("dis")); - Serial.printf_P(PSTR("abled\n")); -} - -static void -print_accel_offsets(void) -{ - Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), - (float)imu.ax(), - (float)imu.ay(), - (float)imu.az()); -} - -static void -print_gyro_offsets(void) -{ - Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), - (float)imu.gx(), - (float)imu.gy(), - (float)imu.gz()); -} - - -#endif // CLI_ENABLED -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/system.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/***************************************************************************** -The init_ardupilot function processes everything we need for an in - air restart - We will determine later if we are actually on the ground and process a - ground start in that case. - -*****************************************************************************/ - -#if CLI_ENABLED == ENABLED - -// Functions called from the top-level menu -static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp -static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Commands:\n" - " logs log readback/setup mode\n" - " setup setup mode\n" - " test test mode\n" - "\n" - "Move the slide switch and reset to FLY.\n" - "\n")); - return(0); -} - -// Command/function table for the top-level menu. -static const struct Menu::command main_menu_commands[] PROGMEM = { -// command function called -// ======= =============== - {"logs", process_logs}, - {"setup", setup_mode}, - {"test", test_mode}, - {"help", main_menu_help}, - {"planner", planner_mode} -}; - -// Create the top-level menu object. -MENU(main_menu, THISFIRMWARE, main_menu_commands); - -// the user wants the CLI. It never exits -static void run_cli(void) -{ - while (1) { - main_menu.run(); - } -} - -#endif // CLI_ENABLED - -static void init_ardupilot() -{ - // Console serial port - // - // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. - // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - Serial.begin(SERIAL0_BAUD, 128, 128); - - // GPS serial port. - // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // - // standard gps running - Serial1.begin(38400, 128, 16); - - Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE - "\n\nFree RAM: %u\n"), - memcheck_available_memory()); - - // - // Check the EEPROM format version before loading any parameters from EEPROM. - // - if (!g.format_version.load()) { - - Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n")); - delay(100); // wait for serial msg to flush - - AP_Var::erase_all(); - - // save the current format version - g.format_version.set_and_save(Parameters::k_format_version); - - } else if (g.format_version != Parameters::k_format_version) { - - Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" - "\n\nForcing complete parameter reset..."), - g.format_version.get(), Parameters::k_format_version); - delay(100); // wait for serial msg to flush - - // erase all parameters - AP_Var::erase_all(); - - // save the new format version - g.format_version.set_and_save(Parameters::k_format_version); - - Serial.println_P(PSTR("done.")); - } else { - unsigned long before = micros(); - // Load all auto-loaded EEPROM variables - AP_Var::load_all(); - - Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); - Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"), - AP_Var::get_memory_use(), (unsigned)g.num_resets); - } - - // keep a record of how many resets have happened. This can be - // used to detect in-flight resets - g.num_resets.set_and_save(g.num_resets+1); - - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); - - mavlink_system.sysid = g.sysid_this_mav; - - -#if HIL_MODE != HIL_MODE_ATTITUDE - adc.Init(); // APM ADC library initialization - barometer.Init(); // APM Abs Pressure sensor initialization - - if (g.compass_enabled==true) { - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - g.compass_enabled = false; - } else { - dcm.set_compass(&compass); - compass.get_offsets(); // load offsets to account for airframe magnetic interference - } - } - /* - Init is depricated - Jason - if(g.sonar_enabled){ - sonar.init(SONAR_PIN, &adc); - Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC); - } - */ -#endif - -#if LOGGING_ENABLED == ENABLED - DataFlash.Init(); // DataFlash log initialization -#endif - - // Do GPS init - g_gps = &g_gps_driver; - g_gps->init(); // GPS Initialization - g_gps->callback = mavlink_delay; - - // init the GCS - gcs0.init(&Serial); - gcs3.init(&Serial3); - - //mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav - mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id - mavlink_system.type = MAV_FIXED_WING; - - rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override - init_rc_in(); // sets up rc channels from radio - init_rc_out(); // sets up the timer libs - - pinMode(C_LED_PIN, OUTPUT); // GPS status LED - pinMode(A_LED_PIN, OUTPUT); // GPS status LED - pinMode(B_LED_PIN, OUTPUT); // GPS status LED - pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode - pinMode(PUSHBUTTON_PIN, INPUT); // unused - DDRL |= B00000100; // Set Port L, pin 2 to output for the relay - - // If the switch is in 'menu' mode, run the main menu. - // - // Since we can't be sure that the setup or test mode won't leave - // the system in an odd state, we don't let the user exit the top - // menu; they must reset in order to fly. - // -#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED - if (digitalRead(SLIDE_SWITCH_PIN) == 0) { - digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED - Serial.printf_P(PSTR("\n" - "Entering interactive setup mode...\n" - "\n" - "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n" - "Type 'help' to list commands, 'exit' to leave a submenu.\n" - "Visit the 'setup' menu for first-time configuration.\n")); - Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); - run_cli(); - } -#else - Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n")); -#endif // CLI_ENABLED - - if(g.log_bitmask != 0){ - // TODO - Here we will check on the length of the last log - // We don't want to create a bunch of little logs due to powering on and off - byte last_log_num = get_num_logs(); - start_new_log(last_log_num); - } - - // read in the flight switches - update_servo_switches(); - - if (ENABLE_AIR_START == 1) { - // Perform an air start and get back to flying - gcs_send_text_P(SEVERITY_LOW,PSTR(" AIR START")); - - // Get necessary data from EEPROM - //---------------- - //read_EEPROM_airstart_critical(); -#if HIL_MODE != HIL_MODE_ATTITUDE - imu.init(IMU::WARM_START); - dcm.set_centripetal(1); -#endif - - // This delay is important for the APM_RC library to work. - // We need some time for the comm between the 328 and 1280 to be established. - int old_pulse = 0; - while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(g.flight_mode_channel)) > 5 || - APM_RC.InputCh(g.flight_mode_channel) == 1000 || - APM_RC.InputCh(g.flight_mode_channel) == 1200)) { - old_pulse = APM_RC.InputCh(g.flight_mode_channel); - delay(25); - } - GPS_enabled = false; - g_gps->update(); - if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED) GPS_enabled = true; - - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_AIRSTART_MSG); - reload_commands_airstart(); // Get set to resume AUTO from where we left off - - }else { - startup_ground(); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - } - - set_mode(MANUAL); - - // set the correct flight mode - // --------------------------- - reset_control_switch(); -} - -//******************************************************************************** -//This function does all the calibrations, etc. that we need during a ground start -//******************************************************************************** -static void startup_ground(void) -{ - set_mode(INITIALISING); - - gcs_send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); - - #if(GROUND_START_DELAY > 0) - gcs_send_text_P(SEVERITY_LOW,PSTR(" With Delay")); - delay(GROUND_START_DELAY * 1000); - #endif - - // Makes the servos wiggle - // step 1 = 1 wiggle - // ----------------------- - demo_servos(1); - - //IMU ground start - //------------------------ - // - startup_IMU_ground(); - - // read the radio to set trims - // --------------------------- - trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. - -#if HIL_MODE != HIL_MODE_ATTITUDE -if (g.airspeed_enabled == true) - { - // initialize airspeed sensor - // -------------------------- - zero_airspeed(); - gcs_send_text_P(SEVERITY_LOW,PSTR(" zero airspeed calibrated")); - } -else - { - gcs_send_text_P(SEVERITY_LOW,PSTR(" NO airspeed")); - } -#endif - - // Save the settings for in-air restart - // ------------------------------------ - //save_EEPROM_groundstart(); - - // initialize commands - // ------------------- - init_commands(); - - // Read in the GPS - see if one is connected - GPS_enabled = false; - for (byte counter = 0; ; counter++) { - g_gps->update(); - if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){ - GPS_enabled = true; - break; - } - - if (counter >= 2) { - GPS_enabled = false; - break; - } - } - - // Makes the servos wiggle - 3 times signals ready to fly - // ----------------------- - demo_servos(3); - - gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); -} - -static void set_mode(byte mode) -{ - if(control_mode == mode){ - // don't switch modes if we are already in the correct mode. - return; - } - if(g.auto_trim > 0 && control_mode == MANUAL) - trim_control_surfaces(); - - control_mode = mode; - crash_timer = 0; - - switch(control_mode) - { - case INITIALISING: - case MANUAL: - case CIRCLE: - case STABILIZE: - case FLY_BY_WIRE_A: - case FLY_BY_WIRE_B: - break; - - case AUTO: - update_auto(); - break; - - case RTL: - do_RTL(); - break; - - case LOITER: - do_loiter_at_location(); - break; - - case GUIDED: - set_guided_WP(); - break; - - default: - do_RTL(); - break; - } - - if (g.log_bitmask & MASK_LOG_MODE) - Log_Write_Mode(control_mode); -} - -static void check_long_failsafe() -{ - // only act on changes - // ------------------- - if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){ - if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { - failsafe = FAILSAFE_LONG; - failsafe_long_on_event(); - } - if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) { - failsafe = FAILSAFE_LONG; - failsafe_long_on_event(); - } - if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { - failsafe = FAILSAFE_GCS; - failsafe_long_on_event(); - } - } else { - // We do not change state but allow for user to change mode - if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE; - if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE; - if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE; - } -} - -static void check_short_failsafe() -{ - // only act on changes - // ------------------- - if(failsafe == FAILSAFE_NONE){ - if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde - failsafe_short_on_event(); - } - } - - if(failsafe == FAILSAFE_SHORT){ - if(!ch3_failsafe) { - failsafe_short_off_event(); - } - } -} - - -static void startup_IMU_ground(void) -{ -#if HIL_MODE != HIL_MODE_ATTITUDE - gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); - mavlink_delay(500); - - // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! - // ----------------------- - demo_servos(2); - gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); - mavlink_delay(1000); - - imu.init(IMU::COLD_START, mavlink_delay); - dcm.set_centripetal(1); - - // read Baro pressure at ground - //----------------------------- - init_barometer(); - -#endif // HIL_MODE_ATTITUDE - - digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready - digitalWrite(A_LED_PIN, LOW); - digitalWrite(C_LED_PIN, LOW); -} - - -static void update_GPS_light(void) -{ - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - switch (g_gps->status()) { - case(2): - digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case(1): - if (g_gps->valid_read == true){ - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light){ - digitalWrite(C_LED_PIN, LOW); - } else { - digitalWrite(C_LED_PIN, HIGH); - } - g_gps->valid_read = false; - } - break; - - default: - digitalWrite(C_LED_PIN, LOW); - break; - } -} - - -static void resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; - dcm.gyro_sat_count = 0; - imu.adc_constraints = 0; - dcm.renorm_sqrt_count = 0; - dcm.renorm_blowup_count = 0; - gps_fix_count = 0; - pmTest1 = 0; - perf_mon_timer = millis(); -} - - -/* - map from a 8 bit EEPROM baud rate to a real baud rate - */ -static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) -{ - switch (rate) { - case 9: return 9600; - case 19: return 19200; - case 38: return 38400; - case 57: return 57600; - case 111: return 111100; - case 115: return 115200; - } - Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); - return default_baud; -} -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduPlane/test.pde" -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CLI_ENABLED == ENABLED - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); -static int8_t test_radio(uint8_t argc, const Menu::arg *argv); -static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); -static int8_t test_gps(uint8_t argc, const Menu::arg *argv); -static int8_t test_adc(uint8_t argc, const Menu::arg *argv); -static int8_t test_imu(uint8_t argc, const Menu::arg *argv); -static int8_t test_gyro(uint8_t argc, const Menu::arg *argv); -static int8_t test_battery(uint8_t argc, const Menu::arg *argv); -static int8_t test_current(uint8_t argc, const Menu::arg *argv); -static int8_t test_relay(uint8_t argc, const Menu::arg *argv); -static int8_t test_wp(uint8_t argc, const Menu::arg *argv); -static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); -static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); -static int8_t test_mag(uint8_t argc, const Menu::arg *argv); -static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); -static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); -static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); -static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Common for implementation details -static const struct Menu::command test_menu_commands[] PROGMEM = { - {"pwm", test_radio_pwm}, - {"radio", test_radio}, - {"failsafe", test_failsafe}, - {"battery", test_battery}, - {"relay", test_relay}, - {"waypoints", test_wp}, - {"xbee", test_xbee}, - {"eedump", test_eedump}, - {"modeswitch", test_modeswitch}, - {"dipswitches", test_dipswitches}, - - // Tests below here are for hardware sensors only present - // when real sensors are attached or they are emulated -#if HIL_MODE == HIL_MODE_DISABLED - {"adc", test_adc}, - {"gps", test_gps}, - {"rawgps", test_rawgps}, - {"imu", test_imu}, - {"gyro", test_gyro}, - {"airspeed", test_airspeed}, - {"airpressure", test_pressure}, - {"compass", test_mag}, - {"current", test_current}, -#elif HIL_MODE == HIL_MODE_SENSORS - {"adc", test_adc}, - {"gps", test_gps}, - {"imu", test_imu}, - {"gyro", test_gyro}, - {"compass", test_mag}, -#elif HIL_MODE == HIL_MODE_ATTITUDE -#endif - -}; - -// A Macro to create the Menu -MENU(test_menu, "test", test_menu_commands); - -static int8_t -test_mode(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Test Mode\n\n")); - test_menu.run(); - return 0; -} - -static void print_hit_enter() -{ - Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); -} - -static int8_t -test_eedump(uint8_t argc, const Menu::arg *argv) -{ - int i, j; - - // hexdump the EEPROM - for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { - Serial.printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); - Serial.println(); - } - return(0); -} - -static int8_t -test_radio_pwm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - g.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_radio(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - #if THROTTLE_REVERSE == 1 - Serial.printf_P(PSTR("Throttle is reversed in config: \n")); - delay(1000); - #endif - - // read the radio to set trims - // --------------------------- - trim_radio(); - - while(1){ - delay(20); - read_radio(); - update_servo_switches(); - - g.channel_roll.calc_pwm(); - g.channel_pitch.calc_pwm(); - g.channel_throttle.calc_pwm(); - g.channel_rudder.calc_pwm(); - - // write out the servo PWM values - // ------------------------------ - set_servos(); - - Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), - g.channel_roll.control_in, - g.channel_pitch.control_in, - g.channel_throttle.control_in, - g.channel_rudder.control_in, - g.rc_5.control_in, - g.rc_6.control_in, - g.rc_7.control_in, - g.rc_8.control_in); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_failsafe(uint8_t argc, const Menu::arg *argv) -{ - byte fail_test; - print_hit_enter(); - for(int i = 0; i < 50; i++){ - delay(20); - read_radio(); - } - - // read the radio to set trims - // --------------------------- - trim_radio(); - - oldSwitchPosition = readSwitch(); - - Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); - while(g.channel_throttle.control_in > 0){ - delay(20); - read_radio(); - } - - while(1){ - delay(20); - read_radio(); - - if(g.channel_throttle.control_in > 0){ - Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in); - fail_test++; - } - - if(oldSwitchPosition != readSwitch()){ - Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); - Serial.println(flight_mode_strings[readSwitch()]); - fail_test++; - } - - if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){ - Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in); - Serial.println(flight_mode_strings[readSwitch()]); - fail_test++; - } - - if(fail_test > 0){ - return (0); - } - if(Serial.available() > 0){ - Serial.printf_P(PSTR("LOS caused no change in APM.\n")); - return (0); - } - } -} - -static int8_t -test_battery(uint8_t argc, const Menu::arg *argv) -{ -if (g.battery_monitoring >=1 && g.battery_monitoring < 4) { - for (int i = 0; i < 80; i++){ // Need to get many samples for filter to stabilize - delay(20); - read_battery(); - } - Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"), - battery_voltage1, - battery_voltage2, - battery_voltage3, - battery_voltage4); -} else { - Serial.printf_P(PSTR("Not enabled\n")); -} - return (0); -} - -static int8_t -test_current(uint8_t argc, const Menu::arg *argv) -{ -if (g.battery_monitoring == 4) { - print_hit_enter(); - delta_ms_medium_loop = 100; - - while(1){ - delay(100); - read_radio(); - read_battery(); - Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), - battery_voltage, - current_amps, - current_total); - - // write out the servo PWM values - // ------------------------------ - set_servos(); - - if(Serial.available() > 0){ - return (0); - } - } -} else { - Serial.printf_P(PSTR("Not enabled\n")); - return (0); -} - -} - -static int8_t -test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - Serial.printf_P(PSTR("Relay on\n")); - relay.on(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } - - Serial.printf_P(PSTR("Relay off\n")); - relay.off(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_wp(uint8_t argc, const Menu::arg *argv) -{ - delay(1000); - - // save the alitude above home option - if(g.RTL_altitude < 0){ - Serial.printf_P(PSTR("Hold current altitude\n")); - }else{ - Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100); - } - - Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); - Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); - Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - - for(byte i = 0; i <= g.command_total; i++){ - struct Location temp = get_cmd_with_index(i); - test_wp_print(&temp, i); - } - - return (0); -} - -static void -test_wp_print(struct Location *cmd, byte wp_index) -{ - Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), - (int)wp_index, - (int)cmd->id, - (int)cmd->options, - (int)cmd->p1, - cmd->alt, - cmd->lat, - cmd->lng); -} - -static int8_t -test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); - - while(1){ - - if (Serial3.available()) - Serial3.write(Serial3.read()); - - if(Serial.available() > 0){ - return (0); - } - } -} - - -static int8_t -test_modeswitch(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - Serial.printf_P(PSTR("Control CH ")); - - Serial.println(FLIGHT_MODE_CHANNEL, DEC); - - while(1){ - delay(20); - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - Serial.printf_P(PSTR("Position %d\n"), switchPosition); - oldSwitchPosition = switchPosition; - } - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_dipswitches(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - if (!g.switch_enable) { - Serial.println_P(PSTR("dip switches disabled, using EEPROM")); - } - - while(1){ - delay(100); - update_servo_switches(); - - if (g.mix_mode == 0) { - Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), - (int)g.channel_roll.get_reverse(), - (int)g.channel_pitch.get_reverse(), - (int)g.channel_rudder.get_reverse()); - } else { - Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), - (int)g.reverse_elevons, - (int)g.reverse_ch1_elevon, - (int)g.reverse_ch2_elevon); - } - if(Serial.available() > 0){ - return (0); - } - } -} - -//------------------------------------------------------------------------------------------- -// tests in this section are for real sensors or sensors that have been simulated - -#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS -static int8_t -test_adc(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - adc.Init(); - delay(1000); - Serial.printf_P(PSTR("ADC\n")); - delay(1000); - - while(1){ - for (int i=0;i<9;i++) Serial.printf_P(PSTR("%u\t"),adc.Ch(i)); - Serial.println(); - delay(100); - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_gps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(333); - - // Blink GPS LED if we don't have a fix - // ------------------------------------ - update_GPS_light(); - - g_gps->update(); - - if (g_gps->new_data){ - Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"), - g_gps->latitude, - g_gps->longitude, - g_gps->altitude/100, - g_gps->num_sats); - }else{ - Serial.printf_P(PSTR(".")); - } - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_imu(uint8_t argc, const Menu::arg *argv) -{ - //Serial.printf_P(PSTR("Calibrating.")); - - imu.init(IMU::COLD_START); - - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - if (millis() - fast_loopTimer > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer; - G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator - fast_loopTimer = millis(); - - // IMU - // --- - dcm.update_DCM(); - - if(g.compass_enabled) { - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - medium_loopCounter = 0; - } - } - - // We are using the IMU - // --------------------- - Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), - (int)dcm.roll_sensor / 100, - (int)dcm.pitch_sensor / 100, - (uint16_t)dcm.yaw_sensor / 100); - } - if(Serial.available() > 0){ - return (0); - } - } -} - - -static int8_t -test_gyro(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - adc.Init(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - delay(1000); - - while(1){ - imu.update(); // need this because we are not calling the DCM - Vector3f gyros = imu.get_gyro(); - Vector3f accels = imu.get_accel(); - Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), - (int)gyros.x, - (int)gyros.y, - (int)gyros.z, - (int)accels.x, - (int)accels.y, - (int)accels.z); - delay(100); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_mag(uint8_t argc, const Menu::arg *argv) -{ - if (!g.compass_enabled) { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); - } - - compass.set_orientation(MAG_ORIENTATION); - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - return 0; - } - dcm.set_compass(&compass); - report_compass(); - - // we need the DCM initialised for this test - imu.init(IMU::COLD_START); - - int counter = 0; - //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); - - print_hit_enter(); - - while(1) { - delay(20); - if (millis() - fast_loopTimer > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer; - G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator - fast_loopTimer = millis(); - - // IMU - // --- - dcm.update_DCM(); - - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - medium_loopCounter = 0; - } - - counter++; - if (counter>20) { - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z, - maggy.x, - maggy.y, - maggy.z); - counter=0; - } - } - if (Serial.available() > 0) { - break; - } - } - - // save offsets. This allows you to get sane offset values using - // the CLI before you go flying. - Serial.println_P(PSTR("saving offsets")); - compass.save_offsets(); - return (0); -} - -#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS - -//------------------------------------------------------------------------------------------- -// real sensors that have not been simulated yet go here - -#if HIL_MODE == HIL_MODE_DISABLED - -static int8_t -test_airspeed(uint8_t argc, const Menu::arg *argv) -{ - unsigned airspeed_ch = adc.Ch(AIRSPEED_CH); - // Serial.println(adc.Ch(AIRSPEED_CH)); - Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch); - - if (g.airspeed_enabled == false){ - Serial.printf_P(PSTR("airspeed: ")); - print_enabled(false); - return (0); - - }else{ - print_hit_enter(); - zero_airspeed(); - Serial.printf_P(PSTR("airspeed: ")); - print_enabled(true); - - while(1){ - delay(20); - read_airspeed(); - Serial.printf_P(PSTR("%fm/s\n"), airspeed / 100.0); - - if(Serial.available() > 0){ - return (0); - } - } - } -} - - -static int8_t -test_pressure(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Uncalibrated relative airpressure\n")); - print_hit_enter(); - - home.alt = 0; - wp_distance = 0; - init_barometer(); - - while(1){ - delay(100); - current_loc.alt = read_barometer() + home.alt; - - Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld\n"), - current_loc.alt / 100.0, - abs_pressure); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_rawgps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - if (Serial3.available()){ - digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS - Serial1.write(Serial3.read()); - digitalWrite(B_LED_PIN, LOW); - } - if (Serial1.available()){ - digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS - Serial3.write(Serial1.read()); - digitalWrite(C_LED_PIN, LOW); - } - if(Serial.available() > 0){ - return (0); - } - } -} -#endif // HIL_MODE == HIL_MODE_DISABLED - -#endif // CLI_ENABLED diff --git a/ArduRover/ArduRover.cpp b/ArduRover/ArduRover.cpp deleted file mode 100644 index 53d510ad0b..0000000000 --- a/ArduRover/ArduRover.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#line 1 "/home/jgoppert/Projects/ardupilotone/ArduRover/ArduRover.pde" -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Vehicle Configuration -#include "CarStampede.h" -//#include "TankGeneric.h" - -// ArduPilotOne Default Setup -#include "APO_DefaultSetup.h" -#line 1 "autogenerated" -#include "WProgram.h" diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b5c21454d..652ed40c39 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,26 +78,37 @@ if (NOT DEFINED BOARD) endif() message(STATUS "Board configured as: ${BOARD}") -set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) - -# standard apm project setup -macro(apm_project PROJECT_NAME BOARD SRCS) - message(STATUS "creating apo project ${PROJECT_NAME}") - set(${PROJECT_NAME}_BOARD ${BOARD}) - set(${PROJECT_NAME}_PORT ${PORT}) - set(${PROJECT_NAME}_SRCS ${SRCS}) - set(${PROJECT_NAME}_LIBS m c) - generate_arduino_firmware(${PROJECT_NAME}) - set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) +# add a sketch +macro(add_sketch SKETCH_NAME BOARD PORT) + message(STATUS "Generating sketch ${SKETCH_NAME}") + set(SKETCH_CPP ${CMAKE_CURRENT_BINARY_DIR}/${SKETCH_NAME}/${SKETCH_NAME}.cpp) + set(SKETCH_PDE ${SKETCH_NAME}/${SKETCH_NAME}.pde) + set(${SKETCH_NAME}_BOARD ${BOARD}) + set(${SKETCH_NAME}_PORT ${PORT}) + set(${SKETCH_NAME}_SRCS ${SKETCH_CPP}) + set(${SKETCH_NAME}_LIBS m c) + file(GLOB PDE_SOURCES ${SKETCH_NAME}/*.pde) + file(READ ${SKETCH_PDE} FILE) + file(WRITE ${SKETCH_CPP} ${FILE}) + file(APPEND ${SKETCH_CPP} "#include \"WProgram.h\"") + list(REMOVE_ITEM PDE_SOURCES ${SKETCH_PDE}) + list(SORT PDE_SOURCES) + foreach(PDE ${PDE_SOURCES}) + file(READ ${PDE} FILE) + file(APPEND ${SKETCH_CPP} ${FILE}) + endforeach() + include_directories(${SKETCH_NAME}) + generate_arduino_firmware(${SKETCH_NAME}) + set_target_properties(${SKETCH_NAME} PROPERTIES LINKER_LANGUAGE CXX) install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex + ${CMAKE_CURRENT_BINARY_DIR}/${SKETCH_NAME}.hex DESTINATION bin ) endmacro() # projects -apm_project(apo ${BOARD} apo/apo.cpp) -apm_project(ArduRover ${BOARD} ArduRover/ArduRover.cpp) -apm_project(ArduBoat ${BOARD} ArduBoat/ArduBoat.cpp) -apm_project(ArduPlane ${BOARD} ArduPlane/ArduPlane.cpp) -apm_project(ArduCopter ${BOARD} ArduCopter/ArduCopter.cpp) +add_sketch(apo ${BOARD} ${PORT}) +add_sketch(ArduRover ${BOARD} ${PORT}) +add_sketch(ArduBoat ${BOARD} ${PORT}) +#add_sketch(ArduPlane ${BOARD} ${PORT}) +#add_sketch(ArduCopter ${BOARD} ${PORT})