Пример #1
0
float AP_Landing_Deepstall::update_steering()
{
    Location current_loc;
    if (!landing.ahrs.get_position(current_loc)) {
        // panic if no position source is available
        // continue the  but target just holding the wings held level as deepstall should be a minimal energy
        // configuration on the aircraft, and if a position isn't available aborting would be worse
        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Deepstall: No position available. Attempting to hold level");
        memcpy(&current_loc, &landing_point, sizeof(Location));
    }
    uint32_t time = AP_HAL::millis();
    float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) / 1000.0;
    last_time = time;


    Vector2f ab = location_diff(arc_exit, extended_approach);
    ab.normalize();
    Vector2f a_air = location_diff(arc_exit, current_loc);

    crosstrack_error = a_air % ab;
    float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
    float nu1 = asinf(sine_nu1);

    if (L1_i > 0) {
        L1_xtrack_i += nu1 * L1_i / dt;
        L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
        nu1 += L1_xtrack_i;
    }

    float desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw);

    float yaw_rate = landing.ahrs.get_gyro().z;
    float yaw_rate_limit_rps = radians(yaw_rate_limit);
    float error = wrap_PI(constrain_float(desired_change / time_constant,
                                          -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);

#ifdef DEBUG_PRINTS
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
                                    (double)crosstrack_error,
                                    (double)error,
                                    (double)degrees(yaw_rate),
                                    (double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS

    return ds_PID.get_pid(error);
}
Пример #2
0
// report_compass - displays compass information.  Also called by compassmot.pde
void Copter::report_compass()
{
    cliSerial->println("Compass");
    print_divider();

    print_enabled(g.compass_enabled);

    // mag declination
    cliSerial->printf("Mag Dec: %4.4f\n",
            (double)degrees(compass.get_declination()));

    // mag offsets
    Vector3f offsets;
    for (uint8_t i=0; i<compass.get_count(); i++) {
        offsets = compass.get_offsets(i);
        // mag offsets
        cliSerial->printf("Mag%d off: %4.4f, %4.4f, %4.4f\n",
                        (int)i,
                        (double)offsets.x,
                        (double)offsets.y,
                        (double)offsets.z);
    }

    // motor compensation
    cliSerial->print("Motor Comp: ");
    if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
        cliSerial->println("Off");
    }else{
        if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
            cliSerial->print("Throttle");
        }
        if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
            cliSerial->print("Current");
        }
        Vector3f motor_compensation;
        for (uint8_t i=0; i<compass.get_count(); i++) {
            motor_compensation = compass.get_motor_compensation(i);
            cliSerial->printf("\nComMot%d: %4.2f, %4.2f, %4.2f\n",
                        (int)i,
                        (double)motor_compensation.x,
                        (double)motor_compensation.y,
                        (double)motor_compensation.z);
        }
    }
    print_blanks(1);
}
Пример #3
0
/*************************************************************************
 * //Function to calculate the course between two waypoints
 * //I'm using the real formulas--no lookup table fakes!
 *************************************************************************/
int get_gps_course(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float bear_calc;

  float x = 69.1 * (flat2 - flat1); 
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

  calc=atan2(y,x);

  bear_calc= degrees(calc);

  if(bear_calc<=1){
    bear_calc=360+bear_calc; 
  }
  return bear_calc;
}
void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
{
    float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);

    Vector3f wind = landing.ahrs.wind_estimate();
    // TODO: Support a user defined approach heading
    target_heading_deg = use_current_heading ? landing.ahrs.yaw_sensor * 1e-2 : (degrees(atan2f(-wind.y, -wind.x)));

    memcpy(&extended_approach, &landing_point, sizeof(Location));
    memcpy(&arc_exit, &landing_point, sizeof(Location));

    //extend the approach point to 1km away so that there is always a navigational target
    location_update(extended_approach, target_heading_deg, 1000.0);

    float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ?  landing_point.alt * 0.01f : approach_alt_offset,
                                                             false);
    float approach_extension_m = expected_travel_distance + approach_extension;
    float loiter_radius_m_abs = fabsf(loiter_radius);
    // an approach extensions must be at least half the loiter radius, or the aircraft has a
    // decent chance to be misaligned on final approach
    approach_extension_m = MAX(approach_extension_m, loiter_radius_m_abs * 0.5f);

    location_update(arc_exit, target_heading_deg + 180, approach_extension_m);
    memcpy(&arc, &arc_exit, sizeof(Location));
    memcpy(&arc_entry, &arc_exit, sizeof(Location));

    float arc_heading_deg = target_heading_deg + (landing_point.flags.loiter_ccw ? -90.0f : 90.0f);
    location_update(arc, arc_heading_deg, loiter_radius_m_abs);
    location_update(arc_entry, arc_heading_deg, loiter_radius_m_abs * 2);

#ifdef DEBUG_PRINTS
    // TODO: Send this information via a MAVLink packet
    gcs().send_text(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
                                     (double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
                                     (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
                                     (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
                                     (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
                                     (double)expected_travel_distance);
    gcs().send_text(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
#endif // DEBUG_PRINTS

}
Пример #5
0
/*
  send PID tuning message
 */
void Rover::send_pid_tuning(mavlink_channel_t chan)
{
    const Vector3f &gyro = ahrs.get_gyro();
    if (g.gcs_pid_mask & 1) {
        const DataFlash_Class::PID_Info &pid_info = steerController.get_pid_info();
        mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
                                    pid_info.desired,
                                    degrees(gyro.z),
                                    pid_info.FF,
                                    pid_info.P,
                                    pid_info.I,
                                    pid_info.D);
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
            return;
        }
    }
}
Пример #6
0
TEST_P (RandomWalkerTest, BuildLinearSystem)
{
  RandomWalker rw (g.graph,
                   boost::get (boost::edge_weight, g.graph),
                   boost::get (boost::vertex_color, g.graph));

  rw.computeVertexDegrees ();
  rw.buildLinearSystem ();

  ASSERT_EQ (g.rows, rw.L.rows ());
  ASSERT_EQ (g.rows, rw.L.cols ());
  ASSERT_EQ (g.rows, rw.B.rows ());
  ASSERT_EQ (g.cols, rw.B.cols ());

  std::vector<Weight> degrees (g.rows, 0.0);
  std::vector<Weight> L_sums (g.rows, 0.0);
  std::vector<Weight> B_sums (g.rows, 0.0);
  for (int k = 0; k < rw.L.outerSize (); ++k)
  {
    for (SparseMatrix::InnerIterator it (rw.L, k); it; ++it)
    {
      EXPECT_GE (it.row (), it.col ()); // the matrix should be lower triangular
      if (it.row () == it.col ())
      {
        degrees[it.row ()] = it.value ();
      }
      else
      {
        L_sums[it.row ()] -= it.value ();
        L_sums[it.col ()] -= it.value ();
      }
    }
  }
  for (int k = 0; k < rw.B.outerSize (); ++k)
  {
    for (SparseMatrix::InnerIterator it (rw.B, k); it; ++it)
    {
      B_sums[it.row ()] += it.value ();
    }
  }
  for (size_t i = 0; i < g.rows; ++i)
  {
    float sum = L_sums[i] + B_sums[i];
    EXPECT_FLOAT_EQ (degrees[i], sum);
  }
}
Пример #7
0
/*
 Calculate the angles needed to raise the leg from standing position to a certain vertical height (mInches)
 
 */
void  glStaticMovesRobot::lift_leg_from_standing      ( float mInches, bool mLeftLeg )
{
    if (mInches<0)  return ;
        
    float upper_lower_length = m_left_leg.m_upper_leg.m_joint_length + m_left_leg.m_lower_leg.m_joint_length;
    float upper_distance     = mInches * (m_left_leg.m_upper_leg.m_joint_length / upper_lower_length);
    float upper_vertical     = m_left_leg.m_upper_leg.m_joint_length - upper_distance;
    
    // Now find the angle needed to raise upper leg from m_upper_leg.m_joint_length by upper_distance;
    float hypotenus = m_left_leg.m_upper_leg.m_joint_length;
    
    // upper_vertical = hypotenus * cos(mAngle);
    float hip_angle = degrees( acos(upper_vertical / hypotenus) );
    if (mLeftLeg)
        lift_left_leg(hip_angle);
    else
        lift_right_leg(hip_angle);
}
Пример #8
0
/*
   update location from position
*/
void Aircraft::update_position(void)
{
    float bearing = degrees(atan2f(position.y, position.x));
    float distance = sqrtf(sq(position.x) + sq(position.y));

    location = home;
    location_update(location, bearing, distance);

    location.alt  = home.alt - position.z*100.0f;

    // we only advance time if it hasn't been advanced already by the
    // backend
    if (last_time_us == time_now_us) {
        time_now_us += frame_time_us;
    }
    last_time_us = time_now_us;
    sync_frame_time();
}
Пример #9
0
void create_projected_internal_degrees(const std::set< edge_t > edge_list, const spa_network_t & network, std::vector<std::vector<unsigned int> > & projected_internal_degrees) {
  projected_internal_degrees.clear();
  projected_internal_degrees.resize(network.size_in_clusters());
  for (id_t cluster = 0 ; cluster < network.size_in_clusters(); ++cluster) {
    LOGGER_WRITE(Logger::DEBUG,VAR_PRINT(cluster))
    // Fetch subgraph info
    std::multiset<edge_t> duplicate_edges = network.get_subgraph(cluster).get_global_links();
    std::set<id_t> nodes = network.get_subgraph(cluster).get_global_ids();
    std::map<id_t, id_t> global_to_local;
    

    // Keep a single copy of each internal edge 
    LOGGER_WRITE(Logger::DEBUG,"Keep a single copy of each internal edge.")
    std::set<edge_t> unique_edges;
    for (auto it = duplicate_edges.begin(); it!=duplicate_edges.end(); ++it) 
      unique_edges.insert(*it);
    duplicate_edges.clear();

    // Project the network onto the subgraph (and build global_to_local map)
    LOGGER_WRITE(Logger::DEBUG,"Project the network onto the subgraph")
    id_t id_counter = 0;
    for (auto it1 = nodes.begin(); it1 != nodes.end();++it1) {
      global_to_local[*it1] = id_counter;
      for (auto it2 = std::next(it1); it2 != nodes.end();++it2) {
        edge_t edge = *it1 < *it2 ? std::make_pair(*it1,*it2) : std::make_pair(*it2,*it1);
        if (edge_list.find(edge)!=edge_list.end()) {
          unique_edges.insert(edge);
        }
      }
      ++id_counter;
    }

    // Get degrees
    LOGGER_WRITE(Logger::DEBUG,"Get degrees")
    std::vector< unsigned int > degrees(nodes.size(),0);
    for (auto it = unique_edges.begin();it!=unique_edges.end();++it) {
      ++degrees[global_to_local.at(it->first)];
      ++degrees[global_to_local.at(it->second)];
    }

    projected_internal_degrees[cluster] = degrees;
  }
  return;
}
Пример #10
0
void AP_Landing_Deepstall::build_approach_path(void)
{
    float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);

    Vector3f wind = landing.ahrs.wind_estimate();
    // TODO: Support a user defined approach heading
    target_heading_deg = (degrees(atan2f(-wind.y, -wind.x)));

    memcpy(&extended_approach, &landing_point, sizeof(Location));
    memcpy(&arc_exit, &landing_point, sizeof(Location));

    //extend the approach point to 1km away so that there is always a navigational target
    location_update(extended_approach, target_heading_deg, 1000.0);

    float expected_travel_distance = predict_travel_distance(wind, landing_point.alt * 0.01f, false);
    float approach_extension_m = expected_travel_distance + approach_extension;
    // an approach extensions must be at least half the loiter radius, or the aircraft has a
    // decent chance to be misaligned on final approach
    approach_extension_m = MAX(approach_extension_m, loiter_radius * 0.5f);

    location_update(arc_exit, target_heading_deg + 180, approach_extension_m);
    memcpy(&arc, &arc_exit, sizeof(Location));
    memcpy(&arc_entry, &arc_exit, sizeof(Location));

    // TODO: Support loitering on either side of the approach path
    location_update(arc, target_heading_deg + 90.0, loiter_radius);
    location_update(arc_entry, target_heading_deg + 90.0, loiter_radius * 2);

#ifdef DEBUG_PRINTS
    // TODO: Send this information via a MAVLink packet
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
                                     (double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
                                     (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
                                     (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
                                     (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
                                     (double)expected_travel_distance);
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
#endif // DEBUG_PRINTS

}
Пример #11
0
/*
  calculate a new aerodynamic_load_factor and limit nav_roll_cd to
  ensure that the load factor does not take us below the sustainable
  airspeed
 */
void Plane::update_load_factor(void)
{
    float demanded_roll = fabsf(nav_roll_cd*0.01f);
    if (demanded_roll > 85) {
        // limit to 85 degrees to prevent numerical errors
        demanded_roll = 85;
    }
    aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));

    if (!aparm.stall_prevention) {
        // stall prevention is disabled
        return;
    }
    if (fly_inverted()) {
        // no roll limits when inverted
        return;
    }
    if (quadplane.tailsitter_active()) {
        // no limits while hovering
        return;
    }
       

    float max_load_factor = smoothed_airspeed / aparm.airspeed_min;
    if (max_load_factor <= 1) {
        // our airspeed is below the minimum airspeed. Limit roll to
        // 25 degrees
        nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);
        roll_limit_cd = constrain_int32(roll_limit_cd, -2500, 2500);
    } else if (max_load_factor < aerodynamic_load_factor) {
        // the demanded nav_roll would take us past the aerodymamic
        // load limit. Limit our roll to a bank angle that will keep
        // the load within what the airframe can handle. We always
        // allow at least 25 degrees of roll however, to ensure the
        // aircraft can be maneuvered with a bad airspeed estimate. At
        // 25 degrees the load factor is 1.1 (10%)
        int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100;
        if (roll_limit < 2500) {
            roll_limit = 2500;
        }
        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
        roll_limit_cd = constrain_int32(roll_limit_cd, -roll_limit, roll_limit);
    }    
}
Пример #12
0
void GCS_MAVLINK_Rover::send_nav_controller_output() const
{
    if (!rover.control_mode->is_autopilot_mode()) {
        return;
    }

    const Mode *control_mode = rover.control_mode;

    mavlink_msg_nav_controller_output_send(
        chan,
        0,  // roll
        degrees(rover.g2.attitude_control.get_desired_pitch()),
        control_mode->nav_bearing(),
        control_mode->wp_bearing(),
        MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
        0,
        control_mode->speed_error(),
        control_mode->crosstrack_error());
}
Пример #13
0
float TinyGPS::course_to (float lat1, float long1, float lat2, float long2) 
{
  // returns course in degrees (North=0, West=270) from position 1 to position 2,
  // both specified as signed decimal-degrees latitude and longitude.
  // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
  // Courtesy of Maarten Lamers
  float dlon = radians(long2-long1);
  lat1 = radians(lat1);
  lat2 = radians(lat2);
  float a1 = sin(dlon) * cos(lat2);
  float a2 = sin(lat1) * cos(lat2) * cos(dlon);
  a2 = cos(lat1) * sin(lat2) - a2;
  a2 = atan2(a1, a2);
  if (a2 < 0.0)
  {
    a2 += TWO_PI;
  }
  return degrees(a2);
}
Пример #14
0
// correct a bearing in centi-degrees for wind
void AP_AHRS::wind_correct_bearing(int32_t &nav_bearing_cd)
{
    if (!use_compass() || !_flags.wind_estimation) {
        // we are not using the compass - no wind correction,
        // as GPS gives course over ground already
        return;
    }

    // if we are using a compass for navigation, then adjust the
    // heading to account for wind
    Vector3f wind = wind_estimate();
    Vector2f wind2d = Vector2f(wind.x, wind.y);
    float speed;
    if (airspeed_estimate(&speed)) {
        Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
        Vector2f nav_adjusted = nav_vector - wind2d;
        nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;
    }
}
Пример #15
0
// runs a seasonally forced gillespie simulation on a exponential network
int main(int argc,char *argv[]) {
    if(argc != 9) {
            printf("Wrong Number of Arguments\n");
            exit(0);
    }

    double alpha1, alpha2, beta1_max, beta2_max, gamma1, gamma2, phi1, phi2, eta1, eta2;
    alpha1 = alpha2 = atof( argv[1] );
    beta1_max = atof( argv[2] );  
    beta2_max = atof( argv[3] );
    eta1 = eta2 = 1.0/2.62;
    gamma1 = gamma2 = 1.0/3.38;
    phi1 = phi2 = atof( argv[4]);
    int intro_time, start_ind, shift;
    intro_time = atoi( argv[5] );
    start_ind = atoi( argv[6] );
    shift = atoi( argv[7] );
    string network_type = argv[8];
    
    int num_reps = 4000;

    Network net = Network("gillespie toy", Network::Undirected);
    net.populate(10000);
    for(int i =1; i <= num_reps; i++){
        net.clear_edges();
        if(network_type == "exp"){
            net.rand_connect_exponential(0.06453487);
        } else if(network_type == "unif"){
            vector<int> degrees(10000, 16);
            net.rand_connect_explicit(degrees);
        } else{
            cerr << "Unrecognized network type" << endl;
        }
        // cout << net.mean_deg() << endl;
        Gillespie_SEIR_TwoStrain_Network sim(&net, alpha1, alpha2, eta1, eta2, gamma1, gamma2, beta1_max, beta2_max, phi1, phi2, intro_time, start_ind, shift);
        cout << "Simulation number: " << i << endl;
        sim.reset();
        sim.rand_infect(5, 1);
        sim.run_simulation(10000.0);
    }
    return 0;
}
Пример #16
0
/*
*  Called to determine if Manual CR Pitch has hit range limits
*/
void Tracker::check_manual_pitch_limits(int16_t pitch_pwm)
{
    //Check measured pitch angle of ahrs
    float ahrs_pitch = degrees(ahrs.pitch);
    //float pitch_range = g.pitch_range/2;	

    if (ahrs_pitch >= g.pitch_max) { //MAX LIMIT
        //Check if servo is reversed and limit servo in one direction only
        if (!RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm < RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Normal Servo Direction
            pitch_lock = false;
        } else if (RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm > RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Reversed Servo Direction
            pitch_lock = false;
            } else {
            pitch_lock_angle = g.pitch_max;
            pitch_lock = true; //Stop if not in a direction to clear violation
        }
        return;
    }

    if (ahrs_pitch <= g.pitch_min) { //MIN LIMIT
        //Check if servo is reversed and limit servo in one direction only
        if (!RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm > RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Normal Servo Direction
            pitch_lock = false;
        } else if (RC_Channels::rc_channel(CH_PITCH)->get_reverse() && (pitch_pwm < RC_Channels::rc_channel(CH_PITCH)->get_radio_trim())) { //Reversed Servo Direction
            pitch_lock = false;
        } else {
            pitch_lock_angle = g.pitch_min;
            pitch_lock = true; //Stop if not in a direction to clear violation
        }
        return;
    }

    if (pitch_pwm > (RC_Channels::rc_channel(CH_PITCH)->get_radio_trim() - 10) && pitch_pwm < (RC_Channels::rc_channel(CH_PITCH)->get_radio_trim() + 10)) {
        if (!pitch_lock) {
        pitch_lock_angle = ahrs_pitch;
        pitch_lock = true;
        }
        return;
    } 

    pitch_lock = false; //Not in pitch violation
}
Пример #17
0
int main(int argc,char *argv[]) {
    if(argc != 10) {
            printf("Wrong Number of Arguments\n");
            exit(0);
    }

    //double alpha1 = argv[1]-0.0;
    double beta_vax, beta_dis, gamma_vax, gamma_dis;
    int init_inf, init_vax;

    beta_vax = atof( argv[1] ); // vaccine transmissibility
    beta_dis = atof( argv[2] ); // disease transmissibility
    gamma_vax = atof( argv[3] ); // vaccine recovery rate
    gamma_dis = atof( argv[4] ); // disease recovery rate
    init_vax = atoi( argv[5] ); // number initially vaccinated
    init_inf = atoi( argv[6] ); // number initially infected

    string network_type = argv[7] ;
    string vax_type = argv[8];
    int num_reps = atoi( argv[9]);

    Network net = Network("gillespie toy", Network::Undirected);
    net.populate(10000);
    for(int i =1; i <= num_reps; i++){
        net.clear_edges();
        if(network_type == "er"){
            net.erdos_renyi(10);
        } else if(network_type == "exp"){
            net.rand_connect_exponential(0.1);
        } else if(network_type == "unif"){
            vector<int> degrees(10000, 10);
            net.rand_connect_explicit(degrees);
        } else{
            cerr << "Unrecognized network type" << endl;
        }
        Gillespie_trans_vaccine sim(&net, gamma_vax, gamma_dis, beta_vax, beta_dis, vax_type);
        cout << "Simulation number: " << i << endl;
        sim.reset();
        sim.run_simulation(10000.0, init_vax, init_inf);
    }
    return 0;
}
Пример #18
0
int EEregionCheck (int ics, int ips,
                   int radStart, int radEnd,
                   int phiStart, int phiEnd)  
{
  int x = ics-50;
  int y = ips-50;
  double radius2 = x*x + y*y ;
  if (radius2 < 10*10) return 1;  //center of the donut
  if (radius2 > 50*50) return 1;  //outer part of the donut
  if (radius2 < radStart * radStart) return 2 ;
  if (radius2 >= radEnd * radEnd) return 2 ;
  double phi = atan2 (static_cast<double> (y),static_cast<double> (x));
  phi = degrees (phi);
  if (phi < 0) phi += 360; 
  if (phiStart < phiEnd 
     && phi > phiStart && phi < phiEnd ) return 0; 
  if (phiStart > phiEnd 
      && (phi > phiStart|| phi < phiEnd )) return 0; 
   return 3;
}
void CubeMovingState::Execute(float dt)
{
    BSpline* spline = cubeModel->GetSpline();
    if (spline != nullptr)
    {
        cubeModel->SetPosition(spline->GetPosition() + spline->GetNextPoint());
        if (cubeModel->LooksForward())
        {
            glm::vec3 velocity = glm::vec3(spline->GetVelocityUnitVector());
            glm::vec3 forwardVector = cubeModel->mForward;
            glm::vec3 rotationAxis = glm::cross(forwardVector, velocity);
            float rotationAngleInDegrees = degrees(glm::acos(glm::dot(forwardVector, velocity) / (length(forwardVector) * length(velocity))));
            
            cubeModel->SetRotation(rotationAxis, rotationAngleInDegrees);
        }
        spline->Update(dt);
    }

    duration -= dt;
}
Пример #20
0
 int order(const RCP<OrderingSolution<lno_t, gno_t> > &solution)
 {
   int ierr= 0;
 
   HELLO;
 
   lno_t *perm;
   perm = (lno_t *) (solution->getPermutation());
   if (perm==0){
     // Throw exception
     std::cerr << "perm is NULL" << std::endl;
     ierr = -1;
   }
 
   // Get local graph.
   const size_t nVtx = model->getLocalNumVertices();
   ArrayView<const gno_t> edgeIds;
   ArrayView<const lno_t> offsets;
   ArrayView<StridedData<lno_t, scalar_t> > wgts;
   model->getEdgeList(edgeIds, offsets, wgts);
 
   // Store degrees together with index so we can sort.
   Teuchos::Array<std::pair<lno_t, size_t> >  degrees(nVtx);
   for (lno_t i=0; i<(lno_t)nVtx; i++){
     degrees[i].first = i;
     degrees[i].second  = offsets[i+1] - offsets[i];
   }
 
   // Sort degrees.
   SortPairs<lno_t,size_t> zort;
   bool inc = true; // TODO: Add user parameter
   zort.sort(degrees, inc);
 
   // Copy permuted indices to perm.
   for (lno_t i=0; i<(lno_t)nVtx; i++){
     perm[i] = degrees[i].first;
   }
 
   solution->setHavePerm(true);
   return ierr;
 }
Пример #21
0
/**
  update_bearing_and_distance - updates bearing and distance to the vehicle
  should be called at 50hz
 */
void Tracker::update_bearing_and_distance()
{
    // exit immediately if we do not have a valid vehicle position
    if (!vehicle.location_valid) {
        return;
    }

    // calculate bearing to vehicle
    // To-Do: remove need for check of control_mode
    if (control_mode != SCAN && !nav_status.manual_control_yaw) {
        nav_status.bearing  = get_bearing_cd(current_loc, vehicle.location_estimate) * 0.01f;
    }

    // calculate distance to vehicle
    nav_status.distance = get_distance(current_loc, vehicle.location_estimate);

    // calculate pitch to vehicle
    // To-Do: remove need for check of control_mode
    if (control_mode != SCAN && !nav_status.manual_control_pitch) {
        nav_status.pitch    = degrees(atan2f(nav_status.altitude_difference, nav_status.distance));
    }
}
Пример #22
0
/*
 * return the average of all circular (0-360) values in DEGREES added with an addRotationalValue call
 * Range is 0-360 DEGREES, return is -180 to 180 degrees
 */
int Wind::getRotationalAverage() {
	long x =0;
	long y = 0;
	float xf, yf = 0;

	byte i = 0;

	for (; i < dirList.getCurrentSize(); i++) {
		x = x + icosLong(dirList.getValue(i));
		y = y + isinLong(dirList.getValue(i));

	}
	//TODO:watch out for zeros
	if (x == 0l && y == 0l) {
		return 0l;
	}
	xf = (x* 0.0000152590219) / i;
	yf = (y* 0.0000152590219) / i;

	return degrees(atan2(yf, xf));

}
Пример #23
0
// Write an attitude packet
void Rover::Log_Write_Attitude()
{
    float desired_pitch_cd = degrees(g2.attitude_control.get_desired_pitch()) * 100.0f;
    const Vector3f targets(0.0f, desired_pitch_cd, 0.0f);

    DataFlash.Log_Write_Attitude(ahrs, targets);

#if AP_AHRS_NAVEKF_AVAILABLE
    DataFlash.Log_Write_EKF(ahrs);
    DataFlash.Log_Write_AHRS2(ahrs);
#endif
    DataFlash.Log_Write_POS(ahrs);

    // log steering rate controller
    DataFlash.Log_Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info());
    DataFlash.Log_Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid().get_pid_info());

    // log pitch control for balance bots
    if (is_balancebot()) {
        DataFlash.Log_Write_PID(LOG_PIDP_MSG, g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info());
    }
}
Пример #24
0
void create_collapsed_internal_degrees(const spa_network_t & network, std::vector<std::vector<unsigned int> > & collapsed_internal_degrees) {
  collapsed_internal_degrees.clear();
  collapsed_internal_degrees.resize(network.size_in_clusters());
  for (id_t cluster = 0 ; cluster < network.size_in_clusters(); ++cluster) {
    LOGGER_WRITE(Logger::DEBUG,VAR_PRINT(cluster))
    // Fetch subgraph info
    std::multiset<edge_t> duplicate_edges = network.get_subgraph(cluster).get_global_links();
    std::set<id_t> nodes = network.get_subgraph(cluster).get_global_ids();
    std::map<id_t, id_t> global_to_local;
    

    // Keep a single copy of each internal edge 
    LOGGER_WRITE(Logger::DEBUG,"Keep a single copy of each internal edge.")
    std::set<edge_t> unique_edges;
    for (auto it = duplicate_edges.begin(); it!=duplicate_edges.end(); ++it) 
      unique_edges.insert(*it);
    duplicate_edges.clear();


    // Build global_to_local map
    LOGGER_WRITE(Logger::DEBUG,"Build global_to_local map")
    id_t id_counter = 0;
    for (auto it1 = nodes.begin(); it1 != nodes.end();++it1) {
      global_to_local[*it1] = id_counter;
      ++id_counter;
    }

    // Get degrees
    LOGGER_WRITE(Logger::DEBUG,"Get degrees")
    std::vector< unsigned int > degrees(nodes.size(),0);
    for (auto it = unique_edges.begin();it!=unique_edges.end();++it) {
      ++degrees[global_to_local.at(it->first)];
      ++degrees[global_to_local.at(it->second)];
    }

    collapsed_internal_degrees[cluster] = degrees;
  }
  return;
}
Пример #25
0
void LinkedStructure::draw()
{
    // Drawing the linke structureo on the scren
    glPushMatrix();
    glTranslatef(mBasePosition(0),mBasePosition(1),0.0f);
    glutSolidSphere(2.0f, 20, 20);

    // going through each link and at the tip of each link
    // We draw a sphere
    for (int i = 0; i < mList.size(); i++)
    {
        glRotatef(degrees(mList[i]->mAngle), 1.0f, 0.0f, 0.0f);
	mList[i]->mColor.apply();
        gluCylinder(mList[i]->mObj, 1, 1, mList[i]->mLength, 20, 20);
        glTranslatef(0, 0, mList[i]->mLength);
	Color c = { 1.0, 0, 0};
	c.apply();
	// Draw the sphere to show the joint position
        glutSolidSphere(1.5, 20, 20);
    }

    glPopMatrix();
}
Пример #26
0
/**
   update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
   requested pitch, so the board (and therefore the antenna) will be pointing at the target
 */
void Tracker::update_pitch_onoff_servo(float pitch)
{
    // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal
    // pitch argument is -90 to 90, where 0 is horizontal
    // get_servo_out() is in 100ths of a degree
    float ahrs_pitch = degrees(ahrs.pitch);
    float err = ahrs_pitch - pitch;
    int32_t pitch_min_cd = g.pitch_min*100;
    int32_t pitch_max_cd = g.pitch_max*100;

    float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
    if (fabsf(err) < acceptable_error) {
        channel_pitch.set_servo_out(0);
    } else if ((err > 0) && (pitch*100>pitch_min_cd)) {
        // positive error means we are pointing too high, so push the
        // servo down
        channel_pitch.set_servo_out(-9000);
    } else if (pitch*100<pitch_max_cd){
        // negative error means we are pointing too low, so push the
        // servo up
        channel_pitch.set_servo_out(9000);
    }
}
Пример #27
0
	void convertToAbsolute(float groundSpeed, float course, float heading) {
		// Correct from relative to absolute speed here
		/*float relX, relY, vesselX, vesselY, absX, absY;

		relX = apparentWindSpeed*cos(radians(apparentWindDirection));
		relY = apparentWindSpeed*sin(radians(apparentWindDirection));

		float angle = radians(heading-course);
		if (angle > PI)
			angle -= 2*PI;
		else if (angle < -PI)
			angle += 2*PI;

		vesselX = groundSpeed*cos(angle);
		vesselY = groundSpeed*sin(angle);

		absX = relX+vesselX;
		absY = relY+vesselY;

		windSpeed = sqrt(absX*absX+absY*absY);
		windDirection = degrees(atan2(absX,absY));*/

		float a = apparentWindSpeed*sin(radians(apparentWindDirection+heading-course));
		float b = apparentWindSpeed*cos(radians(apparentWindDirection+heading-course))+groundSpeed;

		windSpeed = sqrt(a*a+b*b);
		windDirection = degrees(atan2(a,b))+course;

		if (windDirection > 180)
			windDirection -= 360;
		else if (windDirection <= -180)
			windDirection += 360;		

		/*windSpeed = sqrt(apparentWindSpeed^2+groundSpeed^2-2*apparentWindSpeed*groundSpeed*cos(radians(apparentWindDirection)));

		windDirection = acos((apparentWindSpeed*cos(radians(apparentWindDirection))-groundSpeed)/windSpeed);*/
	}
Пример #28
0
// Special purpose function to return the guiding declination (radians) - either the actual scope position or the
// default values defined in mount.cpp.  Doesn't throw exceptions to callers.
double ScopeASCOM::GetGuidingDeclination(void)
{
    double dReturn = Scope::GetDefGuidingDeclination();

    try
    {
        if (!IsConnected())
        {
            throw ERROR_INFO("ASCOM Scope: cannot get Declination when not connected to mount");
        }

        if (!m_bCanGetCoordinates)
        {
            throw THROW_INFO("!m_bCanGetCoordinates");
        }

        GITObjRef scope(m_gitEntry);

        Variant vRes;
        if (!scope.GetProp(&vRes, dispid_declination))
        {
            throw ERROR_INFO("GetDeclination() fails: " + ExcepMsg(scope.Excep()));
        }

        dReturn = radians(vRes.dblVal);
    }
    catch (wxString Msg)
    {
        POSSIBLY_UNUSED(Msg);
        m_bCanGetCoordinates = false;
    }

    Debug.AddLine("ScopeASCOM::GetDeclination() returns %.1f", degrees(dReturn));

    return dReturn;
}
Пример #29
0
//------------------------------------------------------------
//
// return the average of directions
//------------------------------------------------------------
float Particle::GetAverageBearing(bool IsOpenMode, int head_par, int rear_par, Particle* thispar)
{
	bool Is_headpar = false, Is_rearpar = false;
	if(/*IsOpenMode && */thispar->id==head_par)	// for the head particle in open-area mode
	{
		Is_headpar = true;
	}
	if(/*IsOpenMode && */thispar->id==rear_par)	// for the rear particle in open-area mode
	{
		Is_rearpar = true;
	}
	ofVec2f mid_vec = (thispar->prev->bearing_vec + thispar->next->bearing_vec)/2;
	if(Is_headpar)
		mid_vec = (thispar->next->bearing_vec + thispar->next->next->bearing_vec)/2;
	else if(Is_rearpar)
		mid_vec = (thispar->prev->bearing_vec + thispar->prev->prev->bearing_vec)/2;
	mid_vec.normalize();
	float d_aver = acos(mid_vec.dot(ofVec2f(0,1)));
	if(mid_vec.x<0)
		d_aver = 2*PI-d_aver;	// adjust signs
			float test_aver = degrees(d_aver);
	return d_aver;

}
 vector<int> eventualSafeNodes(vector<vector<int>>& graph) {
     int n = graph.size();
     vector<vector<int>> edges(n);
     vector<int> degrees(n);
     queue<int> Q;
     for (int i = 0; i < n; ++i) {
         degrees[i] = graph[i].size();
         if (degrees[i] == 0) {
             Q.push(i);
         }
         else {
             for (int j : graph[i]) {
                 edges[j].push_back(i);
             }
         }           
     }
   
     vector<bool> safe(n);
     while (!Q.empty()) {
         int u = Q.front();
         safe[u] = true;
         Q.pop();
         for (int v : edges[u]) {
             if (--degrees[v] == 0) {
                 Q.push(v);
             }
         }
     }
     vector<int> result;
     for (int i = 0; i < n; ++i) {
         if (safe[i]) {
             result.push_back(i);
         }
     }
     return result;
 }