a

Dependencies:   HMC5883 MPL3115A MPU605 MU2Class SDFileSystem mbed

Files at this revision

API Documentation at this revision

Comitter:
pyonta2017
Date:
Sat Sep 09 23:18:12 2017 +0000
Commit message:
a

Changed in this revision

HMC5883L.lib Show annotated file Show diff for this revision Revisions of this file
MPL3115A2.lib Show annotated file Show diff for this revision Revisions of this file
MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
MU2Class.lib Show annotated file Show diff for this revision Revisions of this file
PowerControl/EthernetPowerControl.cpp Show annotated file Show diff for this revision Revisions of this file
PowerControl/EthernetPowerControl.h Show annotated file Show diff for this revision Revisions of this file
PowerControl/PowerControl.h Show annotated file Show diff for this revision Revisions of this file
SDFileSystem.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
xprintf.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r ac7a5495d95c HMC5883L.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.lib	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/pyonta2017/code/HMC5883/#41931afb09f2
diff -r 000000000000 -r ac7a5495d95c MPL3115A2.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPL3115A2.lib	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/pyonta2017/code/MPL3115A/#79cff85db78c
diff -r 000000000000 -r ac7a5495d95c MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/pyonta2017/code/MPU605/#b07d973e8c04
diff -r 000000000000 -r ac7a5495d95c MU2Class.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MU2Class.lib	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/ATKINZ117/code/MU2Class/#a7d413a2889d
diff -r 000000000000 -r ac7a5495d95c PowerControl/EthernetPowerControl.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PowerControl/EthernetPowerControl.cpp	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,138 @@
+#include "EthernetPowerControl.h"
+
+static void write_PHY (unsigned int PhyReg, unsigned short Value) {
+   /* Write a data 'Value' to PHY register 'PhyReg'. */
+   unsigned int tout;
+   /* Hardware MII Management for LPC176x devices. */
+   LPC_EMAC->MADR = DP83848C_DEF_ADR | PhyReg;
+   LPC_EMAC->MWTD = Value;
+
+   /* Wait utill operation completed */
+   for (tout = 0; tout < MII_WR_TOUT; tout++) {
+      if ((LPC_EMAC->MIND & MIND_BUSY) == 0) {
+         break;
+      }
+   }
+}
+
+static unsigned short read_PHY (unsigned int PhyReg) {
+   /* Read a PHY register 'PhyReg'. */
+   unsigned int tout, val;
+
+   LPC_EMAC->MADR = DP83848C_DEF_ADR | PhyReg;
+   LPC_EMAC->MCMD = MCMD_READ;
+
+   /* Wait until operation completed */
+   for (tout = 0; tout < MII_RD_TOUT; tout++) {
+      if ((LPC_EMAC->MIND & MIND_BUSY) == 0) {
+         break;
+      }
+   }
+   LPC_EMAC->MCMD = 0;
+   val = LPC_EMAC->MRDD;
+   
+   return (val);
+}
+
+void EMAC_Init()
+{
+  unsigned int tout,regv;
+  /* Power Up the EMAC controller. */
+  Peripheral_PowerUp(LPC1768_PCONP_PCENET);
+  
+  LPC_PINCON->PINSEL2 = 0x50150105;
+  LPC_PINCON->PINSEL3 &= ~0x0000000F;
+  LPC_PINCON->PINSEL3 |= 0x00000005;
+
+  /* Reset all EMAC internal modules. */
+  LPC_EMAC->MAC1 = MAC1_RES_TX | MAC1_RES_MCS_TX | MAC1_RES_RX | MAC1_RES_MCS_RX |
+             MAC1_SIM_RES | MAC1_SOFT_RES;
+  LPC_EMAC->Command = CR_REG_RES | CR_TX_RES | CR_RX_RES;
+
+  /* A short delay after reset. */
+  for (tout = 100; tout; tout--);
+
+  /* Initialize MAC control registers. */
+  LPC_EMAC->MAC1 = MAC1_PASS_ALL;
+  LPC_EMAC->MAC2 = MAC2_CRC_EN | MAC2_PAD_EN;
+  LPC_EMAC->MAXF = ETH_MAX_FLEN;
+  LPC_EMAC->CLRT = CLRT_DEF;
+  LPC_EMAC->IPGR = IPGR_DEF;
+
+  /* Enable Reduced MII interface. */
+  LPC_EMAC->Command = CR_RMII | CR_PASS_RUNT_FRM;
+
+  /* Reset Reduced MII Logic. */
+  LPC_EMAC->SUPP = SUPP_RES_RMII;
+  for (tout = 100; tout; tout--);
+  LPC_EMAC->SUPP = 0;
+
+  /* Put the DP83848C in reset mode */
+  write_PHY (PHY_REG_BMCR, 0x8000);
+
+  /* Wait for hardware reset to end. */
+  for (tout = 0; tout < 0x100000; tout++) {
+    regv = read_PHY (PHY_REG_BMCR);
+    if (!(regv & 0x8000)) {
+      /* Reset complete */
+      break;
+    }
+  }
+}
+
+
+void PHY_PowerDown()
+{
+    if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+        EMAC_Init(); //init EMAC if it is not already init'd
+    
+    unsigned int regv;
+    regv = read_PHY(PHY_REG_BMCR);
+    write_PHY(PHY_REG_BMCR, regv | (1 << PHY_REG_BMCR_POWERDOWN));
+    regv = read_PHY(PHY_REG_BMCR);
+    
+    //shouldn't need the EMAC now.
+    Peripheral_PowerDown(LPC1768_PCONP_PCENET);
+    
+    //and turn off the PHY OSC
+     LPC_GPIO1->FIODIR |= 0x8000000;
+     LPC_GPIO1->FIOCLR = 0x8000000;
+}
+
+void PHY_PowerUp()
+{   
+    if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+        EMAC_Init(); //init EMAC if it is not already init'd
+        
+    LPC_GPIO1->FIODIR |= 0x8000000;
+    LPC_GPIO1->FIOSET = 0x8000000;
+    
+    //wait for osc to be stable
+    wait_ms(200);
+    
+    unsigned int regv;
+    regv = read_PHY(PHY_REG_BMCR);
+    write_PHY(PHY_REG_BMCR, regv & ~(1 << PHY_REG_BMCR_POWERDOWN));
+    regv = read_PHY(PHY_REG_BMCR);
+}
+
+void PHY_EnergyDetect_Enable()
+{
+    if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+        EMAC_Init(); //init EMAC if it is not already init'd
+        
+    unsigned int regv;
+    regv = read_PHY(PHY_REG_EDCR);
+    write_PHY(PHY_REG_BMCR, regv | (1 << PHY_REG_EDCR_ENABLE));
+    regv = read_PHY(PHY_REG_EDCR);   
+}
+
+void PHY_EnergyDetect_Disable()
+{
+    if (!Peripheral_GetStatus(LPC1768_PCONP_PCENET))
+        EMAC_Init(); //init EMAC if it is not already init'd
+    unsigned int regv;
+    regv = read_PHY(PHY_REG_EDCR);
+    write_PHY(PHY_REG_BMCR, regv & ~(1 << PHY_REG_EDCR_ENABLE));
+    regv = read_PHY(PHY_REG_EDCR);   
+}
\ No newline at end of file
diff -r 000000000000 -r ac7a5495d95c PowerControl/EthernetPowerControl.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PowerControl/EthernetPowerControl.h	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,299 @@
+/* mbed PowerControl Library
+  * Copyright (c) 2010 Michael Wei
+  */ 
+  
+#ifndef MBED_POWERCONTROL_ETH_H 
+#define MBED_POWERCONTROL_ETH_H 
+
+#include "mbed.h"
+#include "PowerControl.h"
+
+#define PHY_REG_BMCR_POWERDOWN    0xB
+#define PHY_REG_EDCR_ENABLE       0xF
+
+
+void EMAC_Init();
+static unsigned short read_PHY (unsigned int PhyReg);
+static void write_PHY (unsigned int PhyReg, unsigned short Value);
+
+void PHY_PowerDown(void);
+void PHY_PowerUp(void);
+void PHY_EnergyDetect_Enable(void);
+void PHY_EnergyDetect_Disable(void);
+
+//From NXP Sample Code .... Probably from KEIL sample code
+/* EMAC Memory Buffer configuration for 16K Ethernet RAM. */
+#define NUM_RX_FRAG         4           /* Num.of RX Fragments 4*1536= 6.0kB */
+#define NUM_TX_FRAG         3           /* Num.of TX Fragments 3*1536= 4.6kB */
+#define ETH_FRAG_SIZE       1536        /* Packet Fragment size 1536 Bytes   */
+
+#define ETH_MAX_FLEN        1536        /* Max. Ethernet Frame Size          */
+
+/* EMAC variables located in 16K Ethernet SRAM */
+#define RX_DESC_BASE        0x20080000
+#define RX_STAT_BASE        (RX_DESC_BASE + NUM_RX_FRAG*8)
+#define TX_DESC_BASE        (RX_STAT_BASE + NUM_RX_FRAG*8)
+#define TX_STAT_BASE        (TX_DESC_BASE + NUM_TX_FRAG*8)
+#define RX_BUF_BASE         (TX_STAT_BASE + NUM_TX_FRAG*4)
+#define TX_BUF_BASE         (RX_BUF_BASE  + NUM_RX_FRAG*ETH_FRAG_SIZE)
+
+/* RX and TX descriptor and status definitions. */
+#define RX_DESC_PACKET(i)   (*(unsigned int *)(RX_DESC_BASE   + 8*i))
+#define RX_DESC_CTRL(i)     (*(unsigned int *)(RX_DESC_BASE+4 + 8*i))
+#define RX_STAT_INFO(i)     (*(unsigned int *)(RX_STAT_BASE   + 8*i))
+#define RX_STAT_HASHCRC(i)  (*(unsigned int *)(RX_STAT_BASE+4 + 8*i))
+#define TX_DESC_PACKET(i)   (*(unsigned int *)(TX_DESC_BASE   + 8*i))
+#define TX_DESC_CTRL(i)     (*(unsigned int *)(TX_DESC_BASE+4 + 8*i))
+#define TX_STAT_INFO(i)     (*(unsigned int *)(TX_STAT_BASE   + 4*i))
+#define RX_BUF(i)           (RX_BUF_BASE + ETH_FRAG_SIZE*i)
+#define TX_BUF(i)           (TX_BUF_BASE + ETH_FRAG_SIZE*i)
+
+/* MAC Configuration Register 1 */
+#define MAC1_REC_EN         0x00000001  /* Receive Enable                    */
+#define MAC1_PASS_ALL       0x00000002  /* Pass All Receive Frames           */
+#define MAC1_RX_FLOWC       0x00000004  /* RX Flow Control                   */
+#define MAC1_TX_FLOWC       0x00000008  /* TX Flow Control                   */
+#define MAC1_LOOPB          0x00000010  /* Loop Back Mode                    */
+#define MAC1_RES_TX         0x00000100  /* Reset TX Logic                    */
+#define MAC1_RES_MCS_TX     0x00000200  /* Reset MAC TX Control Sublayer     */
+#define MAC1_RES_RX         0x00000400  /* Reset RX Logic                    */
+#define MAC1_RES_MCS_RX     0x00000800  /* Reset MAC RX Control Sublayer     */
+#define MAC1_SIM_RES        0x00004000  /* Simulation Reset                  */
+#define MAC1_SOFT_RES       0x00008000  /* Soft Reset MAC                    */
+
+/* MAC Configuration Register 2 */
+#define MAC2_FULL_DUP       0x00000001  /* Full Duplex Mode                  */
+#define MAC2_FRM_LEN_CHK    0x00000002  /* Frame Length Checking             */
+#define MAC2_HUGE_FRM_EN    0x00000004  /* Huge Frame Enable                 */
+#define MAC2_DLY_CRC        0x00000008  /* Delayed CRC Mode                  */
+#define MAC2_CRC_EN         0x00000010  /* Append CRC to every Frame         */
+#define MAC2_PAD_EN         0x00000020  /* Pad all Short Frames              */
+#define MAC2_VLAN_PAD_EN    0x00000040  /* VLAN Pad Enable                   */
+#define MAC2_ADET_PAD_EN    0x00000080  /* Auto Detect Pad Enable            */
+#define MAC2_PPREAM_ENF     0x00000100  /* Pure Preamble Enforcement         */
+#define MAC2_LPREAM_ENF     0x00000200  /* Long Preamble Enforcement         */
+#define MAC2_NO_BACKOFF     0x00001000  /* No Backoff Algorithm              */
+#define MAC2_BACK_PRESSURE  0x00002000  /* Backoff Presurre / No Backoff     */
+#define MAC2_EXCESS_DEF     0x00004000  /* Excess Defer                      */
+
+/* Back-to-Back Inter-Packet-Gap Register */
+#define IPGT_FULL_DUP       0x00000015  /* Recommended value for Full Duplex */
+#define IPGT_HALF_DUP       0x00000012  /* Recommended value for Half Duplex */
+
+/* Non Back-to-Back Inter-Packet-Gap Register */
+#define IPGR_DEF            0x00000012  /* Recommended value                 */
+
+/* Collision Window/Retry Register */
+#define CLRT_DEF            0x0000370F  /* Default value                     */
+
+/* PHY Support Register */
+#define SUPP_SPEED          0x00000100  /* Reduced MII Logic Current Speed   */
+#define SUPP_RES_RMII       0x00000800  /* Reset Reduced MII Logic           */
+
+/* Test Register */
+#define TEST_SHCUT_PQUANTA  0x00000001  /* Shortcut Pause Quanta             */
+#define TEST_TST_PAUSE      0x00000002  /* Test Pause                        */
+#define TEST_TST_BACKP      0x00000004  /* Test Back Pressure                */
+
+/* MII Management Configuration Register */
+#define MCFG_SCAN_INC       0x00000001  /* Scan Increment PHY Address        */
+#define MCFG_SUPP_PREAM     0x00000002  /* Suppress Preamble                 */
+#define MCFG_CLK_SEL        0x0000001C  /* Clock Select Mask                 */
+#define MCFG_RES_MII        0x00008000  /* Reset MII Management Hardware     */
+
+/* MII Management Command Register */
+#define MCMD_READ           0x00000001  /* MII Read                          */
+#define MCMD_SCAN           0x00000002  /* MII Scan continuously             */
+
+#define MII_WR_TOUT         0x00050000  /* MII Write timeout count           */
+#define MII_RD_TOUT         0x00050000  /* MII Read timeout count            */
+
+/* MII Management Address Register */
+#define MADR_REG_ADR        0x0000001F  /* MII Register Address Mask         */
+#define MADR_PHY_ADR        0x00001F00  /* PHY Address Mask                  */
+
+/* MII Management Indicators Register */
+#define MIND_BUSY           0x00000001  /* MII is Busy                       */
+#define MIND_SCAN           0x00000002  /* MII Scanning in Progress          */
+#define MIND_NOT_VAL        0x00000004  /* MII Read Data not valid           */
+#define MIND_MII_LINK_FAIL  0x00000008  /* MII Link Failed                   */
+
+/* Command Register */
+#define CR_RX_EN            0x00000001  /* Enable Receive                    */
+#define CR_TX_EN            0x00000002  /* Enable Transmit                   */
+#define CR_REG_RES          0x00000008  /* Reset Host Registers              */
+#define CR_TX_RES           0x00000010  /* Reset Transmit Datapath           */
+#define CR_RX_RES           0x00000020  /* Reset Receive Datapath            */
+#define CR_PASS_RUNT_FRM    0x00000040  /* Pass Runt Frames                  */
+#define CR_PASS_RX_FILT     0x00000080  /* Pass RX Filter                    */
+#define CR_TX_FLOW_CTRL     0x00000100  /* TX Flow Control                   */
+#define CR_RMII             0x00000200  /* Reduced MII Interface             */
+#define CR_FULL_DUP         0x00000400  /* Full Duplex                       */
+
+/* Status Register */
+#define SR_RX_EN            0x00000001  /* Enable Receive                    */
+#define SR_TX_EN            0x00000002  /* Enable Transmit                   */
+
+/* Transmit Status Vector 0 Register */
+#define TSV0_CRC_ERR        0x00000001  /* CRC error                         */
+#define TSV0_LEN_CHKERR     0x00000002  /* Length Check Error                */
+#define TSV0_LEN_OUTRNG     0x00000004  /* Length Out of Range               */
+#define TSV0_DONE           0x00000008  /* Tramsmission Completed            */
+#define TSV0_MCAST          0x00000010  /* Multicast Destination             */
+#define TSV0_BCAST          0x00000020  /* Broadcast Destination             */
+#define TSV0_PKT_DEFER      0x00000040  /* Packet Deferred                   */
+#define TSV0_EXC_DEFER      0x00000080  /* Excessive Packet Deferral         */
+#define TSV0_EXC_COLL       0x00000100  /* Excessive Collision               */
+#define TSV0_LATE_COLL      0x00000200  /* Late Collision Occured            */
+#define TSV0_GIANT          0x00000400  /* Giant Frame                       */
+#define TSV0_UNDERRUN       0x00000800  /* Buffer Underrun                   */
+#define TSV0_BYTES          0x0FFFF000  /* Total Bytes Transferred           */
+#define TSV0_CTRL_FRAME     0x10000000  /* Control Frame                     */
+#define TSV0_PAUSE          0x20000000  /* Pause Frame                       */
+#define TSV0_BACK_PRESS     0x40000000  /* Backpressure Method Applied       */
+#define TSV0_VLAN           0x80000000  /* VLAN Frame                        */
+
+/* Transmit Status Vector 1 Register */
+#define TSV1_BYTE_CNT       0x0000FFFF  /* Transmit Byte Count               */
+#define TSV1_COLL_CNT       0x000F0000  /* Transmit Collision Count          */
+
+/* Receive Status Vector Register */
+#define RSV_BYTE_CNT        0x0000FFFF  /* Receive Byte Count                */
+#define RSV_PKT_IGNORED     0x00010000  /* Packet Previously Ignored         */
+#define RSV_RXDV_SEEN       0x00020000  /* RXDV Event Previously Seen        */
+#define RSV_CARR_SEEN       0x00040000  /* Carrier Event Previously Seen     */
+#define RSV_REC_CODEV       0x00080000  /* Receive Code Violation            */
+#define RSV_CRC_ERR         0x00100000  /* CRC Error                         */
+#define RSV_LEN_CHKERR      0x00200000  /* Length Check Error                */
+#define RSV_LEN_OUTRNG      0x00400000  /* Length Out of Range               */
+#define RSV_REC_OK          0x00800000  /* Frame Received OK                 */
+#define RSV_MCAST           0x01000000  /* Multicast Frame                   */
+#define RSV_BCAST           0x02000000  /* Broadcast Frame                   */
+#define RSV_DRIB_NIBB       0x04000000  /* Dribble Nibble                    */
+#define RSV_CTRL_FRAME      0x08000000  /* Control Frame                     */
+#define RSV_PAUSE           0x10000000  /* Pause Frame                       */
+#define RSV_UNSUPP_OPC      0x20000000  /* Unsupported Opcode                */
+#define RSV_VLAN            0x40000000  /* VLAN Frame                        */
+
+/* Flow Control Counter Register */
+#define FCC_MIRR_CNT        0x0000FFFF  /* Mirror Counter                    */
+#define FCC_PAUSE_TIM       0xFFFF0000  /* Pause Timer                       */
+
+/* Flow Control Status Register */
+#define FCS_MIRR_CNT        0x0000FFFF  /* Mirror Counter Current            */
+
+/* Receive Filter Control Register */
+#define RFC_UCAST_EN        0x00000001  /* Accept Unicast Frames Enable      */
+#define RFC_BCAST_EN        0x00000002  /* Accept Broadcast Frames Enable    */
+#define RFC_MCAST_EN        0x00000004  /* Accept Multicast Frames Enable    */
+#define RFC_UCAST_HASH_EN   0x00000008  /* Accept Unicast Hash Filter Frames */
+#define RFC_MCAST_HASH_EN   0x00000010  /* Accept Multicast Hash Filter Fram.*/
+#define RFC_PERFECT_EN      0x00000020  /* Accept Perfect Match Enable       */
+#define RFC_MAGP_WOL_EN     0x00001000  /* Magic Packet Filter WoL Enable    */
+#define RFC_PFILT_WOL_EN    0x00002000  /* Perfect Filter WoL Enable         */
+
+/* Receive Filter WoL Status/Clear Registers */
+#define WOL_UCAST           0x00000001  /* Unicast Frame caused WoL          */
+#define WOL_BCAST           0x00000002  /* Broadcast Frame caused WoL        */
+#define WOL_MCAST           0x00000004  /* Multicast Frame caused WoL        */
+#define WOL_UCAST_HASH      0x00000008  /* Unicast Hash Filter Frame WoL     */
+#define WOL_MCAST_HASH      0x00000010  /* Multicast Hash Filter Frame WoL   */
+#define WOL_PERFECT         0x00000020  /* Perfect Filter WoL                */
+#define WOL_RX_FILTER       0x00000080  /* RX Filter caused WoL              */
+#define WOL_MAG_PACKET      0x00000100  /* Magic Packet Filter caused WoL    */
+
+/* Interrupt Status/Enable/Clear/Set Registers */
+#define INT_RX_OVERRUN      0x00000001  /* Overrun Error in RX Queue         */
+#define INT_RX_ERR          0x00000002  /* Receive Error                     */
+#define INT_RX_FIN          0x00000004  /* RX Finished Process Descriptors   */
+#define INT_RX_DONE         0x00000008  /* Receive Done                      */
+#define INT_TX_UNDERRUN     0x00000010  /* Transmit Underrun                 */
+#define INT_TX_ERR          0x00000020  /* Transmit Error                    */
+#define INT_TX_FIN          0x00000040  /* TX Finished Process Descriptors   */
+#define INT_TX_DONE         0x00000080  /* Transmit Done                     */
+#define INT_SOFT_INT        0x00001000  /* Software Triggered Interrupt      */
+#define INT_WAKEUP          0x00002000  /* Wakeup Event Interrupt            */
+
+/* Power Down Register */
+#define PD_POWER_DOWN       0x80000000  /* Power Down MAC                    */
+
+/* RX Descriptor Control Word */
+#define RCTRL_SIZE          0x000007FF  /* Buffer size mask                  */
+#define RCTRL_INT           0x80000000  /* Generate RxDone Interrupt         */
+
+/* RX Status Hash CRC Word */
+#define RHASH_SA            0x000001FF  /* Hash CRC for Source Address       */
+#define RHASH_DA            0x001FF000  /* Hash CRC for Destination Address  */
+
+/* RX Status Information Word */
+#define RINFO_SIZE          0x000007FF  /* Data size in bytes                */
+#define RINFO_CTRL_FRAME    0x00040000  /* Control Frame                     */
+#define RINFO_VLAN          0x00080000  /* VLAN Frame                        */
+#define RINFO_FAIL_FILT     0x00100000  /* RX Filter Failed                  */
+#define RINFO_MCAST         0x00200000  /* Multicast Frame                   */
+#define RINFO_BCAST         0x00400000  /* Broadcast Frame                   */
+#define RINFO_CRC_ERR       0x00800000  /* CRC Error in Frame                */
+#define RINFO_SYM_ERR       0x01000000  /* Symbol Error from PHY             */
+#define RINFO_LEN_ERR       0x02000000  /* Length Error                      */
+#define RINFO_RANGE_ERR     0x04000000  /* Range Error (exceeded max. size)  */
+#define RINFO_ALIGN_ERR     0x08000000  /* Alignment Error                   */
+#define RINFO_OVERRUN       0x10000000  /* Receive overrun                   */
+#define RINFO_NO_DESCR      0x20000000  /* No new Descriptor available       */
+#define RINFO_LAST_FLAG     0x40000000  /* Last Fragment in Frame            */
+#define RINFO_ERR           0x80000000  /* Error Occured (OR of all errors)  */
+
+#define RINFO_ERR_MASK     (RINFO_FAIL_FILT | RINFO_CRC_ERR   | RINFO_SYM_ERR | \
+                            RINFO_LEN_ERR   | RINFO_ALIGN_ERR | RINFO_OVERRUN)
+
+/* TX Descriptor Control Word */
+#define TCTRL_SIZE          0x000007FF  /* Size of data buffer in bytes      */
+#define TCTRL_OVERRIDE      0x04000000  /* Override Default MAC Registers    */
+#define TCTRL_HUGE          0x08000000  /* Enable Huge Frame                 */
+#define TCTRL_PAD           0x10000000  /* Pad short Frames to 64 bytes      */
+#define TCTRL_CRC           0x20000000  /* Append a hardware CRC to Frame    */
+#define TCTRL_LAST          0x40000000  /* Last Descriptor for TX Frame      */
+#define TCTRL_INT           0x80000000  /* Generate TxDone Interrupt         */
+
+/* TX Status Information Word */
+#define TINFO_COL_CNT       0x01E00000  /* Collision Count                   */
+#define TINFO_DEFER         0x02000000  /* Packet Deferred (not an error)    */
+#define TINFO_EXCESS_DEF    0x04000000  /* Excessive Deferral                */
+#define TINFO_EXCESS_COL    0x08000000  /* Excessive Collision               */
+#define TINFO_LATE_COL      0x10000000  /* Late Collision Occured            */
+#define TINFO_UNDERRUN      0x20000000  /* Transmit Underrun                 */
+#define TINFO_NO_DESCR      0x40000000  /* No new Descriptor available       */
+#define TINFO_ERR           0x80000000  /* Error Occured (OR of all errors)  */
+
+/* DP83848C PHY Registers */
+#define PHY_REG_BMCR        0x00        /* Basic Mode Control Register       */
+#define PHY_REG_BMSR        0x01        /* Basic Mode Status Register        */
+#define PHY_REG_IDR1        0x02        /* PHY Identifier 1                  */
+#define PHY_REG_IDR2        0x03        /* PHY Identifier 2                  */
+#define PHY_REG_ANAR        0x04        /* Auto-Negotiation Advertisement    */
+#define PHY_REG_ANLPAR      0x05        /* Auto-Neg. Link Partner Abitily    */
+#define PHY_REG_ANER        0x06        /* Auto-Neg. Expansion Register      */
+#define PHY_REG_ANNPTR      0x07        /* Auto-Neg. Next Page TX            */
+
+/* PHY Extended Registers */
+#define PHY_REG_STS         0x10        /* Status Register                   */
+#define PHY_REG_MICR        0x11        /* MII Interrupt Control Register    */
+#define PHY_REG_MISR        0x12        /* MII Interrupt Status Register     */
+#define PHY_REG_FCSCR       0x14        /* False Carrier Sense Counter       */
+#define PHY_REG_RECR        0x15        /* Receive Error Counter             */
+#define PHY_REG_PCSR        0x16        /* PCS Sublayer Config. and Status   */
+#define PHY_REG_RBR         0x17        /* RMII and Bypass Register          */
+#define PHY_REG_LEDCR       0x18        /* LED Direct Control Register       */
+#define PHY_REG_PHYCR       0x19        /* PHY Control Register              */
+#define PHY_REG_10BTSCR     0x1A        /* 10Base-T Status/Control Register  */
+#define PHY_REG_CDCTRL1     0x1B        /* CD Test Control and BIST Extens.  */
+#define PHY_REG_EDCR        0x1D        /* Energy Detect Control Register    */
+
+#define PHY_FULLD_100M      0x2100      /* Full Duplex 100Mbit               */
+#define PHY_HALFD_100M      0x2000      /* Half Duplex 100Mbit               */
+#define PHY_FULLD_10M       0x0100      /* Full Duplex 10Mbit                */
+#define PHY_HALFD_10M       0x0000      /* Half Duplex 10MBit                */
+#define PHY_AUTO_NEG        0x3000      /* Select Auto Negotiation           */
+
+#define DP83848C_DEF_ADR    0x0100      /* Default PHY device address        */
+#define DP83848C_ID         0x20005C90  /* PHY Identifier                    */
+#endif
\ No newline at end of file
diff -r 000000000000 -r ac7a5495d95c PowerControl/PowerControl.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PowerControl/PowerControl.h	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,192 @@
+/* mbed PowerControl Library
+  * Copyright (c) 2010 Michael Wei
+  */ 
+  
+#ifndef MBED_POWERCONTROL_H 
+#define MBED_POWERCONTROL_H 
+
+//shouldn't have to include, but fixes weird problems with defines
+//#include "LPC1768.h"
+
+//System Control Register
+// bit 0: Reserved
+// bit 1: Sleep on Exit
+#define LPC1768_SCR_SLEEPONEXIT     0x2
+// bit 2: Deep Sleep
+#define LPC1768_SCR_SLEEPDEEP       0x4
+// bit 3: Resereved
+// bit 4: Send on Pending
+#define LPC1768_SCR_SEVONPEND        0x10
+// bit 5-31: Reserved
+
+//Power Control Register
+// bit 0: Power mode control bit 0 (power-down mode)
+#define LPC1768_PCON_PM0            0x1
+// bit 1: Power mode control bit 1 (deep power-down mode)
+#define LPC1768_PCON_PM1            0x2
+// bit 2: Brown-out reduced power mode
+#define LPC1768_PCON_BODRPM         0x4
+// bit 3: Brown-out global disable
+#define LPC1768_PCON_BOGD           0x8
+// bit 4: Brown-out reset disable
+#define LPC1768_PCON_BORD           0x10
+// bit 5-7 : Reserved
+// bit 8: Sleep Mode Entry Flag
+#define LPC1768_PCON_SMFLAG         0x100
+// bit 9: Deep Sleep Entry Flag
+#define LPC1768_PCON_DSFLAG         0x200
+// bit 10: Power Down Entry Flag
+#define LPC1768_PCON_PDFLAG         0x400
+// bit 11: Deep Power Down Entry Flag
+#define LPC1768_PCON_DPDFLAG        0x800
+// bit 12-31: Reserved
+
+//"Sleep Mode" (WFI).
+inline void Sleep(void)
+{
+    __WFI();
+}
+
+//"Deep Sleep" Mode
+inline void DeepSleep(void)
+{
+   SCB->SCR |= LPC1768_SCR_SLEEPDEEP;
+   __WFI();
+}
+
+//"Power-Down" Mode
+inline void PowerDown(void)
+{
+   SCB->SCR |= LPC1768_SCR_SLEEPDEEP;
+   LPC_SC->PCON &= ~LPC1768_PCON_PM1;
+   LPC_SC->PCON |= LPC1768_PCON_PM0;
+   __WFI();
+   //reset back to normal
+  LPC_SC->PCON &= ~(LPC1768_PCON_PM1 | LPC1768_PCON_PM0);
+}
+
+//"Deep Power-Down" Mode
+inline void DeepPowerDown(void)
+{
+   SCB->SCR |= LPC1768_SCR_SLEEPDEEP;
+   LPC_SC->PCON |= LPC1768_PCON_PM1 | LPC1768_PCON_PM0;
+   __WFI();
+   //reset back to normal
+  LPC_SC->PCON &= ~(LPC1768_PCON_PM1 | LPC1768_PCON_PM0);
+}
+
+//shut down BOD during power-down/deep sleep
+inline void BrownOut_ReducedPowerMode_Enable(void)
+{
+    LPC_SC->PCON |= LPC1768_PCON_BODRPM;
+}
+
+//turn on BOD during power-down/deep sleep
+inline void BrownOut_ReducedPowerMode_Disable(void)
+{
+    LPC_SC->PCON &= ~LPC1768_PCON_BODRPM;
+}
+
+//turn off brown out circutry
+inline void BrownOut_Global_Disable(void)
+{
+    LPC_SC->PCON |= LPC1768_PCON_BOGD;
+}
+
+//turn on brown out circutry
+inline void BrownOut_Global_Enable(void)
+{
+    LPC_SC->PCON &= !LPC1768_PCON_BOGD;
+}
+
+//turn off brown out reset circutry
+inline void BrownOut_Reset_Disable(void)
+{
+    LPC_SC->PCON |= LPC1768_PCON_BORD;
+}
+
+//turn on brown outreset  circutry
+inline void BrownOut_Reset_Enable(void)
+{
+    LPC_SC->PCON &= ~LPC1768_PCON_BORD;
+}
+//Peripheral Control Register
+// bit 0: Reserved
+// bit 1: PCTIM0: Timer/Counter 0 power/clock enable
+#define LPC1768_PCONP_PCTIM0        0x2
+// bit 2: PCTIM1: Timer/Counter 1 power/clock enable
+#define LPC1768_PCONP_PCTIM1        0x4
+// bit 3: PCUART0: UART 0 power/clock enable
+#define LPC1768_PCONP_PCUART0       0x8
+// bit 4: PCUART1: UART 1 power/clock enable
+#define LPC1768_PCONP_PCUART1       0x10
+// bit 5: Reserved
+// bit 6: PCPWM1: PWM 1 power/clock enable
+#define LPC1768_PCONP_PCPWM1        0x40
+// bit 7: PCI2C0: I2C interface 0 power/clock enable
+#define LPC1768_PCONP_PCI2C0        0x80
+// bit 8: PCSPI: SPI interface power/clock enable
+#define LPC1768_PCONP_PCSPI         0x100
+// bit 9: PCRTC: RTC power/clock enable
+#define LPC1768_PCONP_PCRTC         0x200
+// bit 10: PCSSP1: SSP interface 1 power/clock enable
+#define LPC1768_PCONP_PCSSP1        0x400
+// bit 11: Reserved
+// bit 12: PCADC: A/D converter power/clock enable
+#define LPC1768_PCONP_PCADC         0x1000
+// bit 13: PCCAN1: CAN controller 1 power/clock enable
+#define LPC1768_PCONP_PCCAN1        0x2000
+// bit 14: PCCAN2: CAN controller 2 power/clock enable
+#define LPC1768_PCONP_PCCAN2        0x4000
+// bit 15: PCGPIO: GPIOs power/clock enable
+#define LPC1768_PCONP_PCGPIO        0x8000
+// bit 16: PCRIT: Repetitive interrupt timer power/clock enable
+#define LPC1768_PCONP_PCRIT         0x10000
+// bit 17: PCMCPWM: Motor control PWM power/clock enable
+#define LPC1768_PCONP_PCMCPWM       0x20000
+// bit 18: PCQEI: Quadrature encoder interface power/clock enable
+#define LPC1768_PCONP_PCQEI         0x40000
+// bit 19: PCI2C1: I2C interface 1 power/clock enable
+#define LPC1768_PCONP_PCI2C1        0x80000
+// bit 20: Reserved
+// bit 21: PCSSP0: SSP interface 0 power/clock enable
+#define LPC1768_PCONP_PCSSP0        0x200000
+// bit 22: PCTIM2: Timer 2 power/clock enable
+#define LPC1768_PCONP_PCTIM2        0x400000
+// bit 23: PCTIM3: Timer 3 power/clock enable
+#define LPC1768_PCONP_PCQTIM3       0x800000
+// bit 24: PCUART2: UART 2 power/clock enable
+#define LPC1768_PCONP_PCUART2       0x1000000
+// bit 25: PCUART3: UART 3 power/clock enable
+#define LPC1768_PCONP_PCUART3       0x2000000
+// bit 26: PCI2C2: I2C interface 2 power/clock enable
+#define LPC1768_PCONP_PCI2C2        0x4000000
+// bit 27: PCI2S: I2S interface power/clock enable
+#define LPC1768_PCONP_PCI2S         0x8000000
+// bit 28: Reserved
+// bit 29: PCGPDMA: GP DMA function power/clock enable
+#define LPC1768_PCONP_PCGPDMA       0x20000000
+// bit 30: PCENET: Ethernet block power/clock enable
+#define LPC1768_PCONP_PCENET        0x40000000
+// bit 31: PCUSB: USB interface power/clock enable
+#define LPC1768_PCONP_PCUSB         0x80000000
+
+//Powers Up specified Peripheral(s)
+inline unsigned int Peripheral_PowerUp(unsigned int bitMask)
+{
+    return LPC_SC->PCONP |= bitMask;
+}   
+
+//Powers Down specified Peripheral(s) 
+inline unsigned int Peripheral_PowerDown(unsigned int bitMask)
+{
+    return LPC_SC->PCONP &= ~bitMask;
+}
+
+//returns if the peripheral is on or off
+inline bool Peripheral_GetStatus(unsigned int peripheral)
+{
+    return (LPC_SC->PCONP & peripheral) ? true : false;
+}
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r ac7a5495d95c SDFileSystem.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/SDFileSystem/#8db0d3b02cec
diff -r 000000000000 -r ac7a5495d95c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,799 @@
+#include "mbed.h"
+#include "xprintf.h"
+#include "MPL3115A2.h"
+#include "HMC5883L.h"
+#include "MPU6050.h"
+#include "SDFileSystem.h"
+#include "EthernetPowerControl.h"
+#define M_PI 3.141592
+
+//hmc calibration mode
+#define STOP 0  //initial
+#define CAL 1   //calibration
+#define RUN 2   //run
+
+//Sequence phase
+#define Preparing 1
+#define Rising 2
+#define Descending 3 
+#define Landing_Fusing 4 
+#define Moving 5
+
+//Moving step
+#define get_goaldirection 0
+#define attitude_determination 1
+#define calculate_direction 2
+#define direction_control 3
+#define jump 4
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+//ピンアサイン確認
+Serial gps(p13, p14);
+Serial Mu2(p28, p27);
+I2C i2c(p9, p10);       // sda, scl
+Serial mp(USBTX, USBRX);
+SDFileSystem sd(p5, p6, p7, p8, "sd"); 
+LocalFileSystem local("local");
+DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター
+PwmOut pin21(p21);
+DigitalOut jumpmotor(p24);
+
+//溶断
+DigitalOut para(p18); 
+DigitalOut marker(p19);
+DigitalOut jumparm(p20);
+//
+DigitalOut Flightpin(p26);
+
+Timer timer;
+int val;
+unsigned char phase;
+unsigned char step;
+
+//気圧///////
+MPL3115A2 APsensor(&i2c, &mp);
+int alt;
+int ini_alt;
+int min_alt;
+int min_alt5;
+int max_alt;
+int rela_max;
+int rela_ini;
+int flag1;
+int flag2;
+int flag3;
+unsigned char alttimes;
+void getAltitude(){
+    //wait_ms(300); 
+    Altitude a;
+    Temperature t;
+    Pressure p;    
+    APsensor.readAltitude(&a);
+    APsensor.readTemperature(&t);        
+    APsensor.setModeStandby();
+    APsensor.setModeBarometer();
+    APsensor.setModeActive();
+    APsensor.readPressure(&p);
+    alt = a;
+    //気球試験用
+    if(phase == Preparing){
+        rela_ini = alt - ini_alt;
+        if(rela_ini > 7)flag1 = flag1+1;
+    }
+    
+    if(phase == Rising){
+        if(max_alt < alt){
+            max_alt = alt;
+        }
+        else{
+            rela_max = max_alt -alt;
+            if(rela_max > 7)flag2 = flag2+1;
+        }
+    }    
+    //着地判定
+    if(phase == Descending){
+        if (min_alt > alt){
+            min_alt = alt;
+            min_alt5 = min_alt + 5;
+            flag3 -= 1;
+        }
+        else {
+            if( alt < min_alt5){
+                flag3 +=2;
+            }
+        }
+    }
+    mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase);
+    mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max);
+    mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3);
+    //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure()); 
+    APsensor.setModeStandby();
+    APsensor.setModeAltimeter();
+    APsensor.setModeActive();
+}
+/////
+
+//GPS
+int i,rlock,mode;
+char gps_data[256];
+char ns,ew;
+float w_time,hokui,tokei;
+float g_hokui,g_tokei;
+float d_hokui,m_hokui,d_tokei,m_tokei;
+unsigned char c;
+//目標経度,緯度
+float goal_tokei,goal_hokui;
+float goal_direction;
+float direction_y;
+float direction_x;
+float angular_difference;
+ 
+void getGPS(){
+  //目的地 ブラックロック
+    goal_tokei = 118.9833;
+    goal_hokui = 40.92493;
+    c = gps.getc();
+  if( c=='$' || i == 256){
+    mode = 0;
+    i = 0;
+    for(int j=0; j<256; j++){
+        gps_data[j]=NULL;
+    }
+  }
+  if(mode==0){
+    if((gps_data[i]=c) != '\r'){
+      i++;
+    }else{
+      gps_data[i]='\0';
+      
+      if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
+          if(rlock==1){
+          //mp.printf("Status:Lock(%d)\n\r",rlock);
+          //logitude
+          d_tokei= int(tokei/100);
+          m_tokei= (tokei-d_tokei*100)/60;
+          g_tokei= d_tokei+m_tokei;
+          //Latitude
+          d_hokui=int(hokui/100);
+          m_hokui=(hokui-d_hokui*100)/60;
+          g_hokui=d_hokui+m_hokui;
+          mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui);
+          Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui);
+          float longitudinalDifference =-(goal_tokei - g_tokei);
+          float latitudinalDifference = goal_hokui - g_hokui;  
+          
+          //球面三角法
+          direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
+          direction_x = cos(g_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(g_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
+          goal_direction = atan2f(direction_y,direction_x);
+          if(goal_direction < 0)goal_direction += 2*M_PI;
+           
+          //磁北に対する角度に調節 ネバダ東に14度
+          goal_direction -= 0.244346;
+          if (goal_direction < 0) goal_direction += 2*M_PI;
+          mp.printf("goal:%f\n\r",goal_direction*180/M_PI);
+          //mp.printf("longitudinalDifference:%f",longitudinalDifference);
+          //mp.printf("latitudinalDifference:%f",latitudinalDifference);
+          if(phase == Descending)getAltitude();
+          if(phase == Moving)step = attitude_determination;
+        }
+        else{
+          mp.printf("\n\rStatus:unLock(%d)\n\r",rlock);
+          mp.printf("%s",gps_data);
+          Mu2.printf("@DT02No\r\n");
+          if(phase == Descending)getAltitude();
+          //if(phase == Moving)test_get_direction();
+        }
+        sprintf(gps_data, "");
+      }
+    }
+  }
+}
+
+void MU2initialize(){
+    Mu2.baud(19200);
+    Mu2.printf("@EI34\r\n");
+    wait(1);
+    Mu2.printf("@DI25\r\n");
+    wait(1);
+    Mu2.printf("@CH0F\r\n");
+    wait(1);
+    Mu2.printf("@GI34\r\n");
+    wait(1);
+ }
+ 
+/////////////////////
+//地磁気
+HMC5883L compass(p9, p10);
+Ticker interrupt;
+double heading0 = 0.0;
+double heading1 = 0.0;
+double heading2 = 0.0;
+double heading3 = 0.0;
+double headingLPF = 0.0;
+double initHeading;
+double tgtHeading;
+double preHeading = 0.0;
+unsigned char hmc_mode;
+ 
+int maxX, minX, maxY, minY;
+int ofsX = 0;                   //calibration x
+int ofsY = 0;                   //calibration y
+int counter = 0;
+int ofsXset = 1328;
+int ofsYset = -554;
+int ofsZset = -750;
+
+//地磁気キャリブレーション用
+void intrpt() {
+    int16_t raw[3];     
+    compass.getXYZ(raw);
+    //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
+    double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset));
+    if(heading < 0)heading += 2*M_PI;
+    if(heading > 2*M_PI)heading -= 2*M_PI;
+    heading3 = heading2;
+    heading2 = heading1;
+    heading1 = heading0;
+    heading0 = heading;
+    headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
+    
+    switch(hmc_mode) {
+    case STOP:
+        if(counter == 100) {
+            initHeading = headingLPF;
+            hmc_mode = CAL;
+            maxX = raw[0];
+            minX = raw[0];
+            maxY = raw[2];
+            minY = raw[2];
+            counter = 0;
+            //mp.printf("STOP end\n\r");
+        }
+        break;
+    case CAL:
+        if(raw[0] > maxX)maxX = raw[0];
+        if(raw[0] < minX)minX = raw[0];
+        if(raw[2] > maxY)maxY = raw[2];
+        if(raw[2] < minY)minY = raw[2];
+        if((counter > 100) 
+            && (headingLPF > (initHeading-0.01)) 
+            && (headingLPF < (initHeading+0.01))) {
+            ofsX = (maxX + minX)/2;
+            ofsY = (maxY + minY)/2;
+            //hmc_mode = RUN;
+            counter = 0;
+            //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY);
+        }
+        break;
+    case RUN:
+        mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset);
+        //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]);
+        //step = calculate_direction;
+        break;
+    
+    }
+    counter++;
+}
+
+//加速度
+MPU6050 mpu;
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+int32_t axa, aya, aza;
+int32_t gxa, gya, gza;
+
+//オフセット値
+double axoffset = 586.711;
+double ayoffset = -174.847;
+double azoffset = -2195.34375;
+
+//センサの出力あたりの加速度[m/s^2]
+double axrate = 0.000601615;
+double ayrate = 0.000598334;
+double azrate = 0.0005845;
+
+//3軸キャリブレーション
+double phi;
+double theta;
+double heading3axis;
+
+double bfy;
+double bfx;
+
+//3軸からheading算出
+void getheading3axis() {
+    //加速度算出
+    mpu.getAcceleration(&ax, &ay, &az);
+    double axcal = -axrate * (static_cast<double>(ax) - axoffset);
+    double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
+    double azcal = -azrate * (static_cast<double>(az) - azoffset);
+    
+    phi = atan2(aycal,azcal);
+    //phi += M_PI;
+    theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi));
+    //theta += M_PI;
+    
+    //地磁気算出
+    int16_t raw[3];     
+    compass.getXYZ(raw);
+    bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi);
+    bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta) 
+        + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi)
+        + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi);
+    double heading = atan2(-bfy, bfx);
+    if(heading < 0)heading += 2*M_PI;
+    if(heading > 2*M_PI)heading -= 2*M_PI;
+    heading3 = heading2;
+    heading2 = heading1;
+    heading1 = heading0;
+    heading0 = heading;
+    headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
+    //headingLPF += 0.349066;
+    if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI;
+    //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal);
+    mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI);        
+ }
+ 
+//ADXL375
+const int addr = 0xA6;
+char adxl[6];
+short int axout = 0;
+short int ayout = 0;
+short int azout = 0;
+
+void adxl_init(){
+    char fifo[2] = {0x38,0x80};
+    i2c.write(addr,fifo,2);     //FIFO_Mode -> Stream
+    char power[2] = {0x2D,0x08};
+    i2c.write(addr,power,2);    //measurebit -> 1
+}
+
+void adxl_get(){
+    adxl[0] = 0x32;
+    i2c.write(addr,adxl,1);
+    i2c.read(0xA7,adxl,6);        
+    axout = (((int16_t )adxl[1]) << 8) | adxl[0];
+    ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
+    azout = (((int16_t )adxl[5]) << 8) | adxl[4];
+}
+
+
+//モーター駆動方向制御
+void motor_drive(){
+        controlmotor1.write(1);
+        controlmotor2.write(0);
+        pin21.write(0.8);
+        //if(pin1)mp.printf("Yes!\r\n");
+}
+
+//溶断プログラム
+void burning(){
+    wait(5);
+    myled1 = 1;
+    //para = 1;
+    wait(1);
+    myled1 = 0;
+    //para = 0;
+    wait(5);
+    myled2 = 1;
+    //marker = 1;
+    wait(1);
+    myled2 = 0;
+    //marker = 0;
+    wait(5);
+    myled3 = 1;
+    //jumparm = 1;
+    wait(1);
+    myled3 = 0;
+    //jumparm = 0;    
+}
+//跳躍モーター駆動まいまいかわいい
+void jumping(){
+    timer.start();
+    //モーター駆動時間調整
+    while(val < 4){
+        val = timer.read();
+        jumpmotor = 1;
+        myled4 != myled4;
+    }
+    timer.reset();
+    timer.stop();
+    jumpmotor = 0;
+    val = 0;
+}
+
+///test
+//GPSなし方向制御,跳躍テスト
+float test_tokei,test_hokui;
+void test_get_direction(){
+    //目的地 大岡山駅
+    goal_tokei = 139.686234;
+    goal_hokui = 35.614330;
+    
+    //テスト場所石川台一号館
+    test_tokei = 139.681529;
+    test_hokui = 35.605263;
+    
+    float longitudinalDifference = goal_tokei - test_tokei;
+    float latitudinalDifference = goal_hokui - test_hokui;
+    //球面三角法
+    direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
+    direction_x = cos(test_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(test_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
+    goal_direction = atan2f(direction_y,direction_x);
+    if(goal_direction < 0)goal_direction += 2*M_PI;
+           
+    //磁北に対する角度に調節 東京7度
+    goal_direction += 0.122173;
+    //goal_direction2 += 0.122173;
+    if (goal_direction < 0) goal_direction += 2*M_PI;
+} 
+
+/////////////////////////main////////////////////////////////////
+int main(){
+    PHY_PowerDown();//省電力
+    wait(2);
+    //センサ初期化
+    Mu2.baud(19200);
+    Mu2.printf("@EI34\r\n");
+    wait(1);
+    Mu2.printf("@DI25\r\n");
+    wait(1);
+    Mu2.printf("@CH0F\r\n");
+    wait(1);
+    Mu2.printf("@GI34\r\n");
+    wait(1);
+    APsensor.init();
+    wait(0.2);
+    hmc_mode = RUN;
+    mpu.initialize();
+    wait(0.2);
+    compass.init();
+    wait(0.2);
+    adxl_init();
+    wait(0.2);
+    APsensor.setOffsetAltitude(100);
+    APsensor.setOffsetTemperature(10);
+    APsensor.setOffsetPressure(-32);
+    mkdir("/sd/mydir", 0777);
+    min_alt = 4000.0;
+    Flightpin = 1;
+    //高度初期値
+    for(int h = 0; h < 10; h++){
+        getAltitude();
+        ini_alt = ini_alt + alt;
+        wait(0.5);
+    }
+    ini_alt = ini_alt/10;
+    mp.printf("ini_alt:%d\r\n",ini_alt);
+    int jump_count;
+    phase = Preparing;
+    //phase = Moving;
+    /////////////////////////////単機能////////
+    
+    //wait(5);
+   
+    while(1){
+        myled1 = 1;
+        //Flightpin = 1;
+        //GPS_MU2
+        getGPS(); 
+        //方向制御用モーター
+        /*
+        controlmotor1.write(1);
+        controlmotor2.write(0);
+        pin21.write(0.8);
+        wait(3);
+        controlmotor1.write(0);
+        controlmotor2.write(0);
+        pin21.write(0.0);
+        */
+        //跳躍用モーター
+        //jumpmotor = 1;
+        //wait(5);
+        //jumpmotor = 0;
+        //溶断
+        //burning();
+        //気圧
+        //getAltitude();
+        //mpu
+        /*
+        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        double axcal = -axrate * (static_cast<double>(ax) - axoffset);
+        double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
+        double azcal = -azrate * (static_cast<double>(az) - azoffset);
+        mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal);
+        */
+        //hmc
+        /*
+        hmc_mode = RUN;
+        intrpt();
+        wait(0.5);
+        */
+        //hmc_mpu
+        //getheading3axis() ;
+        //wait(0.3);   
+        //adxl_SDカード 跳躍高さ測定
+        /*
+        mkdir("/sd/mydir", 0777);
+        wait(1);
+        FILE *fp = fopen("/sd/mydir/height1.txt", "a");
+        wait(1);
+        if(fp == NULL) {
+            error("Could not open file for write\n");
+        }
+        timer.reset();
+        timer.start();
+        val = 0;
+        while(val < 5000){
+            jumpmotor = 1;
+            val = timer.read_ms();
+            adxl[0] = 0x32;
+            i2c.write(addr,adxl,1);
+            i2c.read(0xA7,adxl,6);
+            axout = (((int16_t )adxl[1]) << 8) | adxl[0];
+            ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
+            azout = (((int16_t )adxl[5]) << 8) | adxl[4];
+            fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 
+            wait_ms(2);
+            myled1 !=myled1;
+        }
+        myled1 = 0;
+        jumpmotor = 0;
+        wait(1);
+        fclose(fp);
+        */
+        //
+                
+    }
+    /////////////////////////    
+        
+    
+    /*
+    //////////////////////////安全試験用/////////
+    //振動試験
+    myled1 = 0;
+    //wait(5);//test
+    wait(900);//end to end
+    //静荷重
+    FILE *sl = fopen("/local/static2.txt", "a");
+    wait(10);
+    myled1 = 1;
+    wait(5);
+    timer.reset();
+    timer.start(); 
+    //while(val < 30000){ //test 
+    while(val < 600000){ //end to end
+        val = timer.read_ms();
+        adxl[0] = 0x32;
+        i2c.write(addr,adxl,1);
+        i2c.read(0xA7,adxl,6);
+        axout = (((int16_t )adxl[1]) << 8) | adxl[0];
+        ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
+        azout = (((int16_t )adxl[5]) << 8) | adxl[4];
+        //pc.printf("time:%d,X:%d,Y:%d,Z:%d\r\n",val,axout,ayout,azout);
+        fprintf(sl, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
+        wait_ms(200); 
+        //wait必要、SDがbusyの状態で送っちゃってるんじゃない?
+    }
+    fclose(sl);
+    timer.reset();
+    timer.stop();
+    val = 0;
+    myled1 = 0;
+    //wait(5); //test
+    wait(960); //end to end
+        
+    //開傘衝撃
+    FILE *im = fopen("/local/impact2.txt", "a");
+    wait(10);
+    myled2 = 1;
+    timer.reset();
+    timer.start();
+    //while(val < 10000){ //test   
+    while(val < 30000){ //end to end
+        val = timer.read_ms();
+        adxl[0] = 0x32;
+        i2c.write(addr,adxl,1);
+        i2c.read(0xA7,adxl,6);
+        axout = (((int16_t )adxl[1]) << 8) | adxl[0];
+        ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
+        azout = (((int16_t )adxl[5]) << 8) | adxl[4];
+        fprintf(im, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
+        wait_ms(3); 
+        //wait必要、SDがbusyの状態で送っちゃってるんじゃない?
+    }
+    fclose(im);
+    timer.reset();
+    timer.stop();
+    val = 0;
+    myled2 = 0;
+    //wait(30); //test
+    wait(600); //end to end
+    ///////////////////////////
+    */
+    //着地検知用timer
+    timer.reset();
+    timer.start();
+    val = 0;  
+    while(1){
+        switch(phase){
+        case Preparing:
+            myled1 = 1;
+            getAltitude();
+            wait(0.5);
+            val = timer.read();
+            if(flag1 > 10) phase = Rising;
+            if( val > 300) phase = Rising;
+            break; 
+        case Rising:
+            myled1 = 0;
+            myled2 = 1;
+            getAltitude();
+            wait(0.5);
+            val = timer.read();
+            if(flag2 > 10) phase =  Descending;
+            if(val > 600) phase =  Descending; 
+            break;    
+        case Descending:
+            val = timer.read();  
+            myled2 = 0;
+            myled3 = 1;
+            getGPS();
+            //wait(0.5);
+            if ( flag3 >= 100){
+                //mpuの条件もいれます
+                if( val > 180){
+                phase = Landing_Fusing;
+                }
+            }
+            if( val > 900) phase = Landing_Fusing;//タイマー検知15分               
+            break;
+        case Landing_Fusing:
+            timer.reset();
+            timer.stop();
+            val = 0;
+            myled3 = 0;
+            myled4 = 1;
+            wait(30);
+            burning();
+            wait(5);
+            Mu2.printf("@DT04FIRE\r\n");
+            for ( int gp; gp <= 10; gp++){
+                getGPS();
+            }
+            wait(5);
+            jumpmotor = 1;
+            wait(15);
+            jumpmotor = 0;
+            phase = Moving;           
+            break;
+        case Moving:
+            myled2 = 0;
+            myled1 = 0;
+            myled3 = 0;
+            myled4 = 0;
+            Mu2.printf("@DT04MOVE\r\n");
+            wait(1);
+            switch(step){
+            case get_goaldirection:
+                myled1 = 1;
+                //getGPS();
+                test_get_direction();
+                step = attitude_determination;
+                wait(1);
+                break;
+            case attitude_determination:
+                myled1 = 0;
+                myled2 = 1;
+                timer.reset();
+                timer.start();
+                val = 0;
+                while(val < 5){
+                    val = timer.read();
+                    getheading3axis();
+                    wait(0.5);
+                }
+                getheading3axis();
+                step = calculate_direction;
+                wait(1);
+                break;
+            case calculate_direction:
+                myled2 = 0;
+                myled3 = 1;
+                if(headingLPF < M_PI){
+                    if(goal_direction < headingLPF+M_PI){
+                        angular_difference = headingLPF - goal_direction;
+                    }
+                    else{
+                        angular_difference = 2*M_PI + (headingLPF - goal_direction);
+                    }
+                }
+                else{
+                    if(goal_direction < headingLPF - M_PI){
+                        angular_difference = headingLPF - goal_direction - 2*M_PI;
+                    }
+                    else{
+                        angular_difference = headingLPF - goal_direction;
+                    }   
+                }
+                mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI);
+                //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI);
+                //40度以下でジャンプ
+                if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){
+                    step = jump;
+                }
+                else {
+                    step = direction_control;
+                }
+                wait(1);
+                break;
+                
+            case direction_control:
+                myled1 = 0;
+                myled3 = 0;
+                myled4 = 1;
+                //int drive_time;
+                timer.reset();
+                timer.stop();
+                timer.start();
+                val = 0;
+                while(val < 2){
+                    val = timer.read();
+                    motor_drive();            
+                    }
+                controlmotor1.write(0);
+                controlmotor2.write(0);
+                pin21.write(0.0);
+                timer.reset();
+                timer.stop();
+                val = 0;
+                //step = jump;
+                if(jump_count == 3){
+                    step = jump;
+                }
+                else{
+                    step = attitude_determination;
+                }
+                jump_count = jump_count+1;
+                break;
+                
+            case jump:
+                //ログファイル
+                myled1 = 1;
+                myled2 = 1;
+                wait(2);
+                FILE *fp = fopen("/sd/mydir/height1.txt", "a");
+                wait(1);
+                timer.reset();
+                timer.start();
+                val = 0;
+                jumpmotor = 1;
+                while(val < 4000){
+                    val = timer.read_ms();
+                    adxl[0] = 0x32;
+                    i2c.write(addr,adxl,1);
+                    i2c.read(0xA7,adxl,6);
+                    axout = (((int16_t )adxl[1]) << 8) | adxl[0];
+                    ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
+                    azout = (((int16_t )adxl[5]) << 8) | adxl[4];
+                    fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 
+                    wait_ms(2);
+                }
+                jumpmotor = 0;
+                wait(1);
+                fclose(fp);
+                myled3 = 1;
+                jump_count = 0;
+                wait(3);
+                step = get_goaldirection;
+                //step = direction_control;
+                break;
+            }
+        }                       
+    }
+}       
diff -r 000000000000 -r ac7a5495d95c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90
\ No newline at end of file
diff -r 000000000000 -r ac7a5495d95c xprintf.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/xprintf.h	Sat Sep 09 23:18:12 2017 +0000
@@ -0,0 +1,39 @@
+/*------------------------------------------------------------------------*/
+/* Universal string handler for user console interface  (C)ChaN, 2011     */
+/*------------------------------------------------------------------------*/
+
+#ifndef _STRFUNC
+#define _STRFUNC
+
+#define _USE_XFUNC_OUT	1	/* 1: Use output functions */
+#define	_CR_CRLF		1	/* 1: Convert \n ==> \r\n in the output char */
+
+#define _USE_XFUNC_IN	1	/* 1: Use input function */
+#define	_LINE_ECHO		1	/* 1: Echo back input chars in xgets function */
+
+
+#if _USE_XFUNC_OUT
+#define xdev_out(func) xfunc_out = (void(*)(unsigned char))(func)
+extern void (*xfunc_out)(unsigned char);
+void xputc (char c);
+void xputs (const char* str);
+void xfputs (void (*func)(unsigned char), const char* str);
+void xprintf (const char* fmt, ...);
+void xsprintf (char* buff, const char* fmt, ...);
+void xfprintf (void (*func)(unsigned char), const char*	fmt, ...);
+void put_dump (const void* buff, unsigned long addr, int len, int width);
+#define DW_CHAR		sizeof(char)
+#define DW_SHORT	sizeof(short)
+#define DW_LONG		sizeof(long)
+#endif
+
+#if _USE_XFUNC_IN
+#define xdev_in(func) xfunc_in = (unsigned char(*)(void))(func)
+extern unsigned char (*xfunc_in)(void);
+int xgets (char* buff, int len);
+int xfgets (unsigned char (*func)(void), char* buff, int len);
+int xatoi (char** str, long* res);
+#endif
+
+#endif
+