航空研究会
aaa
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Revision 1:f31e17341659, committed 2018-08-28
- Comitter:
- TUATBM
- Date:
- Tue Aug 28 07:12:16 2018 +0000
- Parent:
- 0:92024886c0be
- Commit message:
- aaa
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HCSR04.lib Tue Aug 28 07:12:16 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/TUATBM/code/HCSR04_2/#f077c49e84f6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Tue Aug 28 07:12:16 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/1519026547/code/MPU6050_2/#e1e51291024e
--- a/MPU9250/MPU9250.cpp Tue Aug 01 12:27:13 2017 +0000
+++ b/MPU9250/MPU9250.cpp Tue Aug 28 07:12:16 2018 +0000
@@ -3,7 +3,7 @@
#include "MPU9250.h"
-MPU9250::MPU9250(PinName sda, PinName scl, Serial* serial_p)
+MPU9250::MPU9250(PinName sda, PinName scl, RawSerial* serial_p)
:
i2c_p(new I2C(sda,scl)),
i2c(*i2c_p),
@@ -90,6 +90,21 @@
degree[2] = yaw;
}
+
+float MPU9250::calculateYawByMg(){
+ transformCoordinateFromCompassToMPU();
+ lpmag[0] = LPGAIN_MAG *lpmag[0] + (1 - LPGAIN_MAG)*mx;
+ lpmag[1] = LPGAIN_MAG *lpmag[1] + (1 - LPGAIN_MAG)*my;
+ lpmag[2] = LPGAIN_MAG *lpmag[2] + (1 - LPGAIN_MAG)*mz;
+
+ float radroll = PI/180.0f * roll;
+ float radpitch = PI/180.0f * pitch;
+
+ return 180.0f/PI * atan2(lpmag[2]*sin(radpitch) - lpmag[1]*cos(radpitch),
+ lpmag[0]*cos(radroll) - lpmag[1]*sin(radroll)*sin(radpitch) + lpmag[2]*sin(radroll)*cos(radpitch));
+}
+
+
// Accelerometer and gyroscope self test; check calibration wrt factory settings
void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
{
@@ -295,6 +310,8 @@
magbias[i] = 0;
eInt[i] = 0.0f;
+
+ lpmag[i] = 0.0f;
}
q[0] = 1.0f;
@@ -874,4 +891,11 @@
yaw *= 180.0f / PI;
yaw -= DECLINATION;
roll *= 180.0f / PI;
+}
+
+void MPU9250::transformCoordinateFromCompassToMPU(){
+ float buf = mx;
+ mx = my;
+ my = buf;
+ mz = -mz;
}
\ No newline at end of file
--- a/MPU9250/MPU9250.h Tue Aug 01 12:27:13 2017 +0000
+++ b/MPU9250/MPU9250.h Tue Aug 28 07:12:16 2018 +0000
@@ -27,6 +27,8 @@
#define DECLINATION -7.45f //Declination at Tokyo is -7.45 degree 2017/06/14
const float PI = 3.14159265358979323846f;
+const float LPGAIN_MAG = 0.0;
+
enum Ascale {
AFS_2G = 0,
@@ -53,13 +55,13 @@
public:
//MPU9250();
- MPU9250(PinName sda, PinName scl, Serial* serial_p);
+ MPU9250(PinName sda, PinName scl, RawSerial* serial_p);
~MPU9250();
bool Initialize(void);
bool sensingAcGyMg(void);
void calculatePostureAngle(float degree[3]);
-
+ float calculateYawByMg(void);
void MPU9250SelfTest(float * destination);
@@ -82,7 +84,7 @@
I2C *i2c_p;
I2C &i2c;
- Serial* pc_p;
+ RawSerial* pc_p;
Timer timer;
@@ -114,6 +116,7 @@
float q[4]; // vector to hold quaternion
float eInt[3]; // vector to hold integral error for Mahony method
+ float lpmag[3];
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
char readByte(uint8_t address, uint8_t subAddress);
@@ -148,6 +151,7 @@
void translateQuaternionToDeg(float quaternion[4]);
void calibrateDegree(void);
+ void transformCoordinateFromCompassToMPU();
protected:
};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Rawserial.bld Tue Aug 28 07:12:16 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/fd96258d940d \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Tue Aug 28 07:12:16 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/1519026547/code/SDFileSystem3/#85fbe134acff
--- a/config/Estrela.h Tue Aug 01 12:27:13 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,111 +0,0 @@ -#ifndef ESTRELA_H_ -#define ESTRELA_H_ - -#include "mbed.h" - -//===========OUT channel====================== -/* -Servo servo_AIL1(SERVO8); -Servo servo_ELE(SERVO7); -Servo servo_THR(SERVO6); -Servo servo_RUD(SERVO5); -Servo servo_O(SERVO4); -Servo servo_AIL2(SERVO3); -*/ -//===========IN channel====================== - -//===========machine setting================= - -/*<<<<<<<<<< Tips >>>>>>>>> -サーボ入出力は0~1000(us) これはPWMのパルス幅である -だけど機体によってトリムがあるので、サーボ初期位置は必ず500とは限らない -あたりまえだけど、プロポのほうの設定変更（舵角やミキシング量等）はここには反映されない -反映したけりゃ自分でこのファイルを書き直すこと - -AIL エルロン -ELE エレベーター -THR スロットル -RUD ラダー -SW1 (7ch) -SW2 (8CH) - -*/ -//1ch AIL1 -//2ch ELE -//3ch THR -//4ch RUD -//6cH AIL2 - - -/*自動操縦スイッチのしきい値 ch7のトリムやEPAを変えたらかならずチェックすること*/ -#define SWITCH_CHECK 1400 //下1052 上1972 -/*各舵のニュートラル値*/ -#define RUD_N 1573 // L 1790 N 1332 R 1032 width 822 -#define THR_N 1508 // low 1184 N 1507 hi 1832 width 813 -#define ELE_N 1461 // up 1192 N 1412 dow 1690 width 739 -/*各舵の最大値*/ -#define RUD_MAX 1836 -#define ELE_MAX 1628 -/*各舵の最小値*/ -#define RUD_MIN 1262 -#define ELE_MIN 1304 -#define THR_MIN 1416 -/*各舵のリバース設定 １かー１のみ 直接掛け算するので*/ -#define THROTTLE_REV 1 -#define ELEVATOR_REV 1 //1 -#define RUDDER_REV 1 //1 -/*各舵の比例ゲイン値*/ -#define THROTTLE_GN 0.2 -#define ELEVATOR_GN 1 //0.9 -#define RUDDER_GN 1 //0.6 -/*各舵の微分ゲイン*/ -#define ELE_DGN 0.5 -#define RUD_DGN 0.5 -/*旋回時のラダー中心位置*/ -#define RUDDER_TURN 164 -/*旋回時のロール角度*/ -#define SENKAI_D 10 -/*通常迎角*/ -#define GEIKAKU 31 -/*機体設置状態ピッチ方向の傾き*/ -#define LAND_ANGLE 5 -/*旋回 積分量設定*/ -#define SENKAIDDD 10000000 //旋回時間 傾きの積分値と比較 -/*落下機構の動作位置*/ -#define OTEDAMA1 710 -#define OTEDAMA2 500 -#define OTEDAMA3 250 - -#define CHECKSW1_t 0.1 - -#define MANUAL_t 0.5 -#define SCALE_t 0.05 -#define GHPF 512 -#define ALPF 32 -#define ALPF_ 528 - -#define MANUAL_MS 25 -#define CHECKMODE_MS 0 -#define SENSING_MS 0 -#define AUTO_MS 25 -//#define G_MS 50 -//#define A_MS 50 -#define MOBIUS_T 144 -#define MOBIUS_NN 12 -#define MOBIUS_LO 60 - -//===================================================== - -//===============global=============== -int Auto_RUD=RUD_N; -int Auto_ELE=ELE_N; -int Auto_THR=THR_N; - - -#endif /* ESTRELA_H_ */ - -/* -x軸回り ロール -y軸回り ピッチ -z軸回り ヨー -*/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/config/falfalla.h Tue Aug 28 07:12:16 2018 +0000
@@ -0,0 +1,55 @@
+#ifndef FALFALLA_H
+#define FALFALLA_H
+
+#define SWITCH_CHECK 1400
+#define NUMBER_FALFALLA 2 //何号機かを入れるとそれに応じて#if以下が変わって設定が変わる
+
+static float g_kpELE=2.0;
+static float g_kiELE=0.0;
+static float g_kdELE=0.0;
+static float g_kpRUD=3.0;
+static float g_kiRUD=0.0;
+static float g_kdRUD=0.0;
+
+static float g_rightloopROLL=-17.0;
+static float g_rightloopPITCH=-6.3;
+static float g_leftloopROLL=16.0;
+static float g_leftloopPITCH=-6.0;
+static float g_gostraightROLL=2.0;
+static float g_gostraightPITCH=-3.0;
+static float g_takeoffTHR=1.0;
+static float g_loopTHR=0.58;
+
+static float g_rightloopROLLshort=-20.0;
+static float g_rightloopPITCHshort=-7.0;
+static float g_leftloopROLLshort=22.0;
+static float g_leftloopPITCHshort=-6.0;
+
+static float g_glideloopROLL = -16.0;
+static float g_glideloopPITCH = 0.0;
+
+#if NUMBER_FALFALLA == 1 //1号機
+
+const int16_t Trim_Falfalla[4] = {0,14,-100,-10};
+const float ExpMax_Falfalla[4] = {100,115,100,89};
+const float ExpMin_Falfalla[4] = {100,47,100,110};
+const int8_t Reverce_falfalla[4] = {1,-1,1,1}; //Nutral:1 , Reverce:-1
+
+#elif NUMBER_FALFALLA == 2 //2号機
+
+const int16_t Trim_Falfalla[4] = {0,12,-100,2};
+const float ExpMax_Falfalla[4] = {100,115,100,103};
+const float ExpMin_Falfalla[4] = {100,97,100,97};
+const int16_t Reverce_falfalla[4] = {1,-1,1,-1}; //Nutral:1 , Reverce:-1
+
+
+#endif
+
+
+#endif /* ESTRELA_H_ */
+
+/*
+x軸回り ロール
+y軸回り ピッチ
+z軸回り ヨー
+*/
\ No newline at end of file
--- a/main.cpp Tue Aug 01 12:27:13 2017 +0000
+++ b/main.cpp Tue Aug 28 07:12:16 2018 +0000
@@ -1,85 +1,144 @@
+//mbed
#include "mbed.h"
-//#include "rtos.h"
-
+#include "FATFileSystem.h"
+#include "SDFileSystem.h"
+//C
#include "math.h"
-#include "MPU9250.h"
-#include "BMP280.h"
+//sensor
+#include "MPU6050_DMP6.h"
+//#include "MPU9250.h"
+//#include "BMP280.h"
+#include "hcsr04.h"
+//device
+#include "sbus.h"
+//config
#include "SkipperSv2.h"
-#include "Estrela.h"
-#include "sbus.h"
+#include "falfalla.h"
+//other
#include "pid.h"
-#define DEBUG_SEMIAUTO 1
-#define DEBUG_PRINT_INLOOP 1
+#define DEBUG_SEMIAUTO 0
+#define DEBUG_PRINT_INLOOP
+
+#define KP_ELE 2.0
+#define KI_ELE 0.0
+#define KD_ELE 0.0
+#define KP_RUD 3.0
+#define KI_RUD 0.0
+#define KD_RUD 0.0
+
+#define GAIN_CONTROLVALUE_TO_PWM 3.0
+
+#define RIGHT_ROLL -17.0
+#define RIGHT_PITCH -6.3
+#define LEFT_ROLL 16.0
+#define LEFT_PITCH -6.0
+#define STRAIGHT_ROLL 2.0
+#define STRAIGHT_PITCH -3.0
+#define TAKEOFF_THR 1.0
+#define LOOP_THR 0.58
-#define KP_ELE 2
-#define KI_ELE 0
-#define KD_ELE 0
-#define KP_RUD 2
-#define KI_RUD 0
-#define KD_RUD 0
+#define RIGHT_ROLL_SHORT -20.0
+#define RIGHT_PITCH_SHORT -7.0
+#define LEFT_ROLL_SHORT 22.0
+#define LEFT_PITCH_SHORT -6.0
+
+#define GLIDE_ROLL 16.0
+#define GLIDE_PITCH 0.0
-#define GAIN_CONTROLVALUE_TO_PWM 3
-
+//コンパスキャリブレーション
+//SkipperS2基板
+/*
+#define MAGBIAS_X -35.0
+#define MAGBIAS_Y 535.0
+#define MAGBIAS_Z -50.0
+*/
+//S2v2 1番基板
+#define MAGBIAS_X 395.0
+#define MAGBIAS_Y 505.0
+#define MAGBIAS_Z -725.0
+//S2v2 2番基板
+/*
#define MAGBIAS_X 185.0
#define MAGBIAS_Y 220.0
#define MAGBIAS_Z -350.0
+*/
-const uint16_t changeModeCount = 50;
-const int16_t lengthdivpwm = 320;
+#define ELEMENT 1
+#define LIMIT_STRAIGHT_YAW 5.0
+#define LIMIT_LOOPSTOP_YAW 1.0
+#define THRESHOLD_TURNINGRADIUS_YAW 60.0
+#define ALLOWHEIGHT 15
-const int16_t trim[4] = {0,14,-100,-10};
-const float expMax[4] = {100,115,100,89};
-const float expMin[4] = {100,47,100,110};
+#ifndef PI
+#define PI 3.14159265358979
+#endif
+const int16_t lengthdivpwm = 320;
+const int16_t changeModeCount = 6;
+
+const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]};
+const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]};
+const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]};
+const int16_t reverce[4] = {ExpMin_Falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
SBUS sbus(PA_9, PA_10); //SBUS
-PwmOut servo1(PC_6); // TIM3_CH1
+//PwmOut servo1(PC_6); // TIM3_CH1 //echo
PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
PwmOut servo3(PB_0); // TIM3_CH3
PwmOut servo4(PB_1); // TIM3_CH4
PwmOut servo5(PB_6); // TIM4_CH1
-PwmOut servo6(PB_7); // TIM4_CH2
-PwmOut servo7(PB_8); // TIM4_CH3 //PB_8
-PwmOut servo8(PB_9); // TIM4_CH4
+//PwmOut servo6(PB_7); // TIM4_CH2 //trigger
+//PwmOut servo7(PB_8); // TIM4_CH3 //PB_8
+//PwmOut servo8(PB_9); // TIM4_CH4
-Serial pc(PA_2, PA_3);
+RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
+SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
-DigitalOut led1(PA_0); //緑
-DigitalOut led2(PA_1); //赤
+DigitalOut led1(PA_0); //黄色のコネクタ
+DigitalOut led2(PA_1);
DigitalOut led3(PB_4);
DigitalOut led4(PB_5);
-InterruptIn switch2(PC_14);
-
-MPU9250 mpu9250(PC_9,PA_8,&pc);
+//InterruptIn switch2(PC_14);
+MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
+HCSR04 usensor(PB_7,PC_6); //trig,echo 9,8
-PID pid_ELE(KP_ELE,KI_ELE,KD_ELE);
-PID pid_RUD(KP_RUD,KI_RUD,KD_RUD);
+PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
+PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
-enum Channel{AIL, ELE, THR, RUD, Ch5, Ch6, Ch7, Ch8};
-enum Angle{ROLL, PITCH, YAW};
-enum OperationMode{StartUp, SemiAuto};
+enum Channel{AIL, ELE, THR, RUD, DROP, Ch6, Ch7, Ch8};
+enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
+enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide};
+enum BombingMode{Takeoff, Chicken, Transition, Approach};
enum OutputStatus{Manual, Auto};
static OutputStatus output_status = Manual;
OperationMode operation_mode = StartUp;
-static int16_t autopwm[8] = {1500,1500,1000,1500};
-
-static int16_t trimpwm[4] = {1500,1500,1000,1500};
-int16_t maxpwm[4] = {1820,1820,1820,1820};
-int16_t minpwm[4] = {1180,1180,1180,1180};
+BombingMode bombing_mode = Takeoff;
+static int16_t autopwm[8] = {1500,1500,1180,1500,1446,1500};
+static int16_t trimpwm[6] = {1500,1500,1180,1500,1446,1500};
+int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820};
+int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180};
int16_t oldTHR = 1000;
static float nowAngle[3] = {0,0,0};
-const float trimAngle[3] = {0,0,0};
+const float trimAngle[3] = {0.0, 0.0, 0.0};
const float maxAngle[2] = {90, 90};
const float minAngle[2] = {-90, -90};
-Timer t;
+float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
-//IWDG_HandleTypeDef hiwdg;
+unsigned int g_distance;
+Ticker USsensor;
+static char g_buf[16];
+char g_landingcommand;
+float g_SerialTargetYAW = 0.0;
+
+Timer t;
+Timeout RerurnChickenServo1;
+Timeout RerurnChickenServo2;
/*-----関数のプロトタイプ宣言-----*/
void setup();
@@ -89,9 +148,15 @@
void Init_servo(); //サーボ初期化
void Init_sbus(); //SBUS初期化
void Init_sensors();
-void DisplayClock(); //クロック状態確認
+void DisplayClock(); //クロック状態確認
+//センサの値取得
void SensingMPU();
+void UpdateDist();
+
+//void offsetRollPitch(float FirstROLL, float FirstPITCH);
+//void TransYaw(float FirstYAW);
+float TranslateNewYaw(float beforeYaw, float newzeroYaw);
void UpdateTargetAngle(float targetAngle[3]);
void CalculateControlValue(float targetAngle[3], float controlValue[3]);
void UpdateAutoPWM(float controlValue[3]);
@@ -99,14 +164,56 @@
inline float CalcRatio(float value, float trim, float limit);
bool CheckSW_Up(Channel ch);
int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
+inline int16_t SetTHRinRatio(float ratio);
//sbus割り込み
void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
-void Output_PWM(int16_t pwm[8]); //pwmをサーボへ出力
+void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力
+
+//シリアル割り込み
+void SendSerial(); //1文字きたら送り返す
+void SendArray();
+void getSF_Serial();
+float ConvertByteintoFloat(char high, char low);
+//SD設定
+int GetParameter(FILE *fp, const char *paramName,char parameter[]);
+int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE,
+ float *g_kpRUD, float *g_kiRUD, float *g_kdRUD,
+ float *g_rightloopROLL, float *g_rightloopPITCH,
+ float *g_leftloopROLL, float *g_leftloopPITCH,
+ float *g_gostraightROLL, float *g_gostraightPITCH,
+ float *g_takeoffTHR, float *g_loopTHR,
+ float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
+ float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
+ float *g_glideloopROLL, float *g_glideloopPITCH);
//switch2割り込み
void ResetTrim();
+//自動操縦
+void UpdateTargetAngle_GoStraight(float targetAngle[3]);
+void UpdateTargetAngle_Rightloop(float targetAngle[3]);
+void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Leftloop(float targetAngle[3]);
+void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Moebius(float targetAngle[3]);
+void UpdateTargetAngle_Glide(float targetAngle[3]);
+void UpdateTargetAngle_Takeoff(float targetAngle[3]);
+void UpdateTargetAngle_Approach(float targetAngle[3]);
+void Take_off_and_landing(float targetAngle[3]);
+
+char Rotate(float targetAngle[3], float TargetYAW, char mode);
+
+//投下
+void Chicken_Drop();
+void ReturnChickenServo1();
+void ReturnChickenServo2();
+
+//超音波による高度補正
+void checkHeight(float targetAngle[3]);
+void UpdateTargetAngle_NoseUP(float targetAngle[3]);
+void UpdateTargetAngle_NoseDOWN(float targetAngle[3]);
+
//デバッグ用
void DebugPrint();
@@ -114,59 +221,102 @@
int main()
{
- setup();
+ setup();
+
while(1){
+
loop();
+
+ if(!CheckSW_Up(Ch7)){
+ led3=0;
+ }else{
+ led3=1;
+ }
}
}
void setup(){
+ //buzzer = 0;
led1 = 1;
led2 = 1;
- led3 = 0;
- led4 = 0;
+ led3 = 1;
+ led4 = 1;
- pc.baud(115200);
+ SetOptions(&g_kpELE, &g_kiELE, &g_kdELE,
+ &g_kpRUD, &g_kiRUD, &g_kdRUD,
+ &g_rightloopROLL, &g_rightloopPITCH,
+ &g_leftloopROLL, &g_leftloopPITCH,
+ &g_gostraightROLL, &g_gostraightPITCH,
+ &g_takeoffTHR, &g_loopTHR,
+ &g_rightloopROLLshort, &g_rightloopPITCHshort,
+ &g_leftloopROLLshort, &g_leftloopPITCHshort,
+ &g_glideloopROLL, &g_glideloopPITCH);
+
+
Init_PWM();
Init_servo();
Init_sbus();
Init_sensors();
- switch2.rise(ResetTrim);
+ //switch2.rise(ResetTrim);
+ pc.attach(getSF_Serial, Serial::RxIrq);
+ USsensor.attach(&UpdateDist, 0.05);
+
+ NVIC_SetPriority(USART1_IRQn,0);
+ NVIC_SetPriority(EXTI0_IRQn,1);
+ NVIC_SetPriority(TIM5_IRQn,2);
+ NVIC_SetPriority(EXTI9_5_IRQn,3);
+ NVIC_SetPriority(USART2_IRQn,4);
+ DisplayClock();
t.start();
- DisplayClock();
-
+
+
+ pc.printf("MPU calibration start\r\n");
+
+ float offsetstart = t.read();
+ while(t.read() - offsetstart < 26){
+ SensingMPU();
+ for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
+ pc.printf("\r\n");
+ led1 = !led1;
+ led2 = !led2;
+ led3 = !led3;
+ led4 = !led4;
+ }
+
+ FirstROLL = nowAngle[ROLL];
+ FirstPITCH = nowAngle[PITCH];
+ nowAngle[ROLL] -=FirstROLL;
+ nowAngle[PITCH] -=FirstPITCH;
+
led1 = 0;
led2 = 0;
led3 = 0;
led4 = 0;
+ wait(0.2);
+
pc.printf("All initialized\r\n");
-
- mpu9250.sensingAcGyMg();
- mpu9250.calculatePostureAngle(nowAngle);
}
void loop(){
- static float targetAngle[3], controlValue[2];
+ static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
SensingMPU();
UpdateTargetAngle(targetAngle);
+ //Rotate(targetAngle, 30.0);
CalculateControlValue(targetAngle, controlValue);
UpdateAutoPWM(controlValue);
-
-#if DEBUG_PRINT_INLOOP
+ wait_ms(23);
+//#if DEBUG_PRINT_INLOOP
DebugPrint();
-#endif
+//#endif
}
-
-
//サーボ初期化関数
-void Init_servo()
-{
+void Init_servo(){
- servo1.period_ms(14);
- servo1.pulsewidth_us(trimpwm[AIL]);
+ //servo1.period_ms(14);
+ //servo1.pulsewidth_us(trimpwm[AIL]);
servo2.period_ms(14);
servo2.pulsewidth_us(trimpwm[ELE]);
@@ -178,21 +328,12 @@
servo4.pulsewidth_us(trimpwm[RUD]);
servo5.period_ms(14);
- servo5.pulsewidth_us(1500);
-
- servo6.period_ms(14);
- servo6.pulsewidth_us(1500);
-
- servo7.period_ms(14);
- servo7.pulsewidth_us(1500);
-
- servo8.period_ms(14);
- servo8.pulsewidth_us(1500);
+ servo5.pulsewidth_us(1392);
pc.printf("servo initialized\r\n");
}
-//Sbusを初期化する関数
+//Sbus初期化
void Init_sbus(){
sbus.initialize();
sbus.setLastfuncPoint(Update_PWM);
@@ -200,16 +341,20 @@
}
void Init_sensors(){
- if(!mpu9250.Initialize()){
+ if(mpu6050.setup() == -1){
pc.printf("failed initialize\r\n");
- while(1);
+ while(1){
+ led1 = 1; led2 = 0; led3 = 1; led4 = 0;
+ wait(1);
+ led1 = 0; led2 = 1; led3 = 0; led4 = 1;
+ wait(1);
+ }
}
- mpu9250.setMagBias(MAGBIAS_X,MAGBIAS_Y,MAGBIAS_Z);
}
void Init_PWM(){
for (uint8_t i = 0; i < 4; ++i){
- trimpwm[i] = 1500 + trim[i];
+ trimpwm[i] = 1500 + (int16_t)(lengthdivpwm * (trim[i]/100));
maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100));
minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100));
}
@@ -225,12 +370,11 @@
}
void UpdateTargetAngle(float targetAngle[3]){
- static uint16_t count_op = 0, count_out = 0;
-
+ static int16_t count_op = 0, count_out = 0;
#if DEBUG_SEMIAUTO
switch(operation_mode){
case StartUp:
- if(!CheckSW_Up(Ch5) && CheckSW_Up(Ch6)){
+ if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
count_op++;
if(count_op > changeModeCount){
operation_mode = SemiAuto;
@@ -261,16 +405,308 @@
break;
}
- if(CheckSW_Up(Ch5)){
- output_status = Auto;
- led1 = 1;
- }else{
- output_status = Manual;
- led1 = 0;
+#else
+
+ switch(operation_mode){
+ case StartUp:
+ if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ //ch7;自動・手動切り替え ch8;自動操縦モード切替
+ count_op++;
+ if(count_op > changeModeCount){
+ operation_mode = LeftLoop;
+ pc.printf("Goto LeftLoop mode\r\n");
+ count_op = 0;
+ }
+ }else count_op = 0;
+ break;
+
+ case LeftLoop:
+ if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
+ count_op++;
+ if(count_op > changeModeCount){
+ operation_mode = RightLoop;
+ pc.printf("Goto RightLoop mode\r\n");
+ count_op = 0;
+ }
+ }else count_op = 0;
+ UpdateTargetAngle_Leftloop(targetAngle);
+ break;
+
+ case RightLoop:
+ if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
+ count_op++;
+ if(count_op > changeModeCount){
+ operation_mode = Moebius;
+ pc.printf("Goto Moebius mode\r\n");
+ count_op = 0;
+ }
+ }else count_op = 0;
+ UpdateTargetAngle_Rightloop(targetAngle);
+
+ //Rotate確認用
+ /*
+ static char mode = 'G';
+ mode = Rotate(targetAngle,g_SerialTargetYAW,mode);
+ */
+ break;
+
+ case Moebius:
+ if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
+ count_op++;
+ if(count_op > changeModeCount){
+ operation_mode = Glide;
+ pc.printf("Goto Glide mode\r\n");
+ count_op = 0;
+ }
+ }else count_op = 0;
+ UpdateTargetAngle_Moebius(targetAngle);
+ break;
+
+ case Glide:
+ if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
+ count_op++;
+ if(count_op > changeModeCount){
+ operation_mode = BombwithPC;
+ pc.printf("Goto Bombing mode\r\n");
+ count_op = 0;
+ }
+ }else count_op = 0;
+ UpdateTargetAngle_Glide(targetAngle);
+ break;
+
+ case BombwithPC:
+ if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
+ count_op++;
+ if(count_op > changeModeCount){
+ operation_mode = GoStraight;
+ pc.printf("Goto GoStraight mode\r\n");
+ count_op = 0;
+ }
+ }else count_op = 0;
+ Take_off_and_landing(targetAngle);
+ break;
+
+ case GoStraight:
+ if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
+ count_op++;
+ if(count_op > changeModeCount){
+ operation_mode = LeftLoop;
+ pc.printf("Goto Leftloop mode\r\n");
+ count_op = 0;
+ }
+ }else count_op = 0;
+ UpdateTargetAngle_GoStraight(targetAngle);
+ break;
+
+
+ default:
+
+ operation_mode = StartUp;
+ break;
}
#endif
+
+ if(CheckSW_Up(Ch7) && output_status == Manual){
+ count_out++;
+ if(count_out > changeModeCount){
+ output_status = Auto;
+ //led3 = 1;
+ count_out = 0;
+ }
+ }else if(!CheckSW_Up(Ch7) && output_status == Auto){
+ count_out++;
+ if(count_out > changeModeCount){
+ output_status = Manual;
+ count_out = 0;
+ //led3 = 0;
+ }
+ }else count_out = 0;
}
+int GetParameter(FILE *fp, const char *paramName,char parameter[]){
+ int i=0, j=0;
+ int strmax = 200;
+ char str[strmax];
+
+ rewind(fp); //ファイル位置を先頭に
+ while(1){
+ if (fgets(str, strmax, fp) == NULL) {
+ return 0;
+ }
+ if (!strncmp(str, paramName, strlen(paramName))) {
+ while (str[i++] != '=') {}
+ while (str[i] != '\n') {
+ parameter[j++] = str[i++];
+ }
+ parameter[j] = '\0';
+ return 1;
+ }
+ }
+ return 0;
+}
+
+
+//sdによる設定
+int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE,
+ float *g_kpRUD, float *g_kiRUD, float *g_kdRUD,
+ float *g_rightloopROLL, float *g_rightloopPITCH,
+ float *g_leftloopROLL, float *g_leftloopPITCH,
+ float *g_gostraightROLL, float *g_gostraightPITCH,
+ float *g_takeoffTHR, float *g_loopTHR,
+ float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
+ float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
+ float *g_glideloopROLL, float *g_glideloopPITCH
+ ){
+
+ pc.printf("SDsetup start.\r\n");
+
+ FILE *fp;
+ char parameter[30]; //文字列渡す用の配列
+ int SDerrorcount = 0; //取得できなかった数を返す
+ const char *paramNames[] = {
+ "KP_ELEVATOR",
+ "KI_ELEVATOR",
+ "KD_ELEVATOR",
+ "KP_RUDDER",
+ "KI_RUDDER",
+ "KD_RUDDER",
+ "RIGHTLOOP_ROLL",
+ "RIGHTLOOP_PITCH",
+ "LEFTLOOP_ROLL",
+ "LEFTLOOP_PITCH",
+ "GOSTRAIGHT_ROLL",
+ "GOSTRAIGHT_PITCH",
+ "TAKEOFF_THR_RATE",
+ "LOOP_THR_RATE",
+ "RIGHTLOOP_ROLL_SHORT",
+ "RIGHTLOOP_PITCH_SHORT",
+ "LEFTLOOP_ROLL_SHORT",
+ "LEFTLOOP_PITCH_SHORT",
+ "AUTOGLIDE_ROLL",
+ "AUTOGLIDE PITCH"
+ };
+
+ fp = fopen("/sd/option.txt","r");
+
+ if(fp != NULL){ //開けたら
+ pc.printf("File was openned.\r\n");
+ if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter);
+ else{ *g_kpELE = KP_ELE;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter);
+ else{ *g_kiELE = KI_ELE;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter);
+ else{ *g_kdELE = KD_ELE;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter);
+ else{ *g_kpRUD = KP_RUD;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter);
+ else{ *g_kiRUD = KI_RUD;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter);
+ else{ *g_kdRUD = KD_RUD;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter);
+ else{ *g_rightloopROLL = RIGHT_ROLL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter);
+ else{ *g_rightloopPITCH = RIGHT_PITCH;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter);
+ else{ *g_leftloopROLL = LEFT_ROLL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter);
+ else{ *g_leftloopPITCH = LEFT_PITCH;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter);
+ else{ *g_gostraightROLL = STRAIGHT_ROLL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter);
+ else{ *g_gostraightPITCH = STRAIGHT_PITCH;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter);
+ else{ *g_takeoffTHR = TAKEOFF_THR;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter);
+ else{ *g_loopTHR = LOOP_THR;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter);
+ else{ *g_rightloopROLLshort = RIGHT_ROLL_SHORT;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter);
+ else{ *g_rightloopPITCHshort = RIGHT_PITCH_SHORT;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter);
+ else{ *g_leftloopROLLshort = LEFT_ROLL_SHORT;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter);
+ else{ *g_leftloopPITCHshort = LEFT_PITCH_SHORT;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter);
+ else{ *g_glideloopROLL = GLIDE_ROLL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter);
+ else{ *g_glideloopPITCH = GLIDE_PITCH;
+ SDerrorcount++;
+ }
+ fclose(fp);
+
+ }else{ //ファイルがなかったら
+ pc.printf("fp was null.\r\n");
+ *g_kpELE = KP_ELE;
+ *g_kiELE = KI_ELE;
+ *g_kdELE = KD_ELE;
+ *g_kpRUD = KP_RUD;
+ *g_kiRUD = KI_RUD;
+ *g_kdRUD = KD_RUD;
+ *g_rightloopROLL = RIGHT_ROLL;
+ *g_rightloopPITCH = RIGHT_PITCH;
+ *g_leftloopROLL = LEFT_ROLL;
+ *g_leftloopPITCH = LEFT_PITCH;
+ *g_gostraightROLL = STRAIGHT_ROLL;
+ *g_gostraightPITCH = STRAIGHT_PITCH;
+ *g_takeoffTHR = TAKEOFF_THR;
+ *g_loopTHR = LOOP_THR;
+ SDerrorcount = -1;
+ }
+ pc.printf("SDsetup finished.\r\n");
+ if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
+ else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
+ else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
+
+ pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD);
+ pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE);
+ pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH);
+ pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", *g_leftloopROLL, *g_leftloopPITCH);
+ pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH);
+ pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR);
+ pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort);
+ pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort);
+ pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH);
+
+ return SDerrorcount;
+}
+
void CalculateControlValue(float targetAngle[3], float controlValue[3]){
static int t_last;
int t_now;
@@ -286,37 +722,17 @@
void UpdateAutoPWM(float controlValue[3]){
int16_t addpwm[2]; //-500~500
- addpwm[ROLL] = GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正 レバー:右回転正
- addpwm[PITCH] = -1 * GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正 レバー:機首上げ正
-
- autopwm[ELE] = trimpwm[ELE] + addpwm[PITCH];
- autopwm[RUD] = trimpwm[RUD] + addpwm[ROLL];
- autopwm[THR] = oldTHR;
+ addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正 レバー:機首上げ正
+ addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！） レバー:右回転正
+
+ autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH];
+ autopwm[RUD] = trimpwm[RUD] + reverce[RUD] * addpwm[ROLL];
+ //autopwm[THR] = oldTHR;
autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
autopwm[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]);
}
-void ConvertPWMintoRAD(float targetAngle[3]){
- float ratio[2];
-
- if(sbus.manualpwm[ELE] > trimpwm[ELE]){
- ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)maxpwm[ELE]);
- targetAngle[PITCH] = ratio[0]*(maxAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];
- }else{
- ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)minpwm[ELE]);
- targetAngle[PITCH] = ratio[0]*(minAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];
- }
-
- if(sbus.manualpwm[RUD] > trimpwm[RUD]){
- ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)maxpwm[RUD]);
- targetAngle[ROLL] = -1*ratio[1]*(maxAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];
- }else{
- ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)minpwm[RUD]);
- targetAngle[ROLL] = -1*ratio[1]*(minAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];
- }
-}
-
inline float CalcRatio(float value, float trim, float limit){
return (value - trim) / (limit - trim);
}
@@ -335,7 +751,9 @@
return value;
}
-
+inline int16_t SetTHRinRatio(float ratio){
+ return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio);
+}
/*---SBUS割り込み処理---*/
@@ -343,98 +761,567 @@
//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
void Update_PWM()
{
- static int16_t pwm[8];
+ static int16_t pwm[5], sbuspwm[5];
+ static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]};
+ static int16_t tempsbuspwm[5] = {trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]};
+ static int counter_abnormalpwm=0;
if(sbus.flg_ch_update == true){
switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
- case Manual:
- for(uint8_t i=0;i<8;i++){
- pwm[i] = sbus.manualpwm[i];
+ case Manual:
+ for(uint8_t i=1;i<4;i++){
+ if(abs(sbus.manualpwm[i]-tempsbuspwm[i])>300){
+ counter_abnormalpwm++;
+ if(counter_abnormalpwm<=1) sbuspwm[i]=tempsbuspwm[i];
+ else {
+ counter_abnormalpwm = 0;
+ sbuspwm[i] = sbus.manualpwm[i];
+ }
+ }
+ else{
+ counter_abnormalpwm = 0;
+ sbuspwm[i] = sbus.manualpwm[i];
+ }
+ tempsbuspwm[i]=sbus.manualpwm[i];
}
- oldTHR = sbus.manualpwm[THR];
+ sbuspwm[4] = sbus.manualpwm[4];
+
+ for(uint8_t i=1;i<5;i++){
+ pwm[i] = sbuspwm[i];
+ }
+ oldTHR = sbuspwm[THR];
+
//pc.printf("update_manual\r\n");
break;
case Auto:
- pwm[AIL] = sbus.manualpwm[AIL];
pwm[ELE] = autopwm[ELE];
pwm[THR] = autopwm[THR];
pwm[RUD] = autopwm[RUD];
- pwm[Ch5] = sbus.manualpwm[Ch5];
- pwm[Ch6] = sbus.manualpwm[Ch6];
- pwm[Ch7] = sbus.manualpwm[Ch7];
- pwm[Ch8] = sbus.manualpwm[Ch8];
-
+ pwm[DROP] = autopwm[DROP];
//pc.printf("update_auto\r\n");
break;
default:
- for(uint8_t i=0;i<8;i++){
+ for(uint8_t i=1;i<5;i++){
pwm[i] = sbus.manualpwm[i];
} //pc.printf("update_manual\r\n");
break;
}
+ for(uint8_t i=1;i<5;i++) if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
+
+ if(pwm[4]>1600 || pwm[4]<1420) pwm[4]=temppwm[4];
+ temppwm[4] = pwm[4];
+
}else{
pc.printf("0\r\n");
}
- sbus.flg_ch_update = false;
- //for(uint8_t i=0; i<8; i++){pc.printf("%d, ",pwm[i]);}
- //pc.printf("\r\n");
- Output_PWM(pwm);
+ sbus.flg_ch_update = false;
+
+ Output_PWM(pwm);
}
-//pwmをサーボに出力する関数。
-void Output_PWM(int16_t pwm[8])
+//pwmをサーボに出力。
+void Output_PWM(int16_t pwm[5])
{
- servo1.pulsewidth_us(pwm[0]);
+ //servo1.pulsewidth_us(pwm[0]);
servo2.pulsewidth_us(pwm[1]);
servo3.pulsewidth_us(pwm[2]);
servo4.pulsewidth_us(pwm[3]);
servo5.pulsewidth_us(pwm[4]);
- servo6.pulsewidth_us(pwm[5]);
- servo7.pulsewidth_us(pwm[6]);
- servo8.pulsewidth_us(pwm[7]);
}
void ResetTrim(){
- for(uint8_t i=0; i<4; i++){
+ for(uint8_t i=1; i<4; i++){
trimpwm[i] = sbus.manualpwm[i];
}
pc.printf("reset PWM trim\r\n");
}
+
void SensingMPU(){
//static int16_t deltaT = 0, t_start = 0;
//t_start = t.read_us();
+
+ float rpy[3] = {0}, oldrpy[3] = {0};
+ static uint16_t count_changeRPY = 0;
+ static bool flg_checkoutlier = false;
NVIC_DisableIRQ(USART1_IRQn);
- if(!mpu9250.sensingAcGyMg()){
- //pc.printf("failed\r\n");
- return;
- }
+ NVIC_DisableIRQ(USART2_IRQn);
+ NVIC_DisableIRQ(TIM5_IRQn);
+ NVIC_DisableIRQ(EXTI0_IRQn);
+ NVIC_DisableIRQ(EXTI9_5_IRQn);
+
+ mpu6050.getRollPitchYaw_Skipper(rpy);
+
NVIC_EnableIRQ(USART1_IRQn);
- mpu9250.calculatePostureAngle(nowAngle);
- /*
- deltaT = t.read_us() - t_start;
- pc.printf("t:%d us, ",deltaT);
- mpu9250.displayAngle();
- */
+ NVIC_EnableIRQ(USART2_IRQn);
+ NVIC_EnableIRQ(TIM5_IRQn);
+ NVIC_EnableIRQ(EXTI0_IRQn);
+ NVIC_EnableIRQ(EXTI9_5_IRQn);
+
+
+ //外れ値対策
+ for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
+ rpy[ROLL] -= FirstROLL;
+ rpy[PITCH] -= FirstPITCH;
+ rpy[YAW] -= FirstYAW;
+
+ for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
+ if(!flg_checkoutlier || count_changeRPY >= 2){
+ for(uint8_t i=0; i<3; i++){
+ nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
+ }
+ count_changeRPY = 0;
+ }else count_changeRPY++;
+ flg_checkoutlier = false;
+
+}
+
+float TranslateNewYaw(float beforeYaw, float newzeroYaw){
+ float newYaw = beforeYaw - newzeroYaw;
+
+ if(newYaw<-180.0f) newYaw += 360.0f;
+ else if(newYaw>180.0f) newYaw -= 360.0f;
+ return newYaw;
+}
+
+
+void getSF_Serial(){
+
+ static char SFbuf[16];
+ static int bufcounter=0;
+
+ SFbuf[bufcounter]=pc.getc();
+ if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
+
+ if(bufcounter==5 && SFbuf[4]=='F'){
+ g_landingcommand = SFbuf[1];
+ if(g_landingcommand=='Y' || g_landingcommand=='C')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
+ bufcounter = 0;
+ memset(SFbuf, 0, strlen(SFbuf));
+ //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
+ }
+ else if(bufcounter>=5 && SFbuf[4]!='F'){
+ //pc.printf("Communication Falsed.\r\n");
+ bufcounter = 0;
+ }
+ }
+
+float ConvertByteintoFloat(char high, char low){
+
+ //int16_t intvalue = (int16_t)high*256 + (int16_t)low;
+ int16_t intvalue = (int16_t)(((int16_t)high << 8) | low); // Turn the MSB and LSB into a signed 16-bit value
+ float floatvalue = (float)intvalue;
+ return floatvalue;
+}
+
+
+//超音波割り込み
+void UpdateDist(){
+ g_distance = usensor.get_dist_cm();
+ usensor.start();
+}
+
+//8の字旋回
+void UpdateTargetAngle_Moebius(float targetAngle[3]){
+ static uint8_t RotateCounter=0;
+ static uint8_t precounter=0;
+ static bool flg_setInStartAuto = false;
+ static float FirstYAW_Moebius = 0.0;
+ float newYaw_Moebius;
+
+ if(!flg_setInStartAuto && CheckSW_Up(Ch7)){
+ FirstYAW_Moebius = nowAngle[YAW];
+ RotateCounter = 0;
+ flg_setInStartAuto = true;
+ }else if(!CheckSW_Up(Ch7)){
+ flg_setInStartAuto = false;
+ led2 = 0;
+ }
+
+ newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius);
+
+ if(RotateCounter == 0 && newYaw_Moebius >90.0f && newYaw_Moebius < 180.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;}
+ }
+ if(RotateCounter == 1 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;}
+
+ }
+ if(RotateCounter == 2 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;}
+ }
+ if(RotateCounter == 3 && newYaw_Moebius >10.0f && newYaw_Moebius < 90.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;}
+ }
+ if(RotateCounter == 4 && newYaw_Moebius >90.0f && newYaw_Moebius < 180.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;}
+ }
+ if(RotateCounter == 5 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;}
+ }
+ if(RotateCounter == 6 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;}
+ }
+ if(RotateCounter == 7 && newYaw_Moebius >10.0f && newYaw_Moebius < 180.0f){
+ precounter++;
+ if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;}
+ }
+
+
+ if(RotateCounter < 6) UpdateTargetAngle_Rightloop_short(targetAngle);
+ else UpdateTargetAngle_Leftloop_short(targetAngle); //左旋回
+}
+
+//自動滑空
+void UpdateTargetAngle_Glide(float targetAngle[3]){
+ static int THRcount = 0;
+ static int t_start = 0;
+ static bool flg_tstart = false;
+ int t_diff = 0;
+ static int groundcount = 0;
+
+ targetAngle[ROLL] = g_glideloopROLL;
+ targetAngle[PITCH] = g_glideloopPITCH;
+
+ //時間計測開始設定
+ if(!flg_tstart && CheckSW_Up(Ch7)){
+ t_start = t.read();
+ flg_tstart = true;
+ pc.printf("timer start\r\n");
+ }else if(!CheckSW_Up(Ch7)){
+ t_start = 0;
+ flg_tstart = false;
+ }
+
+
+ //フラグが偽であれば計測は行わない
+ if(flg_tstart){
+ t_diff = t.read() - t_start;
+ //一定高度or15秒でled点灯
+ if((groundcount>5 && g_distance>0) || t_diff > 15){
+ led2 = 1;
+ //pc.printf("Call [Stop!] calling!\r\n");
+ }
+ if(g_distance<180 && g_distance > 0) groundcount++;
+ }else{
+ t_diff = 0;
+ groundcount = 0;
+ led2 = 0;
+ }
+
+ if(t_diff > 17){
+ autopwm[THR] = SetTHRinRatio(0.5);
+ }else{
+ if(g_distance<150 && g_distance>0 ){
+ THRcount++;
+ if(THRcount>5){
+ autopwm[THR] = SetTHRinRatio(0.6);
+ //pc.printf("throttle ON\r\n");
+ }
+ }else{
+ autopwm[THR] = 1180;
+ THRcount = 0;
+ }
+ }
+}
+
+//離陸-投下-着陸一連
+void Take_off_and_landing(float targetAngle[3]){
+ if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
- //mpu9250.sensingPostureAngle(nowAngle);
+ switch(bombing_mode){
+ case Takeoff:
+ static bool flg_setFirstYaw = false;
+ static int TakeoffCount = 0;
+
+ if(!flg_setFirstYaw && CheckSW_Up(Ch7)){
+ FirstYAW = nowAngle[YAW] + FirstYAW;
+ flg_setFirstYaw = true;
+ }else if(flg_setFirstYaw && !CheckSW_Up(Ch7)){
+ flg_setFirstYaw = false;
+ }
+
+ UpdateTargetAngle_Takeoff(targetAngle);
+
+ if(g_distance>150) TakeoffCount++;
+ else TakeoffCount = 0;
+ if(TakeoffCount>5){
+ led2=1;
+ autopwm[THR] = SetTHRinRatio(g_loopTHR);
+ pc.printf("Now go to Approach mode!!");
+ bombing_mode = Approach;
+ led2=0;
+ }
+
+ break;
+ /*
+ case Chicken:
+ break;
+ */
+ case Transition:
+ /*
+ static int ApproachCount = 0;
+ targetAngle[YAW]=180.0;
+ int Judge = Rotate(targetAngle, g_SerialTargetYAW);
+
+ if(Judge==0) ApproachCount++;
+ if(ApproachCount>5) bombing_mode = Approach;
+ */
+ break;
+
+ case Approach:
+ UpdateTargetAngle_Approach(targetAngle);
+ break;
+
+ default:
+ bombing_mode = Takeoff;
+ break;
+ }
+}
+
+//離陸モード
+void UpdateTargetAngle_Takeoff(float targetAngle[3]){
+ static int tELE_start = 0;
+ static bool flg_ELEup = false;
+ int t_def = 0;
+ if(!flg_ELEup && CheckSW_Up(Ch7)){
+ tELE_start = t.read_ms();
+ flg_ELEup = true;
+ pc.printf("timer start\r\n");
+ }else if(!CheckSW_Up(Ch7)){
+ tELE_start = 0;
+ flg_ELEup = false;
+ }
+ if(flg_ELEup){
+ t_def = t.read_ms() - tELE_start;
+
+ //1.5秒経過すればELE上げ舵へ
+ if(t_def>500) targetAngle[PITCH]=-25.0;
+ else{
+ t_def = 0;
+ targetAngle[PITCH]=g_gostraightPITCH;
+ }
+ }
+ autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
+ targetAngle[ROLL] = 3.5;
+/*
+ //pc.printf("%d \r\n",g_distance);
+ targetAngle[ROLL] = g_gostraightROLL;
+ if UptargetAngle[PITCH] = -10.0;
+ autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
+*/
+}
+
+//ヨーを目標値にして許容角度になるまで水平旋回
+char Rotate(float targetAngle[3], float TargetYAW, char mode){
+ float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
+
+ switch(mode){
+ case 'G': //直進
+ UpdateTargetAngle_GoStraight(targetAngle);
+ if(diffYaw > LIMIT_STRAIGHT_YAW) mode = 'R';
+ else if(diffYaw < -LIMIT_STRAIGHT_YAW) mode = 'L';
+ break;
+
+ case 'R': //右旋回
+ UpdateTargetAngle_Rightloop_short(targetAngle);
+ if(diffYaw < LIMIT_LOOPSTOP_YAW && diffYaw > -LIMIT_STRAIGHT_YAW) mode = 'G';
+ break;
+
+ case 'L': //左旋回
+ UpdateTargetAngle_Leftloop_short(targetAngle);
+ if(diffYaw > -LIMIT_LOOPSTOP_YAW && diffYaw < LIMIT_STRAIGHT_YAW) mode = 'G';
+ break;
+ }
+
+ return mode;
+
+/*
+ if(diffYaw > LIMIT_STRAIGHT_YAW){
+ UpdateTargetAngle_Rightloop_short(targetAngle);
+ return 1;
+ }else if(diffYaw < -LIMIT_STRAIGHT_YAW){
+ UpdateTargetAngle_Leftloop_short(targetAngle);
+ return 1;
+ }else{
+ UpdateTargetAngle_GoStraight(targetAngle);
+ return 0;
+ }
+*/
+}
+
+//チキラー投下
+void Chicken_Drop(){
+ if(CheckSW_Up(Ch7)){
+ autopwm[DROP] = 1572;
+ pc.printf("Bombed!\r\n");
+ RerurnChickenServo1.attach(&ReturnChickenServo1, 3);
+ //operation_mode = Approach;
+ //buzzer = 0;
+ }
+}
+
+void ReturnChickenServo1(){
+ autopwm[DROP] = 1438;
+ pc.printf("first reverse\r\n");
+ RerurnChickenServo2.attach(&ReturnChickenServo2, 1);
+}
+
+void ReturnChickenServo2(){
+ autopwm[DROP] = 1446;
+ pc.printf("second reverse\r\n");
+}
+
+//着陸モード（PCからの指令に従う）
+void UpdateTargetAngle_Approach(float targetAngle[3]){
+ static char rotatemode = 'G';
+ if(output_status == Manual) rotatemode = 'G';
+
+ //pc.putc(g_landingcommand);
+ switch(g_landingcommand){
+ case 'R': //右旋回セヨ
+ UpdateTargetAngle_Rightloop(targetAngle);
+ break;
+
+ case 'L': //左旋回セヨ
+ UpdateTargetAngle_Leftloop(targetAngle);
+ break;
+
+ case 'G': //直進セヨ
+ UpdateTargetAngle_GoStraight(targetAngle);
+ break;
+
+ case 'Y': //指定ノヨー方向ニ移動セヨ
+ rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode);
+ break;
+
+ case 'B': //ブザーヲ鳴ラセ
+ //buzzer = 1;
+ break;
+
+ case 'D': //物資ヲ落トセ
+ Chicken_Drop();
+ break;
+
+ case 'C': //停止セヨ
+ rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode);
+ autopwm[THR] = minpwm[THR];
+ break;
+ }
+}
+
+void checkHeight(float targetAngle[3]){
+
+ static int targetHeight = 200;
+
+ if(g_distance < targetHeight + ALLOWHEIGHT){
+ UpdateTargetAngle_NoseUP(targetAngle);
+ if(CheckSW_Up(Ch7)) led2 = 1;
+ }
+ else if(g_distance > targetHeight - ALLOWHEIGHT){
+ UpdateTargetAngle_NoseDOWN(targetAngle);
+ if(CheckSW_Up(Ch7)) led2 = 1;
+ }
+ else led2=0;
+}
+
+void UpdateTargetAngle_NoseUP(float targetAngle[3]){
+
+ //targetAngle[PITCH] += 2.0f;
+ //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
+ autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05f);
+ //pc.printf("nose UP");
+}
+
+void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){
+
+ //targetAngle[PITCH] -= 2.0f;
+ autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05f);
+ //pc.printf("nose DOWN");
+}
+
+//直進
+void UpdateTargetAngle_GoStraight(float targetAngle[3]){
+
+ targetAngle[ROLL] = g_gostraightROLL;
+ targetAngle[PITCH] = g_gostraightPITCH;
+ autopwm[THR] = SetTHRinRatio(g_loopTHR);
+
+ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
+//右旋回
+void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
+
+ targetAngle[ROLL] = g_rightloopROLL;
+ targetAngle[PITCH] = g_rightloopPITCH ;
+ autopwm[THR] = SetTHRinRatio(g_loopTHR);
+ //checkHeight(targetAngle);
+ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
+void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回
+
+ targetAngle[ROLL] = g_rightloopROLLshort;
+ targetAngle[PITCH] = g_rightloopPITCHshort;
+ autopwm[THR] = SetTHRinRatio(g_loopTHR);
+
+
+ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
+//左旋回
+void UpdateTargetAngle_Leftloop(float targetAngle[3]){
+
+ targetAngle[ROLL] = g_leftloopROLL;
+ targetAngle[PITCH] = g_leftloopPITCH;
+ autopwm[THR] = SetTHRinRatio(g_loopTHR);
+ //checkHeight(targetAngle);
+
+ //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
+void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){
+
+ targetAngle[ROLL] = g_leftloopROLLshort;
+ targetAngle[PITCH] = g_leftloopPITCHshort;
+ autopwm[THR] = SetTHRinRatio(g_loopTHR);
+
+ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}
//デバッグ用
void DebugPrint(){
-
- //static int16_t deltaT = 0, t_start = 0;
- //t_start = t.read_us();
- for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
+ /*
+ static int16_t deltaT = 0, t_start = 0;
+ deltaT = t.read_u2s() - t_start;
+ pc.printf("t:%d us, ",deltaT);
pc.printf("\r\n");
- //deltaT = t.read_us() - t_start;
- //pc.printf("t:%d us, ",deltaT);
+ t_start = t.read_us();
+ */
+
+ //pc.printf("%d", sbus.manualpwm[4]);
+
+ //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
+ //for(uint8_t i=1; i<5; i++) pc.printf("%d ",sbus.manualpwm[i]);
//pc.printf("\r\n");
+ //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
+ //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ
+ //pc.printf("%d\t",autopwm[ELE]); pc.printf("%d\t",autopwm[RUD]);
+ //pc.printf("%d",g_distance);
- //mpu9250.displayAngle();
- //for(uint8_t i=0; i<3; i++) pc.printf("%f ",nowAngle[i]);
+ //pc.printf("Mode: %c: ",g_buf[0]);
+ //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
+ //pc.printf("%x ",sbus.failsafe_status);
+
//pc.printf("\r\n");
-}
-
+}
\ No newline at end of file
--- a/mbed.bld Tue Aug 01 12:27:13 2017 +0000 +++ b/mbed.bld Tue Aug 28 07:12:16 2018 +0000 @@ -1,1 +1,1 @@ -https://mbed.org/users/mbed_official/code/mbed/builds/0f02307a0877 \ No newline at end of file +https://mbed.org/users/mbed_official/code/mbed/builds/e2bfab296f20 \ No newline at end of file