diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..1d314f6281 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.metadata/ +Tools/ArdupilotMegaPlanner/bin/Release/logs/ +config.mk diff --git a/ArduCopter/CMakeLists.txt b/ArduCopter/CMakeLists.txt new file mode 100644 index 0000000000..bef981f926 --- /dev/null +++ b/ArduCopter/CMakeLists.txt @@ -0,0 +1,162 @@ +#=============================================================================# +# Author: Niklaa Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") + +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +#include(ArduinoProcessing) + +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + + +message(STATUS "DIR: ${CMAKE_SOURCE_DIR}") + +cmake_minimum_required(VERSION 2.8) +#====================================================================# +# Setup Project # +#====================================================================# +project(ArduCopter C CXX) + +find_package(Arduino 22 REQUIRED) + +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter) +#add_subdirectory(testtool) + +PRINT_BOARD_SETTINGS(mega2560) + + + +#=============================================================================# +# Author: Niklas Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME arducopter) + +set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ArduCopter.pde + Attitude.pde + Camera.pde + commands.pde + commands_logic.pde + commands_process.pde + control_modes.pde + events.pde + flight_control.pde + flip.pde + GCS.pde + GCS_Ardupilot.pde + #GCS_IMU_output.pde + GCS_Jason_text.pde + GCS_Mavlink.pde + GCS_Standard.pde + GCS_Xplane.pde + heli.pde + HIL_Xplane.pde + leds.pde + Log.pde + motors_hexa.pde + motors_octa.pde + motors_octa_quad.pde + motors_quad.pde + motors_tri.pde + motors_y6.pde + navigation.pde + planner.pde + radio.pde + read_commands.pde + sensors.pde + setup.pde + system.pde + test.pde + ) # Firmware sketches + +#create dummy sourcefile +file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit") + +set(${FIRMWARE_NAME}_SRCS + #test.cpp + ${FIRMWARE_NAME}.cpp + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + APM_Config.h + APM_Config_mavlink_hil.h + APM_Config_xplane.h + config.h + defines.h + GCS.h + HIL.h + Mavlink_Common.h + Parameters.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + DataFlash + AP_Math + PID + RC_Channel + AP_OpticalFlow + ModeFilter + memcheck + #AP_PID + APM_PI + #APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + c + m +) +SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/DataFlash +${CMAKE_SOURCE_DIR}/libraries/AP_Math +${CMAKE_SOURCE_DIR}/libraries/PID +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/RC_Channel +${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/memcheck +#${CMAKE_SOURCE_DIR}/libraries/AP_PID +${CMAKE_SOURCE_DIR}/libraries/APM_PI +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 4ebb342fd1..40fcc180a9 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -21,15 +21,4 @@ #define HIL_PORT 0 #define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #define GCS_PORT 3 -*/ - - -// ----- Camera definitions ------ -// ------------------------------ -#define CAMERA ENABLED -#define CAM_DEBUG DISABLED - -// - Options to reduce code size - -// ------------------------------- -// Disable text based terminal configuration -#define CLI_ENABLED DISABLED +*/ diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index a01fd6501f..1c71b6b5be 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.24" +#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 @@ -41,9 +41,6 @@ version 2.1 of the License, or (at your option) any later version. #include // RC Channel Library #include // Range finder library #include -#include // Photo or video camera -#include // Camera mount - #include // MAVLink GCS definitions #include @@ -420,14 +417,6 @@ static float load; // % MCU cycles used RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -//Camera tracking and stabilisation stuff -// -------------------------------------- -#if CAMERA == ENABLED -AP_Mount camera_mount(g_gps, &dcm); - -//pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) -#endif - //////////////////////////////////////////////////////////////////////////////// // Top-level logic //////////////////////////////////////////////////////////////////////////////// @@ -575,18 +564,6 @@ static void fast_loop() static void medium_loop() { -#if CAMERA == ENABLED - // TODO replace home with a POI coming from a MavLink message or command - //camera_mount.set_GPS_target(home); - - // For now point the camera manually via the RC inputs (later remove these two lines) - // for simple dcm tests, replace k_manual with k_stabilise - camera_mount.set_mode(AP_Mount::k_stabilise); - camera_mount.update_mount(); - - g.camera.trigger_pic_cleanup(); -#endif - // This is the start of the medium (10 Hz) loop pieces // ----------------------------------------- switch(medium_loopCounter) { @@ -736,9 +713,6 @@ static void slow_loop() update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); -#if CAMERA == ENABLED - camera_mount.update_mount_type(); -#endif break; case 2: diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt new file mode 100644 index 0000000000..1509de51a6 --- /dev/null +++ b/ArduPlane/CMakeLists.txt @@ -0,0 +1,162 @@ +#=============================================================================# +# Author: Niklaa Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") + +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +#include(ArduinoProcessing) + +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + +cmake_minimum_required(VERSION 2.8) +#====================================================================# +# Setup Project # +#====================================================================# +project(ArduPlane C CXX) + +find_package(Arduino 22 REQUIRED) + +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter) +#add_subdirectory(testtool) + +PRINT_BOARD_SETTINGS(mega2560) + + + +#=============================================================================# +# Author: Niklas Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ArduPlane) + +set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ArduPlane.pde + Attitude.pde + climb_rate.pde + commands.pde + commands_logic.pde + commands_process.pde + control_modes.pde + events.pde + #flight_control.pde + #flip.pde + #GCS.pde + #GCS_Ardupilot.pde + #GCS_IMU_output.pde + #GCS_Jason_text.pde + GCS_Mavlink.pde + #GCS_Standard.pde + #GCS_Xplane.pde + #heli.pde + HIL_Xplane.pde + #leds.pde + Log.pde + #motors_hexa.pde + #motors_octa.pde + #motors_octa_quad.pde + #motors_quad.pde + #motors_tri.pde + #motors_y6.pde + navigation.pde + planner.pde + radio.pde + #read_commands.pde + sensors.pde + setup.pde + system.pde + test.pde + ) # Firmware sketches + +#create dummy sourcefile +file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit") + +set(${FIRMWARE_NAME}_SRCS + #test.cpp + ${FIRMWARE_NAME}.cpp + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + APM_Config.h + APM_Config_mavlink_hil.h + APM_Config_xplane.h + config.h + defines.h + GCS.h + HIL.h + Mavlink_Common.h + Parameters.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + DataFlash + AP_Math + PID + RC_Channel + AP_OpticalFlow + ModeFilter + memcheck + #AP_PID + APM_PI + #APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + c + m +) +SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/DataFlash +${CMAKE_SOURCE_DIR}/libraries/AP_Math +${CMAKE_SOURCE_DIR}/libraries/PID +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/RC_Channel +${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/memcheck +#${CMAKE_SOURCE_DIR}/libraries/AP_PID +${CMAKE_SOURCE_DIR}/libraries/APM_PI +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +#new +#${CMAKE_SOURCE_DIR}/libraries/Wire +#${CMAKE_SOURCE_DIR}/libraries/SPI +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7b62d9a8fa..70976ce887 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -112,13 +112,6 @@ public: k_param_RTL_altitude, k_param_inverted_flight_ch, - // - // Camera parameters - // -#if CAMERA == ENABLED - k_param_camera, -#endif - // // 170: Radio settings // @@ -324,11 +317,6 @@ public: AP_Int8 flap_2_percent; AP_Int8 flap_2_speed; - // Camera -#if CAMERA == ENABLED - AP_Camera camera; -#endif - // RC channels RC_Channel channel_roll; RC_Channel channel_pitch; @@ -434,10 +422,6 @@ public: // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! -#if CAMERA == ENABLED - camera (k_param_camera, PSTR("CAM_")), -#endif - // RC channel group key name //---------------------------------------------------------------------- channel_roll (k_param_channel_roll, PSTR("RC1_")), diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde deleted file mode 100644 index 474644cb90..0000000000 --- a/ArduPlane/WP_activity.pde +++ /dev/null @@ -1,56 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CAMERA == ENABLED -void waypoint_check() -{ - if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you - g.camera.trigger_pic(); - } // DO SOMETHIMNG - } - if(g.waypoint_index == 20){ // When here do whats underneath - g.camera.trigger_pic(); - } -} - -void picture_time_check() -{ - if (g.camera.picture_time == 1){ - if (wp_distance < g.camera.wp_distance_min){ - g.camera.trigger_pic(); // or any camera activation command - } - } -} -#endif - -void egg_waypoint() -{ - if (g_rc_function[RC_Channel_aux::k_egg_drop]) - { - float temp = (float)(current_loc.alt - home.alt) * .01; - float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; - - if(g.waypoint_index == 3){ - if(wp_distance < egg_dist){ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 100; - } - }else{ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 0; - } - } -} - -#if CAMERA == ENABLED -void johann_check() // if you aren't Johann it doesn't really matter :D -{ - APM_RC.OutputCh(CH_7,1500 + (350)); - if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you - g.camera.trigger_pic(); - } - } - if(g.waypoint_index == 20){ // When here do whats underneath - g.camera.trigger_pic(); - } -} -#endif diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 5b26479ce6..674c096e25 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -24,26 +24,6 @@ static void init_rc_in() //set auxiliary ranges update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); - G_RC_AUX(k_flap)->set_range(0,100); - G_RC_AUX(k_flap_auto)->set_range(0,100); - G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); - G_RC_AUX(k_flaperon)->set_range(0,100); -#if CAMERA == ENABLED - G_RC_AUX(k_mount_yaw)->set_range( - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); - G_RC_AUX(k_mount_pitch)->set_range( - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); - G_RC_AUX(k_mount_roll)->set_range( - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); - G_RC_AUX(k_cam_trigger)->set_range( - g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); - G_RC_AUX(k_cam_open)->set_range(0,100); -#endif - G_RC_AUX(k_egg_drop)->set_range(0,100); } static void init_rc_out() @@ -56,7 +36,7 @@ static void init_rc_out() 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.OutputCh(CH_8, g.rc_8.radio_trim); APM_RC.Init(); // APM Radio initialization diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 6ac38ed914..ddfdd3e8d6 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -232,7 +232,6 @@ UserControl - Component @@ -302,7 +301,6 @@ ElevationProfile.cs - Component @@ -338,6 +336,7 @@ Splash.cs + Form @@ -374,6 +373,7 @@ Firmware.cs + Designer FlightData.cs @@ -424,6 +424,7 @@ Firmware.cs Always + Designer FlightData.cs diff --git a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs index 99660f265f..94e3281f84 100644 --- a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs @@ -6,7 +6,6 @@ using System.IO.Ports; using System.Threading; using System.Net; // dns, ip address using System.Net.Sockets; // tcplistner -using SerialProxy; namespace System.IO.Ports { diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs index 9dba66f3d1..8758775aac 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs @@ -257,6 +257,8 @@ namespace ArdupilotMega.GCSViews internal void processToScreen() { + + Params.Rows.Clear(); // process hashdefines and update display @@ -541,7 +543,10 @@ namespace ArdupilotMega.GCSViews private void BUT_writePIDS_Click(object sender, EventArgs e) { - foreach (string value in changes.Keys) + + Hashtable temp = (Hashtable)changes.Clone(); + + foreach (string value in temp.Keys) { try { @@ -566,6 +571,7 @@ namespace ArdupilotMega.GCSViews if (row.Cells[0].Value.ToString() == value) { row.Cells[1].Style.BackColor = Color.FromArgb(0x43, 0x44, 0x45); + changes.Remove(value); break; } } @@ -575,8 +581,6 @@ namespace ArdupilotMega.GCSViews } catch { MessageBox.Show("Set " + value + " Failed"); } } - - changes.Clear(); } const float rad2deg = (float)(180 / Math.PI); @@ -600,9 +604,11 @@ namespace ArdupilotMega.GCSViews MainV2.fixtheme(temp); - TabSetup.Controls.Clear(); + temp.ShowDialog(); - TabSetup.Controls.Add(temp.Controls[0]); + startup = true; + processToScreen(); + startup = false; } } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index 1e7c2544c7..15f3a849f7 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -519,13 +519,18 @@ namespace ArdupilotMega.GCSViews } catch (Exception ex) { lbl_status.Text = "Failed download"; MessageBox.Show("Failed to download new firmware : " + ex.Message); return; } + UploadFlash(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex", board); + } + + void UploadFlash(string filename, string board) + { byte[] FLASH = new byte[1]; StreamReader sr = null; try { lbl_status.Text = "Reading Hex File"; this.Refresh(); - sr = new StreamReader(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex"); + sr = new StreamReader(filename); FLASH = readIntelHEXv2(sr); sr.Close(); Console.WriteLine("\n\nSize: {0}\n\n", FLASH.Length); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx index 95f999f7a7..7283c0a474 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx @@ -343,16 +343,16 @@ NoControl - 101, 165 + 113, 167 - 79, 13 + 56, 13 8 - ArduPilot Mega + ArduPlane label1 @@ -403,16 +403,16 @@ NoControl - 57, 362 + 74, 361 - 168, 13 + 142, 13 10 - ArduPilot Mega (Xplane simulator) + ArduPlane (Xplane simulator) label3 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 21732c373e..e144fa180a 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -347,7 +347,7 @@ namespace ArdupilotMega.GCSViews { TXT_mouselat.Text = lat.ToString(); TXT_mouselong.Text = lng.ToString(); - TXT_mousealt.Text = alt.ToString(); + TXT_mousealt.Text = srtm.getAltitude(lat, lng).ToString("0"); try { diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs index b4e8e178f4..b95cd51577 100644 --- a/Tools/ArdupilotMegaPlanner/HUD.cs +++ b/Tools/ArdupilotMegaPlanner/HUD.cs @@ -236,7 +236,7 @@ System.ComponentModel.Category("Values")] { //Console.WriteLine("ms "+(DateTime.Now - starttime).TotalMilliseconds); //e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); - return; + //return; } starttime = DateTime.Now; @@ -1365,6 +1365,18 @@ System.ComponentModel.Category("Values")] charbitmaps = new Bitmap[charbitmaps.Length]; + try + { + + foreach (int texid in charbitmaptexid) + { + if (texid != 0) + GL.DeleteTexture(texid); + } + + } + catch { } + GC.Collect(); try diff --git a/Tools/ArdupilotMegaPlanner/Log.cs b/Tools/ArdupilotMegaPlanner/Log.cs index e47cb2c35d..e51d159de0 100644 --- a/Tools/ArdupilotMegaPlanner/Log.cs +++ b/Tools/ArdupilotMegaPlanner/Log.cs @@ -366,45 +366,6 @@ namespace ArdupilotMega } } - private byte[] readline(NetSerial comport) - { - byte[] temp = new byte[1024]; - int count = 0; - int timeout = 0; - - while (timeout <= 100) - { - if (!comport.IsOpen) { break; } - if (comport.BytesToRead > 0) - { - byte letter = (byte)comport.ReadByte(); - - temp[count] = letter; - - if (letter == '\n') // normal line - { - break; - } - - - count++; - if (count == temp.Length) - break; - timeout = 0; - } else { - timeout++; - System.Threading.Thread.Sleep(5); - } - } - if (timeout >= 100) { - Console.WriteLine("readline timeout"); - } - - Array.Resize(ref temp, count + 1); - - return temp; - } - List modelist = new List(); List[] position = new List[200]; int positionindex = 0; diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 0c26ebb6cd..242b2818aa 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -951,6 +951,8 @@ namespace ArdupilotMega req.seq = index; + //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); + // request generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req); @@ -972,11 +974,14 @@ namespace ArdupilotMega MainV2.givecomport = false; throw new Exception("Timeout on read - getWP"); } + //Console.WriteLine("getwp read " + DateTime.Now.Millisecond); byte[] buffer = readPacket(); + //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT) { + //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); __mavlink_waypoint_t wp = new __mavlink_waypoint_t(); object temp = (object)wp; @@ -1070,7 +1075,7 @@ namespace ArdupilotMega } else { - //Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); + Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } @@ -1455,16 +1460,18 @@ namespace ArdupilotMega else { int to = 0; - while (BaseStream.BytesToRead < length) + while (BaseStream.BytesToRead < (length - 4)) { if (to > 1000) { Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length); break; } - System.Threading.Thread.Sleep(2); - System.Windows.Forms.Application.DoEvents(); + System.Threading.Thread.Sleep(1); + //System.Windows.Forms.Application.DoEvents(); to++; + + //Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead); } int read = BaseStream.Read(temp, 6, length - 4); } diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 48dac31e32..451e8acea8 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -76,6 +76,8 @@ namespace ArdupilotMega //System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US"); //System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US"); + srtm.datadirectory = @"C:\srtm"; + var t = Type.GetType("Mono.Runtime"); MAC = (t != null); diff --git a/Tools/ArdupilotMegaPlanner/MyButton.cs b/Tools/ArdupilotMegaPlanner/MyButton.cs index 12508be1f7..11bf6cd531 100644 --- a/Tools/ArdupilotMegaPlanner/MyButton.cs +++ b/Tools/ArdupilotMegaPlanner/MyButton.cs @@ -50,7 +50,7 @@ namespace ArdupilotMega if (!this.Enabled) { - SolidBrush brush = new SolidBrush(Color.FromArgb(200, 0x2b, 0x3a, 0x03)); + SolidBrush brush = new SolidBrush(Color.FromArgb(150, 0x2b, 0x3a, 0x03)); gr.FillRectangle(brush, 0, 0, this.Width, this.Height); } diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 25abe98b99..d4f74b5646 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.67")] +[assembly: AssemblyFileVersion("1.0.68")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 53279ab634..17c296b914 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -231,7 +231,6 @@ namespace ArdupilotMega.Setup MainV2.comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS, oldrc); - MainV2.comPort.param = MainV2.comPort.getParamList(); if (Configuration != null) { Configuration.startup = true; diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx index 95f999f7a7..7283c0a474 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx @@ -343,16 +343,16 @@ NoControl - 101, 165 + 113, 167 - 79, 13 + 56, 13 8 - ArduPilot Mega + ArduPlane label1 @@ -403,16 +403,16 @@ NoControl - 57, 362 + 74, 361 - 168, 13 + 142, 13 10 - ArduPilot Mega (Xplane simulator) + ArduPlane (Xplane simulator) label3 diff --git a/Tools/ArdupilotMegaPlanner/srtm.cs b/Tools/ArdupilotMegaPlanner/srtm.cs new file mode 100644 index 0000000000..e11c6ecb9b --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/srtm.cs @@ -0,0 +1,76 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO; + +namespace ArdupilotMega +{ + class srtm + { + public static string datadirectory; + + public static int getAltitude(double lat, double lng) + { + short alt = -32768; + + lat += 0.0008; + //lng += 0.0008; + + int x = (int)Math.Floor(lng); + int y = (int)Math.Floor(lat); + + string ns; + if (y > 0) + ns = "N"; + else + ns = "S"; + + string ew; + if (x > 0) + ew = "E"; + else + ew = "W"; + + string filename = ns+ Math.Abs(y).ToString("00")+ew+ Math.Abs(x).ToString("000")+".hgt"; + + if (!File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) + { + return alt; + } + + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open,FileAccess.Read); + + float posx = 0; + float row = 0; + + if (fs.Length <= (1201 * 1201 * 2)) { + posx = (int)(((float)(lng - x)) * (1201 * 2)); + row = (int)(((float)(lat - y)) * 1201) * (1201 * 2); + row = (1201 * 1201 * 2) - row; + } else { + posx = (int)(((float)(lng - x)) * (3601 * 2)); + row = (int)(((float)(lat - y)) * 3601) * (3601 * 2); + row = (3601 * 3601 * 2) - row; + } + + if (posx % 2 == 1) + { + posx--; + } + + //Console.WriteLine(filename + " row " + row + " posx" + posx); + + byte[] data = new byte[2]; + + fs.Seek((int)(row + posx), SeekOrigin.Begin); + fs.Read(data, 0, data.Length); + + Array.Reverse(data); + + alt = BitConverter.ToInt16(data,0); + + return alt; + } + } +} diff --git a/cmake/PdeP.jar b/cmake/PdeP.jar new file mode 100644 index 0000000000..ae76f9b762 Binary files /dev/null and b/cmake/PdeP.jar differ diff --git a/cmake/modules/ArduinoProcessing.cmake b/cmake/modules/ArduinoProcessing.cmake new file mode 100644 index 0000000000..c7a2407a2a --- /dev/null +++ b/cmake/modules/ArduinoProcessing.cmake @@ -0,0 +1,103 @@ +# 1. Concatenate all PDE files +# 2. Write #include "WProgram.h" +# 3. Write prototypes +# 4. Write original sources +# +# +# Prefix Writer +# 1. Scrub comments +# 2. Optionally subsitute Unicode +# 3. Find imports +# 4. Find prototypes +# +# Find prototypes +# 1. Strip comments, quotes, preprocessor directives +# 2. Collapse braches +# 3. Regex + + +set(SINGLE_QUOTES_REGEX "('.')") +set(DOUBLE_QUOTES_REGEX "(\"([^\"\\\\]|\\\\.)*\")") +set(SINGLE_COMMENT_REGEX "([ ]*//[^\n]*)") +set(MULTI_COMMENT_REGEX "(/[*][^/]*[*]/)") +set(PREPROC_REGEX "([ ]*#(\\\\[\n]|[^\n])*)") + +#"[\w\[\]\*]+\s+[&\[\]\*\w\s]+\([&,\[\]\*\w\s]*\)(?=\s*\{)" +set(PROTOTPYE_REGEX "([a-zA-Z0-9]+[ ]*)*[a-zA-Z0-9]+[ ]*\([^{]*\)[ ]*{") + +function(READ_SKETCHES VAR_NAME ) + set(SKETCH_SOURCE) + foreach(SKETCH ${ARGN}) + if(EXISTS ${SKETCH}) + message(STATUS "${SKETCH}") + file(READ ${SKETCH} SKETCH_CONTENTS) + set(SKETCH_SOURCE "${SKETCH_SOURCE}\n${SKETCH_CONTENTS}") + else() + message(FATAL_ERROR "Sketch file does not exist: ${SKETCH}") + endif() + endforeach() + set(${VAR_NAME} "${SKETCH_SOURCE}" PARENT_SCOPE) +endfunction() + +function(STRIP_SOURCES VAR_NAME SOURCES) + string(REGEX REPLACE "${SINGLE_QUOTES_REGEX}|${DOUBLE_QUOTES_REGEX}|${SINGLE_COMMENT_REGEX}|${MULTI_COMMENT_REGEX}|${PREPROC_REGEX}" + "" + SOURCES + "${SOURCES}") + set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE) +endfunction() + +function(COLLAPSE_BRACES VAR_NAME SOURCES) + set(PARSED_SOURCES) + string(LENGTH "${SOURCES}" SOURCES_LENGTH) + math(EXPR SOURCES_LENGTH "${SOURCES_LENGTH}-1") + + set(NESTING 0) + set(START 0) + foreach(INDEX RANGE ${SOURCES_LENGTH}) + string(SUBSTRING "${SOURCES}" ${INDEX} 1 CURRENT_CHAR) + #message("${CURRENT_CHAR}") + if(CURRENT_CHAR STREQUAL "{") + if(NESTING EQUAL 0) + math(EXPR SUBLENGTH "${INDEX}-${START} +1") + string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK) + set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}") + #message("INDEX: ${INDEX} START: ${START} LENGTH: ${SUBLENGTH}") + endif() + math(EXPR NESTING "${NESTING}+1") + elseif(CURRENT_CHAR STREQUAL "}") + math(EXPR NESTING "${NESTING}-1") + if(NESTING EQUAL 0) + set(START ${INDEX}) + endif() + endif() + endforeach() + + math(EXPR SUBLENGTH "${SOURCES_LENGTH}-${START} +1") + string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK) + set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}") + + set(${VAR_NAME} "${PARSED_SOURCES}" PARENT_SCOPE) +endfunction() + +function(extract_prototypes VAR_NAME SOURCES) + string(REGEX MATCHALL "${PROTOTPYE_REGEX}" + SOURCES + "${SOURCES}") + set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE) +endfunction() + +read_sketches(SKETCH_SOURCE ${FILES}) +strip_sources(SKETCH_SOURCE "${SKETCH_SOURCE}") +collapse_braces(SKETCH_SOURCE "${SKETCH_SOURCE}") +extract_prototypes(SKETCH_SOURCE "${SKETCH_SOURCE}") + + + + +message("===============") +foreach(ENTRY ${SKETCH_SOURCE}) + message("START]]]${ENTRY}[[[END") +endforeach() +message("===============") +#message("${SKETCH_SOURCE}") diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake new file mode 100644 index 0000000000..7faadeba15 --- /dev/null +++ b/cmake/modules/FindArduino.cmake @@ -0,0 +1,581 @@ +# - Generate firmware and libraries for Arduino Devices +# generate_arduino_firmware(TARGET_NAME) +# TARGET_NAME - Name of target +# Creates a Arduino firmware target. +# +# The target options can be configured by setting options of +# the following format: +# ${TARGET_NAME}${SUFFIX} +# The following suffixes are availabe: +# _SRCS # Sources +# _HDRS # Headers +# _SKETCHES # Arduino sketch files +# _LIBS # Libraries to linked in +# _BOARD # Board name (such as uno, mega2560, ...) +# _PORT # Serial port, for upload and serial targets [OPTIONAL] +# _AFLAGS # Override global Avrdude flags for target +# _SERIAL # Serial command for serial target [OPTIONAL] +# _NO_AUTOLIBS # Disables Arduino library detection +# Here is a short example for a target named test: +# set(test_SRCS test.cpp) +# set(test_HDRS test.h) +# set(test_BOARD uno) +# +# generate_arduino_firmware(test) +# +# +# generate_arduino_library(TARGET_NAME) +# TARGET_NAME - Name of target +# Creates a Arduino firmware target. +# +# The target options can be configured by setting options of +# the following format: +# ${TARGET_NAME}${SUFFIX} +# The following suffixes are availabe: +# +# _SRCS # Sources +# _HDRS # Headers +# _LIBS # Libraries to linked in +# _BOARD # Board name (such as uno, mega2560, ...) +# _NO_AUTOLIBS # Disables Arduino library detection +# +# Here is a short example for a target named test: +# set(test_SRCS test.cpp) +# set(test_HDRS test.h) +# set(test_BOARD uno) +# +# generate_arduino_library(test) + + +find_path(ARDUINO_SDK_PATH + NAMES lib/version.txt hardware libraries + PATH_SUFFIXES share/arduino + DOC "Arduino Development Kit path.") + + +# load_board_settings() +# +# Load the Arduino SDK board settings from the boards.txt file. +# +function(LOAD_BOARD_SETTINGS) + if(NOT ARDUINO_BOARDS AND ARDUINO_BOARDS_PATH) + file(STRINGS ${ARDUINO_BOARDS_PATH} BOARD_SETTINGS) + foreach(BOARD_SETTING ${BOARD_SETTINGS}) + if("${BOARD_SETTING}" MATCHES "^.*=.*") + string(REGEX MATCH "^[^=]+" SETTING_NAME ${BOARD_SETTING}) + string(REGEX MATCH "[^=]+$" SETTING_VALUE ${BOARD_SETTING}) + string(REPLACE "." ";" SETTING_NAME_TOKENS ${SETTING_NAME}) + + list(LENGTH SETTING_NAME_TOKENS SETTING_NAME_TOKENS_LEN) + + + # Add Arduino to main list of arduino boards + list(GET SETTING_NAME_TOKENS 0 BOARD_ID) + list(FIND ARDUINO_BOARDS ${BOARD_ID} ARDUINO_BOARD_INDEX) + if(ARDUINO_BOARD_INDEX LESS 0) + list(APPEND ARDUINO_BOARDS ${BOARD_ID}) + endif() + + # Add setting to board settings list + list(GET SETTING_NAME_TOKENS 1 BOARD_SETTING) + list(FIND ${BOARD_ID}.SETTINGS ${BOARD_SETTING} BOARD_SETTINGS_LEN) + if(BOARD_SETTINGS_LEN LESS 0) + list(APPEND ${BOARD_ID}.SETTINGS ${BOARD_SETTING}) + set(${BOARD_ID}.SETTINGS ${${BOARD_ID}.SETTINGS} + CACHE INTERNAL "Arduino ${BOARD_ID} Board settings list") + endif() + + set(ARDUINO_SETTING_NAME ${BOARD_ID}.${BOARD_SETTING}) + + # Add sub-setting to board sub-settings list + if(SETTING_NAME_TOKENS_LEN GREATER 2) + list(GET SETTING_NAME_TOKENS 2 BOARD_SUBSETTING) + list(FIND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING} BOARD_SUBSETTINGS_LEN) + if(BOARD_SUBSETTINGS_LEN LESS 0) + list(APPEND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING}) + set(${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS} + CACHE INTERNAL "Arduino ${BOARD_ID} Board sub-settings list") + endif() + set(ARDUINO_SETTING_NAME ${ARDUINO_SETTING_NAME}.${BOARD_SUBSETTING}) + endif() + + # Save setting value + set(${ARDUINO_SETTING_NAME} ${SETTING_VALUE} CACHE INTERNAL "Arduino ${BOARD_ID} Board setting") + + + endif() + endforeach() + set(ARDUINO_BOARDS ${ARDUINO_BOARDS} CACHE STRING "List of detected Arduino Board configurations") + mark_as_advanced(ARDUINO_BOARDS) + endif() +endfunction() + +# print_board_settings(ARDUINO_BOARD) +# +# ARDUINO_BOARD - Board id +# +# Print the detected Arduino board settings. +# +function(PRINT_BOARD_SETTINGS ARDUINO_BOARD) + if(${ARDUINO_BOARD}.SETTINGS) + + message(STATUS "Arduino ${ARDUINO_BOARD} Board:") + foreach(BOARD_SETTING ${${ARDUINO_BOARD}.SETTINGS}) + if(${ARDUINO_BOARD}.${BOARD_SETTING}) + message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}}") + endif() + if(${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS) + foreach(BOARD_SUBSETTING ${${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS}) + if(${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}) + message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}}") + endif() + endforeach() + endif() + message(STATUS "") + endforeach() + endif() +endfunction() + + + +# generate_arduino_library(TARGET_NAME) +# +# see documentation at top +function(GENERATE_ARDUINO_LIBRARY TARGET_NAME) + load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources + _HDRS # Headers + _LIBS # Libraries to linked in + _BOARD) # Board name (such as uno, mega2560, ...) + set(INPUT_AUTOLIBS True) + if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS) + set(INPUT_AUTOLIBS False) + endif() + + message(STATUS "Generating ${TARGET_NAME}") + + set(ALL_LIBS) + set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS}) + + setup_arduino_compiler(${INPUT_BOARD}) + setup_arduino_core(CORE_LIB ${INPUT_BOARD}) + + if(INPUT_AUTOLIBS) + setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}") + endif() + + list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS}) + + add_library(${TARGET_NAME} ${ALL_SRCS}) + target_link_libraries(${TARGET_NAME} ${ALL_LIBS}) +endfunction() + +# generate_arduino_firmware(TARGET_NAME) +# +# see documentation at top +function(GENERATE_ARDUINO_FIRMWARE TARGET_NAME) + load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources + _HDRS # Headers + _LIBS # Libraries to linked in + _BOARD # Board name (such as uno, mega2560, ...) + _PORT # Serial port, for upload and serial targets + _AFLAGS # Override global Avrdude flags for target + _SKETCHES # Arduino sketch files + _SERIAL) # Serial command for serial target + + set(INPUT_AUTOLIBS True) + if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS) + set(INPUT_AUTOLIBS False) + endif() + + message(STATUS "Generating ${TARGET_NAME}") + message(STATUS "Sketches ${INPUT_SKETCHES}") + + set(ALL_LIBS) + set(ALL_SRCS ${INPUT_SKETCHES} ${INPUT_SRCS} ${INPUT_HDRS} ) + #set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS} ) + + #set compile flags and file properties for pde files + #SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES LANGUAGE CXX) + #SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES COMPILE_FLAGS "-x c++" ) + + setup_arduino_compiler(${INPUT_BOARD}) + setup_arduino_core(CORE_LIB ${INPUT_BOARD}) + + #setup_arduino_sketch(SKETCH_SRCS ${INPUT_SKETCHES}) + + if(INPUT_AUTOLIBS) + setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}") + endif() + + + list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS}) + + setup_arduino_target(${TARGET_NAME} "${ALL_SRCS}" "${ALL_LIBS}") + #SET_TARGET_PROPERTIES(${TARGET_NAME} PROPERTIES LINKER_LANGUAGE CXX) + #setup_arduino_target(${TARGET_NAME} "${INPUT_SKETCHES}" ${INPUT_HDRS} "${ALL_LIBS}") + + if(INPUT_PORT) + setup_arduino_upload(${INPUT_BOARD} ${TARGET_NAME} ${INPUT_PORT}) + endif() + + if(INPUT_SERIAL) + setup_serial_target(${TARGET_NAME} "${INPUT_SERIAL}") + endif() +endfunction() + + +# load_generator_settings(TARGET_NAME PREFIX [SUFFIX_1 SUFFIX_2 .. SUFFIX_N]) +# +# TARGET_NAME - The base name of the user settings +# PREFIX - The prefix name used for generator settings +# SUFFIX_XX - List of suffixes to load +# +# Loads a list of user settings into the generators scope. User settings have +# the following syntax: +# +# ${BASE_NAME}${SUFFIX} +# +# The BASE_NAME is the target name and the suffix is a specific generator settings. +# +# For every user setting found a generator setting is created of the follwoing fromat: +# +# ${PREFIX}${SUFFIX} +# +# The purpose of loading the settings into the generator is to not modify user settings +# and to have a generic naming of the settings within the generator. +# +function(LOAD_GENERATOR_SETTINGS TARGET_NAME PREFIX) + foreach(GEN_SUFFIX ${ARGN}) + if(${TARGET_NAME}${GEN_SUFFIX}) + set(${PREFIX}${GEN_SUFFIX} ${${TARGET_NAME}${GEN_SUFFIX}} PARENT_SCOPE) + endif() + endforeach() +endfunction() + +# setup_arduino_compiler(BOARD_ID) +# +# BOARD_ID - The board id name +# +# Configures the the build settings for the specified Arduino Board. +# +macro(setup_arduino_compiler BOARD_ID) + set(BOARD_CORE ${${BOARD_ID}.build.core}) + if(BOARD_CORE) + set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE}) + include_directories(${BOARD_CORE_PATH}) + include_directories(${ARDUINO_LIBRARIES_PATH}) + add_definitions(-DF_CPU=${${BOARD_ID}.build.f_cpu} + -DARDUINO=${ARDUINO_SDK_VERSION} + -mmcu=${${BOARD_ID}.build.mcu} + ) + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + endif() +endmacro() + +# setup_arduino_core(VAR_NAME BOARD_ID) +# +# VAR_NAME - Variable name that will hold the generated library name +# BOARD_ID - Arduino board id +# +# Creates the Arduino Core library for the specified board, +# each board gets it's own version of the library. +# +function(setup_arduino_core VAR_NAME BOARD_ID) + set(CORE_LIB_NAME ${BOARD_ID}_CORE) + set(BOARD_CORE ${${BOARD_ID}.build.core}) + if(BOARD_CORE AND NOT TARGET ${CORE_LIB_NAME}) + set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE}) + find_sources(CORE_SRCS ${BOARD_CORE_PATH}) + add_library(${CORE_LIB_NAME} ${CORE_SRCS}) + set(${VAR_NAME} ${CORE_LIB_NAME} PARENT_SCOPE) + endif() +endfunction() + +# find_arduino_libraries(VAR_NAME SRCS) +# +# VAR_NAME - Variable name which will hold the results +# SRCS - Sources that will be analized +# +# returns a list of paths to libraries found. +# +# Finds all Arduino type libraries included in sources. Available libraries +# are ${ARDUINO_SDK_PATH}/libraries and ${CMAKE_CURRENT_SOURCE_DIR}. +# +# A Arduino library is a folder that has the same name as the include header. +# For example, if we have a include "#include " then the following +# directory structure is considered a Arduino library: +# +# LibraryName/ +# |- LibraryName.h +# `- LibraryName.c +# +# If such a directory is found then all sources within that directory are considred +# to be part of that Arduino library. +# +function(find_arduino_libraries VAR_NAME SRCS) + set(ARDUINO_LIBS ) + foreach(SRC ${SRCS}) + file(STRINGS ${SRC} SRC_CONTENTS) + foreach(SRC_LINE ${SRC_CONTENTS}) + if("${SRC_LINE}" MATCHES "^ *#include *[<\"](.*)[>\"]") + get_filename_component(INCLUDE_NAME ${CMAKE_MATCH_1} NAME_WE) + foreach(LIB_SEARCH_PATH ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR}) + if(EXISTS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}/${CMAKE_MATCH_1}) + list(APPEND ARDUINO_LIBS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}) + break() + endif() + endforeach() + endif() + endforeach() + endforeach() + if(ARDUINO_LIBS) + list(REMOVE_DUPLICATES ARDUINO_LIBS) + endif() + set(${VAR_NAME} ${ARDUINO_LIBS} PARENT_SCOPE) +endfunction() + +# setup_arduino_library(VAR_NAME BOARD_ID LIB_PATH) +# +# VAR_NAME - Vairable wich will hold the generated library names +# BOARD_ID - Board name +# LIB_PATH - path of the library +# +# Creates an Arduino library, with all it's library dependencies. +# +function(setup_arduino_library VAR_NAME BOARD_ID LIB_PATH) + set(LIB_TARGETS) + get_filename_component(LIB_NAME ${LIB_PATH} NAME) + set(TARGET_LIB_NAME ${BOARD_ID}_${LIB_NAME}) + if(NOT TARGET ${TARGET_LIB_NAME}) + find_sources(LIB_SRCS ${LIB_PATH}) + if(LIB_SRCS) + + message(STATUS "Generating Arduino ${LIB_NAME} library") + include_directories(${LIB_PATH} ${LIB_PATH}/utility) + add_library(${TARGET_LIB_NAME} STATIC ${LIB_SRCS}) + + find_arduino_libraries(LIB_DEPS "${LIB_SRCS}") + foreach(LIB_DEP ${LIB_DEPS}) + setup_arduino_library(DEP_LIB_SRCS ${BOARD_ID} ${LIB_DEP}) + list(APPEND LIB_TARGETS ${DEP_LIB_SRCS}) + endforeach() + + target_link_libraries(${TARGET_LIB_NAME} ${BOARD_ID}_CORE ${LIB_TARGETS}) + list(APPEND LIB_TARGETS ${TARGET_LIB_NAME}) + endif() + else() + # Target already exists, skiping creating + include_directories(${LIB_PATH} ${LIB_PATH}/utility) + list(APPEND LIB_TARGETS ${TARGET_LIB_NAME}) + endif() + if(LIB_TARGETS) + list(REMOVE_DUPLICATES LIB_TARGETS) + endif() + set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE) +endfunction() + +# setup_arduino_libraries(VAR_NAME BOARD_ID SRCS) +# +# VAR_NAME - Vairable wich will hold the generated library names +# BOARD_ID - Board ID +# SRCS - source files +# +# Finds and creates all dependency libraries based on sources. +# +function(setup_arduino_libraries VAR_NAME BOARD_ID SRCS) + set(LIB_TARGETS) + find_arduino_libraries(TARGET_LIBS ${SRCS}) + foreach(TARGET_LIB ${TARGET_LIBS}) + setup_arduino_library(LIB_DEPS ${BOARD_ID} ${TARGET_LIB}) # Create static library instead of returning sources + list(APPEND LIB_TARGETS ${LIB_DEPS}) + endforeach() + set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE) +endfunction() + + +# setup_arduino_target(TARGET_NAME ALL_SRCS ALL_LIBS) +# +# TARGET_NAME - Target name +# ALL_SRCS - All sources +# ALL_LIBS - All libraries +# +# Creates an Arduino firmware target. +# +function(setup_arduino_target TARGET_NAME ALL_SRCS ALL_LIBS) + add_executable(${TARGET_NAME} ${ALL_SRCS}) + target_link_libraries(${TARGET_NAME} ${ALL_LIBS}) + set_target_properties(${TARGET_NAME} PROPERTIES SUFFIX ".elf") + + set(TARGET_PATH ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}) + add_custom_command(TARGET ${TARGET_NAME} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} + ARGS ${ARDUINO_OBJCOPY_EEP_FLAGS} + ${TARGET_PATH}.elf + ${TARGET_PATH}.eep + VERBATIM) + add_custom_command(TARGET ${TARGET_NAME} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} + ARGS ${ARDUINO_OBJCOPY_HEX_FLAGS} + ${TARGET_PATH}.elf + ${TARGET_PATH}.hex + VERBATIM) +endfunction() + +# setup_arduino_upload(BOARD_ID TARGET_NAME PORT) +# +# BOARD_ID - Arduino board id +# TARGET_NAME - Target name +# PORT - Serial port for upload +# +# Create an upload target (${TARGET_NAME}-upload) for the specified Arduino target. +# +function(setup_arduino_upload BOARD_ID TARGET_NAME PORT) + set(AVRDUDE_FLAGS ${ARDUINO_AVRDUDE_FLAGS}) + if(DEFINED ${TARGET_NAME}_AFLAGS) + set(AVRDUDE_FLAGS ${${TARGET_NAME}_AFLAGS}) + endif() + add_custom_target(${TARGET_NAME}-upload + ${ARDUINO_AVRDUDE_PROGRAM} + -U flash:w:${TARGET_NAME}.hex + ${AVRDUDE_FLAGS} + -C ${ARDUINO_AVRDUDE_CONFIG_PATH} + -p ${${BOARD_ID}.build.mcu} + -c ${${BOARD_ID}.upload.protocol} + -b ${${BOARD_ID}.upload.speed} + -P ${PORT} + DEPENDS ${TARGET_NAME}) + if(NOT TARGET upload) + add_custom_target(upload) + endif() + add_dependencies(upload ${TARGET_NAME}-upload) +endfunction() + +# find_sources(VAR_NAME LIB_PATH) +# +# VAR_NAME - Variable name that will hold the detected sources +# LIB_PATH - The base path +# +# Finds all C/C++ sources located at the specified path. +# +function(find_sources VAR_NAME LIB_PATH) + file(GLOB_RECURSE LIB_FILES ${LIB_PATH}/*.cpp + ${LIB_PATH}/*.c + ${LIB_PATH}/*.cc + ${LIB_PATH}/*.cxx + ${LIB_PATH}/*.h + ${LIB_PATH}/*.hh + ${LIB_PATH}/*.hxx) + if(LIB_FILES) + set(${VAR_NAME} ${LIB_FILES} PARENT_SCOPE) + endif() +endfunction() + +# setup_serial_target(TARGET_NAME CMD) +# +# TARGET_NAME - Target name +# CMD - Serial terminal command +# +# Creates a target (${TARGET_NAME}-serial) for launching the serial termnial. +# +function(setup_serial_target TARGET_NAME CMD) + string(CONFIGURE "${CMD}" FULL_CMD @ONLY) + add_custom_target(${TARGET_NAME}-serial + ${FULL_CMD}) +endfunction() + + +# detect_arduino_version(VAR_NAME) +# +# VAR_NAME - Variable name where the detected version will be saved +# +# Detects the Arduino SDK Version based on the revisions.txt file. +# +function(detect_arduino_version VAR_NAME) + if(ARDUINO_VERSION_PATH) + file(READ ${ARDUINO_VERSION_PATH} ARD_VERSION) + if("${ARD_VERSION}" MATCHES " *[0]+([0-9]+)") + set(${VAR_NAME} ${CMAKE_MATCH_1} PARENT_SCOPE) + endif() + endif() +endfunction() + + +function(convert_arduino_sketch VAR_NAME SRCS) +endfunction() + + +# Setting up Arduino enviroment settings +if(NOT ARDUINO_FOUND) + find_file(ARDUINO_CORES_PATH + NAMES cores + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_LIBRARIES_PATH + NAMES libraries + PATHS ${ARDUINO_SDK_PATH}) + + find_file(ARDUINO_BOARDS_PATH + NAMES boards.txt + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_PROGRAMMERS_PATH + NAMES programmers.txt + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_REVISIONS_PATH + NAMES revisions.txt + PATHS ${ARDUINO_SDK_PATH}) + + find_file(ARDUINO_VERSION_PATH + NAMES lib/version.txt + PATHS ${ARDUINO_SDK_PATH}) + + find_program(ARDUINO_AVRDUDE_PROGRAM + NAMES avrdude + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/tools) + + find_program(ARDUINO_AVRDUDE_CONFIG_PATH + NAMES avrdude.conf + PATHS ${ARDUINO_SDK_PATH} /etc/avrdude + PATH_SUFFIXES hardware/tools + hardware/tools/avr/etc) + + set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0 + CACHE STRING "") + set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom + CACHE STRING "") + set(ARDUINO_AVRDUDE_FLAGS -V -F + CACHE STRING "Arvdude global flag list.") + + if(ARDUINO_SDK_PATH) + detect_arduino_version(ARDUINO_SDK_VERSION) + set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version") + endif(ARDUINO_SDK_PATH) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Arduino + REQUIRED_VARS ARDUINO_SDK_PATH + ARDUINO_SDK_VERSION + VERSION_VAR ARDUINO_SDK_VERSION) + + + mark_as_advanced(ARDUINO_CORES_PATH + ARDUINO_SDK_VERSION + ARDUINO_LIBRARIES_PATH + ARDUINO_BOARDS_PATH + ARDUINO_PROGRAMMERS_PATH + ARDUINO_REVISIONS_PATH + ARDUINO_AVRDUDE_PROGRAM + ARDUINO_AVRDUDE_CONFIG_PATH + ARDUINO_OBJCOPY_EEP_FLAGS + ARDUINO_OBJCOPY_HEX_FLAGS) + load_board_settings() + +endif() + diff --git a/cmake/modules/Platform/Arduino.cmake b/cmake/modules/Platform/Arduino.cmake new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cmake/modules/Platform/ArduinoPaths.cmake b/cmake/modules/Platform/ArduinoPaths.cmake new file mode 100644 index 0000000000..27372098cc --- /dev/null +++ b/cmake/modules/Platform/ArduinoPaths.cmake @@ -0,0 +1,21 @@ +if(UNIX) + include(Platform/UnixPaths) + if(APPLE) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ~/Applications + /Applications + /Developer/Applications + /sw # Fink + /opt/local) # MacPorts + endif() +elseif(WIN32) + include(Platform/WindowsPaths) +endif() + +if(ARDUINO_SDK_PATH) + if(WIN32) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/utils/bin) + elseif(APPLE) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin) + endif() +endif() diff --git a/cmake/toolchains/Arduino.cmake b/cmake/toolchains/Arduino.cmake new file mode 100644 index 0000000000..616c7b9160 --- /dev/null +++ b/cmake/toolchains/Arduino.cmake @@ -0,0 +1,66 @@ +set(CMAKE_SYSTEM_NAME Arduino) + +set(CMAKE_C_COMPILER avr-gcc) +set(CMAKE_CXX_COMPILER avr-g++) + +#=============================================================================# +# C Flags # +#=============================================================================# +set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections") +set(CMAKE_C_FLAGS "-g -Os ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_DEBUG "-g ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_RELEASE "-Os -DNDEBUG -w ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_RELWITHDEBINFO "-Os -g -w ${ARDUINO_C_FLAGS}" CACHE STRING "") + +#=============================================================================# +# C++ Flags # +#=============================================================================# +set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions") +set(CMAKE_CXX_FLAGS "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_DEBUG "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_RELEASE "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-Os -g ${ARDUINO_CXX_FLAGS}" CACHE STRING "") + +#=============================================================================# +# Executable Linker Flags # +#=============================================================================# +set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections -lc -lm") +set(CMAKE_EXE_LINKER_FLAGS "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_MINSIZEREL "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") + +#=============================================================================# +# Shared Lbrary Linker Flags # +#=============================================================================# +set(CMAKE_SHARED_LINKER_FLAGS "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "") + +set(CMAKE_MODULE_LINKER_FLAGS "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_DEBUG "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_RELEASE "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "") + + + + +set(ARDUINO_PATHS) +foreach(VERSION RANGE 22 1) + list(APPEND ARDUINO_PATHS arduino-00${VERSION}) +endforeach() + +find_path(ARDUINO_SDK_PATH + NAMES lib/version.txt + PATH_SUFFIXES share/arduino + Arduino.app/Contents/Resources/Java/ + ${ARDUINO_PATHS} + DOC "Arduino Development Kit path.") + +include(Platform/ArduinoPaths) diff --git a/libraries/APM_BMP085/CMakeLists.txt b/libraries/APM_BMP085/CMakeLists.txt new file mode 100644 index 0000000000..342fe4668f --- /dev/null +++ b/libraries/APM_BMP085/CMakeLists.txt @@ -0,0 +1,22 @@ +set(LIB_NAME APM_BMP085) + +set(${LIB_NAME}_SRCS + APM_BMP085.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_BMP085.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_PI/CMakeLists.txt b/libraries/APM_PI/CMakeLists.txt new file mode 100644 index 0000000000..e65e76f6a4 --- /dev/null +++ b/libraries/APM_PI/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME APM_PI) + +set(${LIB_NAME}_SRCS + APM_PI.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_PI.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_RC/CMakeLists.txt b/libraries/APM_RC/CMakeLists.txt new file mode 100644 index 0000000000..3e4aab0011 --- /dev/null +++ b/libraries/APM_RC/CMakeLists.txt @@ -0,0 +1,22 @@ +set(LIB_NAME APM_RC) + +set(${LIB_NAME}_SRCS + APM_RC.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_RC.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_ADC/CMakeLists.txt b/libraries/AP_ADC/CMakeLists.txt new file mode 100644 index 0000000000..df85645fb2 --- /dev/null +++ b/libraries/AP_ADC/CMakeLists.txt @@ -0,0 +1,26 @@ +set(LIB_NAME AP_ADC) + +set(${LIB_NAME}_SRCS + AP_ADC_HIL.cpp + AP_ADC_ADS7844.cpp + AP_ADC.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_ADC_HIL.h + AP_ADC_ADS7844.h + AP_ADC.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp deleted file mode 100644 index ddfbc2d8a3..0000000000 --- a/libraries/AP_Camera/AP_Camera.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include -#include <../RC_Channel/RC_Channel_aux.h> - -extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -extern long wp_distance; -extern "C" { -void relay_on(); -void relay_off(); -} - -void -AP_Camera::servo_pic() // Servo operated camera -{ - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) - { - g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max; - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles - } -} - -void -AP_Camera::relay_pic() // basic relay activation -{ - relay_on(); - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles -} - -void -AP_Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ -// TODO find a way to do this without using the global parameter g -// g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic == 10){ - servo_pic(); // triggering method - thr_pic = 0; -// g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void -AP_Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ -// TODO find a way to do this without using the global parameter g -// g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method -// g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void -AP_Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // TODO: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles -} - -// single entry point to take pictures -void -AP_Camera::trigger_pic() -{ - switch (trigger_type) - { - case 0: - servo_pic(); // Servo operated camera - break; - case 1: - relay_pic(); // basic relay activation - break; - case 2: - throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - break; - case 3: - distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - break; - case 4: - NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - break; - } -} - -// de-activate the trigger after some delay, but without using a delay() function -void -AP_Camera::trigger_pic_cleanup() -{ - if (keep_cam_trigg_active_cycles) - { - keep_cam_trigg_active_cycles --; - } - else - { - switch (trigger_type) - { - case 0: - case 2: - case 3: - G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; - break; - case 1: - // TODO for some strange reason the function call bellow gives a linker error - //relay_off(); - PORTL &= ~B00000100; // hardcoded version of relay_off(). Replace with a proper function call later. - break; - case 4: - digitalWrite(camtrig, LOW); - break; - } - } -} diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h deleted file mode 100644 index e1c520131a..0000000000 --- a/libraries/AP_Camera/AP_Camera.h +++ /dev/null @@ -1,57 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file AP_Camera.h -/// @brief Photo or video camera manager, with EEPROM-backed storage of constants. - -#ifndef AP_CAMERA_H -#define AP_CAMERA_H - -#include - -/// @class Camera -/// @brief Object managing a Photo or video camera -class AP_Camera{ -protected: - AP_Var_group _group; // must be before all vars to keep ctor init order correct - -public: - /// Constructor - /// - /// @param key EEPROM storage key for the camera configuration parameters. - /// @param name Optional name for the group. - /// - AP_Camera(AP_Var::Key key, const prog_char_t *name) : - _group(key, name), - trigger_type(&_group, 0, 0, name ? PSTR("TRIGG_TYPE") : 0), - picture_time (0), // waypoint trigger variable - thr_pic (0), // timer variable for throttle_pic - camtrig (83), // PK6 chosen as it not near anything so safer for soldering - keep_cam_trigg_active_cycles (0), - wp_distance_min (10) - {} - - // single entry point to take pictures - void trigger_pic(); - - // de-activate the trigger after some delay, but without using a delay() function - void trigger_pic_cleanup(); - - int picture_time; // waypoint trigger variable - long wp_distance_min; // take picture if distance to WP is smaller than this - -private: - uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active - int thr_pic; // timer variable for throttle_pic - int camtrig; // PK6 chosen as it not near anything so safer for soldering - - AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor - - void servo_pic(); // Servo operated camera - void relay_pic(); // basic relay activation - void throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - void distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - void NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - -}; - -#endif /* AP_CAMERA_H */ diff --git a/libraries/AP_Common/CMakeLists.txt b/libraries/AP_Common/CMakeLists.txt new file mode 100644 index 0000000000..7d89ba1e40 --- /dev/null +++ b/libraries/AP_Common/CMakeLists.txt @@ -0,0 +1,33 @@ +set(LIB_NAME AP_Common) + +set(${LIB_NAME}_SRCS + AP_Common.cpp + AP_Loop.cpp + AP_MetaClass.cpp + AP_Var.cpp + AP_Var_menufuncs.cpp + c++.cpp + menu.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Common.h + AP_Loop.h + AP_MetaClass.h + AP_Var.h + AP_Test.h + c++.h + AP_Vector.h +) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Compass/CMakeLists.txt b/libraries/AP_Compass/CMakeLists.txt new file mode 100644 index 0000000000..613b751ef4 --- /dev/null +++ b/libraries/AP_Compass/CMakeLists.txt @@ -0,0 +1,27 @@ +set(LIB_NAME AP_Compass) + +set(${LIB_NAME}_SRCS + AP_Compass_HIL.cpp + AP_Compass_HMC5843.cpp + Compass.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + Compass.h + AP_Compass_HIL.h + AP_Compass_HMC5843.h + AP_Compass.h + ) + + + +include_directories( + + - + ${ARDUINO_LIBRARIES_PATH}/Wire + #${CMAKE_SOURCE_DIR}/libraries/FastSerial + # +) +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_DCM/CMakeLists.txt b/libraries/AP_DCM/CMakeLists.txt new file mode 100644 index 0000000000..0131eeb495 --- /dev/null +++ b/libraries/AP_DCM/CMakeLists.txt @@ -0,0 +1,25 @@ +set(LIB_NAME AP_DCM) + +set(${LIB_NAME}_SRCS + AP_DCM.cpp + AP_DCM_HIL.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_DCM.h + AP_DCM_HIL.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_DCM + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_GPS/CMakeLists.txt b/libraries/AP_GPS/CMakeLists.txt new file mode 100644 index 0000000000..57409c6d24 --- /dev/null +++ b/libraries/AP_GPS/CMakeLists.txt @@ -0,0 +1,44 @@ +set(LIB_NAME AP_GPS) + +set(${LIB_NAME}_SRCS + AP_GPS_406.cpp + AP_GPS_Auto.cpp + AP_GPS_HIL.cpp + AP_GPS_IMU.cpp + AP_GPS_MTK.cpp + AP_GPS_MTK16.cpp + AP_GPS_NMEA.cpp + AP_GPS_SIRF.cpp + AP_GPS_UBLOX.cpp + GPS.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_GPS_406.h + AP_GPS_Auto.h + AP_GPS_HIL.h + AP_GPS_IMU.h + AP_GPS_MTK.h + AP_GPS_MTK_Common.h + AP_GPS_MTK16.h + AP_GPS_NMEA.h + AP_GPS_None.h + AP_GPS_Shim.h + AP_GPS_SIRF.h + AP_GPS_UBLOX.h + AP_GPS.h + GPS.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_IMU/CMakeLists.txt b/libraries/AP_IMU/CMakeLists.txt new file mode 100644 index 0000000000..2447a078c4 --- /dev/null +++ b/libraries/AP_IMU/CMakeLists.txt @@ -0,0 +1,25 @@ +set(LIB_NAME AP_IMU) + +set(${LIB_NAME}_SRCS + AP_IMU_Oilpan.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_IMU.h + AP_IMU_Shim.h + AP_IMU_Oilpan.h + IMU.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Math/CMakeLists.txt b/libraries/AP_Math/CMakeLists.txt new file mode 100644 index 0000000000..f4a83b3102 --- /dev/null +++ b/libraries/AP_Math/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_Math) + +set(${LIB_NAME}_SRCS + + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Math.h + matrix3.h + vector2.h + vector3.h + ) + +include_directories( + + - +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp deleted file mode 100644 index ef931eb58c..0000000000 --- a/libraries/AP_Mount/AP_Mount.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include -#include <../RC_Channel/RC_Channel_aux.h> - -extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function - -AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) -{ - _dcm=dcm; - _gps=gps; -} - -void AP_Mount::set_pitch_yaw(int pitchCh, int yawCh) -{ -} - -void AP_Mount::set_GPS_target(Location targetGPSLocation) -{ - _target_GPS_location=targetGPSLocation; - - //set mode - _mount_mode=k_gps_target; - - //update mount position - update_mount(); -} - -void AP_Mount::set_assisted(int roll, int pitch, int yaw) -{ - _assist_angles.x = roll; - _assist_angles.y = pitch; - _assist_angles.z = yaw; - - //set mode - _mount_mode=k_assisted; - - //update mount position - update_mount(); -} - -//sets the servo angles for FPV, note angles are * 100 -void AP_Mount::set_mount_free_roam(int roll, int pitch, int yaw) -{ - _roam_angles.x=roll; - _roam_angles.y=pitch; - _roam_angles.z=yaw; - - //set mode - _mount_mode=k_roam; - - //now update mount position - update_mount(); -} - -//sets the servo angles for landing, note angles are * 100 -void AP_Mount::set_mount_landing(int roll, int pitch, int yaw) -{ - _landing_angles.x=roll; - _landing_angles.y=pitch; - _landing_angles.z=yaw; - - //set mode - _mount_mode=k_landing; - - //now update mount position - update_mount(); -} - -void AP_Mount::set_none() -{ - //set mode - _mount_mode=k_none; - - //now update mount position - update_mount(); -} - -void AP_Mount::update_mount() -{ - Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs - Vector3f targ; //holds target vector, var is used as temp in calcs - - switch(_mount_mode) - { - case k_gps_target: - { - if(_gps->fix) - { - calc_GPS_target_vector(&_target_GPS_location); - } - m = _dcm->get_dcm_transposed(); - targ = m*_GPS_vector; - roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll - pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch - break; - } - case k_stabilise: - { - // TODO replace this simplified implementation with a proper one - roll_angle = -_dcm->roll_sensor; - pitch_angle = -_dcm->pitch_sensor; - yaw_angle = -_dcm->yaw_sensor; - break; - } - case k_roam: - { - roll_angle=100*_roam_angles.x; - pitch_angle=100*_roam_angles.y; - yaw_angle=100*_roam_angles.z; - break; - } - case k_assisted: - { - m = _dcm->get_dcm_transposed(); - //rotate vector - targ = m*_assist_vector; - roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll - pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch - break; - } - case k_landing: - { - roll_angle=100*_roam_angles.x; - pitch_angle=100*_roam_angles.y; - yaw_angle=100*_roam_angles.z; - break; - } - case k_manual: // radio manual control - if (g_rc_function[RC_Channel_aux::k_mount_roll]) - roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max); - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) - pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max); - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) - yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max); - break; - case k_none: - default: - { - //do nothing - break; - } - } - - // write the results to the servos - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); - G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); - G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); -} - -void AP_Mount::set_mode(MountMode mode) -{ - _mount_mode=mode; -} - -void AP_Mount::calc_GPS_target_vector(struct Location *target) -{ - _GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195; - _GPS_vector.y = (target->lat-_gps->latitude)*.01113195; - _GPS_vector.z = (_gps->altitude-target->alt); -} - -void -AP_Mount::update_mount_type() -{ - // Auto-detect the mount gimbal type depending on the functions assigned to the servos - if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) - { - _mount_type = k_pan_tilt; - } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL)) - { - _mount_type = k_tilt_roll; - } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) - { - _mount_type = k_pan_tilt_roll; - } -} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h deleted file mode 100644 index e80c3e294a..0000000000 --- a/libraries/AP_Mount/AP_Mount.h +++ /dev/null @@ -1,90 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/************************************************************ -* AP_mount -- library to control a 2 or 3 axis mount. * -* * -* Author: Joe Holdsworth; * -* Ritchie Wilson; * -* Amilcar Lucas; * -* * -* Purpose: Move a 2 or 3 axis mount attached to vehicle, * -* Used for mount to track targets or stabilise * -* camera plus other modes. * -* * -* Usage: Use in main code to control mounts attached to * -* vehicle. * -* * -*Comments: All angles in degrees * 100, distances in meters* -* unless otherwise stated. * - ************************************************************/ -#ifndef AP_Mount_H -#define AP_Mount_H - -//#include -#include -#include -#include - -class AP_Mount -{ -public: - //Constructors - AP_Mount(GPS *gps, AP_DCM *dcm); - - //enums - enum MountMode{ - k_gps_target = 0, - k_stabilise = 1, //note the correct English spelling :) - k_roam = 2, - k_assisted = 3, - k_landing = 4, - k_none = 5, - k_manual = 6 - }; - - enum MountType{ - k_pan_tilt = 0, //yaw-pitch - k_tilt_roll = 1, //pitch-roll - k_pan_tilt_roll = 2, //yaw-pitch-roll - }; - - //Accessors - void set_pitch_yaw(int pitchCh, int yawCh); - void set_pitch_roll(int pitchCh, int rollCh); - void set_pitch_roll_yaw(int pitchCh, int rollCh, int yawCh); - - void set_GPS_target(Location targetGPSLocation); //used to tell the mount to track GPS location - void set_assisted(int roll, int pitch, int yaw); - void set_mount_free_roam(int roll, int pitch, int yaw);//used in the FPV for example, - void set_mount_landing(int roll, int pitch, int yaw); //set mount landing position - void set_none(); - - //methods - void update_mount(); - void update_mount_type(); //Auto-detect the mount gimbal type depending on the functions assigned to the servos - void set_mode(MountMode mode); - - int pitch_angle; //degrees*100 - int roll_angle; //degrees*100 - int yaw_angle; //degrees*100 -protected: - //methods - void calc_GPS_target_vector(struct Location *target); - //void CalculateDCM(int roll, int pitch, int yaw); - //members - AP_DCM *_dcm; - GPS *_gps; - - MountMode _mount_mode; - MountType _mount_type; - - struct Location _target_GPS_location; - Vector3f _GPS_vector; //target vector calculated stored in meters - - Vector3i _roam_angles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _landing_angles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - - Vector3i _assist_angles; //used to keep angles that user has supplied from assisted targeting - Vector3f _assist_vector; //used to keep vector calculated from _AssistAngles -}; -#endif \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/CMakeLists.txt b/libraries/AP_OpticalFlow/CMakeLists.txt new file mode 100644 index 0000000000..74d7457ac4 --- /dev/null +++ b/libraries/AP_OpticalFlow/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_OpticalFlow) + +set(${LIB_NAME}_SRCS + AP_OpticalFlow.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_OpticalFlow.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_PID/CMakeLists.txt b/libraries/AP_PID/CMakeLists.txt new file mode 100644 index 0000000000..14e9782e51 --- /dev/null +++ b/libraries/AP_PID/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_PID) + +set(${LIB_NAME}_SRCS + AP_PID.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_PID.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_RangeFinder/CMakeLists.txt b/libraries/AP_RangeFinder/CMakeLists.txt new file mode 100644 index 0000000000..add2e2928e --- /dev/null +++ b/libraries/AP_RangeFinder/CMakeLists.txt @@ -0,0 +1,27 @@ +set(LIB_NAME AP_RangeFinder) + +set(${LIB_NAME}_SRCS + AP_RangeFinder_MaxsonarXL.cpp + AP_RangeFinder_SharpGP2Y.cpp + RangeFinder.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_RangeFinder.h + AP_RangeFinder_MaxsonarXL.h + AP_RangeFinder_SharpGP2Y.h + RangeFinder.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt new file mode 100644 index 0000000000..ed27448a23 --- /dev/null +++ b/libraries/CMakeLists.txt @@ -0,0 +1,24 @@ +add_subdirectory(DataFlash) +add_subdirectory(AP_Math) +add_subdirectory(PID) +add_subdirectory(AP_Common) +add_subdirectory(RC_Channel) +add_subdirectory(AP_OpticalFlow) +add_subdirectory(ModeFilter) +add_subdirectory(memcheck) +add_subdirectory(APM_PI) +add_subdirectory(AP_GPS) +add_subdirectory(AP_DCM) +add_subdirectory(AP_Compass) +add_subdirectory(AP_ADC) +add_subdirectory(AP_IMU) +add_subdirectory(AP_RangeFinder) + +add_subdirectory(APM_RC) +add_subdirectory(APM_BMP085) + +#add_subdirectory(APO) +add_subdirectory(FastSerial) +add_subdirectory(GCS_MAVLink) + +#add_subdirectory(playgroundlib) diff --git a/libraries/DataFlash/CMakeLists.txt b/libraries/DataFlash/CMakeLists.txt new file mode 100644 index 0000000000..2e13294932 --- /dev/null +++ b/libraries/DataFlash/CMakeLists.txt @@ -0,0 +1,21 @@ +set(LIB_NAME DataFlash) + +set(${LIB_NAME}_SRCS + DataFlash.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + DataFlash.h + ) + +include_directories( + + - +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/FastSerial/CMakeLists.txt b/libraries/FastSerial/CMakeLists.txt new file mode 100644 index 0000000000..78e94e9023 --- /dev/null +++ b/libraries/FastSerial/CMakeLists.txt @@ -0,0 +1,28 @@ +set(LIB_NAME FastSerial) + +set(${LIB_NAME}_SRCS + BetterStream.cpp + FastSerial.cpp + vprintf.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + BetterStream.h + FastSerial.h + ftoa_engine.h + ntz.h + xtoa_fast.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/GCS_MAVLink/CMakeLists.txt b/libraries/GCS_MAVLink/CMakeLists.txt new file mode 100644 index 0000000000..326d2b959c --- /dev/null +++ b/libraries/GCS_MAVLink/CMakeLists.txt @@ -0,0 +1,19 @@ +set(LIB_NAME GCS_MAVLink) + +set(${LIB_NAME}_SRCS + GCS_MAVLink.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + GCS_MAVLink.h +) + +include_directories( + #${CMAKE_SOURCE_DIR}/libraries/ + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/ModeFilter/CMakeLists.txt b/libraries/ModeFilter/CMakeLists.txt new file mode 100644 index 0000000000..cba4eec1b8 --- /dev/null +++ b/libraries/ModeFilter/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME ModeFilter) + +set(${LIB_NAME}_SRCS + ModeFilter.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + ModeFilter.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/PID/CMakeLists.txt b/libraries/PID/CMakeLists.txt new file mode 100644 index 0000000000..c814e841d1 --- /dev/null +++ b/libraries/PID/CMakeLists.txt @@ -0,0 +1,21 @@ +set(LIB_NAME PID) + +set(${LIB_NAME}_SRCS + PID.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + PID.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/RC_Channel/CMakeLists.txt b/libraries/RC_Channel/CMakeLists.txt new file mode 100644 index 0000000000..94ed383fcb --- /dev/null +++ b/libraries/RC_Channel/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME RC_Channel) + +set(${LIB_NAME}_SRCS + RC_Channel.cpp + RC_Channel_aux.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + RC_Channel.h + RC_Channel_aux.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/APM_RC + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 55ed6819af..1ff7b45326 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -5,42 +5,6 @@ extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -int16_t -RC_Channel_aux::closest_limit(int16_t angle) -{ - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - int16_t min = angle_min / 10; - int16_t max = angle_max / 10; - - // Make sure the angle lies in the interval [-180 .. 180[ degrees - while (angle < -1800) angle += 3600; - while (angle >= 1800) angle -= 3600; - - // Make sure the angle limits lie in the interval [-180 .. 180[ degrees - while (min < -1800) min += 3600; - while (min >= 1800) min -= 3600; - while (max < -1800) max += 3600; - while (max >= 1800) max -= 3600; - // This is done every time because the user might change the min, max values on the fly - set_range(min, max); - - // If the angle is outside servo limits, saturate the angle to the closest limit - // On a circle the closest angular position must be carefully calculated to account for wrap-around - if ((angle < min) && (angle > max)){ - // angle error if min limit is used - int16_t err_min = min - angle + (anglemax?0:3600); // add 360 degrees if on the "wrong side" - angle = err_minset_range(0,100); + G_RC_AUX(k_flap_auto)->set_range(0,100); + G_RC_AUX(k_aileron)->set_angle(4500); + G_RC_AUX(k_flaperon)->set_range(0,100); } diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 6d8c228472..7e091bf18f 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -14,7 +14,6 @@ /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function -/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements class RC_Channel_aux : public RC_Channel{ public: /// Constructor @@ -24,9 +23,7 @@ public: /// RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) : RC_Channel(key, name), - function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name - angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection - angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection + function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0) // suppress name if group has no name {} typedef enum @@ -37,20 +34,10 @@ public: k_flap_auto = 3, // flap automated k_aileron = 4, // aileron k_flaperon = 5, // flaperon (flaps and aileron combined, needs two independent servos one for each wing) - k_mount_yaw = 6, // mount yaw (pan) - k_mount_pitch = 7, // mount pitch (tilt) - k_mount_roll = 8, // mount roll - k_cam_trigger = 9, // camera trigger - k_cam_open = 10, // camera open - k_egg_drop = 11, // egg drop k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop - AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units - AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units - - int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it diff --git a/libraries/memcheck/CMakeLists.txt b/libraries/memcheck/CMakeLists.txt new file mode 100644 index 0000000000..ec9121b1ce --- /dev/null +++ b/libraries/memcheck/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME memcheck) + +set(${LIB_NAME}_SRCS + memcheck.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + memcheck.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME})