int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators)
{
static int counter = 0;
static bool initialized = false;
static struct fw_rate_control_params p;
static struct fw_rate_control_param_handles h;
static PID_t roll_rate_controller;
static PID_t pitch_rate_controller;
static PID_t yaw_rate_controller;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if(!initialized)
{
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
}
/* Roll Rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer...
actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now
counter++;
return 0;
}
/*
* Output functions
*
*/
void PID_lacet_Outputs_wrapper(const real_T *t,
const real_T *kp,
const real_T *ki,
const real_T *kd,
const real_T *rc,
const real_T *rmes,
real_T *com)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* This sample sets the output equal to the input
y0[0] = u0[0];
For complex signals use: y0[0].re = u0[0].re;
y0[0].im = u0[0].im;
y1[0].re = u1[0].re;
y1[0].im = u1[0].im;
*/
if (t[0]<=0.01){
t_av=0;
mon_pid = &mon_pid_t;
pid_init(mon_pid, kp[0], ki[0], kd[0], 1000, 1000, PID_MODE_DERIVATIV_SET, 0.00001);
pid_reset_integral(mon_pid);
pid_set_parameters(mon_pid, kp[0], ki[0], kd[0], 1000, 1000);
//pid_init(mon_pid, 6.8, 0, 0, 1000, 1000, PID_MODE_DERIVATIV_SET, 0.1);
//pid_set_parameters(mon_pid, 6.56, 0, 0, 4096, 4096);
toto++;
}
//com[0]=thetames[0];
dt=t[0]-t_av;
t_av=t[0];
com[0]=pid_calculate(mon_pid, rc[0] ,rmes[0], 0, dt);
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
struct vehicle_rates_setpoint_s *rates_sp)
{
static int counter = 0;
static bool initialized = false;
static struct fw_att_control_params p;
static struct fw_pos_control_param_handles h;
static PID_t roll_controller;
static PID_t pitch_controller;
if(!initialized)
{
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0);
/* Pitch (P) */
float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan;
rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
//TODO
counter++;
return 0;
}
/**
* @brief Init Wall follow controller
* @param pid param
*/
void pid_Wallfollow_init(PID_PARAMETERS pid_param)
{
pid_init();
pid_set_parameters(pid_param);
// if (pid_TimerID != 0xff)
// {
// TIMER_UnregisterEvent(pid_TimerID);
// }
pid_Runtimeout(&Pid_process_callback, pid_param.Ts);
ui32_msLoop = pid_param.Ts;
}
/**
* @brief Update the controller gains
*
* Calling this function reads the controller gains from the global_data.param[]
* data structure. This structure can be written from the GCS and is read from
* EEPROM on startup.
*/
void update_controller_parameters(void)
{
// "Zero position drift" attitude output
// This the same as trimming an aircraft (the trim levers on a remote control)
// To compensate mechanical imprecision
// Using this parameter allows to exchange the remote control
// and leave the trim at zero
global_data.attitude_setpoint_offset.x
= global_data.param[PARAM_ATT_OFFSET_X];
global_data.attitude_setpoint_offset.y
= global_data.param[PARAM_ATT_OFFSET_Y];
global_data.attitude_setpoint_offset.z
= global_data.param[PARAM_ATT_OFFSET_Z];
/// ATTITUDE PID PARAMETERS
pid_set_parameters(&nick_controller, global_data.param[PARAM_PID_ATT_P],
global_data.param[PARAM_PID_ATT_I],
global_data.param[PARAM_PID_ATT_D],
global_data.param[PARAM_PID_ATT_AWU]);
pid_set_parameters(&roll_controller, global_data.param[PARAM_PID_ATT_P],
global_data.param[PARAM_PID_ATT_I],
global_data.param[PARAM_PID_ATT_D],
global_data.param[PARAM_PID_ATT_AWU]);
pid_set_parameters(&yaw_pos_controller,
global_data.param[PARAM_PID_YAWPOS_P],
global_data.param[PARAM_PID_YAWPOS_I],
global_data.param[PARAM_PID_YAWPOS_D],
global_data.param[PARAM_PID_YAWPOS_AWU]);
pid_set_parameters(&yaw_speed_controller,
global_data.param[PARAM_PID_YAWSPEED_P],
global_data.param[PARAM_PID_YAWSPEED_I],
global_data.param[PARAM_PID_YAWSPEED_D],
global_data.param[PARAM_PID_YAWSPEED_AWU]);
/// POSITION PID PARAMETERS
pid_set_parameters(&x_axis_controller, global_data.param[PARAM_PID_POS_P],
global_data.param[PARAM_PID_POS_I],
global_data.param[PARAM_PID_POS_D],
global_data.param[PARAM_PID_POS_AWU]);
pid_set_parameters(&y_axis_controller, global_data.param[PARAM_PID_POS_P],
global_data.param[PARAM_PID_POS_I],
global_data.param[PARAM_PID_POS_D],
global_data.param[PARAM_PID_POS_AWU]);
if (global_data.state.fly != FLY_RAMP_UP)
{
pid_set_parameters(&z_axis_controller,
global_data.param[PARAM_PID_POS_Z_P],
global_data.param[PARAM_PID_POS_Z_I],
global_data.param[PARAM_PID_POS_Z_D],
global_data.param[PARAM_PID_POS_Z_AWU]);
}
}
void
GroundRoverAttitudeControl::parameters_update()
{
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
param_get(_parameter_handles.w_d, &(_parameters.w_d));
param_get(_parameter_handles.w_imax, &(_parameters.w_imax));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
/* Steering pid parameters*/
pid_init(&_steering_ctrl, PID_MODE_DERIVATIV_SET, 0.01f);
pid_set_parameters(&_steering_ctrl,
_parameters.w_p,
_parameters.w_i,
_parameters.w_d,
_parameters.w_imax,
1.0f);
}
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
{
static PID_t distance_controller;
static int read_ret;
static global_data_position_t position_estimated;
static uint16_t counter;
static bool initialized;
static uint16_t pm_counter;
static float lat_next;
static float lon_next;
static float pitch_current;
static float thrust_total;
if (initialized == false) {
pid_init(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
PID_MODE_DERIVATIV_CALC, 150);//150
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
thrust_total = 0.0f;
/* Position initialization */
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
lat_next = position_estimated.lat;
lon_next = position_estimated.lon;
/* attitude initialization */
global_data_lock(&global_data_attitude->access_conf);
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
initialized = true;
}
/* load new parameters with 10Hz */
if (counter % 50 == 0) {
if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
/* check whether new parameters are available */
if (global_data_parameter_storage->counter > pm_counter) {
pid_set_parameters(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
//
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter;
printf("Position controller changed pid parameters\n");
}
}
global_data_unlock(&global_data_parameter_storage->access_conf);
}
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
/* Get next waypoint */ //TODO: add local copy
if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
lat_next = global_data_position_setpoint->x;
lon_next = global_data_position_setpoint->y;
global_data_unlock(&global_data_position_setpoint->access_conf);
}
/* Get distance to waypoint */
float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// if(counter % 5 == 0)
// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
/* Get bearing to waypoint (direction on earth surface to next waypoint) */
float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
if (counter % 5 == 0)
printf("bearing: %.4f\n", bearing);
/* Calculate speed in direction of bearing (needed for controller) */
float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// if(counter % 5 == 0)
// printf("speed_norm: %.4f\n", speed_norm);
float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
/* Control Thrust in bearing direction */
float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// if(counter % 5 == 0)
// printf("horizontal thrust: %.4f\n", horizontal_thrust);
/* Get total thrust (from remote for now) */
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
global_data_unlock(&global_data_rc_channels->access_conf);
}
const float max_gas = 500.0f;
thrust_total *= max_gas / 20000.0f; //TODO: check this
thrust_total += max_gas / 2.0f;
if (horizontal_thrust > thrust_total) {
horizontal_thrust = thrust_total;
} else if (horizontal_thrust < -thrust_total) {
horizontal_thrust = -thrust_total;
}
//TODO: maybe we want to add a speed controller later...
/* Calclulate thrust in east and north direction */
float thrust_north = cosf(bearing) * horizontal_thrust;
float thrust_east = sinf(bearing) * horizontal_thrust;
if (counter % 10 == 0) {
printf("thrust north: %.4f\n", thrust_north);
printf("thrust east: %.4f\n", thrust_east);
fflush(stdout);
}
/* Get current attitude */
if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
}
/* Get desired pitch & roll */
float pitch_desired = 0.0f;
float roll_desired = 0.0f;
if (thrust_total != 0) {
float pitch_fraction = -thrust_north / thrust_total;
float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
if (roll_fraction < -1) {
roll_fraction = -1;
} else if (roll_fraction > 1) {
roll_fraction = 1;
}
// if(counter % 5 == 0)
// {
// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// fflush(stdout);
// }
pitch_desired = asinf(pitch_fraction);
roll_desired = asinf(roll_fraction);
}
att_sp.roll = roll_desired;
att_sp.pitch = pitch_desired;
counter++;
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
last_run = hrt_absolute_time();
if (last_input != att_sp->timestamp) {
last_input = att_sp->timestamp;
}
static int sensor_delay;
sensor_delay = hrt_absolute_time() - att->timestamp;
static int motor_skip_counter = 0;
static PID_t pitch_controller;
static PID_t roll_controller;
static struct mc_att_control_params p;
static struct mc_att_control_param_handles h;
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
1000.0f, PID_MODE_DERIVATIV_SET);
initialized = true;
}
/* load new parameters with lower rate */
if (motor_skip_counter % 1000 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
//printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
/* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
/* calculate current control outputs */
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
att->pitch, att->pitchspeed, deltaT);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
rates_sp->thrust = att_sp->thrust;
motor_skip_counter++;
}
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 multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
static uint64_t last_input = 0;
if (last_input != rate_sp->timestamp) {
last_input = rate_sp->timestamp;
}
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
parameters_update(&h, &p);
initialized = true;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
// TODO pid_reset_integral(&yaw_rate_controller);
}
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
rates[0], 0.0f, deltaT);
/* control yaw rate */ //XXX use library here
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
/* increase resilience to faulty control inputs */
if (!isfinite(yaw_rate_control)) {
yaw_rate_control = 0.0f;
warnx("rej. NaN ctrl yaw");
}
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
}
void *multirotor_position_control_thread_main()
{
/* welcome user */
fprintf (stdout, "Multirotor position controller started\n");
fflush(stdout);
int i;
bool_t updated;
bool_t reset_mission_sp = 0 /* false */;
bool_t global_pos_sp_valid = 0 /* false */;
bool_t reset_man_sp_z = 1 /* true */;
bool_t reset_man_sp_xy = 1 /* true */;
bool_t reset_int_z = 1 /* true */;
bool_t reset_int_z_manual = 0 /* false */;
bool_t reset_int_xy = 1 /* true */;
bool_t was_armed = 0 /* false */;
bool_t reset_auto_sp_xy = 1 /* true */;
bool_t reset_auto_sp_z = 1 /* true */;
bool_t reset_takeoff_sp = 1 /* true */;
absolute_time t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float i_limit; /* use integral_limit_out = tilt_max / 2 */
float ref_alt = 0.0f;
absolute_time ref_alt_t = 0;
absolute_time local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
/* structures */
struct parameter_update_s ps;
struct vehicle_control_flags_s control_flags;
memset(&control_flags, 0, sizeof(control_flags));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
orb_subscr_t param_sub = orb_subscribe(ORB_ID(parameter_update));
if (param_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to parameter_update topic\n");
exit(-1);
}
orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags));
if (control_flags_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_control_flags topic\n");
exit(-1);
}
orb_subscr_t att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
if (att_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude topic\n");
exit(-1);
}
orb_subscr_t att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
if (att_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude_setpoint topic\n");
exit(-1);
}
orb_subscr_t manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
if (manual_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to manual_control_setpoint topic\n");
exit(-1);
}
orb_subscr_t local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
if (local_pos_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position_setpoint topic\n");
exit(-1);
}
orb_subscr_t local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
if (local_pos_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position topic\n");
exit(-1);
}
orb_subscr_t global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
if (global_pos_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_global_position_setpoint topic\n");
exit(-1);
}
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint));
if (local_pos_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_local_position_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint));
if (global_vel_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_global_velocity_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint));
if (att_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
/* abort on a nonzero return value from the parameter init */
if (multirotor_position_control_params_init() != 0) {
/* parameter setup went wrong, abort */
fprintf (stderr, "Multirotor position controller aborting on startup due to an error\n");
exit(-1);
}
for (i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f,
multirotor_position_control_parameters.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i,
multirotor_position_control_parameters.xy_vel_d, 1.0f, multirotor_position_control_parameters.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (!_shutdown_all_systems) {
updated = orb_check (ORB_ID(parameter_update), param_sub);
if (updated) {
/* clear updated flag */
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update multirotor_position_control_parameters */
multirotor_position_control_params_update();
for (i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p,
0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f);
if (multirotor_position_control_parameters.xy_vel_i > 0.0f) {
i_limit = multirotor_position_control_parameters.tilt_max / multirotor_position_control_parameters.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p,
multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, i_limit, multirotor_position_control_parameters.tilt_max);
}
pid_set_parameters(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min);
}
updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub);
if (updated) {
orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags);
}
updated = orb_check (ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = 1 /* true */;
reset_mission_sp = 1 /* true */;
}
absolute_time t = get_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
if (control_flags.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = 1 /* true */;
reset_man_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
reset_auto_sp_xy = 1 /* true */;
reset_takeoff_sp = 1 /* true */;
reset_int_z = 1 /* true */;
reset_int_xy = 1 /* true */;
}
was_armed = control_flags.flag_armed;
t_prev = t;
if (control_flags.flag_control_altitude_enabled || control_flags.flag_control_velocity_enabled || control_flags.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = multirotor_position_control_parameters.z_vel_max / multirotor_position_control_parameters.z_p * 2.0f;
float xy_sp_offs_max = multirotor_position_control_parameters.xy_vel_max / multirotor_position_control_parameters.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_flags.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_flags.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
//mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.thrust - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * multirotor_position_control_parameters.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_flags.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
//mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / multirotor_position_control_parameters.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / multirotor_position_control_parameters.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * multirotor_position_control_parameters.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_flags.flag_control_position_enabled;
reset_auto_sp_z = !control_flags.flag_control_altitude_enabled;
reset_takeoff_sp = 1 /* true */;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = 1 /* true */;
} else if (control_flags.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_flags.auto_state == navigation_state_auto_ready) {
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_takeoff) {
if (reset_takeoff_sp) {
reset_takeoff_sp = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - multirotor_position_control_parameters.takeoff_alt - multirotor_position_control_parameters.takeoff_gap;
att_sp.yaw_body = att.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
reset_auto_sp_xy = 0 /* false */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_rtl) {
// TODO
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_mission) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
reset_mission_sp = 1 /* true */;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
//mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (reset_mission_sp) {
reset_mission_sp = 0 /* false */;
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.latitude * 1e-7;
double sp_lon = global_pos_sp.longitude * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
att_sp.yaw_body = global_pos_sp.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {
reset_auto_sp_xy = 0 /* false */;
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
}
if (reset_auto_sp_z) {
reset_auto_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
}
//mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
}
if (control_flags.auto_state != navigation_state_auto_takeoff) {
reset_takeoff_sp = 1 /* true */;
}
if (control_flags.auto_state != navigation_state_auto_mission) {
reset_mission_sp = 1 /* true */;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = 1 /* true */;
reset_man_sp_z = 1 /* true */;
} else {
/* no control (failsafe), loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
//mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_man_sp_z = 1 /* true */;
} else {
/* in air: hold altitude */
if (reset_man_sp_z) {
reset_man_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
//mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
reset_auto_sp_z = 0 /* false */;
}
if (control_flags.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
reset_auto_sp_xy = 0 /* false */;
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_flags.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
} else {
reset_man_sp_z = 1 /* true */;
global_vel_sp.vz = 0.0f;
}
if (control_flags.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
/* limit horizontal speed */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / multirotor_position_control_parameters.xy_vel_max;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
} else {
reset_man_sp_xy = 1 /* true */;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
//fprintf (stderr, "global_vel_sp.vx:%.3f\tglobal_vel_sp.vy:%.3f\tglobal_vel_sp.vz:%.3f\n", global_vel_sp.vx, global_vel_sp.vy, global_vel_sp.vz);
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
if (control_flags.flag_control_climb_rate_enabled || control_flags.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
if (control_flags.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = 0 /* false */;
float i = multirotor_position_control_parameters.thr_min;
if (reset_int_z_manual) {
i = manual.thrust;
if (i < multirotor_position_control_parameters.thr_min) {
i = multirotor_position_control_parameters.thr_min;
} else if (i > multirotor_position_control_parameters.thr_max) {
i = multirotor_position_control_parameters.thr_max;
}
}
thrust_pid_set_integral(&z_vel_pid, -i);
//mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
} else {
/* reset thrust integral when altitude control enabled */
reset_int_z = 1 /* true */;
}
if (control_flags.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_int_xy) {
reset_int_xy = 0 /* false */;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
//mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
/* limit horizontal part of thrust */
float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > multirotor_position_control_parameters.tilt_max) {
tilt = multirotor_position_control_parameters.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
} else {
reset_int_xy = 1 /* true */;
}
att_sp.timestamp = get_absolute_time();
//fprintf (stderr, "att_sp.roll:%.3f\tatt_sp.pitch:%.3f\tatt_sp.yaw:%.3f\tatt_sp.thrust:%.3f\n", att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust);
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = 1 /* true */;
reset_man_sp_xy = 1 /* true */;
reset_int_z = 1 /* true */;
reset_int_xy = 1 /* true */;
reset_mission_sp = 1 /* true */;
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_flags.flag_armed && control_flags.flag_control_manual_enabled && !control_flags.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
/*
* do unsubscriptions
*/
orb_unsubscribe(ORB_ID(parameter_update), param_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_attitude), att_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, pthread_self());
orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_local_position), local_pos_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, pthread_self());
/*
* do unadvertises
*/
orb_unadvertise(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, pthread_self());
orb_unadvertise(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, pthread_self());
orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self());
return 0;
}
static int multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
warnx("started");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[mpc] started");
/* structures */
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
bool reset_mission_sp = false;
bool global_pos_sp_valid = false;
bool reset_man_sp_z = true;
bool reset_man_sp_xy = true;
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
bool reset_auto_sp_xy = true;
bool reset_auto_sp_z = true;
bool reset_takeoff_sp = true;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
thread_running = true;
struct multirotor_position_control_params params;
struct multirotor_position_control_param_handles params_h;
parameters_init(¶ms_h);
parameters_update(¶ms_h, ¶ms);
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (!thread_should_exit) {
bool param_updated;
orb_check(param_sub, ¶m_updated);
if (param_updated) {
/* clear updated flag */
struct parameter_update_s ps;
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update params */
parameters_update(¶ms_h, ¶ms);
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i > 0.0f) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
}
bool updated;
orb_check(control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(global_pos_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = true;
reset_mission_sp = true;
}
hrt_abstime t = hrt_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
if (control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_auto_sp_z = true;
reset_auto_sp_xy = true;
reset_takeoff_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = control_mode.flag_armed;
t_prev = t;
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_mode.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
reset_takeoff_sp = true;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = true;
} else if (control_mode.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (reset_takeoff_sp) {
reset_takeoff_sp = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
reset_auto_sp_xy = false;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
// TODO
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
reset_mission_sp = true;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (reset_mission_sp) {
reset_mission_sp = false;
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {
reset_auto_sp_xy = false;
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
}
if (reset_auto_sp_z) {
reset_auto_sp_z = false;
local_pos_sp.z = local_pos.z;
}
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
reset_takeoff_sp = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
reset_mission_sp = true;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = true;
reset_man_sp_z = true;
} else {
/* no control (failsafe), loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_man_sp_z = true;
} else {
/* in air: hold altitude */
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
reset_auto_sp_z = false;
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
reset_auto_sp_xy = false;
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
} else {
reset_man_sp_z = true;
global_vel_sp.vz = 0.0f;
}
if (control_mode.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
/* limit horizontal speed */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
} else {
reset_man_sp_xy = true;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = params.thr_min;
if (reset_int_z_manual) {
i = manual.throttle;
if (i < params.thr_min) {
i = params.thr_min;
} else if (i > params.thr_max) {
i = params.thr_max;
}
}
thrust_pid_set_integral(&z_vel_pid, -i);
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
} else {
/* reset thrust integral when altitude control enabled */
reset_int_z = true;
}
if (control_mode.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_int_xy) {
reset_int_xy = false;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
/* limit horizontal part of thrust */
float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > params.tilt_max) {
tilt = params.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
} else {
reset_int_xy = true;
}
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_int_z = true;
reset_int_xy = true;
reset_mission_sp = true;
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[mpc] stopped");
thread_running = false;
fflush(stdout);
return 0;
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
static uint64_t last_input = 0;
if (last_input != rate_sp->timestamp) {
last_input = rate_sp->timestamp;
}
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
static PID_t pitch_rate_controller;
static PID_t roll_rate_controller;
static PID_t yaw_rate_controller;
static struct mc_rate_control_params p;
static struct mc_rate_control_param_handles h;
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
parameters_init(&h);
parameters_update(&h, &p);
initialized = true;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
}
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
pid_reset_integral(&yaw_rate_controller);
}
/* run pitch, roll and yaw controllers */
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_control;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
}