大 田澤
/
Group_Control_No2_3actions
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 0:65f9be2858c9
- Child:
- 1:12c5a8179ad2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Feb 22 08:59:47 2017 +0000
@@ -0,0 +1,787 @@
+char buf1[0x4000]; // 通常エリア
+char buf2[0x4000] __attribute__((section("AHBSRAM0"))); // Ethernet用エリア (0x2007c000)
+
+#include <stdlib.h>
+#include <string.h>
+#include "xbee.h"
+#include "MPU9250.h"
+#include "mbed.h"
+
+#define PI 3.14159265
+#define A 6378137.000
+#define B 6356752.314245
+#define e2 0.006694379
+#define m 6335439.327292
+
+#define aa 11
+#define bb 21
+#define cc 31
+#define aa_ 12
+#define bb_ 22
+#define cc_ 32
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut request(p11);
+DigitalIn signal(p12);
+Serial pc(USBTX, USBRX);
+Serial sGps(p28, p27);
+//RawSerial _xbee_serial(p9, p10);
+SPI spi(p5, p6, p7);
+mpu9250_spi imu(spi,p8);
+Serial sbus(p13, p14);
+Timer t, anglesT;
+
+double gyro[3],angles_before[3],angles[3],houi;
+float m_min[] = {-35.921, -33.910, -41.263};
+float m_max[] = {41.938, 44.207, 40.922};
+float offsetM[3];
+int i = 0, j = 0, k = 0;
+double myLat_rad, myLong_rad, Latitude_rad, Longitude_rad;
+
+double sekkin = 0.0, sekkin_hani = 10.0;
+
+double Time, Lat, Long, heading;
+double Timef, Latf, Longf, Heading;
+double Lat_before, Latf_before, Long_before, Longf_before, Heading_before, distance_before;
+double distance, direction;
+
+double b_head, b_head_front;
+double range;
+
+int rud = 1024, ele = 1024, ail = 1024;
+double ele_dis = 0.0, ail_dis = 0.0;
+int ele_range, ail_range;
+
+//int marker, marker_me;
+
+char sbus_from[6][6];
+int channels_from[] = {1024,1024,1024,1024,1024,1541};
+int channels_to[] = {1024,1024,1024,1024,1024,1541};
+int not_use;
+int through = 0, through_before = 0;
+bool input_before = true;
+
+/*XBee送受信関連*/
+/*#define API_TXSIZE 255 // 送信用APIデータ長(32～255)
+typedef unsigned char byte;
+
+static byte PACKET_ID = 0;
+
+static byte ADR_DEST[]= {0x00,0x13,0xA2,0x00,0x00,0x00,0x00,0x00}; //宛先のIEEEアドレス(変更可)
+static byte SADR_DEST[]= {0xFF,0xFE}; //ブロード(ショート)アドレス
+*/
+
+/*void xbee_address(const byte *address){
+
+// 処理：送信用の宛先アドレス設定用の関数
+// 入力：byte *address = 宛先(子機)アドレス
+
+ byte i;
+
+ for(i=0; i< 8 ;i++ ) ADR_DEST[i] = address[i];
+ SADR_DEST[0] = 0xFF;
+ SADR_DEST[1] = 0xFE;
+}*/
+
+/* シリアル送受信バッファクリア */
+/*void sci_clear(void){
+ //while( _xbee_serial.readable() ){
+ // _xbee_serial.getc();
+ // wait(0.002);
+ //}
+ while( !_xbee_serial.writeable() ){
+ _xbee_serial.send_break();
+ //wait(1);
+ }
+}*/
+
+/*byte sci_write_check(void){
+ if( !_xbee_serial.writeable() ){
+ //wait(0.1);
+ while( !_xbee_serial.writeable() ) sci_clear();
+ }
+
+ return( 1 );
+}*/
+
+/*byte sci_write( char *data, byte len ){
+ byte ret=1; // 戻り値 0でエラー
+
+ byte i;
+ sci_write_check();
+ for(i=0;i<len;i++){
+ if( _xbee_serial.putc((int)data[i]) < 0 ) ret=0;
+ }
+ if(ret) ret=len;
+
+ return( ret );
+}*/
+
+/*byte xbee_at_tx(const char *at, const byte *value, const byte value_len){
+
+// 処理：リモートＡＴコマンドの送信を行うXBeeドライバ部
+// 入力：ATコマンドat[] ＝ "AT**"はローカルATで、"RAT***"がリモートＡＴ
+// TXがデータ送信モード
+// データvalue[] ＝ 各ＡＴコマンドで引き渡すデータ値
+// value_len ＝ その長さ
+// 戻り値：送信したAPIサービス長。送信しなかった場合は0
+
+ char data_api[API_TXSIZE];
+ byte i;
+ byte len=0; // APIサービス長
+ byte check=0xFF; // チェックサム
+ byte data_position=5; // 送信データdata_api[]の何処にvalue[]を入れるか
+ byte ret=0;
+
+ if( PACKET_ID == 0xFF ){
+ PACKET_ID=0x01;
+ }else{
+ PACKET_ID++;
+ }
+
+ if(at[0] == 'T' && at[1] == 'X'){
+ data_api[3]=(char)0x10; // TXデータ送信モード
+ data_api[4]=(char)0x00; // フレームIDを使用しない(no responce)
+ for( i=5 ; i<=12 ; i++) data_api[i]=(char)ADR_DEST[i-5];
+ for( i=13 ; i<=14 ; i++) data_api[i]=(char)SADR_DEST[i-13];
+ data_api[15]=(char)0x00; // ZigBeeホップ数
+ data_api[16]=(char)0x00; // 暗号化＝しない
+ data_position=17;
+ len=14;
+ } // サービスデータにMD+FI+ADR(8)+SAD(2)+OPT(2)が入る
+
+ if( len ){
+ data_api[0]=(char)0x7E; // デリミタ
+ data_api[1]=(char)0x00; // パケット長の上位(送らない前程)
+ for( i=3 ; i < data_position ; i++) check -= (byte)data_api[i];
+ if( value_len > 0 ){
+ for( i=0 ; i<value_len; i++){
+ data_api[data_position + i] = (char)value[i];
+ check -= (byte)data_api[data_position + i];
+ len++;
+ }
+ }
+ data_api[2] =(char)len;
+ data_api[len+3]=(char)check;
+
+ check = sci_write_check(); // 以降 checkはシリアルバッファ確認に使用する
+ //シリアルデータ送信
+ if( check > 0 ){
+ sci_write( data_api, (byte)(len+4) );
+ ret=len+3;
+ }else{
+ ret=0;
+ }
+ }
+
+ return( ret );
+}*/
+
+/*byte xbee_putstr( const char *s ){
+
+// 処理：文字を送信する
+// 入力：char *s
+// 出力：送信データ長を応答。０の場合は異常
+
+ byte data[API_TXSIZE-17]; // 17バイトはAPIヘッダ+CRC1バイトなのでデータ長は[API_TXSIZE-18]+null文字で+1する-17
+ byte i;
+
+ for(i=0; (i< (API_TXSIZE-18) ) && (s[i] != 0x00) ; i++){ // データ長はAPI_TXSIZE-18
+ data[i] = (byte)s[i]; // テキストデータをバイナリデータ（バイト値）に変換する
+ }
+ data[i] = 0x00;
+ if( xbee_at_tx( "TX", data , i) == 0) i=0;
+
+ return( i );
+}*/
+
+/*byte xbee_uart(const byte *address, const char *in){
+
+// 入力：byte *address = 宛先(子機)アドレス
+// char *in = 送信するテキスト文字。最大文字数はAPI_TXSIZE-1
+// 出力：戻り値 = 送信パケット番号PACKET_ID。0x00は失敗。
+
+ byte ret=0;
+
+ xbee_address( address ); // 宛先のアドレスを設定
+ if( xbee_putstr( in ) > 0 ) ret = PACKET_ID;
+
+ return( ret );
+}*/
+
+
+
+void to_rad(double myLat, double myLong, double Latitude, double Longitude){ //緯度経度をradに変換
+ double myLat_deg = floor(myLat / 100.0) ;
+ myLat_rad = (myLat_deg + (myLat - (myLat_deg * 100.0)) / 60.0) * (PI / 180.0) ; //自機の緯度をradに変換
+ double myLong_deg = floor(myLong / 100.0) ;
+ myLong_rad = (myLong_deg + (myLong - (myLong_deg * 100.0)) / 60.0) * (PI / 180.0); //自機の経度をradに変換
+ double Latitude_deg = floor(Latitude / 100.0) ;
+ Latitude_rad = (Latitude_deg + (Latitude - (Latitude_deg * 100.0)) / 60.0) * (PI / 180.0); //他機の緯度をradに変換
+ double Longitude_deg = floor(Longitude / 100.0) ;
+ Longitude_rad = (Longitude_deg + (Longitude - (Longitude_deg * 100.0)) / 60.0) * (PI / 180.0); //他機の経度をradに変換
+}
+
+double hubeni_distance(double lat1, double long1, double lat2, double long2){//ヒュベニの公式を用いて距離を求める。緯度経度はラジアン。
+ double dx = long1 - long2; //経度の差
+ double dy = lat1 - lat2; //緯度の差
+ double uy = (lat1 + lat2) / 2.0; //緯度の平均
+ double W = sqrt(1.0 - e2 * pow(sin(uy),2)); //各曲率半径の分母
+ double M = m / pow(W,3); //子午線曲率半径
+ double N = A / W; //卯酉線曲率半径
+ double d = sqrt(pow((dy * M),2) + pow((dx * N * cos(uy)),2)); //距離
+ return d;
+}
+
+/*double directions(double lat1, double long1, double lat2, double long2){//自機から見た相手の方位を求める。緯度経度はラジアン。
+ double dir = 90 - atan2(sin(long2-long1), cos(lat1) * tan(lat2) - sin(lat1) * cos(long2-long1));
+ return dir;
+}*/
+
+double directions(double lat1, double long1, double lat2, double long2){//自機から見た相手の方位を求める。緯度経度はラジアン
+ double dir_rad = atan2(cos(lat2) * sin(long2-long1), cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(long2 - long1));
+ if(dir_rad < 0){
+ dir_rad += (2.0 * PI);
+ }
+ double dir = dir_rad * (180.0 / PI);
+ return dir;
+}
+
+double map(double x, double in_min, double in_max, double out_min, double out_max)//範囲変換
+{
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+void SBUS(){
+ while(sbus.readable() > 0){
+ if(sbus.getc() == '~'){
+ for(i = 0; i < 4; i++){
+ for(k = 0; k < 6; k++){
+ sbus_from[i][k] = sbus.getc();
+ if(sbus_from[i][k] == ','){
+ sbus_from[i][k] = '\0';
+ channels_from[i] = atoi(sbus_from[i]);
+ break;
+ }
+ }
+ }
+
+ channels_to[0] = channels_from[0];
+ channels_to[1] = channels_from[1];
+ channels_to[2] = channels_from[2];
+ channels_to[3] = channels_from[3];
+ //channels_to[4] = channels_from[4];
+ //channels_to[5] = channels_from[5];
+
+ pc.printf("\n");
+ pc.printf("%6d%6d%6d%6d",
+ channels_from[0],
+ channels_from[1],
+ channels_from[2],
+ channels_from[3]
+ );
+ if(through == 0){
+ through = 1;
+ }else if(through == 1){
+ through = 0;
+ }else;
+ }
+ //not_use = sbus.getc();
+ sbus.getc();
+ }
+}
+
+int main(){
+ int n = 0;
+ myled1 = true;
+ myled2 = true;
+ myled3 = true;
+ request = false;
+
+ sbus.baud(57600);
+
+ pc.baud(9600);
+ sGps.baud(9600);
+
+ //_xbee_serial.baud(57600);
+ //_xbee_serial.format(8, Serial::None, 1);
+ //_xbee_serial.attach(&_xbee_serial_callback);
+ xbee_init(0);
+
+ imu.init(1,BITS_DLPF_CFG_188HZ);
+ printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_1000DPS));
+ wait(0.2);
+ printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_8G));
+ wait(0.2);
+ imu.AK8963_calib_Magnetometer();
+ wait(0.2);
+ for(i = 0; i < 3; i++){
+ offsetM[i] = (m_max[i] + m_min[i]) / 2.0;
+ imu.AK8963_setoffset(i,offsetM[i]);
+ }
+ pc.printf("%10.3f,%10.3f,%10.3f\n\n\n"
+ ,imu.Magnetometer_offset[0]
+ ,imu.Magnetometer_offset[1]
+ ,imu.Magnetometer_offset[2]
+ );
+ wait(0.2);
+
+ //byte dev1[]={0x00,0x13,0xA2,0x00,0x40,0xAF,0x4F,0x4A};//1
+ byte dev2[]={0x00,0x13,0xA2,0x00,0x40,0xF9,0x87,0xC9};//2
+ byte dev3[]={0x00,0x13,0xA2,0x00,0x40,0xF9,0x88,0x91};//3*/
+
+ t.start();
+ anglesT.start();
+ //sbus.attach(SBUS, Serial::RxIrq);
+ imu.Timer_start();
+
+ while(1){
+ if(n == 500){
+ myled1 = !myled1;
+ n = 0;
+ }else{
+ n++;
+ }
+
+ imu.AK8936_read_Orientation();
+
+ char buf[10], s[70], s_to[40];
+
+ byte discard, re;
+ char a[5][13], b[5][13], c[13];
+
+ /*if(t.read_ms() > 200){
+
+ imu.AK8936_read_Orientation();
+ heading = imu.getdata[0];
+ myled2 = !myled2;
+
+ sprintf(s,"%02d%f,\r",aa_,heading);//1
+ //sprintf(s,"%02d%f,\r",bb_,heading);//2
+ //sprintf(s,"%02d%f,\r",cc_,heading);//3
+
+ //pc.printf("\n");
+ //pc.printf("MY ");
+ //pc.printf(s);
+
+ //xbee_uart(dev1,s);
+ xbee_uart(dev2,s);
+ //xbee_uart(dev3,s);
+
+ myled3 = !myled3;
+
+ t.reset();
+ }*/
+
+ while(sGps.readable() > 0){
+ if(sGps.getc() == '$'){
+ for(i = 0; i < 5; i++){
+ buf[i] = sGps.getc();
+ }
+ if (0 == strncmp("GNRMC", buf, 5)) {
+ char dot = sGps.getc();
+ for(i = 0; i < 5; i++){
+ for(j = 0; j < 13; j++){
+ b[i][j] = sGps.getc();
+ if(b[i][j] == ','){
+ b[i][j] = '\0';
+ break;
+ }
+ }
+ }
+
+ /*pc.printf("\n");
+ pc.printf(b[0]);
+ pc.printf(":");
+ pc.printf(b[1]);
+ pc.printf(":");
+ pc.printf(b[2]);
+ pc.printf(":");
+ pc.printf(b[3]);
+ pc.printf(":");
+ pc.printf(b[4]);
+ pc.printf(":");
+ pc.printf("%f",heading);*/
+
+ Time = atof(b[0]);
+ Lat = atof(b[2]);
+ Long = atof(b[4]);
+ //marker_me = channels_to[5];
+
+ imu.AK8936_read_Orientation();
+ heading = imu.getdata[0];
+ myled2 = !myled2;
+
+ sprintf(s,"%02d%9.2f,%10.5f,%11.5f,%9.5f,\r",aa,Time,Lat,Long,heading);//1
+ //sprintf(s,"%02d%9.2f,%10.5f,%11.5f,%9.5f,%04d,\r",aa,Time,Lat,Long,heading,marker_me);//1
+
+ pc.printf("\n");
+ pc.printf("MY ");
+ pc.printf(s);
+
+ xbee_uart(dev2,s);
+ //xbee_uart(dev3,s);
+ //xbee_send_frame(dev2, s, 46);
+ //xbee_send_frame(dev3, s, sizeof(s));
+
+ t.reset();
+ }
+ }
+ }
+
+ while( xbee_readable() > 0 ){
+ //while( xbee.readable() > 0 ){
+ if(sci_read(1) == 0x7E){
+ //if(xbee.getc() == 0x7E){
+ wait_us(200);
+ for(i = 0; i < 4; i++){
+ discard = sci_read(1);
+ //discard = xbee.getc();
+ wait_us(200);
+ }
+ for(;;){
+ discard = sci_read(1);
+ //discard = xbee.getc();
+ wait_us(200);
+ //if(discard == 49) break;
+ if(discard == 50) break;
+ //if(discard == 51) break;
+ }
+ re = sci_read(1);
+ //re = xbee.getc();
+ wait_us(200);
+ if(re == 49){
+ char No[] = {discard, re};
+ int Noi = atoi(No);
+
+ for(j = 0; j < 4; j++){
+ for(k = 0; k < 13; k++){
+ a[j][k] = sci_read(1);
+ //a[j][k] = xbee.getc();
+ wait_us(200);
+ if(a[j][k] == ','){
+ a[j][k] = '\0';
+ break;
+ }
+ }
+ }
+
+ /*for(j = 0; j < 5; j++){
+ for(k = 0; k < 13; k++){
+ a[j][k] = sci_read(1);
+ //a[j][k] = xbee.getc();
+ wait_us(200);
+ if(a[j][k] == ','){
+ a[j][k] = '\0';
+ break;
+ }
+ }
+ }*/
+
+ Timef = atof(a[0]);
+ Latf = atof(a[1]);
+ Longf = atof(a[2]);
+ Heading = atof(a[3]);
+ //marker = atoi(a[4]);
+
+ pc.printf("\n");
+ if(Noi == 11){
+ pc.printf("A ");
+ }else if(Noi == 21){
+ pc.printf("B ");
+ }else if(Noi == 31){
+ pc.printf("C ");
+ }else{
+ pc.printf("%d ",Noi);
+ }
+
+ //pc.printf("Time = ");
+ pc.printf("%f",Timef);
+ //pc.printf(Time);
+ pc.printf(" : ");
+ //pc.printf("Latitude = ");
+ pc.printf("%f",Latf);
+ //pc.printf(Lat);
+ pc.printf(" : ");
+ //pc.printf("Longitude = ");
+ pc.printf("%f",Longf);
+ //pc.printf(Long);
+ pc.printf(" : ");
+ //pc.printf("Heading = ");
+ pc.printf("%f",Heading);
+ //pc.printf(" : ");
+ //pc.printf("Marker = ");
+ //pc.printf("%d",marker);
+ }
+ if(re == 50){
+ char No[] = {discard, re};
+ int Noi = atoi(No);
+
+ for(j = 0; j < 13; j++){
+ c[j] = sci_read(1);
+ //c[j] = xbee.getc();
+ wait_us(200);
+ if(c[j] == ','){
+ c[j] = '\0';
+ break;
+ }
+ }
+
+ Heading = atof(c);
+
+ /*pc.printf("\n");
+ if(Noi == 12){
+ pc.printf("A ");
+ }else if(Noi == 22){
+ pc.printf("B ");
+ }else if(Noi == 32){
+ pc.printf("C ");
+ }else{
+ pc.printf("%d ",Noi);
+ }
+
+ pc.printf("%f",Heading);*/
+ }
+ }
+ //pc.printf("f");
+ }
+
+ if(input_before != signal && signal == true){
+ channels_to[5] = 1541;
+ input_before = signal;
+ pc.printf("%d\n",channels_to[5]);
+ }else if(input_before != signal && signal == false){
+ channels_to[5] = 1024;
+ input_before = signal;
+ pc.printf("%d\n",channels_to[5]);
+ }else;
+
+ /*if(channels_to[5] == 1541 && marker != 2222){
+ marker = 2222;
+ }*/
+
+ if((Lat_before != Lat) && (Latf_before != Latf)){
+ if((Lat != 0.0) && (Latf != 0.0) && (Long != 0.0) && (Longf != 0.0)){
+ Lat_before = Lat;
+ Latf_before = Latf;
+ Long_before = Long;
+ Longf_before = Longf;
+ to_rad(Lat, Long, Latf, Longf);
+ distance = hubeni_distance(myLat_rad, myLong_rad, Latitude_rad, Longitude_rad);
+ direction = directions(myLat_rad, myLong_rad, Latitude_rad, Longitude_rad);
+
+ //pc.printf("\nDistance = %10.3f Direction = %10.3f\n",distance,direction);
+ }else{
+ if(distance != -2.0){
+ //distance = -1.0;
+ ail = 1024;
+ ele = 1024;
+ rud = 1024;
+ sprintf(s_to,"~%04d,%04d,%04d,%04d,%04d,%04d,",
+ ail,
+ ele,
+ channels_to[2],
+ rud,
+ channels_to[4],
+ channels_to[5]
+ );
+ sbus.printf(s_to);
+ //pc.printf("\n");
+ //pc.printf(s_to);
+
+ char sta[40];
+ sprintf(sta,"Time=%.2f, ail=%d, ele=%d\r",Time,ail,ele);
+ //pc.printf(sta);
+ xbee_uart(dev3,sta);
+ //xbee_send_frame(dev3, sta, sizeof(sta));
+
+ distance = -2.0;
+ }
+ }
+ }
+
+ //if((distance > sekkin) && (Heading_before != Heading) && (sbus.channel(6) != 1541) && (marker == 1111)){
+ //if((distance > sekkin) && (Heading_before != Heading) && (marker == 1111)){
+ if((distance > sekkin) && (Heading_before != Heading)){
+
+ Heading_before = Heading;
+
+ imu.AK8936_read_Orientation();
+ heading = imu.getdata[0];
+ myled2 = !myled2;
+
+ b_head_front = Heading + (180.0 - heading); //先導機と方位を揃える
+ if(b_head_front > 360.0){
+ b_head_front = b_head_front - 360.0;
+ }else if(b_head_front < 0.0){
+ b_head_front = b_head_front + 360.0;
+ }else;
+
+ if(b_head_front >= 180.0){
+ rud = map(b_head_front,180.0,360.0,1084,1384);
+ }else{
+ rud = map(b_head_front,180.0,0.0,963,664);
+ }
+
+
+ b_head = direction + (180.0 - heading); //先導機に追従
+ if(b_head > 360.0){
+ b_head = b_head - 360.0;
+ }else if(b_head < 0.0){
+ b_head = b_head + 360.0;
+ }else ;
+
+ if(distance > sekkin + sekkin_hani){
+ distance = sekkin + sekkin_hani;
+ }
+ range = map(distance,sekkin,sekkin + sekkin_hani,150.0,660.0);
+
+
+ if(b_head >= 0.0 && b_head < 90.0){
+ ele = 1024 - (range * cos(map(b_head,0.0,90.0,0.0,(PI/2.0))));
+ ail = 1024 - (range * sin(map(b_head,0.0,90.0,0.0,(PI/2.0))));
+ }else if(b_head >= 90.0 && b_head < 180.0){
+ ele = 1024 + (range * sin(map(b_head,90.0,180.0,0.0,(PI/2.0))));
+ ail = 1024 - (range * cos(map(b_head,90.0,180.0,0.0,(PI/2.0))));
+ }else if(b_head >= 180.0 && b_head < 270.0){
+ ele = 1024 + (range * cos(map(b_head,180.0,270.0,0.0,(PI/2.0))));
+ ail = 1024 + (range * sin(map(b_head,180.0,270.0,0.0,(PI/2.0))));
+ }else{
+ ele = 1024 - (range * sin(map(b_head,270.0,360.0,0.0,(PI/2.0))));
+ ail = 1024 + (range * cos(map(b_head,270.0,360.0,0.0,(PI/2.0))));
+ }
+
+ /*if(b_head >= 0.0 && b_head < 90.0){
+ ele_dis = (distance * cos(map(b_head,0.0,90.0,0.0,(PI/2.0))));
+ ail_dis = (distance * sin(map(b_head,0.0,90.0,0.0,(PI/2.0))));
+ ele_range = -1;
+ ail_range = -1;
+ }else if(b_head >= 90.0 && b_head < 180.0){
+ ele_dis = (distance * sin(map(b_head,90.0,180.0,0.0,(PI/2.0))));
+ ail_dis = (distance * cos(map(b_head,90.0,180.0,0.0,(PI/2.0))));
+ ele_range = 1;
+ ail_range = -1;
+ }else if(b_head >= 180.0 && b_head < 270.0){
+ ele_dis = (distance * cos(map(b_head,180.0,270.0,0.0,(PI/2.0))));
+ ail_dis = (distance * sin(map(b_head,180.0,270.0,0.0,(PI/2.0))));
+ ele_range = 1;
+ ail_range = 1;
+ }else{
+ ele_dis = (distance * sin(map(b_head,270.0,360.0,0.0,(PI/2.0))));
+ ail_dis = (distance * cos(map(b_head,270.0,360.0,0.0,(PI/2.0))));
+ ele_range = -1;
+ ail_range = 1;
+ }
+
+ if(ele_dis > sekkin + sekkin_hani){
+ ele_dis = sekkin + sekkin_hani;
+ }
+ ele = 1024 + ele_range * map(ele_dis,sekkin,sekkin + sekkin_hani,150.0,660.0);
+
+ if(ail_dis > sekkin + sekkin_hani){
+ ail_dis = sekkin + sekkin_hani;
+ }
+ ail = 1024 + ail_range * map(ail_dis,sekkin,sekkin + sekkin_hani,150.0,660.0);*/
+
+ if(ele < 364){
+ ele = 364;
+ }else if(ele > 1684){
+ ele = 1684;
+ }else;
+
+ if(ail < 364){
+ ail = 364;
+ }else if(ail > 1684){
+ ail = 1684;
+ }else;
+
+
+ //request = true;
+ /*while(1){
+ if(through_before != through){
+ through_before = through;
+ break;
+ }
+ }*/
+
+ /*while(1){
+ if(sbus.readable() > 0){
+ break;
+ }
+ }*/
+ /*while(sbus.readable() > 0){
+ if(sbus.getc() == '~'){
+ for(i = 0; i < 4; i++){
+ for(k = 0; k < 6; k++){
+ sbus_from[i][k] = sbus.getc();
+ if(sbus_from[i][k] == ','){
+ sbus_from[i][k] = '\0';
+ channels_from[i] = atoi(sbus_from[i]);
+ break;
+ }
+ }
+ }
+
+ channels_to[0] = channels_from[0];
+ channels_to[1] = channels_from[1];
+ channels_to[2] = channels_from[2];
+ channels_to[3] = channels_from[3];
+ //channels_to[4] = channels_from[4];
+ //channels_to[5] = channels_from[5];
+
+ pc.printf("\n");
+ pc.printf("%6d%6d%6d%6d",
+ channels_from[0],
+ channels_from[1],
+ channels_from[2],
+ channels_from[3]
+ );
+ }
+ //not_use = sbus.getc();
+ sbus.getc();
+ }*/
+ //request = false;
+
+ sprintf(s_to,"~%04d,%04d,%04d,%04d,%04d,%04d,",
+ ail,
+ ele,
+ channels_to[2],
+ rud,
+ channels_to[4],
+ channels_to[5]
+ );
+ sbus.printf(s_to);
+ //pc.printf("\n");
+ //pc.printf(s_to);
+
+ char sta[60];
+ /*if(ail > 1024 && ele > 1024){
+ sprintf(sta,"ail = Right, ele = Forward\n");
+ }else if(ail > 1024 && ele < 1024){
+ sprintf(sta,"ail = Right, ele = Back\n");
+ }else if(ail > 1024 && ele == 1024){
+ sprintf(sta,"ail = Right, ele = Center\n");
+ }else if(ail < 1024 && ele > 1024){
+ sprintf(sta,"ail = Left, ele = Forward\n");
+ }else if(ail < 1024 && ele < 1024){
+ sprintf(sta,"ail = Left, ele = Back\n");
+ }else if(ail < 1024 && ele == 1024){
+ sprintf(sta,"ail = Left, ele = Center\n");
+ }else if(ail == 1024 && ele > 1024){
+ sprintf(sta,"ail = Center, ele = Forward\n");
+ }else if(ail == 1024 && ele < 1024){
+ sprintf(sta,"ail = Center, ele = Back\n");
+ }else{
+ sprintf(sta,"ail = Center, ele = Center\n");
+ }*/
+ sprintf(sta,"Time=%9.2f,Dis=%5.2f,Dir=%6.2f,ail=%4d,ele=%4d\r",Time,distance,direction,ail,ele);
+ //pc.printf(sta);
+ xbee_uart(dev3,sta);
+ //xbee_send_frame(dev3, sta, sizeof(sta));
+
+ }
+
+ }
+}
\ No newline at end of file