// Called on each vision analysis result after receiving the struct void run_avoid_navigation_onvision(void) { // Send ALL vision data to the ground DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 5, avoid_navigation_data.stereo_bin); switch (avoid_navigation_data.mode) { case 0: // Go to Goal and stop at obstacles //count 4 subsequent obstacles if (avoid_navigation_data.stereo_bin[0] > 1) { counter = counter + 1; if (counter > 1) { counter = 0; //Obstacle detected, go to turn until clear mode obstacle_detected = TRUE; avoid_navigation_data.mode = 1; } } else { counter = 0; } break; case 1: // Turn until clear //count 20 subsequent free frames if (avoid_navigation_data.stereo_bin[0] < 1) { counter = counter + 1; if (counter > 12) { counter = 0; //Stop and put waypoint 2.5 m ahead struct EnuCoor_i new_coor; struct EnuCoor_i *pos = stateGetPositionEnu_i(); float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.z = pos->z; waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y); obstacle_detected = FALSE; avoid_navigation_data.mode = 0; } } else { counter = 0; } break; case 2: break; default: // do nothing break; } avoid_navigation_data.stereo_bin[2] = avoid_navigation_data.stereo_bin[0] > 20; avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode; avoid_navigation_data.stereo_bin[4] = counter; #ifdef STEREO_LED if (obstacle_detected) { LED_ON(STEREO_LED); } else { LED_OFF(STEREO_LED); } #endif }
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); float heading_f; heading_f = ANGLE_FLOAT_OF_BFP(heading); quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f); }
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) { // use float conversion for now... struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); float heading_f = ANGLE_FLOAT_OF_BFP(heading); struct FloatQuat quat_f; quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f); // convert back to fixed point QUAT_BFP_OF_REAL(*quat, quat_f); }
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); /* Rotate horizontal commands to body frame by psi */ float psi = stateGetNedToBodyEulers_f()->psi; float s_psi = sinf(psi); float c_psi = cosf(psi); stab_att_sp_euler.phi = -s_psi * cmd_f.x + c_psi * cmd_f.y; stab_att_sp_euler.theta = -c_psi * cmd_f.x - s_psi * cmd_f.y; stab_att_sp_euler.psi = ANGLE_FLOAT_OF_BFP(heading); }
void ArduIMU_event(void) { // Handle INS I2C event if (ardu_ins_trans.status == I2CTransSuccess) { // received data from I2C transaction recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0]; recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2]; recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4]; recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6]; recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8]; recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10]; recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12]; recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14]; recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16]; // Update ArduIMU data arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral; arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral; arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]); arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]); arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]); arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]); arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]); arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]); arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]); // Update estimator stateSetNedToBodyEulers_f(&arduimu_eulers); stateSetBodyRates_f(&arduimu_rates); stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel)); ardu_ins_trans.status = I2CTransDone; #ifdef ARDUIMU_SYNC_SEND // uint8_t arduimu_id = 102; //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id)); RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); #endif } else if (ardu_ins_trans.status == I2CTransFailed) { ardu_ins_trans.status = I2CTransDone; } // Handle GPS I2C event if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) { ardu_gps_trans.status = I2CTransDone; } }
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 }
void ArduIMU_event( void ) { // Handle INS I2C event if (ardu_ins_trans.status == I2CTransSuccess) { // received data from I2C transaction recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0]; recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2]; recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4]; recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6]; recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8]; recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10]; recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12]; recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14]; recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16]; // Update ArduIMU data arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]); arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]); arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]); arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]); arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]); arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]); arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]); arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]); arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]); // Update estimator estimator_phi = arduimu_eulers.phi - ins_roll_neutral; estimator_theta = arduimu_eulers.theta - ins_pitch_neutral; estimator_p = arduimu_rates.p; ardu_ins_trans.status = I2CTransDone; //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi)); RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); } else if (ardu_ins_trans.status == I2CTransFailed) { ardu_ins_trans.status = I2CTransDone; } // Handle GPS I2C event if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) { ardu_gps_trans.status = I2CTransDone; } }
uint8_t moveWaypointForwards(uint8_t waypoint, float distanceMeters) { struct EnuCoor_i new_coor; struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position // Calculate the sine and cosine of the heading the drone is keeping float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); // Now determine where to place the waypoint you want to go to new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (distanceMeters)); new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (distanceMeters)); new_coor.z = pos->z; // Keep the height the same // Set the waypoint to the calculated position waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y); return false; }
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight) { // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers #if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight); #else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); #endif struct FloatQuat q_rp_cmd; #if USE_EARTH_BOUND_RC_SETPOINT stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); #else stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd); #endif /* get current heading */ const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_yaw; //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. FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, care_free_heading); } else { FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi); } /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd); FLOAT_QUAT_NORMALIZE(q_rp_sp); if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); #else FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi); #endif /* rotation between current yaw and yaw setpoint */ struct FloatQuat q_yaw_diff; FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw); /* compute final setpoint with yaw */ FLOAT_QUAT_COMP_NORM_SHORTEST(*q_sp, q_rp_sp, q_yaw_diff); } else { QUAT_COPY(*q_sp, q_rp_sp); } }
/** Read attitude setpoint from RC as quaternion * Interprets the stick positions as axes. * @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] q_sp attitude setpoint as quaternion */ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers #if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); #else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); #endif struct FloatQuat q_rp_cmd; stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd); /* get current heading */ const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_yaw; //Care Free mode if (in_carefree) { //care_free_heading has been set to current psi when entering care free mode. float_quat_of_axis_angle(&q_yaw, &zaxis, care_free_heading); } else { float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi); } /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd); float_quat_normalize(&q_rp_sp); if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); #else float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi); #endif /* rotation between current yaw and yaw setpoint */ struct FloatQuat q_yaw_diff; float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw); /* compute final setpoint with yaw */ float_quat_comp_norm_shortest(q_sp, &q_rp_sp, &q_yaw_diff); } else { QUAT_COPY(*q_sp, q_rp_sp); } }
/* 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; }
/** Read roll/pitch command from RC as quaternion. * Both angles are are interpreted relative to to the horizontal plane (earth bound). * @param[out] q quaternion representing the RC roll/pitch input */ void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q) { /* only non-zero entries for roll quaternion */ float roll2 = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; float qx_roll = sinf(roll2); float qi_roll = cosf(roll2); //An offset is added if in forward mode /* only non-zero entries for pitch quaternion */ float pitch2 = (ANGLE_FLOAT_OF_BFP(transition_theta_offset) + radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) / 2; float qy_pitch = sinf(pitch2); float qi_pitch = cosf(pitch2); /* only multiply non-zero entries of FLOAT_QUAT_COMP(*q, q_roll, q_pitch) */ q->qi = qi_roll * qi_pitch; q->qx = qx_roll * qi_pitch; q->qy = qi_roll * qy_pitch; q->qz = qx_roll * qy_pitch; }
/** Read roll/pitch command from RC as quaternion. * Both angles are are interpreted relative to to the horizontal plane (earth bound). * @param[out] q quaternion representing the RC roll/pitch input */ void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q) { /* only non-zero entries for roll quaternion */ float roll2 = get_rc_roll_f() / 2.0f; float qx_roll = sinf(roll2); float qi_roll = cosf(roll2); //An offset is added if in forward mode /* only non-zero entries for pitch quaternion */ float pitch2 = (ANGLE_FLOAT_OF_BFP(transition_theta_offset) + get_rc_pitch_f()) / 2.0f; float qy_pitch = sinf(pitch2); float qi_pitch = cosf(pitch2); /* only multiply non-zero entries of float_quat_comp(q, &q_roll, &q_pitch) */ q->qi = qi_roll * qi_pitch; q->qx = qx_roll * qi_pitch; q->qy = qi_roll * qy_pitch; q->qz = qx_roll * qy_pitch; }
//Function that reads the rc setpoint in an earth bound frame void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers #if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); #else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); #endif const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_rp_cmd; stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); #else float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi); #endif float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd); } else { struct FloatQuat q_yaw; float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi); /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd); float_quat_normalize(&q_rp_sp); QUAT_COPY(*q_sp, q_rp_sp); } }
//Function that reads the rc setpoint in an earth bound frame void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat* q_sp, bool_t in_flight) { // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers #if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight); #else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); #endif const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_rp_cmd; stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); #else FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi); #endif FLOAT_QUAT_COMP(*q_sp, q_yaw_sp, q_rp_cmd); } else { struct FloatQuat q_yaw; FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi); /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd); FLOAT_QUAT_NORMALIZE(q_rp_sp); QUAT_COPY(*q_sp, q_rp_sp); } }
void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading) { struct FloatQuat q_rp_cmd; float_quat_of_eulers(&q_rp_cmd, &guidance_euler_cmd); //TODO this is a quaternion without yaw! add the desired yaw before you use it! /* get current heading */ const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_yaw; float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi); /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd); float_quat_normalize(&q_rp_sp); struct FloatQuat 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(heading)); /* rotation between current yaw and yaw setpoint */ struct FloatQuat q_yaw_diff; float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw); /* compute final setpoint with yaw */ float_quat_comp_norm_shortest(&q_sp, &q_rp_sp, &q_yaw_diff); } else { QUAT_COPY(q_sp, q_rp_sp); } QUAT_BFP_OF_REAL(stab_att_sp_quat,q_sp); }
/** 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(); }
/** 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 stereocam_droplet_periodic(void) { static float heading = 0; // Read Serial while (StereoChAvailable()) { stereo_parse(StereoGetch()); } if (avoid_navigation_data.timeout <= 0) return; avoid_navigation_data.timeout --; // Results DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, avoid_navigation_data.stereo_bin); volatile bool_t once = TRUE; // Move waypoint with constant speed in current direction if ( (avoid_navigation_data.stereo_bin[0] == 97) || (avoid_navigation_data.stereo_bin[0] == 100) ) { once = TRUE; struct EnuCoor_f enu; enu.x = waypoint_get_x(WP_GOAL); enu.y = waypoint_get_y(WP_GOAL); enu.z = waypoint_get_alt(WP_GOAL); float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); enu.x += (sin_heading * 1.3 / 20); enu.y += (cos_heading * 1.3 / 20); waypoint_set_enu(WP_GOAL, &enu); } else if (avoid_navigation_data.stereo_bin[0] == 98) { // STOP!!! if (once) { NavSetWaypointHere(WP_GOAL); once = FALSE; } } else { once = TRUE; } switch (avoid_navigation_data.stereo_bin[0]) { case 99: // Turn heading += 4; if (heading > 360) { heading = 0; } nav_set_heading_rad(RadOfDeg(heading)); break; default: // do nothing break; } #ifdef STEREO_LED if (obstacle_detected) { LED_ON(STEREO_LED); } else { LED_OFF(STEREO_LED); } #endif }