a
    ?Cf)  ã                   @   sV   d dl mZ d dlZd dlZd dlmZmZ G dd„ dƒZdd„ Z	dd	„ Z
d
d„ ZdS )é    )ÚminidomN)Úatan2Úpic                   @   s   e Zd Zdd„ Zdd„ ZdS )ÚPointc                 C   s   || _ || _d S )N)ÚxÚy)Úselfr   r   © r	   ú*/var/www/html/django/DPS/api/planformat.pyÚ__init__   s    zPoint.__init__c                 C   s2   t | j|j | j|j ƒ}|dk r.|dt 7 }|S )Nr   é   )r   r   r   r   )r   ÚotherÚangler	   r	   r
   ÚangleFromSelf	   s    zPoint.angleFromSelfN)Ú__name__Ú
__module__Ú__qualname__r   r   r	   r	   r	   r
   r      s   r   c                 C   s   | d t  S )Né´   )r   )r   r	   r	   r
   Ú	rad_2_deg   s    r   c                 C   sÂ  d}t  ¡ }| d¡}| |¡ |D ]è}| d¡}| dt|ƒ¡ | dd¡ | dt|d ƒ¡ | dt|d	 ƒ¡ | d
tdƒ¡ | dd¡ | dd¡ | dd¡ | dd¡ | |¡ |d }d}d	}	tt|ƒƒD ]<}
||
 }|d |d	 g}tj	 
||¡j}||krÞ|}|}	qÞt|	d	 |	d ƒ}t|d |d	 ƒ}t| |¡ƒ}t|	d	 |	d ƒ t|d |d	 ƒ t|ƒ | d¡}| dt|ƒ¡ | dd¡ | dd¡ | dd¡ | d
d¡ | dtt|ƒƒ¡ | dd¡ | dd¡ | dd¡ | |¡ |d }q$| jd d d	 d	 }| d¡}| dt|ƒ¡ | dd¡ | dt|d ƒ¡ | dt|d	 ƒ¡ | d
d¡ | dd¡ | dd¡ | dd¡ | dd¡ | |¡ | ¡ S )Né   ÚmissionZmissionitemÚnoÚactionZWAYPOINTÚlatÚlonr   Úaltr   Z
parameter1Ú0Z
parameter2Z
parameter3Úflagé@B ZSET_HEADÚgeometryÚcoordinatesZLANDÚ2)r   ÚDocumentÚcreateElementÚappendChildÚsetAttributeÚstrÚrangeÚlenÚgeopyÚdistanceÚgeodesicÚmr   r   r   ÚprintÚintÚflight_planÚtoprettyxml)Úplan_instanceÚtree_coordinates_arrayr    ÚiÚrootÚxmlÚ
coordinateZwaypointÚ
lengthLineÚclosestTreeÚjÚtree_coordinatesÚpoint_coordinatesr*   Ú
tree_pointÚflight_pointÚturn_degreesZset_headZlast_coordinateZlandingpointr	   r	   r
   ÚConvertToKML   sr    









r?   c                 C   sÎ  d}|D ]}|d |kr|d }qd}d}|t |ƒ d t |ƒ d }|d }|D ]4}|t |ƒd t |d ƒ d t |d ƒ d t |ƒ d 7 }|d }d	}d}tt|ƒƒD ]<}	||	 }
|d |d g}tj |
|¡j}||kr°|}|
}q°t|d |d ƒ}t|d |d ƒ}t| 	|¡ƒ}|t |ƒd
 t t
|ƒƒ d t |ƒ d 7 }|d }|t |ƒd t |d ƒ d t |ƒ d 7 }|d }qR|t |d ƒ d t |d ƒ d t |d ƒ d t |ƒ d }|S )Nr   r   z$QGC WPL 110
0	1	0	0	0	0	0	0	0	0	0	1
r   z>	0	3	22	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	z	1
z,	0	3	16	0.000000	0.000000	0.000000	0.000000	ú	r   z		0	3	115	z.	0.000000	0.000000	0.000000	0.000000	0.000000	z	0	3	2000	1	0.000000	0.000000	é   z	0.000000	0.000000	z,	0	3	21	0.000000	0.000000	0.000000	0.000000	z	1)r&   r'   r(   r)   r*   r+   r,   r   r   r   r.   )r1   r2   r    Zmax_altituder6   Ú
plain_textr3   r7   r8   r9   r:   r;   r*   r<   r=   r>   r	   r	   r
   ÚConvertToArduPilot\   s:    

<,,
@rC   )Úxml.domr   Úgeopy.distancer)   ÚosÚmathr   r   r   r   r?   rC   r	   r	   r	   r
   Ú<module>   s   	J