void stabilisation_copter_mix_to_servos_cross_quad(control_command_t *control, servos_t* servos)
{
int32_t i;
float motor_command[4];
motor_command[M_FRONT]= control->thrust + control->rpy[PITCH] + M_FRONT_DIR * control->rpy[YAW];
motor_command[M_RIGHT] = control->thrust - control->rpy[ROLL] + M_RIGHT_DIR * control->rpy[YAW];
motor_command[M_REAR] = control->thrust - control->rpy[PITCH] + M_REAR_DIR * control->rpy[YAW];
motor_command[M_LEFT] = control->thrust + control->rpy[ROLL] + M_LEFT_DIR * control->rpy[YAW];
for (i=0; i<4; i++)
{
if (motor_command[i]<MIN_THRUST)
{
motor_command[i]=MIN_THRUST;
}
if (motor_command[i]>MAX_THRUST)
{
motor_command[i]=MAX_THRUST;
}
}
for (i=0; i<4; i++)
{
servos_set_value( servos, i, motor_command[i]);
}
}
void servos_mix_quadcopter_diag_update(servo_mix_quadcotper_diag_t* mix)
{
float motor[4];
// Front Right motor
motor[0] = mix->thrust_command->thrust +
( - mix->torque_command->xyz[0] ) +
( + mix->torque_command->xyz[1] ) +
mix->motor_front_right_dir * mix->torque_command->xyz[2];
// Front Left motor
motor[1] = mix->thrust_command->thrust +
( + mix->torque_command->xyz[0] ) +
( + mix->torque_command->xyz[1] ) +
mix->motor_front_left_dir * mix->torque_command->xyz[2];
// Rear Right motor
motor[2] = mix->thrust_command->thrust +
(- mix->torque_command->xyz[0]) +
(- mix->torque_command->xyz[1]) +
mix->motor_rear_right_dir * mix->torque_command->xyz[2];
// Rear Left motor
motor[3] = mix->thrust_command->thrust +
( + mix->torque_command->xyz[0] ) +
( - mix->torque_command->xyz[1] ) +
mix->motor_rear_left_dir * mix->torque_command->xyz[2];
// Clip values
for (int32_t i = 0; i < 4; i++)
{
if ( motor[i] < mix->min_thrust )
{
motor[i] = mix->min_thrust;
}
else if ( motor[i] > mix->max_thrust )
{
motor[i] = mix->max_thrust;
}
}
servos_set_value(mix->servos, mix->motor_front_right, motor[0]);
servos_set_value(mix->servos, mix->motor_front_left, motor[1]);
servos_set_value(mix->servos, mix->motor_rear_right, motor[2]);
servos_set_value(mix->servos, mix->motor_rear_left, motor[3]);
}
bool servos_mix_birotor_update(servo_mix_birotor_t* mix)
{
//int32_t i;
float motor[4];
// torque_command->xyz[0] ==== ROLL
// torque_command->xyz[1] ==== PITCH
// torque_command->xyz[2] ==== YAW
// Motor left
motor[0] = mix->thrust_command->thrust +
( + mix->torque_command->xyz[2]);
// Motor right
motor[1] = mix->thrust_command->thrust +
( - mix->torque_command->xyz[2]);
// Servo left
motor[2] = mix->servo_left_dir * ( + mix->torque_command->xyz[0]
+ mix->torque_command->xyz[1] );
// Servo right
motor[3] = mix->servo_right_dir * ( - mix->torque_command->xyz[0]
+ mix->torque_command->xyz[1] );
// Clip values
/*for (i=0; i<2; i++)
{
if ( motor[i] < mix->min_thrust )
{
motor[i] = mix->min_thrust;
}
else if ( motor[i] > mix->max_thrust )
{
motor[i] = mix->max_thrust;
}
}
int i=0;
for (i=2; i<4; i++)
{
if ( motor[i] < mix->min_servo )
{
motor[i] = mix->min_servo;
}
else if ( motor[i] > mix->max_servo )
{
motor[i] = mix->max_servo;
}
}
servos_set_value(mix->servos, mix->motor_left , motor[0]);
servos_set_value(mix->servos, mix->motor_right , motor[1]);
servos_set_value(mix->servos, mix->servo_left , motor[2]);
servos_set_value(mix->servos, mix->servo_right , motor[3]);*/
servos_set_value(mix->servos, 0 , motor[0]);
servos_set_value(mix->servos, 1 , motor[1]);
servos_set_value(mix->servos, 2 , motor[2]);
servos_set_value(mix->servos, 3 , motor[3]);
mix->dc_motors->wingrons_angle[0] = motor[2];
mix->dc_motors->wingrons_angle[1] = motor[3];
return true;
}
