diff --git a/3.10/misc/motor/motor.c b/3.10/misc/motor/motor.c index a4ddc2bd3..cb64150b5 100644 --- a/3.10/misc/motor/motor.c +++ b/3.10/misc/motor/motor.c @@ -260,26 +260,30 @@ static void motor_power_off(struct motor_device *mdev) } // motor_power_on re-enables motor GPIO pins as outputs -// Called before motor movement to energize the coils +// Called before motor movement to ensure initial state of the coils. static void motor_power_on(struct motor_device *mdev) { int index; int value; + int step; struct motor_driver *motor = NULL; - value = ((0 ^ invert_gpio_dir) & 0x1); - for (index = 0; index < NUMBER_OF_MOTORS; index++) { motor = &mdev->motors[index]; + // apply torque, assuming cur_steps reflects actual position + step = motor->cur_steps % 8; + step = step < 0 ? step + 8 : step; + value = invert_gpio_dir ? (step_8[step] ^ 0xff) : step_8[step]; + if (motor->pdata->motor_st1_gpio != -1) - gpio_direction_output(motor->pdata->motor_st1_gpio, value); + gpio_set_value(motor->pdata->motor_st1_gpio, value & 0x8); if (motor->pdata->motor_st2_gpio != -1) - gpio_direction_output(motor->pdata->motor_st2_gpio, value); + gpio_set_value(motor->pdata->motor_st2_gpio, value & 0x4); if (motor->pdata->motor_st3_gpio != -1) - gpio_direction_output(motor->pdata->motor_st3_gpio, value); + gpio_set_value(motor->pdata->motor_st3_gpio, value & 0x2); if (motor->pdata->motor_st4_gpio != -1) - gpio_direction_output(motor->pdata->motor_st4_gpio, value); + gpio_set_value(motor->pdata->motor_st4_gpio, value & 0x1); } } @@ -312,29 +316,30 @@ static void motor_move_step(struct motor_device *mdev, int index) step = motor->cur_steps % 8; step = step < 0 ? step + 8 : step; - value = (step_8[step] ^ 0xff); + value = invert_gpio_dir ? (step_8[step] ^ 0xff) : step_8[step]; motor_set_direction(mdev, (index == PAN_MOTOR) ? MOTOR_MOVE_RIGHT_UP : MOTOR_MOVE_LEFT_DOWN); if (motor->pdata->motor_st1_gpio != -1) - gpio_direction_output(motor->pdata->motor_st1_gpio, value & 0x8); + gpio_set_value(motor->pdata->motor_st1_gpio, value & 0x8); if (motor->pdata->motor_st2_gpio != -1) - gpio_direction_output(motor->pdata->motor_st2_gpio, value & 0x4); + gpio_set_value(motor->pdata->motor_st2_gpio, value & 0x4); if (motor->pdata->motor_st3_gpio != -1) - gpio_direction_output(motor->pdata->motor_st3_gpio, value & 0x2); + gpio_set_value(motor->pdata->motor_st3_gpio, value & 0x2); if (motor->pdata->motor_st4_gpio != -1) - gpio_direction_output(motor->pdata->motor_st4_gpio, value & 0x1); + gpio_set_value(motor->pdata->motor_st4_gpio, value & 0x1); } else { - value = ((0 ^ invert_gpio_dir) & 0x1); + // power off the coils + value = invert_gpio_dir ? 1 : 0; if (motor->pdata->motor_st1_gpio != -1) - gpio_direction_output(motor->pdata->motor_st1_gpio, value); + gpio_set_value(motor->pdata->motor_st1_gpio, value); if (motor->pdata->motor_st2_gpio != -1) - gpio_direction_output(motor->pdata->motor_st2_gpio, value); + gpio_set_value(motor->pdata->motor_st2_gpio, value); if (motor->pdata->motor_st3_gpio != -1) - gpio_direction_output(motor->pdata->motor_st3_gpio, value); + gpio_set_value(motor->pdata->motor_st3_gpio, value); if (motor->pdata->motor_st4_gpio != -1) - gpio_direction_output(motor->pdata->motor_st4_gpio, value); + gpio_set_value(motor->pdata->motor_st4_gpio, value); } if (motor->state == MOTOR_OPS_RESET) { @@ -519,12 +524,12 @@ static long motor_ops_move(struct motor_device *mdev, int x, int y) return 0; } - /* Enable motor GPIO outputs before movement */ - motor_power_on(mdev); - mutex_lock(&mdev->dev_mutex); spin_lock_irqsave(&mdev->slock, flags); + /* Enable motor GPIO outputs before movement */ + motor_power_on(mdev); + calc_slow_mode(mdev, x1); mdev->counter = 0; @@ -642,12 +647,12 @@ static long motor_ops_cruise(struct motor_device *mdev) motor_ops_goback(mdev); - /* Enable motor GPIO outputs before cruise */ - motor_power_on(mdev); - mutex_lock(&mdev->dev_mutex); spin_lock_irqsave(&mdev->slock, flags); + /* Enable motor GPIO outputs before cruise */ + motor_power_on(mdev); + mdev->dev_state = MOTOR_OPS_CRUISE; motors[PAN_MOTOR].state = MOTOR_OPS_CRUISE; motors[TILT_MOTOR].state = MOTOR_OPS_CRUISE; @@ -724,12 +729,12 @@ static long motor_ops_reset(struct motor_device *mdev, struct motor_reset_data * mutex_unlock(&mdev->dev_mutex); } else { /* driver calculate max steps. */ - /* Enable motor GPIO outputs before reset/homing */ - motor_power_on(mdev); - mutex_lock(&mdev->dev_mutex); spin_lock_irqsave(&mdev->slock, flags); + /* Enable motor GPIO outputs before reset/homing */ + motor_power_on(mdev); + for (index = 0; index < NUMBER_OF_MOTORS; index++) { struct motor_driver *drv = &mdev->motors[index]; int half = drv->max_steps > 0 ? drv->max_steps / 2 : 0;