/* Command The Camera */
void dc_send_command(uint8_t cmd)
{
switch (cmd) {
case DC_SHOOT:
// Send Photo Position To Camera
dc_shot_msg.data.nr = dc_photo_nr + 1;
dc_shot_msg.data.lat = stateGetPositionLla_i()->lat;
dc_shot_msg.data.lon = stateGetPositionLla_i()->lon;
dc_shot_msg.data.alt = stateGetPositionLla_i()->alt;
dc_shot_msg.data.phi = stateGetNedToBodyEulers_i()->phi;
dc_shot_msg.data.theta = stateGetNedToBodyEulers_i()->theta;
dc_shot_msg.data.psi = stateGetNedToBodyEulers_i()->psi;
dc_shot_msg.data.vground = *stateGetHorizontalSpeedNorm_i();
dc_shot_msg.data.course = *stateGetHorizontalSpeedDir_i();
dc_shot_msg.data.groundalt = POS_BFP_OF_REAL(state.alt_agl_f);
MoraHeader(MORA_SHOOT, MORA_SHOOT_MSG_SIZE);
for (int i = 0; i < (MORA_SHOOT_MSG_SIZE); i++) {
MoraPutUint8(dc_shot_msg.bin[i]);
}
MoraTrailer();
dc_send_shot_position();
break;
case DC_TALLER:
break;
case DC_WIDER:
break;
case DC_ON:
break;
case DC_OFF:
break;
default:
break;
}
}
static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
&radio_control.values[RADIO_ROLL],
&radio_control.values[RADIO_PITCH],
&radio_control.values[RADIO_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_THRUST],
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi));
}
/** Read attitude setpoint from RC as euler angles.
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {
sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ);
sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi;
int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
}
void stabilization_attitude_enter(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
reset_psi_ref_from_body();
INT_EULERS_ZERO( stabilization_att_sum_err );
}
static void send_fp(void) {
int32_t carrot_up = -guidance_v_z_sp;
DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&guidance_h_pos_sp.y,
&guidance_h_pos_sp.x,
&carrot_up,
&guidance_h_heading_sp,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) {
/* Rotate horizontal commands to body frame by psi */
int32_t psi = stateGetNedToBodyEulers_i()->psi;
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.psi = heading;
}
static void send_fp(struct transport_tx *trans, struct link_device *dev)
{
int32_t carrot_up = -guidance_v_z_sp;
pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&guidance_h.sp.pos.y,
&guidance_h.sp.pos.x,
&carrot_up,
&guidance_h.sp.heading,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
}
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&ahrs_ice.ltp_to_imu_euler.phi,
&ahrs_ice.ltp_to_imu_euler.theta,
&ahrs_ice.ltp_to_imu_euler.psi,
&(eulers->phi),
&(eulers->theta),
&(eulers->psi));
}
/**
* Horizontal guidance mode enter resets the errors
* and starts the controller.
*/
void guidance_h_module_enter(void)
{
/* Reset the integrated errors */
opticflow_stab.err_vx_int = 0;
opticflow_stab.err_vy_int = 0;
/* Set rool/pitch to 0 degrees and psi to current heading */
opticflow_stab.cmd.phi = 0;
opticflow_stab.cmd.theta = 0;
opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi;
}
void stabilization_attitude_enter(void) {
#if !USE_SETPOINTS_WITH_TRANSITIONS
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
#endif
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
void rotorcraft_cam_periodic(void)
{
switch (rotorcraft_cam_mode) {
case ROTORCRAFT_CAM_MODE_NONE:
#if ROTORCRAFT_CAM_USE_TILT
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
#endif
#if ROTORCRAFT_CAM_USE_PAN
rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
#endif
break;
case ROTORCRAFT_CAM_MODE_MANUAL:
// nothing to do here, just apply tilt pwm at the end
break;
case ROTORCRAFT_CAM_MODE_HEADING:
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
#if ROTORCRAFT_CAM_USE_PAN
INT32_COURSE_NORMALIZE(rotorcraft_cam_pan);
nav_heading = rotorcraft_cam_pan;
#endif
break;
case ROTORCRAFT_CAM_MODE_WP:
#ifdef ROTORCRAFT_CAM_TRACK_WP
{
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
rotorcraft_cam_pan = int32_atan2(diff.x, diff.y);
nav_heading = rotorcraft_cam_pan;
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
int32_t dist, height;
dist = INT32_VECT2_NORM(diff);
height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
rotorcraft_cam_tilt = int32_atan2(height, dist);
Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
}
#endif
break;
default:
break;
}
#if ROTORCRAFT_CAM_USE_TILT
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
#endif
}
static void send_att(void) {
struct FloatEulers ltp_to_imu_euler;
FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
&euler_i.phi,
&euler_i.theta,
&euler_i.psi,
&(eulers_body->phi),
&(eulers_body->theta),
&(eulers_body->psi));
}
static void send_att(struct transport_tx *trans, struct link_device *dev) {
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&euler_i.phi,
&euler_i.theta,
&euler_i.psi,
&(eulers_body->phi),
&(eulers_body->theta),
&(eulers_body->psi));
}
/**
* Horizontal guidance mode enter resets the errors
* and starts the controller.
*/
void guidance_h_module_enter(void)
{
/* Reset the integrated errors */
opticflow_stab.err_vx_int = 0;
opticflow_stab.err_vy_int = 0;
/* Set rool/pitch to 0 degrees and psi to current heading */
opticflow_stab.cmd.phi = 0;
opticflow_stab.cmd.theta = 0;
opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi;
new_heading = 0;
// register_periodic_telemetry(DefaultPeriodic, "INPUT_CONTROL", send_INPUT_CONTROL);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) {
// stab_att_sp_euler.psi still used in ref..
stab_att_sp_euler.psi = heading;
// compute sp_euler phi/theta for debugging/telemetry
/* Rotate horizontal commands to body frame by psi */
int32_t psi = stateGetNedToBodyEulers_i()->psi;
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
}
void save_shot(struct image_t *img, struct image_t *img_jpeg)
{
// Search for a file where we can write to
char save_name[128];
sprintf(save_name, "%s/img_%05d.jpg", foldername, shotNumber);
shotNumber++;
// Check if file exists or not
if (access(save_name, F_OK) == -1) {
// Create a high quality image (99% JPEG encoded)
jpeg_encode_image(img, img_jpeg, 99, TRUE);
#if VIDEO_USB_LOGGER_JPEG_WITH_EXIF_HEADER
write_exif_jpeg(save_name, img_jpeg->buf, img_jpeg->buf_size, img_jpeg->w, img_jpeg->h);
#else
FILE *fp = fopen(save_name, "w");
if (fp == NULL) {
printf("[video_thread-thread] Could not write shot %s.\n", save_name);
} else {
// Save it to the file and close it
fwrite(img_jpeg->buf, sizeof(uint8_t), img_jpeg->buf_size, fp);
fclose(fp);
}
#endif
/** Log the values to a csv file */
if (video_usb_logger == NULL) {
return;
}
static uint32_t counter = 0;
struct NedCoor_i *ned = stateGetPositionNed_i();
struct Int32Eulers *euler = stateGetNedToBodyEulers_i();
static uint32_t sonar = 0;
// Save current information to a file
fprintf(video_usb_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", counter,
shotNumber, euler->phi, euler->theta, euler->psi, ned->x,
ned->y, ned->z, sonar);
counter++;
}
}
void stabilization_attitude_read_rc(bool_t in_flight) {
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_read_rc_absolute(in_flight);
#else
//FIXME: remove me, do in quaternion directly
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
struct FloatQuat q_rp_cmd;
#if USE_EARTH_BOUND_RC_SETPOINT
stabilization_attitude_read_rc_roll_pitch_earth_quat(&q_rp_cmd);
#else
stabilization_attitude_read_rc_roll_pitch_quat(&q_rp_cmd);
#endif
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(stateGetNedToBodyEulers_i()->psi));
/* apply roll and pitch commands with respect to current heading */
struct FloatQuat q_sp;
FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp_cmd);
FLOAT_QUAT_NORMALIZE(q_sp);
if (in_flight)
{
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
/* rotation between current yaw and yaw setpoint */
struct FloatQuat q_yaw_diff;
FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);
/* temporary copy with roll/pitch command and current heading */
struct FloatQuat q_rp_sp;
QUAT_COPY(q_rp_sp, q_sp);
/* compute final setpoint with yaw */
FLOAT_QUAT_COMP_NORM_SHORTEST(q_sp, q_rp_sp, q_yaw_diff);
}
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
#endif
}
/* This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
However, when rolling more then 90 degrees in combination with pitch it switches. For a
transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
int32_t stabilization_attitude_get_heading_i(void) {
struct Int32Eulers* att = stateGetNedToBodyEulers_i();
int32_t heading;
if(abs(att->phi) < INT32_ANGLE_PI_2) {
int32_t sin_theta;
PPRZ_ITRIG_SIN(sin_theta, att->theta);
heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
}
else if(ANGLE_FLOAT_OF_BFP(att->theta) > 0)
heading = att->psi - att->phi;
else
heading = att->psi + att->phi;
return heading;
}
void stabilization_attitude_run(bool_t in_flight __attribute__ ((unused))) {
/* For roll and pitch we pass trough the desired angles as stabilization command */
const int32_t angle2cmd = (MAX_PPRZ/TRAJ_MAX_BANK);
stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd;
stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd;
//TODO: Fix yaw with PID controller
int32_t yaw_error = stateGetNedToBodyEulers_i()->psi - stab_att_sp_euler.psi;
INT32_ANGLE_NORMALIZE(yaw_error);
// stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI;
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
/** Log the values to a csv file */
void video_usb_logger_periodic(void)
{
if (video_usb_logger == NULL) {
return;
}
static uint32_t counter = 0;
struct NedCoor_i *ned = stateGetPositionNed_i();
struct Int32Eulers *euler = stateGetNedToBodyEulers_i();
static uint32_t sonar = 0;
// Take a new shot
viewvideo_take_shot(TRUE);
// Save to the file
fprintf(video_usb_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", counter,
viewvideo.shot_number, euler->phi, euler->theta, euler->psi, ned->x,
ned->y, ned->z, sonar);
counter++;
}
static void send_att(void) { //FIXME really use this message here ?
struct Int32Rates* body_rate = stateGetBodyRates_i();
struct Int32Eulers* att = stateGetNedToBodyEulers_i();
DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW]);
}
static void send_att(struct transport_tx *trans, struct link_device *dev) //FIXME really use this message here ?
{
struct Int32Rates *body_rate = stateGetBodyRates_i();
struct Int32Eulers *att = stateGetNedToBodyEulers_i();
pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err_quat.qx,
&stabilization_att_sum_err_quat.qy,
&stabilization_att_sum_err_quat.qz,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW]);
}
/**
* Update the controls based on a vision result
* @param[in] *result The opticflow calculation result used for control
*/
void OA_update()
{
float v_x = 0;
float v_y = 0;
if (opti_speed_flag == 1) {
//rotation_vector.psi = stateGetNedToBodyEulers_f()->psi;
//opti_speed = stateGetSpeedNed_f();
//Transform to body frame.
//float_rmat_of_eulers_312(&T, &rotation_vector)Attractforce_goal_send;
//MAT33_VECT3_MUL(*opti_speed, T, *opti_speed);
// Calculate the speed in body frame
struct FloatVect2 speed_cur;
float psi = stateGetNedToBodyEulers_f()->psi;
float s_psi = sin(psi);
float c_psi = cos(psi);
speed_cur.x = c_psi * stateGetSpeedNed_f()->x + s_psi * stateGetSpeedNed_f()->y;
speed_cur.y = -s_psi * stateGetSpeedNed_f()->x + c_psi * stateGetSpeedNed_f()->y;
opti_speed_read.x = speed_cur.x * 100;
opti_speed_read.y = speed_cur.y * 100;
//set result_vel
v_x = speed_cur.y * 100;
v_y = speed_cur.x * 100;
} else {
}
if (OA_method_flag == NO_OBSTACLE_AVOIDANCE) {
/* Calculate the error if we have enough flow */
opticflow_stab.desired_vx = 0;
opticflow_stab.desired_vy = 0;
err_vx = opticflow_stab.desired_vx - v_x;
err_vy = opticflow_stab.desired_vy - v_y;
/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
opticflow_stab.err_vy_int += err_vy / 100;
/* Calculate the commands */
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
+ opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
}
if (OA_method_flag == PINGPONG) {
opticflow_stab.cmd.phi = ANGLE_BFP_OF_REAL(ref_roll);
opticflow_stab.cmd.theta = ANGLE_BFP_OF_REAL(ref_pitch);
}
if (OA_method_flag == 2) {
Total_Kan_x = r_dot_new;
Total_Kan_y = speed_pot;
alpha_fil = 0.1;
yaw_rate = (int32_t)(alpha_fil * ANGLE_BFP_OF_REAL(r_dot_new));
opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi + yaw_rate;
INT32_ANGLE_NORMALIZE(opticflow_stab.cmd.psi);
opticflow_stab.desired_vx = 0;
opticflow_stab.desired_vy = speed_pot;
/* Calculate the error if we have enough flow */
err_vx = opticflow_stab.desired_vx - v_x;
err_vy = opticflow_stab.desired_vy - v_y;
/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
opticflow_stab.err_vy_int += err_vy / 100;
/* Calculate the commands */
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
+ opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
}
if (OA_method_flag == POT_HEADING) {
new_heading = ref_pitch;
opticflow_stab.desired_vx = sin(new_heading) * speed_pot * 100;
opticflow_stab.desired_vy = cos(new_heading) * speed_pot * 100;
/* Calculate the error if we have enough flow */
err_vx = opticflow_stab.desired_vx - v_x;
err_vy = opticflow_stab.desired_vy - v_y;
/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
opticflow_stab.err_vy_int += err_vy / 100;
/* Calculate the commands */
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
+ opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT)
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ);
sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ);
if (in_flight) {
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
if(abs(sp->phi) < max_phi)
omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
else //max 60 degrees roll, then take constant omega
omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)));
sp->psi += omega/RC_UPDATE_FREQ;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = heading + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = heading - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
}
void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.phi = 0;
stab_att_sp_euler.theta = 0;
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{
/* last time this function was called, used to calculate yaw setpoint update */
static float last_ts = 0.f;
sp->phi = get_rc_roll();
sp->theta = get_rc_pitch();
if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
/* make sure nothing drastically weird happens, bound dt to 0.5sec */
Bound(dt, 0, 0.5);
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += get_rc_yaw() * dt;
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
if (abs(sp->phi) < max_phi) {
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
} else { //max 60 degrees roll
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
}
sp->psi += omega * dt;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit) {
sp->psi = heading + delta_limit;
} else if (delta_psi < -delta_limit) {
sp->psi = heading - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
/* update timestamp for dt calculation */
last_ts = get_sys_time_float();
}
void guidance_flip_run(void)
{
uint32_t timer;
int32_t phi;
static uint32_t timer_save = 0;
timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY;
phi = stateGetNedToBodyEulers_i()->phi;
switch (flip_state) {
case 0:
flip_cmd_earth.x = 0;
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
timer_save = 0;
if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
flip_state++;
}
break;
case 1:
stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
flip_state++;
}
break;
case 2:
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
timer_save = timer;
flip_state++;
}
break;
case 3:
flip_cmd_earth.x = 0;
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
flip_state++;
}
break;
default:
autopilot_mode_auto2 = autopilot_mode_old;
autopilot_set_mode(autopilot_mode_old);
stab_att_sp_euler.psi = heading_save;
flip_rollout = false;
flip_counter = 0;
timer_save = 0;
flip_state = 0;
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
break;
}
}
void stabilization_attitude_run(bool_t in_flight) {
/* update reference */
stabilization_attitude_ref_update();
/* compute feedforward command */
stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(stabilization_gains.dd.x * stab_att_ref_accel.p, 5);
stabilization_att_ff_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND(stabilization_gains.dd.y * stab_att_ref_accel.q, 5);
stabilization_att_ff_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND(stabilization_gains.dd.z * stab_att_ref_accel.r, 5);
/* compute feedback command */
/* attitude error */
const struct Int32Eulers att_ref_scaled = {
OFFSET_AND_ROUND(stab_att_ref_euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC))
};
struct Int32Eulers att_err;
struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i();
EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
INT32_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
/* update integrator */
EULERS_ADD(stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
INT_EULERS_ZERO(stabilization_att_sum_err);
}
/* rate error */
const struct Int32Rates rate_ref_scaled = {
OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
};
struct Int32Rates rate_err;
struct Int32Rates* body_rate = stateGetBodyRates_i();
RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
/* PID */
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
OFFSET_AND_ROUND2((stabilization_gains.i.x * stabilization_att_sum_err.phi), 10);
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
OFFSET_AND_ROUND2((stabilization_gains.i.y * stabilization_att_sum_err.theta), 10);
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
OFFSET_AND_ROUND2((stabilization_gains.i.z * stabilization_att_sum_err.psi), 10);
/* with P gain of 100, att_err of 180deg (3.14 rad)
* fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628
* max possible command is 9600
*/
#define CMD_SHIFT 11
/* sum feedforward and feedback */
stabilization_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), CMD_SHIFT);
stabilization_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), CMD_SHIFT);
stabilization_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT);
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}