void guidance_v_mode_changed(uint8_t new_mode) {
if (new_mode == guidance_v_mode)
return;
switch (new_mode) {
case GUIDANCE_V_MODE_HOVER:
guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
break;
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
guidance_v_zd_sp = 0;
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
default:
break;
}
guidance_v_mode = new_mode;
}
void guidance_v_mode_changed(uint8_t new_mode)
{
if (new_mode == guidance_v_mode) {
return;
}
switch (new_mode) {
case GUIDANCE_V_MODE_GUIDED:
case GUIDANCE_V_MODE_HOVER:
/* disable vertical velocity setpoints */
guidance_v_guided_vel_enabled = false;
/* set current altitude as setpoint */
guidance_v_z_sp = stateGetPositionNed_i()->z;
/* reset guidance reference */
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
/* reset speed setting */
guidance_v_zd_sp = 0;
break;
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
guidance_v_zd_sp = 0;
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_enter();
break;
#endif
case GUIDANCE_V_MODE_FLIP:
break;
default:
break;
}
guidance_v_mode = new_mode;
}
static void send_tune_vert(void) {
DOWNLINK_SEND_TUNE_VERT(DefaultChannel, DefaultDevice,
&guidance_v_z_sp,
&(stateGetPositionNed_i()->z),
&guidance_v_z_ref,
&guidance_v_zd_ref);
}
static void send_gh(struct transport_tx *trans, struct link_device *dev)
{
struct NedCoor_i *pos = stateGetPositionNed_i();
pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID,
&guidance_h.sp.pos.x, &guidance_h.sp.pos.y,
&guidance_h.ref.pos.x, &guidance_h.ref.pos.y,
&(pos->x), &(pos->y));
}
static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_TUNE_VERT(trans, dev, AC_ID,
&guidance_v_z_sp,
&(stateGetPositionNed_i()->z),
&guidance_v_z_ref,
&guidance_v_zd_ref);
}
static inline void reset_guidance_reference_from_current_position(void)
{
VECT2_COPY(guidance_h.ref.pos, *stateGetPositionNed_i());
VECT2_COPY(guidance_h.ref.speed, *stateGetSpeedNed_i());
INT_VECT2_ZERO(guidance_h.ref.accel);
gh_set_ref(guidance_h.ref.pos, guidance_h.ref.speed, guidance_h.ref.accel);
INT_VECT2_ZERO(guidance_h_trim_att_integrator);
}
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++;
}
}
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);
}
/** 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_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;
}
}
}
#define FF_CMD_FRAC 18
#define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight) {
/* convert our reference to generic representation */
int64_t tmp = gv_z_ref>>(GV_Z_REF_FRAC - INT32_POS_FRAC);
guidance_v_z_ref = (int32_t)tmp;
guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
/* compute the error to our reference */
int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z;
Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
int32_t err_zd = guidance_v_zd_ref - stateGetSpeedNed_i()->z;
Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
if (in_flight) {
guidance_v_z_sum_err += err_z;
Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
}
else
guidance_v_z_sum_err = 0;
/* our nominal command : (g + zdd)*m */
#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
const int32_t inv_m = BFP_OF_REAL(9.81/(guidance_v_nominal_throttle*MAX_PPRZ), FF_CMD_FRAC);
#else
