From b88057fffd12994e8b32ecc35ca5c8e1d5059898 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Wed, 4 Oct 2023 11:50:02 +0200 Subject: [PATCH] examples/foc: support svm3 state with nxscope --- examples/foc/foc_fixed16_thr.c | 20 +++++++++++++++++++- examples/foc/foc_float_thr.c | 20 +++++++++++++++++++- examples/foc/foc_motor_b16.h | 3 +++ examples/foc/foc_motor_f32.h | 3 +++ examples/foc/foc_nxscope.c | 3 +++ examples/foc/foc_nxscope.h | 1 + 6 files changed, 48 insertions(+), 2 deletions(-) diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index fc0158b396e..569b9e70a27 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -129,7 +129,7 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor, foc_handler_state_b16(&motor->handler, &motor->foc_state, - NULL); + &motor->mod_state); return ret; } @@ -259,6 +259,24 @@ static void foc_fixed16_nxscope(FAR struct foc_nxscope_s *nxs, ptr = (FAR b16_t *)&motor->vdq_comp; nxscope_put_vb16_t(&nxs->nxs, i++, ptr, 2); #endif +#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3) + b16_t svm3_tmp[4]; + + /* Convert sector to b16_t. + * Normally, a sector value is an integer in the range 1-6 but we convert + * it to b16_t and range to 0.1-0.6. This is to send the entire SVM3 state + * as b16_t array and scale the sector value closer to PWM duty values + * (range 0.0 to 0.5) which makes it easier to visualize the data later. + */ + + svm3_tmp[0] = b16mulb16(itob16(motor->mod_state.sector), ftob16(0.1f)); + svm3_tmp[1] = motor->mod_state.d_u; + svm3_tmp[2] = motor->mod_state.d_v; + svm3_tmp[3] = motor->mod_state.d_w; + + ptr = svm3_tmp; + nxscope_put_vfloat(&nxs->nxs, i++, ptr, 4); +#endif nxscope_unlock(&nxs->nxs); } diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index 7598a40aed6..cc362a1da1e 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -129,7 +129,7 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor, foc_handler_state_f32(&motor->handler, &motor->foc_state, - NULL); + &motor->mod_state); return ret; } @@ -260,6 +260,24 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs, ptr = (FAR float *)&motor->vdq_comp; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2); #endif +#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3) + float svm3_tmp[4]; + + /* Convert sector to float. + * Normally, a sector value is an integer in the range 1-6 but we convert + * it to float and range to 0.1-0.6. This is to send the entire SVM3 state + * as float array and scale the sector value closer to PWM duty values + * (range 0.0 to 0.5) which makes it easier to visualize the data later. + */ + + svm3_tmp[0] = (float)motor->mod_state.sector * 0.1f; + svm3_tmp[1] = motor->mod_state.d_u; + svm3_tmp[2] = motor->mod_state.d_v; + svm3_tmp[3] = motor->mod_state.d_w; + + ptr = svm3_tmp; + nxscope_put_vfloat(&nxs->nxs, i++, ptr, 4); +#endif nxscope_unlock(&nxs->nxs); } diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index 5b660bf83ba..0c3605389b9 100644 --- a/examples/foc/foc_motor_b16.h +++ b/examples/foc/foc_motor_b16.h @@ -80,6 +80,9 @@ struct foc_motor_b16_s /* FOC data ***************************************************************/ struct foc_state_b16_s foc_state; /* FOC controller sate */ +#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 + struct svm3_state_b16_s mod_state; /* Modulation state */ +#endif foc_handler_b16_t handler; /* FOC controller */ dq_frame_b16_t dq_ref; /* DQ reference */ dq_frame_b16_t vdq_comp; /* DQ voltage compensation */ diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index 0eb86a789cd..fb4767885ee 100644 --- a/examples/foc/foc_motor_f32.h +++ b/examples/foc/foc_motor_f32.h @@ -80,6 +80,9 @@ struct foc_motor_f32_s /* FOC data ***************************************************************/ struct foc_state_f32_s foc_state; /* FOC controller sate */ +#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 + struct svm3_state_f32_s mod_state; /* Modulation state */ +#endif foc_handler_f32_t handler; /* FOC controller */ dq_frame_f32_t dq_ref; /* DQ reference */ dq_frame_f32_t vdq_comp; /* DQ voltage compensation */ diff --git a/examples/foc/foc_nxscope.c b/examples/foc/foc_nxscope.c index 6a4afdff2ac..a6fbde8801a 100644 --- a/examples/foc/foc_nxscope.c +++ b/examples/foc/foc_nxscope.c @@ -236,6 +236,9 @@ int foc_nxscope_init(FAR struct foc_nxscope_s *nxs) #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP) nxscope_chan_init(&nxs->nxs, i++, "vdqcomp", u.u8, 2, 0); #endif +#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3) + nxscope_chan_init(&nxs->nxs, i++, "svm3", u.u8, 4, 0); +#endif if (i > CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS) { diff --git a/examples/foc/foc_nxscope.h b/examples/foc/foc_nxscope.h index bfbdda833e1..ef7a79a6dc2 100644 --- a/examples/foc/foc_nxscope.h +++ b/examples/foc/foc_nxscope.h @@ -51,6 +51,7 @@ #define FOC_NXSCOPE_SPPOS (1 << 13) /* Position setpoint */ #define FOC_NXSCOPE_DQREF (1 << 14) /* DQ reference */ #define FOC_NXSCOPE_VDQCOMP (1 << 15) /* VDQ compensation */ +#define FOC_NXSCOPE_SVM3 (1 << 16) /* Space-vector modulation sector */ /* Max 32-bit */ /****************************************************************************