C++ (Cpp) cm_att_set_gps_pos Examples

Example #1

File: auto_logic.c Project: erazor83/PenguPilot

bool auto_logic_run(bool *hard_off, bool is_full_auto, uint16_t sensor_status, bool flying, float channels[MAX_CHANNELS], float yaw, vec2_t *ne_gps_pos, float u_baro_pos, float u_ultra_pos)
{ 
   int rc_valid = sensor_status & RC_VALID;
   if (is_full_auto || rc_valid)
   {
      float gas_stick = channels[CH_GAS];
      if (is_full_auto || gas_stick > 0.5)
      {
         pthread_mutex_lock(&mutex);
         if (mode_is_ground)
            cm_u_set_ultra_pos(setp_u);
         else
            cm_u_set_baro_pos(setp_u);
         pthread_mutex_unlock(&mutex);
      }
      else
         cm_u_set_spd((gas_stick - 0.5) * 5.0);
   }
   else
      cm_u_set_spd(-0.5);

   if (u_ultra_pos > 1.0)
   {
      float yaw_stick = channels[CH_YAW];
      if (is_full_auto || (rc_valid && fabs(yaw_stick) < 0.05))
         cm_yaw_set_pos(tsfloat_get(&setp_yaw));
      else
         cm_yaw_set_spd(yaw_stick * 2.0f);
   }
   else
      cm_yaw_set_spd(0.0f);   

   float pitch = channels[CH_PITCH];
   float roll = channels[CH_ROLL];
   float sw_l = channels[CH_SWITCH_L];
   if (!is_full_auto && rc_valid && !is_full_auto && sqrt(pitch * pitch + roll * roll) > 0.1)
      hyst_gps_override = 1.0;
   hyst_gps_override -= 0.006;   

   if (hyst_gps_override > 0.0)
   {
      vec2_t angles;
      vec2_set(&angles, pitch, roll);
      cm_att_set_angles(&angles);
   }
   else
   {
      vec2_t ne_gps_setpoint;
      vec2_set(&ne_gps_setpoint, tsfloat_get(&setp_n), tsfloat_get(&setp_e));
      cm_att_set_gps_pos(&ne_gps_setpoint);
   }

   if (!is_full_auto && (sensor_status & RC_VALID) && sw_l > 0.5)
      *hard_off = true;

   return tsint_get(&motors_enabled);
}

Example #2

File: auto_logic.c Project: matu7151/PenguPilot

bool auto_logic_run(bool *hard_off, bool is_full_auto, uint16_t sensor_status, bool flying, float channels[PP_MAX_CHANNELS], float yaw, vec2_t *ne_gps_pos, float u_baro_pos, float u_ultra_pos)
{ 
   /* set u position: */
   pthread_mutex_lock(&mutex);
   if (mode_is_ground)
      cm_u_set_ultra_pos(setp_u);
   else
      cm_u_set_baro_pos(setp_u);
   pthread_mutex_unlock(&mutex);

   /* set gps position: */
   vec2_t ne_gps_setpoint;
   vec2_set(&ne_gps_setpoint, tsfloat_get(&setp_n), tsfloat_get(&setp_e));
   cm_att_set_gps_pos(&ne_gps_setpoint);
         
   /* set yaw position: */
   cm_yaw_set_pos(tsfloat_get(&setp_yaw));

   /* safe_auto code: */
   if (!is_full_auto && (sensor_status & RC_VALID))
   {
      printf("SAFE_AUTO\n");

      float sw_l = channels[CH_SWITCH_L];
      if (sw_l > 0.5)
      {
         *hard_off = true;
      }

      float gas_stick = channels[CH_GAS];
      if (gas_stick < 0.8)
         cm_u_set_acc((gas_stick - 0.5) * 5.0);
      
      float pitch = channels[CH_PITCH];
      float roll = channels[CH_ROLL];
      if (sqrt(pitch * pitch + roll * roll) > 0.1)
         hyst_gps_override = 1.0;
      hyst_gps_override -= 0.006;   

      if (hyst_gps_override > 0.0)
      {
         vec2_t angles;
         vec2_set(&angles, pitch, roll);
         cm_att_set_angles(&angles);
      }
   }

   return tsint_get(&motors_enabled);
}