void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co guidance_v_thrust_coeff = get_vertical_thrust_coeff(); if (in_flight) { int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref); } else {
static void save_shot_on_disk(struct image_t *img, struct image_t *img_jpeg) { // Search for a file where we can write to char save_name[128]; snprintf(save_name, sizeof(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); printf("Wrote image\n"); } #endif /** Log the values to a csv file */ if (video_usb_logger == NULL) { return; } static uint32_t counter = 0; struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts); struct NedCoor_i *ned = stateGetPositionNed_i(); struct NedCoor_i *accel = stateGetAccelNed_i(); static uint32_t sonar = 0; // Save current information to a file fprintf(video_usb_logger, "%d,%d,%f,%f,%f,%d,%d,%d,%d,%d,%d,%f,%f,%f,%d\n", counter, shotNumber, pose.eulers.phi, pose.eulers.theta, pose.eulers.psi, ned->x, ned->y, ned->z, accel->x, accel->y, accel->z, pose.rates.p, pose.rates.q, pose.rates.r, sonar); counter++; } }
inline static void h_ctl_cl_loop(void) { #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR #if (!defined SITL || defined USE_NPS) struct Int32Vect3 accel_meas_body, accel_ned; struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i(); struct NedCoor_i *accel_tmp = stateGetAccelNed_i(); VECT3_COPY(accel_ned, (*accel_tmp)); accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f); int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned); float nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f; // max load factor to be taken into acount // to prevent negative flap movement du to negative acceleration Bound(nz, 1.f, 2.f); #else float nz = 1.f; #endif #endif // Compute a corrected airspeed corresponding to the current load factor nz // with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V, // the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2, // thus Vn = V / sqrt(nz) #if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT float corrected_airspeed = v_ctl_auto_airspeed_setpoint; #else float corrected_airspeed = stateGetAirspeed_f(); #endif #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR corrected_airspeed /= sqrtf(nz); #endif Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED); float cmd = 0.f; // deadband around NOMINAL_AIRSPEED, rest linear if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) { cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED); } else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) { cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED); } // no control in manual mode if (pprz_mode == PPRZ_MODE_MANUAL) { cmd = 0.f; } // bound max flap angle Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL); // from percent to pprz cmd = cmd * MAX_PPRZ; h_ctl_flaps_setpoint = TRIM_PPRZ(cmd); }
inline static void h_ctl_yaw_loop(void) { #if H_CTL_YAW_TRIM_NY // Actual Acceleration from IMU: #if (!defined SITL || defined USE_NPS) struct Int32Vect3 accel_meas_body, accel_ned; struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i(); struct NedCoor_i *accel_tmp = stateGetAccelNed_i(); VECT3_COPY(accel_ned, (*accel_tmp)); accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f); int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned); float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g) #else float ny = 0.f; #endif if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_yaw_ny_sum_err = 0.; } else { if (h_ctl_yaw_ny_igain > 0.) { // only update when: phi<60degrees and ny<2g if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) { h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT; // max half rudder deflection for trim BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain)); } } else { h_ctl_yaw_ny_sum_err = 0.; } } #endif #ifdef USE_AIRSPEED float Vo = stateGetAirspeed_f(); Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED); #else float Vo = NOMINAL_AIRSPEED; #endif h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC + 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r; float cmd = + h_ctl_yaw_dgain * d_err #if H_CTL_YAW_TRIM_NY + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err #endif ; cmd /= airspeed_ratio2; h_ctl_rudder_setpoint = TRIM_PPRZ(cmd); }
static void send_vert_loop(void) { DOWNLINK_SEND_VERT_LOOP(DefaultChannel, DefaultDevice, &guidance_v_z_sp, &guidance_v_zd_sp, &(stateGetPositionNed_i()->z), &(stateGetSpeedNed_i()->z), &(stateGetAccelNed_i()->z), &guidance_v_z_ref, &guidance_v_zd_ref, &guidance_v_zdd_ref, &gv_adapt_X, &gv_adapt_P, &gv_adapt_Xmeas, &guidance_v_z_sum_err, &guidance_v_ff_cmd, &guidance_v_fb_cmd, &guidance_v_delta_t); }
static void send_vert_loop(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_VERT_LOOP(trans, dev, AC_ID, &guidance_v_z_sp, &guidance_v_zd_sp, &(stateGetPositionNed_i()->z), &(stateGetSpeedNed_i()->z), &(stateGetAccelNed_i()->z), &guidance_v_z_ref, &guidance_v_zd_ref, &guidance_v_zdd_ref, &gv_adapt_X, &gv_adapt_P, &gv_adapt_Xmeas, &guidance_v_z_sum_err, &guidance_v_ff_cmd, &guidance_v_fb_cmd, &guidance_v_delta_t); }
void guidance_v_run(bool in_flight) { guidance_v_thrust_coeff = get_vertical_thrust_coeff(); if (in_flight) { /* Only run adaptive throttle estimation if we are in flight and * the desired vertical velocity (zd) was updated (i.e. we ran hover_loop before). * This means that the estimation is not updated when using direct throttle commands. * * FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co */ if (desired_zd_updated) { int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref); } } else {
static void send_hover_loop(struct transport_tx *trans, struct link_device *dev) { struct NedCoor_i *pos = stateGetPositionNed_i(); struct NedCoor_i *speed = stateGetSpeedNed_i(); struct NedCoor_i *accel = stateGetAccelNed_i(); pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID, &guidance_h.sp.pos.x, &guidance_h.sp.pos.y, &(pos->x), &(pos->y), &(speed->x), &(speed->y), &(accel->x), &(accel->y), &guidance_h_pos_err.x, &guidance_h_pos_err.y, &guidance_h_speed_err.x, &guidance_h_speed_err.y, &guidance_h_trim_att_integrator.x, &guidance_h_trim_att_integrator.y, &guidance_h_cmd_earth.x, &guidance_h_cmd_earth.y, &guidance_h.sp.heading); }
/** * Propagate the received states into the vehicle * state machine */ void ins_vectornav_propagate() { // Acceleration [m/s^2] // in fixed point for sending as ABI and telemetry msgs ACCELS_BFP_OF_REAL(ins_vn.accel_i, ins_vn.accel); // Rates [rad/s] static struct FloatRates body_rate; // in fixed point for sending as ABI and telemetry msgs RATES_BFP_OF_REAL(ins_vn.gyro_i, ins_vn.gyro); float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.gyro); // compute body rates stateSetBodyRates_f(&body_rate); // Set state [rad/s] // Attitude [deg] ins_vectornav_yaw_pitch_roll_to_attitude(&ins_vn.attitude); // convert to correct units and axis [rad] static struct FloatQuat imu_quat; // convert from euler to quat float_quat_of_eulers(&imu_quat, &ins_vn.attitude); static struct FloatRMat imu_rmat; // convert from quat to rmat float_rmat_of_quat(&imu_rmat, &imu_quat); static struct FloatRMat ltp_to_body_rmat; // rotate to body frame float_rmat_comp(<p_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu)); stateSetNedToBodyRMat_f(<p_to_body_rmat); // set body states [rad] // NED (LTP) velocity [m/s] // North east down (NED), also known as local tangent plane (LTP), // is a geographical coordinate system for representing state vectors that is commonly used in aviation. // It consists of three numbers: one represents the position along the northern axis, // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to // up in order to comply with the right-hand rule. // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity. // x = North // y = East // z = Down stateSetSpeedNed_f(&ins_vn.vel_ned); // set state // NED (LTP) acceleration [m/s^2] static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame float_rmat_transp_vmult(&accel_meas_ltp, orientationGetRMat_f(&ins_vn.body_to_imu), &(ins_vn.lin_accel)); static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z); stateSetAccelNed_f(<p_accel); // then set the states ins_vn.ltp_accel_f = ltp_accel; // LLA position [rad, rad, m] //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.pos_lla[0]); // ins_impl.pos_lla[0] = lat ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos); stateSetPositionLla_i(&gps.lla_pos); // ECEF position struct LtpDef_f def; ltp_def_from_lla_f(&def, &ins_vn.lla_pos); struct EcefCoor_f ecef_vel; ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned); ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel); // ECEF velocity gps.ecef_pos.x = stateGetPositionEcef_i()->x; gps.ecef_pos.y = stateGetPositionEcef_i()->y; gps.ecef_pos.z = stateGetPositionEcef_i()->z; #if GPS_USE_LATLONG // GPS UTM /* Computes from (lat, long) in the referenced UTM zone */ struct UtmCoor_f utm_f; utm_f.zone = nav_utm_zone0; /* convert to utm */ //utm_of_lla_f(&utm_f, &lla_f); utm_of_lla_f(&utm_f, &ins_vn.lla_pos); /* copy results of utm conversion */ gps.utm_pos.east = (int32_t)(utm_f.east * 100); gps.utm_pos.north = (int32_t)(utm_f.north * 100); gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000); gps.utm_pos.zone = (uint8_t)nav_utm_zone0; #endif // GPS Ground speed float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y); gps.gspeed = ((uint16_t)(speed * 100)); // GPS course gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x))); // Because we have not HMSL data from Vectornav, we are using LLA-Altitude // as a workaround gps.hmsl = (uint32_t)(gps.lla_pos.alt); // set position uncertainty ins_vectornav_set_pacc(); // set velocity uncertainty ins_vectornav_set_sacc(); // check GPS status gps.last_msg_time = sys_time.nb_sec; gps.last_msg_ticks = sys_time.nb_sec_rem; if (gps.fix == GPS_FIX_3D) { gps.last_3dfix_time = sys_time.nb_sec; gps.last_3dfix_ticks = sys_time.nb_sec_rem; } // read INS status ins_vectornav_check_status(); // update internal states for telemetry purposes // TODO: directly convert vectornav output instead of using state interface // to support multiple INS running at the same time ins_vn.ltp_pos_i = *stateGetPositionNed_i(); ins_vn.ltp_speed_i = *stateGetSpeedNed_i(); ins_vn.ltp_accel_i = *stateGetAccelNed_i(); // send ABI messages uint32_t now_ts = get_sys_time_usec(); AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps); AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.gyro_i); AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.accel_i); }
void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co if (in_flight) { gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref); } switch (guidance_v_mode) { case GUIDANCE_V_MODE_RC_DIRECT: guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; break; case GUIDANCE_V_MODE_RC_CLIMB: guidance_v_zd_sp = guidance_v_rc_zd_sp; gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; case GUIDANCE_V_MODE_CLIMB: #if USE_FMS if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB) { guidance_v_zd_sp = fms.input.v_sp.climb; } #endif gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); #if NO_RC_THRUST_LIMIT stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #else // saturate max authority with RC stick stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); #endif break; case GUIDANCE_V_MODE_HOVER: #if USE_FMS if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER) guidance_v_z_sp = fms.input.v_sp.height; #endif gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); #if NO_RC_THRUST_LIMIT stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #else // saturate max authority with RC stick stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); #endif break; case GUIDANCE_V_MODE_NAV: { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_CLIMB) { guidance_v_zd_sp = -nav_climb; gv_update_ref_from_zd_sp(guidance_v_zd_sp); nav_flight_altitude = -guidance_v_z_sp; run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_MANUAL) { guidance_v_z_sp = -nav_flight_altitude; // For display only guidance_v_delta_t = nav_throttle; } #if NO_RC_THRUST_LIMIT stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #else /* use rc limitation if available */ if (radio_control.status == RC_OK) stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); else stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #endif break; } default: break; } }