void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
{
static int motor_skip_counter = 0;
static PID_t yaw_pos_controller;
static PID_t yaw_speed_controller;
static PID_t nick_controller;
static PID_t roll_controller;
static const float min_gas = 10;
static const float max_gas = 512;
static uint16_t motor_pwm[4] = {0, 0, 0, 0};
static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
// static float remote_control_weight_z = 1;
// static float position_control_weight_z = 0;
static float pid_yawpos_lim;
static float pid_yawspeed_lim;
static float pid_att_lim;
static bool initialized;
static float_vect3 attitude_setpoint_navigationframe_from_positioncontroller;
static hrt_abstime now_time;
static hrt_abstime last_time;
static commander_state_machine_t current_state;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
pid_init(&yaw_pos_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU],
PID_MODE_DERIVATIV_CALC, 154);
pid_init(&yaw_speed_controller,
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU],
PID_MODE_DERIVATIV_CALC, 155);
pid_init(&nick_controller,
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
PID_MODE_DERIVATIV_SET, 156);
pid_init(&roll_controller,
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
PID_MODE_DERIVATIV_SET, 157);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
//TODO: true initialization? get gps while on ground?
attitude_setpoint_navigationframe_from_positioncontroller.x = 0.0f;
attitude_setpoint_navigationframe_from_positioncontroller.y = 0.0f;
attitude_setpoint_navigationframe_from_positioncontroller.z = 0.0f;
last_time = 0;
initialized = true;
}
/* load new parameters with lower rate */
if (motor_skip_counter % 50 == 0) {
pid_set_parameters(&yaw_pos_controller,
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
pid_set_parameters(&yaw_speed_controller,
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]);
pid_set_parameters(&nick_controller,
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
pid_set_parameters(&roll_controller,
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
}
current_state = status->state_machine;
float_vect3 attitude_setpoint_bodyframe = {}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
if (current_state == SYSTEM_STATE_AUTO) {
attitude_setpoint_navigationframe_from_positioncontroller.x = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[0];
attitude_setpoint_navigationframe_from_positioncontroller.y = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[1];
attitude_setpoint_navigationframe_from_positioncontroller.z = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[2];
float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z;
// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
if (yaw_e > M_PI) {
yaw_e -= 2.0f * M_PI_F;
}
if (yaw_e < -M_PI) {
yaw_e += 2.0f * M_PI_F;
}
attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL);
/* limit control output */
if (attitude_setpoint_navigationframe_from_positioncontroller.z > pid_yawpos_lim) {
attitude_setpoint_navigationframe_from_positioncontroller.z = pid_yawpos_lim;
yaw_pos_controller.saturated = 1;
}
if (attitude_setpoint_navigationframe_from_positioncontroller.z < -pid_yawpos_lim) {
attitude_setpoint_navigationframe_from_positioncontroller.z = -pid_yawpos_lim;
yaw_pos_controller.saturated = 1;
}
//transform attitude setpoint from position controller from navi to body frame on xy_plane
float_vect3 attitude_setpoint_bodyframe_from_positioncontroller;
navi2body_xy_plane(&attitude_setpoint_navigationframe_from_positioncontroller, att->yaw , &attitude_setpoint_bodyframe_from_positioncontroller); //yaw angle= att->yaw
//now everything is in body frame
//TODO: here we decide which input (position controller or ppm) we use. For now we have only the ppm, this should be decided dpending on the state machione (manula or auto) ppm should always overwrite auto (?)
attitude_setpoint_bodyframe.x = attitude_setpoint_bodyframe_from_positioncontroller.x;
attitude_setpoint_bodyframe.y = attitude_setpoint_bodyframe_from_positioncontroller.y;
attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
} else if (current_state == SYSTEM_STATE_MANUAL) {
attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * M_PI_F / 8.0f;
attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * M_PI_F / 8.0f;
attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * M_PI_F;
}
/* add an attitude offset which needs to be estimated somewhere */
attitude_setpoint_bodyframe.x += global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET];
attitude_setpoint_bodyframe.y += global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET];
/*Calculate Controllers*/
//control Nick
float nick = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
//control Roll
float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
//control Yaw Speed
float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed!
//compensation to keep force in z-direction
float zcompensation;
if (fabs(att->roll) > 0.5f) {
zcompensation = 1.13949393f;
} else {
zcompensation = 1.0f / cosf(att->roll);
}
if (fabs(att->pitch) > 0.5f) {
zcompensation *= 1.13949393f;
} else {
zcompensation *= 1.0f / cosf(att->pitch);
}
// use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT]
// to compute thrust for Z position control
//
// float motor_thrust = min_gas +
// ( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation )
// + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation );
//calculate the basic thrust
float motor_thrust = 0;
// FLYING MODES
if (current_state == SYSTEM_STATE_MANUAL) {
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;
} else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) {
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO
} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO
} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
motor_thrust = 0; //immediately cut off thrust!
} else {
motor_thrust = 0; // Motor thrust must be zero in any other mode!
}
if (status->rc_signal_lost) motor_thrust = 0;
// Convertion to motor-step units
motor_thrust *= zcompensation;
/* scale up from 0..1 to 10..512) */
motor_thrust *= ((float)max_gas - min_gas);
//limit control output
//yawspeed
if (yaw > pid_yawspeed_lim) {
yaw = pid_yawspeed_lim;
yaw_speed_controller.saturated = 1;
}
if (yaw < -pid_yawspeed_lim) {
yaw = -pid_yawspeed_lim;
yaw_speed_controller.saturated = 1;
}
if (nick > pid_att_lim) {
nick = pid_att_lim;
nick_controller.saturated = 1;
}
if (nick < -pid_att_lim) {
nick = -pid_att_lim;
nick_controller.saturated = 1;
}
if (roll > pid_att_lim) {
roll = pid_att_lim;
roll_controller.saturated = 1;
}
if (roll < -pid_att_lim) {
roll = -pid_att_lim;
roll_controller.saturated = 1;
}
/* Emit controller values */
ar_control->setpoint_thrust_cast = motor_thrust;
ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x;
ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y;
ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z;
ar_control->attitude_control_output[0] = roll;
ar_control->attitude_control_output[1] = nick;
ar_control->attitude_control_output[2] = yaw;
ar_control->zcompensation = zcompensation;
orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control);
static float output_band = 0.f;
static float band_factor = 0.75f;
static float startpoint_full_control = 150.0f; //TODO
static float yaw_factor = 1.0f;
if (motor_thrust <= min_gas) {
motor_thrust = min_gas;
output_band = 0.f;
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) {
output_band = band_factor * (motor_thrust - min_gas);
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) {
output_band = band_factor * startpoint_full_control;
} else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) {
output_band = band_factor * (max_gas - motor_thrust);
}
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw);
// if we are not in the output band
if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
&& motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
&& motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
&& motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
yaw_factor = 0.5f;
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw * yaw_factor);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor);
}
uint8_t i;
for (i = 0; i < 4; i++) {
//check for limits
if (motor_calc[i] < motor_thrust - output_band) {
motor_calc[i] = motor_thrust - output_band;
}
if (motor_calc[i] > motor_thrust + output_band) {
motor_calc[i] = motor_thrust + output_band;
}
}
// Write out actual thrust
motor_pwm[0] = (uint16_t) motor_calc[0];
motor_pwm[1] = (uint16_t) motor_calc[1];
motor_pwm[2] = (uint16_t) motor_calc[2];
motor_pwm[3] = (uint16_t) motor_calc[3];
// Keep motors spinning while armed
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
//SEND MOTOR COMMANDS
if (motor_skip_counter % 5 == 0) {
uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
write(ardrone_write, motorSpeedBuf, 5);
}
motor_skip_counter++;
// now_time = hrt_absolute_time() / 1000000;
// if(now_time - last_time > 0)
// {
// printf("Counter: %ld\n",control_counter);
// last_time = now_time;
// control_counter = 0;
// }
}
//void control_attitude(float roll, float rollRef, float rollSpeed, float rollSpeedRef, float nick, float nickRef, float nickSpeed, float nickSpeedRef){}
inline void control_quadrotor_attitude()
{
float min_gas = 10;
float max_gas = 255;
//uint8_t stand_gas = 30;
uint8_t motor_pwm[4];
float motor_calc[4];
//float global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 1;
//float global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0;
float remote_control_weight_z = 1;
// float position_control_weight_z = 0;
//Calculate setpoints
// Control Yaw POSTION before mixing speed!!!
//TODO Check signs and -+180 degree
float yaw_e = global_data.attitude.z - global_data.yaw_pos_setpoint;
// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
if (yaw_e > 3.14)
{
yaw_e -= 2 * 3.14;
}
if (yaw_e < -3.14)
{
yaw_e += 2 * 3.14;
}
float yaw_pos_corr = pid_calculate(&yaw_pos_controller,
0, yaw_e, global_data.gyros_si.z,
CONTROL_PID_ATTITUDE_INTERVAL);
global_data.position_yaw_control_output = yaw_pos_corr;
global_data.attitude_setpoint_pos.z = yaw_pos_corr;
//limit control output
if (global_data.attitude_setpoint_pos.z
> global_data.param[PARAM_PID_YAWPOS_LIM])
{
global_data.attitude_setpoint_pos.z
= global_data.param[PARAM_PID_YAWPOS_LIM];
yaw_pos_controller.saturated=1;
}
if (global_data.attitude_setpoint_pos.z
< -global_data.param[PARAM_PID_YAWPOS_LIM])
{
global_data.attitude_setpoint_pos.z
= -global_data.param[PARAM_PID_YAWPOS_LIM];
yaw_pos_controller.saturated=1;
}
//transform attitude setpoint from positioncontroller from navi to body frame on xy_plane
navi2body_xy_plane(&global_data.attitude_setpoint_pos,
global_data.attitude.z, &global_data.attitude_setpoint_pos_body); //yaw angle= global_data.attitude.z
//now everything is in body frame
global_data.attitude_setpoint.x
= global_data.param[PARAM_MIX_REMOTE_WEIGHT]
* global_data.attitude_setpoint_remote.x
- global_data.param[PARAM_MIX_OFFSET_WEIGHT]
* global_data.param[PARAM_ATT_OFFSET_X]
+ global_data.param[PARAM_MIX_POSITION_WEIGHT]
* global_data.attitude_setpoint_pos_body.x;
global_data.attitude_setpoint.y
= global_data.param[PARAM_MIX_REMOTE_WEIGHT]
* global_data.attitude_setpoint_remote.y
- global_data.param[PARAM_MIX_OFFSET_WEIGHT]
* global_data.param[PARAM_ATT_OFFSET_Y]
+ global_data.param[PARAM_MIX_POSITION_WEIGHT]
* global_data.attitude_setpoint_pos_body.y;
global_data.attitude_setpoint.z = remote_control_weight_z
* global_data.attitude_setpoint_remote.z
- global_data.param[PARAM_MIX_OFFSET_WEIGHT]
* global_data.param[PARAM_ATT_OFFSET_Z]
+ global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT]
* global_data.attitude_setpoint_pos_body.z;
/*Calculate Controllers*/
// Control Yaw Speed
float yaw = pid_calculate(&yaw_speed_controller,
global_data.attitude_setpoint.z, global_data.gyros_si.z, 0,
CONTROL_PID_ATTITUDE_INTERVAL); //ATTENTION WE ARE CONTROLLING YAWspeed to YAW angle
//Control Nick
float nick = pid_calculate(&nick_controller,
global_data.attitude_setpoint.y, global_data.attitude.y,
global_data.gyros_si.y, CONTROL_PID_ATTITUDE_INTERVAL);
//Control Roll
float roll = pid_calculate(&roll_controller,
global_data.attitude_setpoint.x, global_data.attitude.x,
global_data.gyros_si.x, CONTROL_PID_ATTITUDE_INTERVAL);
//compensation to keep force in z-direction
float zcompensation;
if (fabs(global_data.attitude.x) > 0.5)
{
zcompensation = 1.13949393;
}
else
{
zcompensation = 1 / cos(global_data.attitude.x);
}
if (fabs(global_data.attitude.y) > 0.5)
{
zcompensation *= 1.13949393;
}
else
{
zcompensation *= 1 / cos(global_data.attitude.y);
}
// use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT]
// to compute thrust for Z position control
//
// float motor_thrust = min_gas +
// ( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation )
// + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation );
//calculate the basic thrust
//CALCULATE HOVER THRUST, THEN SWITCH TO Z-CONTROLLER
float motor_thrust = 0;
// GUIDED AND AUTO MODES
if (global_data.state.mav_mode == (uint8_t) MAV_MODE_GUIDED || global_data.state.mav_mode
== (uint8_t) MAV_MODE_AUTO)
{
motor_thrust = quadrotor_start_land_motor_thrust();
}
else if (global_data.state.mav_mode == (uint8_t) MAV_MODE_MANUAL)
{
motor_thrust = global_data.gas_remote;
}
else if (global_data.state.mav_mode == (uint8_t)MAV_MODE_LOCKED)
{
// LOCKED MODE
motor_thrust = 0;
}
else
{
// NO VALID MODE, LOCK DOWN TO ZERO
motor_thrust = 0;
// Message will flood buffer at 200 Hz, which is fine as it it critical
// debug_message_buffer("ERROR: NO VALID MODE, THRUST LIMITED TO 0%");
}
// OLD CODE from AMIR and ANDY
// // GUIDED AND AUTO MODES
// if (global_data.mode == (uint8_t)MAV_MODE_GUIDED || global_data.mode == (uint8_t)MAV_MODE_AUTO)
// {
//
// if (global_data.waiting_over == true)
// {
// if (global_data.ramp_up == true)
// {
// global_data.thrust_hover_offset += global_data.thrust_calibration_increment;
// // check limit:
// if (global_data.thrust_hover_offset > 0.65)
// {
// global_data.thrust_hover_offset = 0.65;
// global_data.ramp_up = false;
// }
// }
// motor_thrust = (global_data.thrust_hover_offset + global_data.position_control_output.z);
// global_data.motor_thrust_actual=motor_thrust;
//
// //Security: thrust never higher than remote control
// if (motor_thrust > global_data.gas_remote)
// {
// motor_thrust = global_data.gas_remote;
// }
// }
// }
// else if (global_data.mode == (uint8_t)MAV_MODE_MANUAL)
// {
//// if (global_data.waiting_over == true)
//// {
// // MANUAL MODE
// motor_thrust = global_data.gas_remote;
//// }
// }
// else if (global_data.mode == (uint8_t)MAV_MODE_LOCKED)
// {
// // LOCKED MODE
// motor_thrust = 0;
// }
// else
// {
// // NO VALID MODE, LOCK DOWN TO ZERO
// motor_thrust = 0;
// // Message will flood buffer at 200 Hz, which is fine as it it critical
// debug_message_buffer("ERROR: NO VALID MODE, THRUST LIMITED TO 0%");
// }
// Convertion to motor-step units
motor_thrust *= zcompensation;
motor_thrust *= (max_gas - min_gas);
motor_thrust += min_gas;
//limit control output
//yawspeed
if (yaw > global_data.param[PARAM_PID_YAWSPEED_LIM])
{
yaw = global_data.param[PARAM_PID_YAWSPEED_LIM];
yaw_speed_controller.saturated = 1;
}
if (yaw < -global_data.param[PARAM_PID_YAWSPEED_LIM])
{
yaw = -global_data.param[PARAM_PID_YAWSPEED_LIM];
yaw_speed_controller.saturated = 1;
}
if (nick > global_data.param[PARAM_PID_ATT_LIM])
{
nick = global_data.param[PARAM_PID_ATT_LIM];
nick_controller.saturated = 1;
}
if (nick < -global_data.param[PARAM_PID_ATT_LIM])
{
nick = -global_data.param[PARAM_PID_ATT_LIM];
nick_controller.saturated = 1;
}
if (roll > global_data.param[PARAM_PID_ATT_LIM])
{
roll = global_data.param[PARAM_PID_ATT_LIM];
roll_controller.saturated = 1;
}
if (roll < -global_data.param[PARAM_PID_ATT_LIM])
{
roll = -global_data.param[PARAM_PID_ATT_LIM];
roll_controller.saturated = 1;
}
// Emit motor values
global_data.attitude_control_output.x = motor_thrust + roll;
global_data.attitude_control_output.y = motor_thrust + nick;
global_data.attitude_control_output.z = yaw;
//add the yaw, nick and roll components to the basic thrust
// RIGHT (MOTOR 3)
motor_calc[0] = motor_thrust + yaw - roll;
// LEFT (MOTOR 4)
motor_calc[1] = motor_thrust + yaw + roll;
// FRONT (MOTOR 1)
motor_calc[2] = motor_thrust - yaw - nick;
// BACK (MOTOR 2)
motor_calc[3] = motor_thrust - yaw + nick;
uint8_t i;
for (i = 0; i < 4; i++)
{
//check for limits
if (motor_calc[i] < min_gas)
{
motor_calc[i] = min_gas;
}
if (motor_calc[i] > max_gas)
{
motor_calc[i] = max_gas;
}
}
//convert to byte
motor_pwm[0] = (uint8_t) motor_calc[0];
motor_pwm[1] = (uint8_t) motor_calc[1];
motor_pwm[2] = (uint8_t) motor_calc[2];
motor_pwm[3] = (uint8_t) motor_calc[3];
//Disable for testing without motors
if ((global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
== MAV_MODE_GUIDED) && (global_data.state.status == MAV_STATE_ACTIVE
|| global_data.state.status == MAV_STATE_CRITICAL || global_data.state.status
== MAV_STATE_EMERGENCY))
{
// Set MOTORS
motor_i2c_set_pwm(MOT1_I2C_SLAVE_ADDRESS, motor_pwm[0]);
motor_i2c_set_pwm(MOT2_I2C_SLAVE_ADDRESS, motor_pwm[1]);
motor_i2c_set_pwm(MOT3_I2C_SLAVE_ADDRESS, motor_pwm[2]);
motor_i2c_set_pwm(MOT4_I2C_SLAVE_ADDRESS, motor_pwm[3]);
}
else
{
if (global_data.state.mav_mode == MAV_MODE_TEST3
&& global_data.param[PARAM_SW_VERSION] >= 0
&& global_data.param[PARAM_SW_VERSION] <= 255)
{
//Testing motor trust with qgroundcontrol. PARAM_SW_VERSION is the thrust
// motor_i2c_set_pwm(MOT1_I2C_SLAVE_ADDRESS,
// global_data.param[PARAM_SW_VERSION]);
// motor_i2c_set_pwm(MOT2_I2C_SLAVE_ADDRESS,
// global_data.param[PARAM_SW_VERSION]);
// motor_i2c_set_pwm(MOT3_I2C_SLAVE_ADDRESS,
// global_data.param[PARAM_SW_VERSION]);
motor_i2c_set_pwm(MOT4_I2C_SLAVE_ADDRESS,
global_data.param[PARAM_SW_VERSION]);
}
else
{
// Set MOTORS to standby thrust
motor_i2c_set_pwm(MOT1_I2C_SLAVE_ADDRESS, 0);
motor_i2c_set_pwm(MOT2_I2C_SLAVE_ADDRESS, 0);
motor_i2c_set_pwm(MOT3_I2C_SLAVE_ADDRESS, 0);
motor_i2c_set_pwm(MOT4_I2C_SLAVE_ADDRESS, 0);
}
}
//DEBUGGING
if (controller_counter++ == 31)
{
controller_counter = 0;
//run every 8th time
if (global_data.param[PARAM_SEND_SLOT_DEBUG_3] == 1)
{
// debug_vect("att_sp", global_data.attitude_setpoint);
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 11, global_data.attitude_setpoint.x);
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 12, global_data.attitude_setpoint.y);
//
// // mavlink_msg_debug_send(MAVLINK_COMM_1, 6, gas_remote);
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 7, yaw);
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 8, nick);
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 9, roll);
// // mavlink_msg_debug_send(MAVLINK_COMM_1, 10, ppm_get_channel(global_data.param[PARAM_PPM_GAS_CHANNEL]));
//
// //
// // message_debug_send(MAVLINK_COMM_1, 29, global_data.gyros_si.x);
// // message_debug_send(MAVLINK_COMM_1, 28, global_data.gyros_si.y);
// // message_debug_send(MAVLINK_COMM_1, 27, global_data.gyros_si.z);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 16,
motor_pwm[0]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 17,
motor_pwm[1]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 18,
motor_pwm[2]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 19,
motor_pwm[3]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 20,
motor_thrust);
}
}
}
