计算vbus电流，加入功率角，测试看看

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

Applications/app/app.c

@@ -112,7 +112,7 @@ static void app_print_log(void) {
 	//throttle_log();
 	//sys_debug("Trq: %f-%f-%f\n", motor.controller.ramp_input_torque.target, motor.controller.ramp_input_torque.interpolation, motor.controller.ramp_input_torque.step);
 	sys_debug("FOC is %s, %d,%d, pwm:%s\n", mot_contrl_is_start(mot_contrl())?"start":"stop", motor.foc_start_cnt, motor.foc_stop_cnt, pwm_channel_is_enabled()?"start":"stop");
-	sys_debug("duty: %d-%d-%d\n", foc()->out.duty[0], foc()->out.duty[1], foc()->out.duty[2]);
+	sys_debug("curr: %f,%f\n", mot_contrl()->dc_curr_calc, mot_contrl()->dc_curr_filted);
 	//F_debug();
 	//eCtrl_debug_log();
 	//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());

Applications/foc/core/controller.c

@@ -894,6 +894,8 @@ void mot_contrl_calc_current(mot_contrl_t *ctrl) {
 		iDC x vDC = 3/2(iq x vq + id x vd);
 	*/
 	float m_pow = (vd * id + vq * iq);
+	float pf_angle = fast_atan2(id, iq) - fast_atan2(vd, vq);
+	m_pow = m_pow * arm_cos_f32(pf_angle);
 	float raw_idc = 0.0f;
 	float v_dc = get_vbus_float();
 	if (v_dc != 0.0f) {