加入蓝鲨电机的支持，电流环带宽1500

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/app/nv_storage.c

@@ -32,7 +32,7 @@ static void nv_default_motor_params(void) {
 	m_params.r = MOTOR_R;
 	m_params.ld = MOTOR_Ld;
 	m_params.lq = MOTOR_Lq;
-	m_params.offset = 180;//(69.0f);
+	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
 	//m_params.offset = (360-128);
 	m_params.est_pll_band = 100;
 	m_params.pos_lock_pll_band = 100;
@@ -45,13 +45,13 @@ static void nv_default_foc_params(void) {
 	foc_params.s_maxiDC = 30;
 	//foc_params.s_maxIdq = 200;
 	//foc_params.s_minIdq = -200;
-	foc_params.s_PhaseCurrLim = 10;
+	foc_params.s_PhaseCurrLim = 150;
 	foc_params.s_maxRPM = 8000;
 	foc_params.s_maxEpmRPM = 200;
 	foc_params.s_maxEpmPhaseCurrLim = 10;
 	foc_params.s_maxTorque = foc_params.s_PhaseCurrLim;
 	foc_params.s_PhaseCurreBrkLim = 0.0f;
-	foc_params.n_currentBand = 500;
+	foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
 	foc_params.n_modulation = 1.0f;
 	foc_params.n_PhaseFilterCeof = 0.2f;
 	foc_params.n_brkShutPower = 1;

Applications/bsp/board_mc_v1.h

@@ -16,7 +16,7 @@
 #define CONFIG_MAX_PHASE_CURR   180.0F
 #define CONFIG_MAX_PHASE_VOL    (CONFIG_MOS_MAX_VOL - 20.0F)
 #define CONFIG_MAX_TORQUE       50.0F
-#define CONFIG_CURRENT_BANDWITH 1000.0f /* 电流环带宽 */
+#define CONFIG_CURRENT_BANDWITH 1500.0f /* 电流环带宽 */
 #define CONFIG_BEEP 
 #define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
 #define CONFIG_STALL_MAX_TIME    3000   //ms, 超过最大堵转电流持续时间，判断堵转
@@ -29,7 +29,7 @@
 #define SCHED_TIMER_IRQ TIMER5_IRQn
 #define SCHED_TIMER_IRQHandler TIMER5_IRQHandler
 
-#define PWM_DEAD_TIME_NS 300u
+#define PWM_DEAD_TIME_NS 400u
 #define HW_DEAD_TIME_NS  200u
 #define HW_RISE_TIME_NS  500u
 #define HW_NOISE_TIME_NS 300u
@@ -228,8 +228,9 @@
 
 #define ENC_Duty(d, t) ((1.0f/128.0f) * (130.0f * (d)/(t) - 1.0f))
 
-
 #define DEBUG_PORT_UART2
 
+//#define CONFIG_DQ_STEP_RESPONSE
+
 #endif /*_BOARD_MC_V1_H__ */
 

Applications/foc/commands.c

@@ -11,6 +11,10 @@
 #include "prot/can_foc_msg.h"
 #include "app/nv_storage.h"
 
+#ifdef CONFIG_DQ_STEP_RESPONSE
+extern float target_d;
+extern float target_q;
+#endif
 static u32 foc_command_task(void *args);
 static void process_foc_command(foc_cmd_body_t *command);
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -243,6 +243,10 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_hallAngle;
 	}
+#ifdef CONFIG_DQ_STEP_RESPONSE
+	gFoc_Ctrl.in.s_hallAngle = 0;
+	gFoc_Ctrl.in.s_motAngle = 0;
+#endif
 	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
 	
 	gFoc_Ctrl.in.s_motRPM = motor_encoder_get_speed();
@@ -296,12 +300,21 @@ static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 }
 
 
+#ifdef CONFIG_DQ_STEP_RESPONSE
+float target_d = 0.0f;
+float target_q = 0.0f;
+#endif
+
 static u32 PMSM_FOC_Debug_Task(void *p) {
 	if (gFoc_Ctrl.in.b_motEnable) {
+#ifdef CONFIG_DQ_STEP_RESPONSE
+		plot_2data16(FtoS16x10(target_q), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
+#else
 		//plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
-		plot_3data16(gFoc_Ctrl.in.s_targetTorque, gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
+		//plot_3data16(gFoc_Ctrl.in.s_targetTorque, gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
 		//plot_3data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q), FtoS16x10(gFoc_Ctrl.idq_ctl[1].s_FinalTgt));
 		//plot_3data16( gFoc_Ctrl.in.s_motRPM, speed_td.target, speed_td.diff);
+#endif
 	}
 	return 1;
 }
@@ -316,12 +329,14 @@ void PMSM_FOC_Schedule(void) {
 	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
 		PMSM_FOC_Update_PI_Idq();
-	
+	#ifndef CONFIG_DQ_STEP_RESPONSE
 		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
+	#endif
 		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
 		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(gFoc_Ctrl.pi_id, err);
-
+	#ifndef CONFIG_DQ_STEP_RESPONSE
 		float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
+	#endif
 		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
 		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(gFoc_Ctrl.pi_iq, err);
 		gFoc_Ctrl.out.test_targetIQ = target_q;
@@ -343,7 +358,7 @@ void PMSM_FOC_Schedule(void) {
 	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
 
 	if (gFoc_Ctrl.ctrl_count % 5 == 0) {
-		//plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
+		plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
 		//plot_3data16((s16)gFoc_Ctrl.out.s_RealIdq.d, (s16)gFoc_Ctrl.out.s_RealIdq.q, (s16)gFoc_Ctrl.idq_ctl[1].s_Cp);
 	}
 }

Applications/foc/motor/encoder.c

@@ -6,7 +6,7 @@
 #include "libs/logger.h"
 #include "app/nv_storage.h"
 #include "math/fast_math.h"
-#include "encoder_off2.h"
+#include "encoder_off3.h"
 
 static void encoder_do_offset_calibrate(void) ;
 static void _detect_off_finished(void);

Applications/foc/motor/encoder_off3.h

@@ -0,0 +1,113 @@
+#include "math/fix_math.h" 
+
+/* Auto gen by my tools, and use matlab to desing a FIR filter-(MC100/Simulink/Fir_angle.m) */
+/* Matlib Command: Fir=Fir_angle; angle_off=filter(Fir.Numerator, 1, angle_in); */
+/* angle_in is the data, by plot MC_tools */
+/* Use angle_out=single(angle_off) convert double to float */
+
+#define FIR_PHASE_SHIFT 226
+const static s16q10_t _encoder_off_map[] = {
+-2418,-2425,-2433,-2440,-2448,-2456,-2464,-2472,-2480,-2488,-2497,-2506,-2514,-2523,-2532,-2541,
+-2550,-2560,-2569,-2578,-2588,-2598,-2607,-2617,-2627,-2637,-2648,-2658,-2668,-2679,-2689,-2700,-2711,-2722,-2733,-2744,
+-2755,-2766,-2777,-2788,-2800,-2811,-2823,-2835,-2846,-2858,-2870,-2882,-2894,-2906,-2918,-2930,-2943,-2955,-2967,-2980,
+-2992,-3004,-3017,-3030,-3042,-3055,-3067,-3080,-3092,-3105,-3118,-3131,-3143,-3156,-3169,-3182,-3194,-3207,-3220,-3233,
+-3245,-3258,-3271,-3284,-3297,-3309,-3322,-3335,-3347,-3360,-3372,-3385,-3398,-3410,-3423,-3435,-3448,-3460,-3473,-3485,
+-3497,-3510,-3522,-3534,-3546,-3558,-3570,-3582,-3594,-3605,-3617,-3629,-3640,-3652,-3663,-3674,-3685,-3696,-3707,-3719,
+-3729,-3740,-3751,-3761,-3772,-3782,-3792,-3802,-3812,-3822,-3832,-3842,-3851,-3861,-3870,-3879,-3889,-3898,-3906,-3915,
+-3924,-3932,-3941,-3949,-3957,-3965,-3973,-3981,-3989,-3996,-4003,-4010,-4017,-4024,-4031,-4037,-4044,-4050,-4056,-4062,
+-4067,-4073,-4078,-4084,-4089,-4094,-4099,-4103,-4108,-4112,-4116,-4120,-4124,-4127,-4131,-4134,-4137,-4140,-4143,-4145,
+-4148,-4150,-4152,-4154,-4156,-4157,-4159,-4160,-4161,-4162,-4163,-4163,-4164,-4164,-4164,-4164,-4163,-4163,-4162,-4161,
+-4160,-4159,-4157,-4156,-4154,-4152,-4150,-4147,-4145,-4142,-4140,-4136,-4133,-4130,-4126,-4123,-4119,-4115,-4111,-4106,
+-4102,-4097,-4092,-4087,-4082,-4077,-4072,-4066,-4060,-4054,-4048,-4042,-4036,-4029,-4023,-4016,-4009,-4002,-3994,-3987,
+-3980,-3972,-3964,-3956,-3948,-3940,-3931,-3923,-3914,-3906,-3897,-3888,-3879,-3870,-3860,-3851,-3842,-3832,-3822,-3812,
+-3802,-3792,-3782,-3772,-3762,-3751,-3741,-3730,-3720,-3709,-3698,-3687,-3677,-3666,-3655,-3643,-3632,-3621,-3610,-3598,
+-3587,-3575,-3564,-3552,-3541,-3529,-3517,-3505,-3494,-3482,-3470,-3458,-3446,-3435,-3423,-3411,-3399,-3387,-3375,-3363,
+-3351,-3339,-3327,-3315,-3303,-3291,-3279,-3267,-3255,-3243,-3232,-3220,-3208,-3197,-3185,-3173,-3162,-3150,-3138,-3127,
+-3116,-3104,-3093,-3081,-3070,-3059,-3048,-3037,-3026,-3015,-3004,-2993,-2982,-2972,-2961,-2951,-2940,-2930,-2920,-2910,
+-2899,-2889,-2879,-2870,-2860,-2850,-2841,-2831,-2822,-2813,-2804,-2795,-2786,-2778,-2769,-2761,-2752,-2744,-2736,-2728,
+-2720,-2712,-2705,-2697,-2689,-2682,-2675,-2668,-2661,-2654,-2647,-2641,-2635,-2628,-2622,-2616,-2610,-2604,-2599,-2593,
+-2588,-2583,-2578,-2573,-2568,-2563,-2559,-2554,-2550,-2546,-2542,-2538,-2534,-2531,-2527,-2524,-2521,-2518,-2515,-2513,
+-2510,-2508,-2505,-2503,-2501,-2499,-2497,-2496,-2494,-2493,-2492,-2490,-2489,-2488,-2488,-2487,-2487,-2486,-2486,-2486,
+-2486,-2486,-2486,-2487,-2487,-2488,-2488,-2489,-2490,-2491,-2492,-2494,-2495,-2497,-2498,-2500,-2502,-2504,-2506,-2508,
+-2511,-2513,-2516,-2518,-2521,-2524,-2527,-2530,-2533,-2536,-2539,-2542,-2545,-2549,-2552,-2556,-2560,-2564,-2567,-2571,
+-2575,-2579,-2584,-2588,-2592,-2596,-2601,-2605,-2610,-2614,-2619,-2624,-2628,-2633,-2638,-2643,-2648,-2653,-2658,-2664,
+-2669,-2674,-2680,-2685,-2690,-2696,-2701,-2707,-2712,-2718,-2723,-2729,-2735,-2740,-2746,-2752,-2758,-2763,-2769,-2775,
+-2781,-2787,-2792,-2798,-2804,-2810,-2816,-2822,-2828,-2834,-2840,-2846,-2852,-2858,-2864,-2870,-2876,-2882,-2888,-2894,
+-2900,-2906,-2912,-2918,-2924,-2930,-2936,-2943,-2949,-2955,-2961,-2967,-2973,-2979,-2985,-2991,-2996,-3002,-3008,-3014,
+-3020,-3026,-3032,-3037,-3043,-3049,-3055,-3060,-3066,-3072,-3078,-3083,-3089,-3094,-3100,-3105,-3111,-3116,-3122,-3127,
+-3132,-3138,-3143,-3148,-3154,-3159,-3164,-3170,-3175,-3180,-3185,-3190,-3195,-3201,-3205,-3210,-3215,-3220,-3225,-3230,
+-3234,-3239,-3244,-3248,-3253,-3257,-3262,-3266,-3271,-3275,-3279,-3284,-3288,-3292,-3296,-3301,-3305,-3309,-3312,-3316,
+-3320,-3324,-3328,-3332,-3335,-3339,-3343,-3346,-3350,-3353,-3357,-3360,-3364,-3367,-3370,-3373,-3377,-3380,-3383,-3386,
+-3389,-3391,-3394,-3397,-3400,-3402,-3405,-3407,-3410,-3412,-3415,-3417,-3420,-3422,-3424,-3426,-3428,-3430,-3432,-3434,
+-3436,-3438,-3439,-3441,-3443,-3444,-3446,-3447,-3449,-3450,-3451,-3453,-3454,-3455,-3456,-3457,-3459,-3459,-3460,-3461,
+-3462,-3463,-3463,-3464,-3465,-3465,-3465,-3466,-3466,-3466,-3466,-3467,-3467,-3467,-3467,-3467,-3467,-3466,-3466,-3466,
+-3466,-3465,-3465,-3464,-3464,-3463,-3462,-3461,-3461,-3460,-3459,-3458,-3457,-3456,-3455,-3454,-3453,-3452,-3450,-3449,
+-3448,-3446,-3445,-3443,-3442,-3440,-3439,-3437,-3435,-3433,-3431,-3429,-3427,-3425,-3423,-3421,-3419,-3417,-3414,-3412,
+-3410,-3408,-3405,-3403,-3400,-3397,-3395,-3392,-3389,-3387,-3384,-3381,-3378,-3375,-3372,-3369,-3366,-3363,-3360,-3357,
+-3354,-3351,-3348,-3344,-3341,-3338,-3335,-3331,-3328,-3324,-3321,-3317,-3313,-3310,-3306,-3302,-3298,-3295,-3291,-3287,
+-3283,-3279,-3275,-3271,-3267,-3263,-3259,-3255,-3251,-3246,-3242,-3238,-3234,-3229,-3225,-3220,-3216,-3212,-3207,-3203,
+-3198,-3194,-3189,-3185,-3180,-3175,-3171,-3166,-3162,-3157,-3152,-3147,-3142,-3137,-3132,-3127,-3122,-3117,-3112,-3107,
+-3102,-3097,-3092,-3087,-3082,-3076,-3071,-3066,-3061,-3055,-3050,-3044,-3039,-3033,-3028,-3022,-3017,-3011,-3006,-3000,
+-2995,-2989,-2984,-2978,-2972,-2967,-2961,-2955,-2950,-2944,-2938,-2932,-2926,-2920,-2915,-2909,-2903,-2897,-2891,-2884,
+-2879,-2872,-2866,-2860,-2854,-2848,-2842,-2836,-2829,-2823,-2817,-2811,-2805,-2798,-2792,-2786,-2779,-2773,-2766,-2760,
+-2754,-2747,-2741,-2735,-2729,-2722,-2716,-2710,-2703,-2697,-2691,-2684,-2678,-2671,-2665,-2659,-2652,-2646,-2639,-2633,
+-2627,-2620,-2614,-2607,-2601,-2595,-2588,-2582,-2576,-2569,-2563,-2557,-2551,-2544,-2538,-2532,-2526,-2519,-2513,-2507,
+-2501,-2495,-2489,-2483,-2477,-2471,-2466,-2460,-2454,-2448,-2443,-2437,-2431,-2426,-2421,-2415,-2410,-2404,-2399,-2394,
+-2388,-2383,-2378,-2373,-2368,-2363,-2358,-2353,-2349,-2344,-2339,-2335,-2330,-2326,-2322,-2317,-2313,-2309,-2305,-2301,
+-2297,-2294,-2290,-2286,-2283,-2280,-2276,-2273,-2270,-2267,-2264,-2262,-2259,-2256,-2254,-2252,-2249,-2247,-2245,-2243,
+-2241,-2240,-2238,-2236,-2235,-2234,-2232,-2231,-2231,-2230,-2229,-2228,-2228,-2228,-2227,-2228,-2228,-2228,-2228,-2228,
+-2229,-2230,-2231,-2231,-2233,-2234,-2235,-2237,-2238,-2240,-2242,-2244,-2246,-2249,-2251,-2254,-2257,-2260,-2263,-2266,
+-2269,-2273,-2276,-2280,-2284,-2288,-2292,-2296,-2301,-2305,-2310,-2315,-2320,-2325,-2330,-2336,-2341,-2347,-2353,-2359,
+-2365,-2371,-2377,-2384,-2390,-2397,-2404,-2411,-2418,-2425,-2433,-2440,-2448,-2456,-2464,-2472,-2480,-2488,-2497,-2506,
+-2514,-2523,-2532,-2541,-2550,-2560,-2569,-2578,-2588,-2598,-2607,-2617,-2627,-2637,-2648,-2658,-2668,-2679,-2689,-2700,
+-2711,-2722,-2733,-2744,-2755,-2766,-2777,-2788,-2800,-2811,-2823,-2835,-2846,-2858,-2870,-2882,-2894,-2906,-2918,-2930,
+-2943,-2955,-2967,-2980,-2992,-3004,-3017,-3030,-3042,-3055,-3067,-3080,-3092,-3105,-3118,-3131,-3143,-3156,-3169,-3182,
+-3194,-3207,-3220,-3233,-3245,-3258,-3271,-3284,-3297,-3309,-3322,-3335,-3347,-3360,-3372,-3385,-3398,-3410,-3423,-3435,
+-3448,-3460,-3473,-3485,-3497,-3510,-3522,-3534,-3546,-3558,-3570,-3582,-3594,-3605,-3617,-3629,-3640,-3652,-3663,-3674,
+-3685,-3696,-3707,-3719,-3729,-3740,-3751,-3761,-3772,-3782,-3792,-3802,-3812,-3822,-3832,-3842,-3851,-3861,-3870,-3879,
+-3889,-3898,-3906,-3915,-3924,-3932,-3941,-3949,-3957,-3965,-3973,-3981,-3989,-3996,-4003,-4010,-4017,-4024,-4031,-4037,
+-4044,-4050,-4056,-4062,-4067,-4073,-4078,-4084,-4089,-4094,-4099,-4103,-4108,-4112,-4116,-4120,-4124,-4127,-4131,-4134,
+-4137,-4140,-4143,-4145,-4148,-4150,-4152,-4154,-4156,-4157,-4159,-4160,-4161,-4162,-4163,-4163,-4164,-4164,-4164,-4164,
+-4163,-4163,-4162,-4161,-4160,-4159,-4157,-4156,-4154,-4152,-4150,-4147,-4145,-4142,-4140,-4136,-4133,-4130,-4126,-4123,
+-4119,-4115,-4111,-4106,-4102,-4097,-4092,-4087,-4082,-4077,-4072,-4066,-4060,-4054,-4048,-4042,-4036,-4029,-4023,-4016,
+-4009,-4002,-3994,-3987,-3980,-3972,-3964,-3956,-3948,-3940,-3931,-3923,-3914,-3906,-3897,-3888,-3879,-3870,-3860,-3851,
+-3842,-3832,-3822,-3812,-3802,-3792,-3782,-3772,-3762,-3751,-3741,-3730,-3720,-3709,-3698,-3687,-3677,-3666,-3655,-3643,
+-3632,-3621,-3610,-3598,-3587,-3575,-3564,-3552,-3541,-3529,-3517,-3505,-3494,-3482,-3470,-3458,-3446,-3435,-3423,-3411,
+-3399,-3387,-3375,-3363,-3351,-3339,-3327,-3315,-3303,-3291,-3279,-3267,-3255,-3243,-3232,-3220,-3208,-3197,-3185,-3173,
+-3162,-3150,-3138,-3127,-3116,-3104,-3093,-3081,-3070,-3059,-3048,-3037,-3026,-3015,-3004,-2993,-2982,-2972,-2961,-2951,
+-2940,-2930,-2920,-2910,-2899,-2889,-2879,-2870,-2860,-2850,-2841,-2831,-2822,-2813,-2804,-2795,-2786,-2778,-2769,-2761,
+-2752,-2744,-2736,-2728,-2720,-2712,-2705,-2697,-2689,-2682,-2675,-2668,-2661,-2654,-2647,-2641,-2635,-2628,-2622,-2616,
+-2610,-2604,-2599,-2593,-2588,-2583,-2578,-2573,-2568,-2563,-2559,-2554,-2550,-2546,-2542,-2538,-2534,-2531,-2527,-2524,
+-2521,-2518,-2515,-2513,-2510,-2508,-2505,-2503,-2501,-2499,-2497,-2496,-2494,-2493,-2492,-2490,-2489,-2488,-2488,-2487,
+-2487,-2486,-2486,-2486,-2486,-2486,-2486,-2487,-2487,-2488,-2488,-2489,-2490,-2491,-2492,-2494,-2495,-2497,-2498,-2500,
+-2502,-2504,-2506,-2508,-2511,-2513,-2516,-2518,-2521,-2524,-2527,-2530,-2533,-2536,-2539,-2542,-2545,-2549,-2552,-2556,
+-2560,-2564,-2567,-2571,-2575,-2579,-2584,-2588,-2592,-2596,-2601,-2605,-2610,-2614,-2619,-2624,-2628,-2633,-2638,-2643,
+-2648,-2653,-2658,-2664,-2669,-2674,-2680,-2685,-2690,-2696,-2701,-2707,-2712,-2718,-2723,-2729,-2735,-2740,-2746,-2752,
+-2758,-2763,-2769,-2775,-2781,-2787,-2792,-2798,-2804,-2810,-2816,-2822,-2828,-2834,-2840,-2846,-2852,-2858,-2864,-2870,
+-2876,-2882,-2888,-2894,-2900,-2906,-2912,-2918,-2924,-2930,-2936,-2943,-2949,-2955,-2961,-2967,-2973,-2979,-2985,-2991,
+-2996,-3002,-3008,-3014,-3020,-3026,-3032,-3037,-3043,-3049,-3055,-3060,-3066,-3072,-3078,-3083,-3089,-3094,-3100,-3105,
+-3111,-3116,-3122,-3127,-3132,-3138,-3143,-3148,-3154,-3159,-3164,-3170,-3175,-3180,-3185,-3190,-3195,-3201,-3205,-3210,
+-3215,-3220,-3225,-3230,-3234,-3239,-3244,-3248,-3253,-3257,-3262,-3266,-3271,-3275,-3279,-3284,-3288,-3292,-3296,-3301,
+-3305,-3309,-3312,-3316,-3320,-3324,-3328,-3332,-3335,-3339,-3343,-3346,-3350,-3353,-3357,-3360,-3364,-3367,-3370,-3373,
+-3377,-3380,-3383,-3386,-3389,-3391,-3394,-3397,-3400,-3402,-3405,-3407,-3410,-3412,-3415,-3417,-3420,-3422,-3424,-3426,
+-3428,-3430,-3432,-3434,-3436,-3438,-3439,-3441,-3443,-3444,-3446,-3447,-3449,-3450,-3451,-3453,-3454,-3455,-3456,-3457,
+-3459,-3459,-3460,-3461,-3462,-3463,-3463,-3464,-3465,-3465,-3465,-3466,-3466,-3466,-3466,-3467,-3467,-3467,-3467,-3467,
+-3467,-3466,-3466,-3466,-3466,-3465,-3465,-3464,-3464,-3463,-3462,-3461,-3461,-3460,-3459,-3458,-3457,-3456,-3455,-3454,
+-3453,-3452,-3450,-3449,-3448,-3446,-3445,-3443,-3442,-3440,-3439,-3437,-3435,-3433,-3431,-3429,-3427,-3425,-3423,-3421,
+-3419,-3417,-3414,-3412,-3410,-3408,-3405,-3403,-3400,-3397,-3395,-3392,-3389,-3387,-3384,-3381,-3378,-3375,-3372,-3369,
+-3366,-3363,-3360,-3357,-3354,-3351,-3348,-3344,-3341,-3338,-3335,-3331,-3328,-3324,-3321,-3317,-3313,-3310,-3306,-3302,
+-3298,-3295,-3291,-3287,-3283,-3279,-3275,-3271,-3267,-3263,-3259,-3255,-3251,-3246,-3242,-3238,-3234,-3229,-3225,-3220,
+-3216,-3212,-3207,-3203,-3198,-3194,-3189,-3185,-3180,-3175,-3171,-3166,-3162,-3157,-3152,-3147,-3142,-3137,-3132,-3127,
+-3122,-3117,-3112,-3107,-3102,-3097,-3092,-3087,-3082,-3076,-3071,-3066,-3061,-3055,-3050,-3044,-3039,-3033,-3028,-3022,
+-3017,-3011,-3006,-3000,-2995,-2989,-2984,-2978,-2972,-2967,-2961,-2955,-2950,-2944,-2938,-2932,-2926,-2920,-2915,-2909,
+-2903,-2897,-2891,-2884,-2879,-2872,-2866,-2860,-2854,-2848,-2842,-2836,-2829,-2823,-2817,-2811,-2805,-2798,-2792,-2786,
+-2779,-2773,-2766,-2760,-2754,-2747,-2741,-2735,-2729,-2722,-2716,-2710,-2703,-2697,-2691,-2684,-2678,-2671,-2665,-2659,
+-2652,-2646,-2639,-2633,-2627,-2620,-2614,-2607,-2601,-2595,-2588,-2582,-2576,-2569,-2563,-2557,-2551,-2544,-2538,-2532,
+-2526,-2519,-2513,-2507,-2501,-2495,-2489,-2483,-2477,-2471,-2466,-2460,-2454,-2448,-2443,-2437,-2431,-2426,-2421,-2415,
+-2410,-2404,-2399,-2394,-2388,-2383,-2378,-2373,-2368,-2363,-2358,-2353,-2349,-2344,-2339,-2335,-2330,-2326,-2322,-2317,
+-2313,-2309,-2305,-2301,-2297,-2294,-2290,-2286,-2283,-2280,-2276,-2273,-2270,-2267,-2264,-2262,-2259,-2256,-2254,-2252,
+-2249,-2247,-2245,-2243,-2241,-2240,-2238,-2236,-2235,-2234,-2232,-2231,-2231,-2230,-2229,-2228,-2228,-2228,-2227,-2228,
+-2228,-2228,-2228,-2228,-2229,-2230,-2231,-2231,-2233,-2234,-2235,-2237,-2238,-2240,-2242,-2244,-2246,-2249,-2251,-2254,
+-2257,-2260,-2263,-2266,-2269,-2273,-2276,-2280,-2284,-2288,-2292,-2296,-2301,-2305,-2310,-2315,-2320,-2325,-2330,-2336,
+-2341,-2347,-2353,-2359,-2365,-2371,-2377,-2384,-2390,-2397,-2404,-2411,
+};

Applications/foc/motor/motor.c

@@ -102,7 +102,9 @@ bool mc_start(u8 mode) {
 	if (motor.b_start) {
 		return true;
 	}
-
+#ifdef CONFIG_DQ_STEP_RESPONSE
+	mode = CTRL_MODE_CURRENT;
+#endif
 	MC_Check_MosVbusThrottle();
 
 	if (PMSM_FOC_GetCriticalError() != 0) {
@@ -317,11 +319,11 @@ void mc_encoder_off_calibrate(s16 vd) {
 	PMSM_FOC_SetOpenVdq(vd, 0);
 	delay_ms(2000);
 	motor_encoder_set_direction(POSITIVE);
-	for (int i = 0; i < 100000; i++) {
+	for (int i = 0; i < 3000; i++) {
 		for (float angle = 0; angle < 360; angle++) {
 			PMSM_FOC_Set_Angle(angle);
-			delay_ms(2);
-			if (i > 10) {
+			delay_ms(1);
+			if (i > 20) {
 				motor_encoder_offset(angle);
 			}
 		}
@@ -332,10 +334,10 @@ void mc_encoder_off_calibrate(s16 vd) {
 	}
 	motor_encoder_set_direction(NEGATIVE);
 	delay_ms(100);
-	for (int i = 0; i < 100000; i++) {
+	for (int i = 0; i < 3000; i++) {
 		for (float angle = 360; angle > 0; angle--) {
 			PMSM_FOC_Set_Angle(angle);
-			delay_ms(2);
+			delay_ms(1);
 			if (i > 10) {
 				motor_encoder_offset(angle);
 			}
@@ -499,6 +501,7 @@ void ADC_IRQHandler(void) {
 	TIME_MEATURE_END();
 }
 
+#ifndef CONFIG_DQ_STEP_RESPONSE
 static bool mc_can_stop_foc(void) {
 	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
 		if (!PMSM_FOC_MotorLocking() && motor.epm_dir == EPM_Dir_None) {
@@ -507,7 +510,7 @@ static bool mc_can_stop_foc(void) {
 	}
 	return false;
 }
-
+#endif
 static bool mc_run_stall_process(u8 run_mode) {
 	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_MotorLocking()) {
 		//堵转判断
@@ -550,6 +553,7 @@ void Sched_MC_mTask(void) {
 		return;
 	}
 	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
+#ifndef CONFIG_DQ_STEP_RESPONSE
 		if (motor.mode != CTRL_MODE_OPEN) {
 			u32 mask;
 			if (mc_can_stop_foc()) {
@@ -579,5 +583,6 @@ void Sched_MC_mTask(void) {
 			}
 			PMSM_FOC_Slow_Task();
 		}
+#endif
 	}
 }

Applications/foc/motor/motor_param.h

@@ -1,11 +1,12 @@
 #ifndef _MOTOR_PARAM_H__
 #define _MOTOR_PARAM_H__
 
-#if 0
-#define MOTOR_R   0.33f
-#define MOTOR_Ld   0.00136f
-#define MOTOR_Lq   0.00136f
-#define MOTOR_POLES     2
+#if 1
+#define MOTOR_R   0.010f
+#define MOTOR_Ld (0.000120f*0.5f)
+#define MOTOR_Lq (0.000200f*0.5f)
+#define MOTOR_POLES  4
+#define MOTOR_ENC_OFFSET 0.0F
 #elif 0
 //编码器电机 3505
 #define MOTOR_R 0.08f
@@ -17,9 +18,7 @@
 #define MOTOR_Ld (0.000143f*0.5f)
 #define MOTOR_Lq (0.000205f*0.5f)
 #define MOTOR_POLES  5
+#define MOTOR_ENC_OFFSET 180.0F
 #endif
 
-
-
-
 #endif /* _MOTOR_PARAM_H__ */

Simulink/Fir_angle3.m

@@ -0,0 +1,26 @@
+function Hd = Fir_angle3
+%FIR_ANGLE3 返回离散时间滤波器对象。
+
+% MATLAB Code
+% Generated by MATLAB(R) 9.9 and Signal Processing Toolbox 8.5.
+% Generated on: 26-Oct-2022 17:32:33
+
+% Equiripple Lowpass filter designed using the FIRPM function.
+
+% All frequency values are in Hz.
+Fs = 500;  % Sampling Frequency
+
+Fpass = 0.2;             % Passband Frequency
+Fstop = 3;               % Stopband Frequency
+Dpass = 0.057501127785;  % Passband Ripple
+Dstop = 0.0001;          % Stopband Attenuation
+dens  = 20;              % Density Factor
+
+% Calculate the order from the parameters using FIRPMORD.
+[N, Fo, Ao, W] = firpmord([Fpass, Fstop]/(Fs/2), [1 0], [Dpass, Dstop]);
+
+% Calculate the coefficients using the FIRPM function.
+b  = firpm(N, Fo, Ao, W, {dens});
+Hd = dfilt.dffir(b);
+
+% [EOF]

Simulink/angle3.csv

@@ -0,0 +1,3072 @@
+-600 
+-654 
+-623 
+-578 
+-669 
+-682 
+-706 
+-674 
+-725 
+-684 
+-738 
+-703 
+-671 
+-721 
+-681 
+-640 
+-600 
+-568 
+-532 
+-528 
+-528 
+-496 
+-465 
+-451 
+-461 
+-434 
+-466 
+-453 
+-412 
+-439 
+-445 
+-440 
+-463 
+-459 
+-436 
+-478 
+-437 
+-482 
+-456 
+-415 
+-461 
+-434 
+-466 
+-453 
+-412 
+-471 
+-486 
+-490 
+-477 
+-486 
+-518 
+-528 
+-523 
+-505 
+-547 
+-515 
+-475 
+-470 
+-493 
+-453 
+-412 
+-371 
+-381 
+-363 
+-340 
+-309 
+-291 
+-305 
+-287 
+-282 
+-279 
+-297 
+-275 
+-293 
+-293 
+-353 
+-312 
+-339 
+-345 
+-340 
+-345 
+-354 
+-354 
+-342 
+-337 
+-341 
+-356 
+-338 
+-338 
+-379 
+-393 
+-403 
+-403 
+-421 
+-467 
+-440 
+-400 
+-459 
+-418 
+-378 
+-437 
+-396 
+-361 
+-329 
+-288 
+-311 
+-293 
+-253 
+-217 
+-230 
+-231 
+-190 
+-177 
+-209 
+-177 
+-223 
+-192 
+-246 
+-215 
+-265 
+-225 
+-284 
+-243 
+-271 
+-262 
+-262 
+-281 
+-290 
+-281 
+-259 
+-273 
+-278 
+-278 
+-296 
+-297 
+-315 
+-325 
+-339 
+-370 
+-371 
+-362 
+-335 
+-340 
+-335 
+-300 
+-282 
+-259 
+-228 
+-187 
+-196 
+-170 
+-142 
+-125 
+-120 
+-102 
+-85 
+-62 
+-121 
+-81 
+-90 
+-100 
+-114 
+-118 
+-128 
+-137 
+-146 
+-106 
+-151 
+-125 
+-184 
+-143 
+-144 
+-162 
+-180 
+-181 
+-190 
+-186 
+-209 
+-254 
+-228 
+-287 
+-282 
+-297 
+-265 
+-325 
+-293 
+-270 
+-262 
+-262 
+-239 
+-217 
+-199 
+-195 
+-164 
+-154 
+-128 
+-123 
+-119 
+-115 
+-110 
+-152 
+-134 
+-148 
+-153 
+-212 
+-207 
+-231 
+-195 
+-250 
+-227 
+-232 
+-228 
+-287 
+-260 
+-270 
+-265 
+-306 
+-298 
+-298 
+-303 
+-362 
+-376 
+-381 
+-390 
+-400 
+-423 
+-413 
+-378 
+-428 
+-396 
+-356 
+-320 
+-361 
+-334 
+-293 
+-276 
+-271 
+-271 
+-231 
+-208 
+-250 
+-214 
+-263 
+-228 
+-287 
+-287 
+-297 
+-301 
+-370 
+-352 
+-348 
+-353 
+-389 
+-371 
+-363 
+-381 
+-377 
+-386 
+-391 
+-378 
+-437 
+-405 
+-456 
+-470 
+-475 
+-520 
+-493 
+-521 
+-512 
+-476 
+-481 
+-454 
+-450 
+-427 
+-418 
+-378 
+-364 
+-346 
+-333 
+-306 
+-293 
+-284 
+-261 
+-303 
+-280 
+-298 
+-281 
+-249 
+-300 
+-309 
+-318 
+-301 
+-323 
+-328 
+-338 
+-324 
+-325 
+-334 
+-338 
+-339 
+-312 
+-330 
+-331 
+-385 
+-354 
+-409 
+-373 
+-428 
+-392 
+-446 
+-406 
+-465 
+-425 
+-384 
+-370 
+-398 
+-362 
+-321 
+-304 
+-272 
+-300 
+-259 
+-218 
+-264 
+-237 
+-210 
+-251 
+-215 
+-275 
+-234 
+-293 
+-294 
+-303 
+-316 
+-304 
+-340 
+-313 
+-359 
+-318 
+-364 
+-337 
+-319 
+-356 
+-379 
+-384 
+-411 
+-411 
+-435 
+-462 
+-439 
+-481 
+-499 
+-500 
+-482 
+-518 
+-478 
+-469 
+-469 
+-456 
+-415 
+-384 
+-366 
+-393 
+-353 
+-348 
+-348 
+-331 
+-313 
+-350 
+-309 
+-304 
+-333 
+-387 
+-355 
+-401 
+-415 
+-429 
+-416 
+-443 
+-435 
+-435 
+-457 
+-426 
+-440 
+-463 
+-459 
+-423 
+-478 
+-514 
+-496 
+-538 
+-524 
+-575 
+-539 
+-557 
+-553 
+-562 
+-539 
+-522 
+-513 
+-500 
+-468 
+-436 
+-433 
+-419 
+-360 
+-356 
+-365 
+-329 
+-352 
+-343 
+-330 
+-362 
+-321 
+-349 
+-340 
+-368 
+-377 
+-368 
+-414 
+-387 
+-391 
+-397 
+-383 
+-425 
+-393 
+-398 
+-403 
+-444 
+-426 
+-431 
+-458 
+-486 
+-500 
+-486 
+-528 
+-551 
+-546 
+-506 
+-529 
+-525 
+-484 
+-461 
+-471 
+-462 
+-421 
+-381 
+-372 
+-400 
+-359 
+-318 
+-333 
+-337 
+-351 
+-356 
+-315 
+-375 
+-375 
+-393 
+-376 
+-426 
+-421 
+-431 
+-435 
+-450 
+-459 
+-418 
+-428 
+-437 
+-464 
+-456 
+-460 
+-515 
+-484 
+-484 
+-503 
+-562 
+-544 
+-554 
+-540 
+-577 
+-559 
+-518 
+-478 
+-537 
+-496 
+-456 
+-415 
+-452 
+-434 
+-393 
+-353 
+-403 
+-371 
+-331 
+-290 
+-350 
+-359 
+-363 
+-328 
+-387 
+-396 
+-374 
+-392 
+-438 
+-434 
+-393 
+-430 
+-412 
+-439 
+-422 
+-390 
+-450 
+-445 
+-432 
+-428 
+-505 
+-496 
+-470 
+-510 
+-529 
+-534 
+-525 
+-517 
+-557 
+-530 
+-517 
+-485 
+-468 
+-454 
+-382 
+-364 
+-378 
+-364 
+-333 
+-310 
+-302 
+-316 
+-261 
+-276 
+-289 
+-321 
+-281 
+-263 
+-300 
+-332 
+-313 
+-278 
+-337 
+-319 
+-288 
+-301 
+-320 
+-329 
+-293 
+-262 
+-312 
+-362 
+-331 
+-308 
+-363 
+-409 
+-368 
+-392 
+-414 
+-446 
+-406 
+-401 
+-425 
+-402 
+-357 
+-362 
+-362 
+-321 
+-281 
+-245 
+-272 
+-259 
+-218 
+-214 
+-237 
+-196 
+-224 
+-215 
+-275 
+-234 
+-270 
+-271 
+-298 
+-316 
+-313 
+-317 
+-300 
+-354 
+-318 
+-305 
+-337 
+-328 
+-356 
+-365 
+-402 
+-411 
+-407 
+-417 
+-462 
+-476 
+-481 
+-440 
+-500 
+-504 
+-500 
+-478 
+-451 
+-464 
+-456 
+-415 
+-388 
+-361 
+-366 
+-353 
+-330 
+-303 
+-299 
+-290 
+-304 
+-309 
+-300 
+-328 
+-373 
+-346 
+-347 
+-365 
+-425 
+-384 
+-379 
+-403 
+-403 
+-403 
+-381 
+-417 
+-400 
+-382 
+-418 
+-423 
+-437 
+-419 
+-456 
+-492 
+-488 
+-489 
+-479 
+-489 
+-512 
+-462 
+-440 
+-440 
+-431 
+-377 
+-368 
+-351 
+-337 
+-296 
+-256 
+-260 
+-238 
+-211 
+-216 
+-203 
+-239 
+-221 
+-181 
+-240 
+-245 
+-209 
+-218 
+-264 
+-282 
+-246 
+-224 
+-265 
+-238 
+-239 
+-243 
+-298 
+-262 
+-221 
+-231 
+-299 
+-309 
+-291 
+-282 
+-355 
+-369 
+-346 
+-315 
+-365 
+-343 
+-293 
+-320 
+-303 
+-276 
+-235 
+-258 
+-240 
+-200 
+-159 
+-132 
+-178 
+-137 
+-110 
+-129 
+-115 
+-129 
+-134 
+-93 
+-153 
+-157 
+-171 
+-163 
+-195 
+-200 
+-159 
+-177 
+-205 
+-187 
+-182 
+-183 
+-201 
+-197 
+-207 
+-243 
+-235 
+-262 
+-262 
+-281 
+-295 
+-300 
+-277 
+-318 
+-278 
+-314 
+-296 
+-256 
+-224 
+-202 
+-211 
+-193 
+-153 
+-162 
+-148 
+-131 
+-90 
+-145 
+-109 
+-68 
+-128 
+-87 
+-146 
+-129 
+-160 
+-225 
+-184 
+-193 
+-203 
+-212 
+-180 
+-181 
+-190 
+-200 
+-204 
+-195 
+-178 
+-237 
+-269 
+-256 
+-279 
+-275 
+-329 
+-293 
+-317 
+-357 
+-321 
+-308 
+-281 
+-300 
+-268 
+-241 
+-228 
+-228 
+-196 
+-156 
+-129 
+-161 
+-134 
+-93 
+-71 
+-112 
+-135 
+-117 
+-90 
+-154 
+-159 
+-118 
+-173 
+-192 
+-196 
+-161 
+-133 
+-175 
+-189 
+-193 
+-153 
+-212 
+-226 
+-226 
+-199 
+-254 
+-300 
+-268 
+-278 
+-337 
+-337 
+-347 
+-315 
+-375 
+-334 
+-293 
+-253 
+-312 
+-271 
+-231 
+-190 
+-222 
+-209 
+-168 
+-128 
+-187 
+-146 
+-147 
+-165 
+-211 
+-202 
+-193 
+-235 
+-212 
+-271 
+-245 
+-285 
+-281 
+-309 
+-277 
+-287 
+-310 
+-337 
+-306 
+-347 
+-365 
+-384 
+-443 
+-403 
+-462 
+-503 
+-481 
+-490 
+-518 
+-509 
+-486 
+-487 
+-487 
+-482 
+-451 
+-424 
+-425 
+-389 
+-366 
+-353 
+-362 
+-366 
+-331 
+-322 
+-350 
+-364 
+-363 
+-373 
+-387 
+-441 
+-406 
+-433 
+-438 
+-484 
+-443 
+-462 
+-462 
+-489 
+-481 
+-467 
+-500 
+-509 
+-509 
+-528 
+-578 
+-560 
+-606 
+-583 
+-625 
+-584 
+-643 
+-603 
+-567 
+-616 
+-581 
+-540 
+-545 
+-523 
+-509 
+-478 
+-437 
+-410 
+-420 
+-415 
+-375 
+-379 
+-393 
+-403 
+-412 
+-421 
+-381 
+-440 
+-400 
+-459 
+-418 
+-478 
+-442 
+-460 
+-456 
+-492 
+-475 
+-452 
+-493 
+-503 
+-512 
+-476 
+-531 
+-581 
+-581 
+-586 
+-591 
+-628 
+-637 
+-601 
+-597 
+-615 
+-611 
+-570 
+-584 
+-553 
+-512 
+-485 
+-458 
+-490 
+-450 
+-423 
+-400 
+-428 
+-455 
+-446 
+-415 
+-465 
+-484 
+-498 
+-493 
+-544 
+-521 
+-571 
+-531 
+-522 
+-550 
+-532 
+-568 
+-546 
+-587 
+-546 
+-606 
+-610 
+-625 
+-625 
+-643 
+-667 
+-676 
+-680 
+-681 
+-690 
+-718 
+-709 
+-668 
+-646 
+-642 
+-628 
+-597 
+-565 
+-556 
+-539 
+-543 
+-503 
+-512 
+-498 
+-481 
+-499 
+-486 
+-509 
+-473 
+-528 
+-587 
+-546 
+-565 
+-565 
+-606 
+-584 
+-575 
+-603 
+-580 
+-612 
+-581 
+-604 
+-600 
+-654 
+-623 
+-578 
+-669 
+-682 
+-706 
+-674 
+-725 
+-684 
+-738 
+-703 
+-671 
+-721 
+-681 
+-640 
+-600 
+-568 
+-532 
+-528 
+-528 
+-496 
+-465 
+-451 
+-461 
+-434 
+-466 
+-453 
+-412 
+-439 
+-445 
+-440 
+-463 
+-459 
+-436 
+-478 
+-437 
+-482 
+-456 
+-415 
+-461 
+-434 
+-466 
+-453 
+-412 
+-471 
+-486 
+-490 
+-477 
+-486 
+-518 
+-528 
+-523 
+-505 
+-547 
+-515 
+-475 
+-470 
+-493 
+-453 
+-412 
+-371 
+-381 
+-363 
+-340 
+-309 
+-291 
+-305 
+-287 
+-282 
+-279 
+-297 
+-275 
+-293 
+-293 
+-353 
+-312 
+-339 
+-345 
+-340 
+-345 
+-354 
+-354 
+-342 
+-337 
+-341 
+-356 
+-338 
+-338 
+-379 
+-393 
+-403 
+-403 
+-421 
+-467 
+-440 
+-400 
+-459 
+-418 
+-378 
+-437 
+-396 
+-361 
+-329 
+-288 
+-311 
+-293 
+-253 
+-217 
+-230 
+-231 
+-190 
+-177 
+-209 
+-177 
+-223 
+-192 
+-246 
+-215 
+-265 
+-225 
+-284 
+-243 
+-271 
+-262 
+-262 
+-281 
+-290 
+-281 
+-259 
+-273 
+-278 
+-278 
+-296 
+-297 
+-315 
+-325 
+-339 
+-370 
+-371 
+-362 
+-335 
+-340 
+-335 
+-300 
+-282 
+-259 
+-228 
+-187 
+-196 
+-170 
+-142 
+-125 
+-120 
+-102 
+-85 
+-62 
+-121 
+-81 
+-90 
+-100 
+-114 
+-118 
+-128 
+-137 
+-146 
+-106 
+-151 
+-125 
+-184 
+-143 
+-144 
+-162 
+-180 
+-181 
+-190 
+-186 
+-209 
+-254 
+-228 
+-287 
+-282 
+-297 
+-265 
+-325 
+-293 
+-270 
+-262 
+-262 
+-239 
+-217 
+-199 
+-195 
+-164 
+-154 
+-128 
+-123 
+-119 
+-115 
+-110 
+-152 
+-134 
+-148 
+-153 
+-212 
+-207 
+-231 
+-195 
+-250 
+-227 
+-232 
+-228 
+-287 
+-260 
+-270 
+-265 
+-306 
+-298 
+-298 
+-303 
+-362 
+-376 
+-381 
+-390 
+-400 
+-423 
+-413 
+-378 
+-428 
+-396 
+-356 
+-320 
+-361 
+-334 
+-293 
+-276 
+-271 
+-271 
+-231 
+-208 
+-250 
+-214 
+-263 
+-228 
+-287 
+-287 
+-297 
+-301 
+-370 
+-352 
+-348 
+-353 
+-389 
+-371 
+-363 
+-381 
+-377 
+-386 
+-391 
+-378 
+-437 
+-405 
+-456 
+-470 
+-475 
+-520 
+-493 
+-521 
+-512 
+-476 
+-481 
+-454 
+-450 
+-427 
+-418 
+-378 
+-364 
+-346 
+-333 
+-306 
+-293 
+-284 
+-261 
+-303 
+-280 
+-298 
+-281 
+-249 
+-300 
+-309 
+-318 
+-301 
+-323 
+-328 
+-338 
+-324 
+-325 
+-334 
+-338 
+-339 
+-312 
+-330 
+-331 
+-385 
+-354 
+-409 
+-373 
+-428 
+-392 
+-446 
+-406 
+-465 
+-425 
+-384 
+-370 
+-398 
+-362 
+-321 
+-304 
+-272 
+-300 
+-259 
+-218 
+-264 
+-237 
+-210 
+-251 
+-215 
+-275 
+-234 
+-293 
+-294 
+-303 
+-316 
+-304 
+-340 
+-313 
+-359 
+-318 
+-364 
+-337 
+-319 
+-356 
+-379 
+-384 
+-411 
+-411 
+-435 
+-462 
+-439 
+-481 
+-499 
+-500 
+-482 
+-518 
+-478 
+-469 
+-469 
+-456 
+-415 
+-384 
+-366 
+-393 
+-353 
+-348 
+-348 
+-331 
+-313 
+-350 
+-309 
+-304 
+-333 
+-387 
+-355 
+-401 
+-415 
+-429 
+-416 
+-443 
+-435 
+-435 
+-457 
+-426 
+-440 
+-463 
+-459 
+-423 
+-478 
+-514 
+-496 
+-538 
+-524 
+-575 
+-539 
+-557 
+-553 
+-562 
+-539 
+-522 
+-513 
+-500 
+-468 
+-436 
+-433 
+-419 
+-360 
+-356 
+-365 
+-329 
+-352 
+-343 
+-330 
+-362 
+-321 
+-349 
+-340 
+-368 
+-377 
+-368 
+-414 
+-387 
+-391 
+-397 
+-383 
+-425 
+-393 
+-398 
+-403 
+-444 
+-426 
+-431 
+-458 
+-486 
+-500 
+-486 
+-528 
+-551 
+-546 
+-506 
+-529 
+-525 
+-484 
+-461 
+-471 
+-462 
+-421 
+-381 
+-372 
+-400 
+-359 
+-318 
+-333 
+-337 
+-351 
+-356 
+-315 
+-375 
+-375 
+-393 
+-376 
+-426 
+-421 
+-431 
+-435 
+-450 
+-459 
+-418 
+-428 
+-437 
+-464 
+-456 
+-460 
+-515 
+-484 
+-484 
+-503 
+-562 
+-544 
+-554 
+-540 
+-577 
+-559 
+-518 
+-478 
+-537 
+-496 
+-456 
+-415 
+-452 
+-434 
+-393 
+-353 
+-403 
+-371 
+-331 
+-290 
+-350 
+-359 
+-363 
+-328 
+-387 
+-396 
+-374 
+-392 
+-438 
+-434 
+-393 
+-430 
+-412 
+-439 
+-422 
+-390 
+-450 
+-445 
+-432 
+-428 
+-505 
+-496 
+-470 
+-510 
+-529 
+-534 
+-525 
+-517 
+-557 
+-530 
+-517 
+-485 
+-468 
+-454 
+-382 
+-364 
+-378 
+-364 
+-333 
+-310 
+-302 
+-316 
+-261 
+-276 
+-289 
+-321 
+-281 
+-263 
+-300 
+-332 
+-313 
+-278 
+-337 
+-319 
+-288 
+-301 
+-320 
+-329 
+-293 
+-262 
+-312 
+-362 
+-331 
+-308 
+-363 
+-409 
+-368 
+-392 
+-414 
+-446 
+-406 
+-401 
+-425 
+-402 
+-357 
+-362 
+-362 
+-321 
+-281 
+-245 
+-272 
+-259 
+-218 
+-214 
+-237 
+-196 
+-224 
+-215 
+-275 
+-234 
+-270 
+-271 
+-298 
+-316 
+-313 
+-317 
+-300 
+-354 
+-318 
+-305 
+-337 
+-328 
+-356 
+-365 
+-402 
+-411 
+-407 
+-417 
+-462 
+-476 
+-481 
+-440 
+-500 
+-504 
+-500 
+-478 
+-451 
+-464 
+-456 
+-415 
+-388 
+-361 
+-366 
+-353 
+-330 
+-303 
+-299 
+-290 
+-304 
+-309 
+-300 
+-328 
+-373 
+-346 
+-347 
+-365 
+-425 
+-384 
+-379 
+-403 
+-403 
+-403 
+-381 
+-417 
+-400 
+-382 
+-418 
+-423 
+-437 
+-419 
+-456 
+-492 
+-488 
+-489 
+-479 
+-489 
+-512 
+-462 
+-440 
+-440 
+-431 
+-377 
+-368 
+-351 
+-337 
+-296 
+-256 
+-260 
+-238 
+-211 
+-216 
+-203 
+-239 
+-221 
+-181 
+-240 
+-245 
+-209 
+-218 
+-264 
+-282 
+-246 
+-224 
+-265 
+-238 
+-239 
+-243 
+-298 
+-262 
+-221 
+-231 
+-299 
+-309 
+-291 
+-282 
+-355 
+-369 
+-346 
+-315 
+-365 
+-343 
+-293 
+-320 
+-303 
+-276 
+-235 
+-258 
+-240 
+-200 
+-159 
+-132 
+-178 
+-137 
+-110 
+-129 
+-115 
+-129 
+-134 
+-93 
+-153 
+-157 
+-171 
+-163 
+-195 
+-200 
+-159 
+-177 
+-205 
+-187 
+-182 
+-183 
+-201 
+-197 
+-207 
+-243 
+-235 
+-262 
+-262 
+-281 
+-295 
+-300 
+-277 
+-318 
+-278 
+-314 
+-296 
+-256 
+-224 
+-202 
+-211 
+-193 
+-153 
+-162 
+-148 
+-131 
+-90 
+-145 
+-109 
+-68 
+-128 
+-87 
+-146 
+-129 
+-160 
+-225 
+-184 
+-193 
+-203 
+-212 
+-180 
+-181 
+-190 
+-200 
+-204 
+-195 
+-178 
+-237 
+-269 
+-256 
+-279 
+-275 
+-329 
+-293 
+-317 
+-357 
+-321 
+-308 
+-281 
+-300 
+-268 
+-241 
+-228 
+-228 
+-196 
+-156 
+-129 
+-161 
+-134 
+-93 
+-71 
+-112 
+-135 
+-117 
+-90 
+-154 
+-159 
+-118 
+-173 
+-192 
+-196 
+-161 
+-133 
+-175 
+-189 
+-193 
+-153 
+-212 
+-226 
+-226 
+-199 
+-254 
+-300 
+-268 
+-278 
+-337 
+-337 
+-347 
+-315 
+-375 
+-334 
+-293 
+-253 
+-312 
+-271 
+-231 
+-190 
+-222 
+-209 
+-168 
+-128 
+-187 
+-146 
+-147 
+-165 
+-211 
+-202 
+-193 
+-235 
+-212 
+-271 
+-245 
+-285 
+-281 
+-309 
+-277 
+-287 
+-310 
+-337 
+-306 
+-347 
+-365 
+-384 
+-443 
+-403 
+-462 
+-503 
+-481 
+-490 
+-518 
+-509 
+-486 
+-487 
+-487 
+-482 
+-451 
+-424 
+-425 
+-389 
+-366 
+-353 
+-362 
+-366 
+-331 
+-322 
+-350 
+-364 
+-363 
+-373 
+-387 
+-441 
+-406 
+-433 
+-438 
+-484 
+-443 
+-462 
+-462 
+-489 
+-481 
+-467 
+-500 
+-509 
+-509 
+-528 
+-578 
+-560 
+-606 
+-583 
+-625 
+-584 
+-643 
+-603 
+-567 
+-616 
+-581 
+-540 
+-545 
+-523 
+-509 
+-478 
+-437 
+-410 
+-420 
+-415 
+-375 
+-379 
+-393 
+-403 
+-412 
+-421 
+-381 
+-440 
+-400 
+-459 
+-418 
+-478 
+-442 
+-460 
+-456 
+-492 
+-475 
+-452 
+-493 
+-503 
+-512 
+-476 
+-531 
+-581 
+-581 
+-586 
+-591 
+-628 
+-637 
+-601 
+-597 
+-615 
+-611 
+-570 
+-584 
+-553 
+-512 
+-485 
+-458 
+-490 
+-450 
+-423 
+-400 
+-428 
+-455 
+-446 
+-415 
+-465 
+-484 
+-498 
+-493 
+-544 
+-521 
+-571 
+-531 
+-522 
+-550 
+-532 
+-568 
+-546 
+-587 
+-546 
+-606 
+-610 
+-625 
+-625 
+-643 
+-667 
+-676 
+-680 
+-681 
+-690 
+-718 
+-709 
+-668 
+-646 
+-642 
+-628 
+-597 
+-565 
+-556 
+-539 
+-543 
+-503 
+-512 
+-498 
+-481 
+-499 
+-486 
+-509 
+-473 
+-528 
+-587 
+-546 
+-565 
+-565 
+-606 
+-584 
+-575 
+-603 
+-580 
+-612 
+-581 
+-604 
+-600 
+-654 
+-623 
+-578 
+-669 
+-682 
+-706 
+-674 
+-725 
+-684 
+-738 
+-703 
+-671 
+-721 
+-681 
+-640 
+-600 
+-568 
+-532 
+-528 
+-528 
+-496 
+-465 
+-451 
+-461 
+-434 
+-466 
+-453 
+-412 
+-439 
+-445 
+-440 
+-463 
+-459 
+-436 
+-478 
+-437 
+-482 
+-456 
+-415 
+-461 
+-434 
+-466 
+-453 
+-412 
+-471 
+-486 
+-490 
+-477 
+-486 
+-518 
+-528 
+-523 
+-505 
+-547 
+-515 
+-475 
+-470 
+-493 
+-453 
+-412 
+-371 
+-381 
+-363 
+-340 
+-309 
+-291 
+-305 
+-287 
+-282 
+-279 
+-297 
+-275 
+-293 
+-293 
+-353 
+-312 
+-339 
+-345 
+-340 
+-345 
+-354 
+-354 
+-342 
+-337 
+-341 
+-356 
+-338 
+-338 
+-379 
+-393 
+-403 
+-403 
+-421 
+-467 
+-440 
+-400 
+-459 
+-418 
+-378 
+-437 
+-396 
+-361 
+-329 
+-288 
+-311 
+-293 
+-253 
+-217 
+-230 
+-231 
+-190 
+-177 
+-209 
+-177 
+-223 
+-192 
+-246 
+-215 
+-265 
+-225 
+-284 
+-243 
+-271 
+-262 
+-262 
+-281 
+-290 
+-281 
+-259 
+-273 
+-278 
+-278 
+-296 
+-297 
+-315 
+-325 
+-339 
+-370 
+-371 
+-362 
+-335 
+-340 
+-335 
+-300 
+-282 
+-259 
+-228 
+-187 
+-196 
+-170 
+-142 
+-125 
+-120 
+-102 
+-85 
+-62 
+-121 
+-81 
+-90 
+-100 
+-114 
+-118 
+-128 
+-137 
+-146 
+-106 
+-151 
+-125 
+-184 
+-143 
+-144 
+-162 
+-180 
+-181 
+-190 
+-186 
+-209 
+-254 
+-228 
+-287 
+-282 
+-297 
+-265 
+-325 
+-293 
+-270 
+-262 
+-262 
+-239 
+-217 
+-199 
+-195 
+-164 
+-154 
+-128 
+-123 
+-119 
+-115 
+-110 
+-152 
+-134 
+-148 
+-153 
+-212 
+-207 
+-231 
+-195 
+-250 
+-227 
+-232 
+-228 
+-287 
+-260 
+-270 
+-265 
+-306 
+-298 
+-298 
+-303 
+-362 
+-376 
+-381 
+-390 
+-400 
+-423 
+-413 
+-378 
+-428 
+-396 
+-356 
+-320 
+-361 
+-334 
+-293 
+-276 
+-271 
+-271 
+-231 
+-208 
+-250 
+-214 
+-263 
+-228 
+-287 
+-287 
+-297 
+-301 
+-370 
+-352 
+-348 
+-353 
+-389 
+-371 
+-363 
+-381 
+-377 
+-386 
+-391 
+-378 
+-437 
+-405 
+-456 
+-470 
+-475 
+-520 
+-493 
+-521 
+-512 
+-476 
+-481 
+-454 
+-450 
+-427 
+-418 
+-378 
+-364 
+-346 
+-333 
+-306 
+-293 
+-284 
+-261 
+-303 
+-280 
+-298 
+-281 
+-249 
+-300 
+-309 
+-318 
+-301 
+-323 
+-328 
+-338 
+-324 
+-325 
+-334 
+-338 
+-339 
+-312 
+-330 
+-331 
+-385 
+-354 
+-409 
+-373 
+-428 
+-392 
+-446 
+-406 
+-465 
+-425 
+-384 
+-370 
+-398 
+-362 
+-321 
+-304 
+-272 
+-300 
+-259 
+-218 
+-264 
+-237 
+-210 
+-251 
+-215 
+-275 
+-234 
+-293 
+-294 
+-303 
+-316 
+-304 
+-340 
+-313 
+-359 
+-318 
+-364 
+-337 
+-319 
+-356 
+-379 
+-384 
+-411 
+-411 
+-435 
+-462 
+-439 
+-481 
+-499 
+-500 
+-482 
+-518 
+-478 
+-469 
+-469 
+-456 
+-415 
+-384 
+-366 
+-393 
+-353 
+-348 
+-348 
+-331 
+-313 
+-350 
+-309 
+-304 
+-333 
+-387 
+-355 
+-401 
+-415 
+-429 
+-416 
+-443 
+-435 
+-435 
+-457 
+-426 
+-440 
+-463 
+-459 
+-423 
+-478 
+-514 
+-496 
+-538 
+-524 
+-575 
+-539 
+-557 
+-553 
+-562 
+-539 
+-522 
+-513 
+-500 
+-468 
+-436 
+-433 
+-419 
+-360 
+-356 
+-365 
+-329 
+-352 
+-343 
+-330 
+-362 
+-321 
+-349 
+-340 
+-368 
+-377 
+-368 
+-414 
+-387 
+-391 
+-397 
+-383 
+-425 
+-393 
+-398 
+-403 
+-444 
+-426 
+-431 
+-458 
+-486 
+-500 
+-486 
+-528 
+-551 
+-546 
+-506 
+-529 
+-525 
+-484 
+-461 
+-471 
+-462 
+-421 
+-381 
+-372 
+-400 
+-359 
+-318 
+-333 
+-337 
+-351 
+-356 
+-315 
+-375 
+-375 
+-393 
+-376 
+-426 
+-421 
+-431 
+-435 
+-450 
+-459 
+-418 
+-428 
+-437 
+-464 
+-456 
+-460 
+-515 
+-484 
+-484 
+-503 
+-562 
+-544 
+-554 
+-540 
+-577 
+-559 
+-518 
+-478 
+-537 
+-496 
+-456 
+-415 
+-452 
+-434 
+-393 
+-353 
+-403 
+-371 
+-331 
+-290 
+-350 
+-359 
+-363 
+-328 
+-387 
+-396 
+-374 
+-392 
+-438 
+-434 
+-393 
+-430 
+-412 
+-439 
+-422 
+-390 
+-450 
+-445 
+-432 
+-428 
+-505 
+-496 
+-470 
+-510 
+-529 
+-534 
+-525 
+-517 
+-557 
+-530 
+-517 
+-485 
+-468 
+-454 
+-382 
+-364 
+-378 
+-364 
+-333 
+-310 
+-302 
+-316 
+-261 
+-276 
+-289 
+-321 
+-281 
+-263 
+-300 
+-332 
+-313 
+-278 
+-337 
+-319 
+-288 
+-301 
+-320 
+-329 
+-293 
+-262 
+-312 
+-362 
+-331 
+-308 
+-363 
+-409 
+-368 
+-392 
+-414 
+-446 
+-406 
+-401 
+-425 
+-402 
+-357 
+-362 
+-362 
+-321 
+-281 
+-245 
+-272 
+-259 
+-218 
+-214 
+-237 
+-196 
+-224 
+-215 
+-275 
+-234 
+-270 
+-271 
+-298 
+-316 
+-313 
+-317 
+-300 
+-354 
+-318 
+-305 
+-337 
+-328 
+-356 
+-365 
+-402 
+-411 
+-407 
+-417 
+-462 
+-476 
+-481 
+-440 
+-500 
+-504 
+-500 
+-478 
+-451 
+-464 
+-456 
+-415 
+-388 
+-361 
+-366 
+-353 
+-330 
+-303 
+-299 
+-290 
+-304 
+-309 
+-300 
+-328 
+-373 
+-346 
+-347 
+-365 
+-425 
+-384 
+-379 
+-403 
+-403 
+-403 
+-381 
+-417 
+-400 
+-382 
+-418 
+-423 
+-437 
+-419 
+-456 
+-492 
+-488 
+-489 
+-479 
+-489 
+-512 
+-462 
+-440 
+-440 
+-431 
+-377 
+-368 
+-351 
+-337 
+-296 
+-256 
+-260 
+-238 
+-211 
+-216 
+-203 
+-239 
+-221 
+-181 
+-240 
+-245 
+-209 
+-218 
+-264 
+-282 
+-246 
+-224 
+-265 
+-238 
+-239 
+-243 
+-298 
+-262 
+-221 
+-231 
+-299 
+-309 
+-291 
+-282 
+-355 
+-369 
+-346 
+-315 
+-365 
+-343 
+-293 
+-320 
+-303 
+-276 
+-235 
+-258 
+-240 
+-200 
+-159 
+-132 
+-178 
+-137 
+-110 
+-129 
+-115 
+-129 
+-134 
+-93 
+-153 
+-157 
+-171 
+-163 
+-195 
+-200 
+-159 
+-177 
+-205 
+-187 
+-182 
+-183 
+-201 
+-197 
+-207 
+-243 
+-235 
+-262 
+-262 
+-281 
+-295 
+-300 
+-277 
+-318 
+-278 
+-314 
+-296 
+-256 
+-224 
+-202 
+-211 
+-193 
+-153 
+-162 
+-148 
+-131 
+-90 
+-145 
+-109 
+-68 
+-128 
+-87 
+-146 
+-129 
+-160 
+-225 
+-184 
+-193 
+-203 
+-212 
+-180 
+-181 
+-190 
+-200 
+-204 
+-195 
+-178 
+-237 
+-269 
+-256 
+-279 
+-275 
+-329 
+-293 
+-317 
+-357 
+-321 
+-308 
+-281 
+-300 
+-268 
+-241 
+-228 
+-228 
+-196 
+-156 
+-129 
+-161 
+-134 
+-93 
+-71 
+-112 
+-135 
+-117 
+-90 
+-154 
+-159 
+-118 
+-173 
+-192 
+-196 
+-161 
+-133 
+-175 
+-189 
+-193 
+-153 
+-212 
+-226 
+-226 
+-199 
+-254 
+-300 
+-268 
+-278 
+-337 
+-337 
+-347 
+-315 
+-375 
+-334 
+-293 
+-253 
+-312 
+-271 
+-231 
+-190 
+-222 
+-209 
+-168 
+-128 
+-187 
+-146 
+-147 
+-165 
+-211 
+-202 
+-193 
+-235 
+-212 
+-271 
+-245 
+-285 
+-281 
+-309 
+-277 
+-287 
+-310 
+-337 
+-306 
+-347 
+-365 
+-384 
+-443 
+-403 
+-462 
+-503 
+-481 
+-490 
+-518 
+-509 
+-486 
+-487 
+-487 
+-482 
+-451 
+-424 
+-425 
+-389 
+-366 
+-353 
+-362 
+-366 
+-331 
+-322 
+-350 
+-364 
+-363 
+-373 
+-387 
+-441 
+-406 
+-433 
+-438 
+-484 
+-443 
+-462 
+-462 
+-489 
+-481 
+-467 
+-500 
+-509 
+-509 
+-528 
+-578 
+-560 
+-606 
+-583 
+-625 
+-584 
+-643 
+-603 
+-567 
+-616 
+-581 
+-540 
+-545 
+-523 
+-509 
+-478 
+-437 
+-410 
+-420 
+-415 
+-375 
+-379 
+-393 
+-403 
+-412 
+-421 
+-381 
+-440 
+-400 
+-459 
+-418 
+-478 
+-442 
+-460 
+-456 
+-492 
+-475 
+-452 
+-493 
+-503 
+-512 
+-476 
+-531 
+-581 
+-581 
+-586 
+-591 
+-628 
+-637 
+-601 
+-597 
+-615 
+-611 
+-570 
+-584 
+-553 
+-512 
+-485 
+-458 
+-490 
+-450 
+-423 
+-400 
+-428 
+-455 
+-446 
+-415 
+-465 
+-484 
+-498 
+-493 
+-544 
+-521 
+-571 
+-531 
+-522 
+-550 
+-532 
+-568 
+-546 
+-587 
+-546 
+-606 
+-610 
+-625 
+-625 
+-643 
+-667 
+-676 
+-680 
+-681 
+-690 
+-718 
+-709 
+-668 
+-646 
+-642 
+-628 
+-597 
+-565 
+-556 
+-539 
+-543 
+-503 
+-512 
+-498 
+-481 
+-499 
+-486 
+-509 
+-473 
+-528 
+-587 
+-546 
+-565 
+-565 
+-606 
+-584 
+-575 
+-603 
+-580 
+-612 
+-581 
+-604 

Simulink/myfilter3.m

@@ -0,0 +1,12 @@
+Fir=Fir_angle3;
+in=csvread('angle3.csv');
+in=in';
+angle_off=filter(Fir.Numerator, 1, in);
+out=single(angle_off);
+t=0:3071;
+plot(t, in);
+hold;
+%plot(t-158, out);
+%plot(t-253, out);
+plot(t-226, out);
+out_off=out';