Пример #1
0
static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
                                      get_sys_time_msec(),
                                      stateGetPositionNed_f()->x,
                                      stateGetPositionNed_f()->y,
                                      stateGetPositionNed_f()->z,
                                      stateGetSpeedNed_f()->x,
                                      stateGetSpeedNed_f()->y,
                                      stateGetSpeedNed_f()->z);
  MAVLinkSendMessage();
}
Пример #2
0
static inline void mavlink_send_local_position_ned(void)
{
  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
                                      get_sys_time_msec(),
                                      stateGetPositionNed_f()->x,
                                      stateGetPositionNed_f()->y,
                                      stateGetPositionNed_f()->z,
                                      stateGetSpeedNed_f()->x,
                                      stateGetSpeedNed_f()->y,
                                      stateGetSpeedNed_f()->z);
  MAVLinkSendMessage();
}
Пример #3
0
void autopilot_check_in_flight(bool motors_on)
{
  if (autopilot_in_flight) {
    if (autopilot_in_flight_counter > 0) {
      /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
      if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
          (fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
          (fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
        autopilot_in_flight_counter--;
        if (autopilot_in_flight_counter == 0) {
          autopilot_in_flight = false;
        }
      } else { /* thrust, speed or accel not above min threshold, reset counter */
        autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
      }
    }
  } else { /* currently not in flight */
    if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
        motors_on) {
      /* if thrust above min threshold, assume in_flight.
       * Don't check for velocity and acceleration above threshold here...
       */
      if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
        autopilot_in_flight_counter++;
        if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) {
          autopilot_in_flight = true;
        }
      } else { /* currently not in_flight and thrust below threshold, reset counter */
        autopilot_in_flight_counter = 0;
      }
    }
  }
}
Пример #4
0
void ins_update_baro() {
#if USE_BAROMETER
  // TODO update kalman filter with baro struct
  if (baro.status == BS_RUNNING) {
    if (!ins_baro_initialised) {
      ins_qfe = baro.absolute;
      ins_baro_initialised = TRUE;
    }
    if (ins.vf_realign) {
      ins.vf_realign = FALSE;
      ins_qfe = baro.absolute;
    }
    else { /* not realigning, so normal update with baro measurement */
      /* altitude decreases with increasing baro.absolute pressure */
      ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS;
      /* run the filter */
      EstimatorSetAlt(ins_baro_alt);
      /* set new altitude, just copy old horizontal position */
      struct UtmCoor_f utm;
      UTM_COPY(utm, *stateGetPositionUtm_f());
      utm.alt = ins_alt;
      stateSetPositionUtm_f(&utm);
      struct NedCoor_f ned_vel;
      memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f));
      ned_vel.z = -ins_alt_dot;
      stateSetSpeedNed_f(&ned_vel);
    }
  }
#endif
}
Пример #5
0
void guidance_indi_run(bool in_flight, int32_t heading) {

  //filter accel to get rid of noise
  //filter attitude to synchronize with accel
  guidance_indi_filter_attitude();
  guidance_indi_filter_accel();

  float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x; //+-ov.y/ (STABILIZATION_ATTITUDE_SP_MAX_THETA)*3.0;
  float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y; //+ ov.x/ (STABILIZATION_ATTITUDE_SP_MAX_PHI)*3.0;

  float speed_sp_x = pos_x_err*guidance_indi_pos_gain;
  float speed_sp_y = pos_y_err*guidance_indi_pos_gain;

  sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x)*guidance_indi_speed_gain;
  sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y)*guidance_indi_speed_gain;
//   sp_accel.x = (radio_control.values[RADIO_PITCH]/9600.0)*8.0;
//   sp_accel.y = -(radio_control.values[RADIO_ROLL]/9600.0)*8.0;

  //   struct FloatMat33 Ga;
  guidance_indi_calcG(&Ga);
  MAT33_INV(Ga_inv, Ga);

  float altitude_sp = POS_FLOAT_OF_BFP(guidance_v_z_sp);
  float vertical_velocity_sp = guidance_indi_pos_gain*(altitude_sp - stateGetPositionNed_f()->z);
//     float vertical_velocity_rc_euler = -(stabilization_cmd[COMMAND_THRUST]-4500.0)/4500.0*2.0;
  float vertical_velocity_err_euler = vertical_velocity_sp - stateGetSpeedNed_f()->z;
  sp_accel.z = vertical_velocity_err_euler*guidance_indi_speed_gain;

  struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, 0.0};

  Bound(a_diff.x, -6.0, 6.0);
  Bound(a_diff.y, -6.0, 6.0);
  Bound(a_diff.z, -9.0, 9.0);

  MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);

  guidance_euler_cmd.phi = roll_filt + euler_cmd.x;
  guidance_euler_cmd.theta = pitch_filt + euler_cmd.y;
  guidance_euler_cmd.psi = 0;//stateGetNedToBodyEulers_f()->psi;

  //Bound euler angles to prevent flipping and keep upright
  Bound(guidance_euler_cmd.phi, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
  Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);

  stabilization_attitude_set_setpoint_rp_quat_f(in_flight, heading);

}
Пример #6
0
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
{
  float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
  if (heading < 0.) {
    heading += 360;
  }
  uint16_t compass_heading = heading * 100;
  int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl;
  /// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs
  mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
                                       get_sys_time_msec(),
                                       stateGetPositionLla_i()->lat,
                                       stateGetPositionLla_i()->lon,
                                       stateGetPositionLla_i()->alt,
                                       relative_alt,
                                       stateGetSpeedNed_f()->x * 100,
                                       stateGetSpeedNed_f()->y * 100,
                                       stateGetSpeedNed_f()->z * 100,
                                       compass_heading);
  MAVLinkSendMessage();
}
Пример #7
0
/**
 * Send Metrics typically displayed on a HUD for fixed wing aircraft.
 */
static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
{
  /* Current heading in degrees, in compass units (0..360, 0=north) */
  int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
  /* Current throttle setting in integer percent, 0 to 100 */
  // is a 16bit unsigned int but supposed to be from 0 to 100??
  uint16_t throttle;
#ifdef COMMAND_THRUST
  throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
#elif defined COMMAND_THROTTLE
  throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
#endif
  mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
                           stateGetAirspeed_f(),
                           stateGetHorizontalSpeedNorm_f(), // groundspeed
                           heading,
                           throttle,
                           stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL
                           stateGetSpeedNed_f()->z); // climb rate
  MAVLinkSendMessage();
}
Пример #8
0
/**
 * 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)

  }