Merge branch 'master' of 192.168.1.3:manufacturer_stm32/iflytop_stm32_os_sdk

components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -483,7 +483,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
 
 void StepMotorCtrlModule::getnowpos(int32_t& pos) {
   int32_t motor_pos = m_stepM1->getXACTUAL();
-  forward_kinematics(motor_pos, pos);
+  inverse_kinematics(motor_pos, pos);
 }
 void StepMotorCtrlModule::_motor_move_to(int32_t pos, int32_t maxv, int32_t acc, int32_t dec) {
   ZLOGI(TAG, "m%d _motor_move_to %d maxv:%d acc:%d dec:%d", m_id, pos, maxv, acc, dec);
@@ -518,11 +518,9 @@ bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTar
 void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) {
   // m_zero_shift_x
   x = motor_pos;
-  if (m_param.x_shaft != 0) x = -x;
   x += m_param.shift_x;
 }
 void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) {
-  if (m_param.x_shaft != 0) x = -x;
   x -= m_param.shift_x;
   motor_pos = x;
 }
@@ -638,4 +636,5 @@ int32_t StepMotorCtrlModule::factory_reset() {
 void StepMotorCtrlModule::active_cfg() {
   m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay);
   m_stepM1->setScale(m_param.distance_scale);
+  m_stepM1->setMotorShaft(m_param.x_shaft);
 }