Beispiel #1
0
void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
{
    switch (msg->msgid) {

    case MAVLINK_MSG_ID_HEARTBEAT:      // MAV ID: 0
    {
        // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
        if(msg->sysid != copter.g.sysid_my_gcs) break;
        copter.failsafe.last_heartbeat_ms = AP_HAL::millis();
        break;
    }

    case MAVLINK_MSG_ID_MANUAL_CONTROL:
    {
        if (msg->sysid != copter.g.sysid_my_gcs) {
            break; // only accept control from our gcs
        }

        mavlink_manual_control_t packet;
        mavlink_msg_manual_control_decode(msg, &packet);

        if (packet.target != copter.g.sysid_this_mav) {
            break; // only accept control aimed at us
        }

        if (packet.z < 0) { // Copter doesn't do negative thrust
            break;
        }

        uint32_t tnow = AP_HAL::millis();

        manual_override(copter.channel_roll, packet.y, 1000, 2000, tnow);
        manual_override(copter.channel_pitch, packet.x, 1000, 2000, tnow, true);
        manual_override(copter.channel_throttle, packet.z, 0, 1000, tnow);
        manual_override(copter.channel_yaw, packet.r, 1000, 2000, tnow);

        // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
        copter.failsafe.last_heartbeat_ms = tnow;
        break;
    }

#if MODE_GUIDED_ENABLED == ENABLED
    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
    {
        // decode packet
        mavlink_set_attitude_target_t packet;
        mavlink_msg_set_attitude_target_decode(msg, &packet);

        // exit if vehicle is not in Guided mode or Auto-Guided mode
        if (!copter.flightmode->in_guided_mode()) {
            break;
        }

        // ensure type_mask specifies to use attitude and thrust
        if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
            break;
        }

        // convert thrust to climb rate
        packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
        float climb_rate_cms = 0.0f;
        if (is_equal(packet.thrust, 0.5f)) {
            climb_rate_cms = 0.0f;
        } else if (packet.thrust > 0.5f) {
            // climb at up to WPNAV_SPEED_UP
            climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up();
        } else {
            // descend at up to WPNAV_SPEED_DN
            climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down());
        }

        // if the body_yaw_rate field is ignored, use the commanded yaw position
        // otherwise use the commanded yaw rate
        bool use_yaw_rate = false;
        if ((packet.type_mask & (1<<2)) == 0) {
            use_yaw_rate = true;
        }

        copter.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
            climb_rate_cms, use_yaw_rate, packet.body_yaw_rate);

        break;
    }

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
    {
        // decode packet
        mavlink_set_position_target_local_ned_t packet;
        mavlink_msg_set_position_target_local_ned_decode(msg, &packet);

        // exit if vehicle is not in Guided mode or Auto-Guided mode
        if (!copter.flightmode->in_guided_mode()) {
            break;
        }

        // check for supported coordinate frames
        if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
            packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
            packet.coordinate_frame != MAV_FRAME_BODY_NED &&
            packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
            break;
        }

        bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
        bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
        bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
        bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
        bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;

        /*
         * for future use:
         * bool force           = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
         */

        // prepare position
        Vector3f pos_vector;
        if (!pos_ignore) {
            // convert to cm
            pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
            // rotate to body-frame if necessary
            if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                copter.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y);
            }
            // add body offset if necessary
            if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
                packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                pos_vector += copter.inertial_nav.get_position();
            } else {
                // convert from alt-above-home to alt-above-ekf-origin
                if (!AP::ahrs().home_is_set()) {
                    break;
                }
                const Location &origin = copter.inertial_nav.get_origin();
                pos_vector.z += AP::ahrs().get_home().alt;
                pos_vector.z -= origin.alt;
            }
        }

        // prepare velocity
        Vector3f vel_vector;
        if (!vel_ignore) {
            // convert to cm
            vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
            // rotate to body-frame if necessary
            if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
            }
        }

        // prepare yaw
        float yaw_cd = 0.0f;
        bool yaw_relative = false;
        float yaw_rate_cds = 0.0f;
        if (!yaw_ignore) {
            yaw_cd = ToDeg(packet.yaw) * 100.0f;
            yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
        }
        if (!yaw_rate_ignore) {
            yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
        }

        // send request
        if (!pos_ignore && !vel_ignore && acc_ignore) {
            copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        } else if (pos_ignore && !vel_ignore && acc_ignore) {
            copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        } else if (!pos_ignore && vel_ignore && acc_ignore) {
            copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        }

        break;
    }

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
    {
        // decode packet
        mavlink_set_position_target_global_int_t packet;
        mavlink_msg_set_position_target_global_int_decode(msg, &packet);

        // exit if vehicle is not in Guided mode or Auto-Guided mode
        if (!copter.flightmode->in_guided_mode()) {
            break;
        }

        bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
        bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
        bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
        bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
        bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;

        /*
         * for future use:
         * bool force           = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
         */

        Vector3f pos_neu_cm;  // position (North, East, Up coordinates) in centimeters

        if(!pos_ignore) {
            // sanity check location
            if (!check_latlng(packet.lat_int, packet.lon_int)) {
                break;
            }
            Location::AltFrame frame;
            if (!mavlink_coordinate_frame_to_location_alt_frame(packet.coordinate_frame, frame)) {
                // unknown coordinate frame
                break;
            }
            const Location loc{
                packet.lat_int,
                packet.lon_int,
                int32_t(packet.alt*100),
                frame,
            };
            if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
                break;
            }
        }

        // prepare yaw
        float yaw_cd = 0.0f;
        bool yaw_relative = false;
        float yaw_rate_cds = 0.0f;
        if (!yaw_ignore) {
            yaw_cd = ToDeg(packet.yaw) * 100.0f;
            yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
        }
        if (!yaw_rate_ignore) {
            yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
        }

        if (!pos_ignore && !vel_ignore && acc_ignore) {
            copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        } else if (pos_ignore && !vel_ignore && acc_ignore) {
            copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        } else if (!pos_ignore && vel_ignore && acc_ignore) {
            copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        }

        break;
    }
#endif

    case MAVLINK_MSG_ID_DISTANCE_SENSOR:
    {
        copter.rangefinder.handle_msg(msg);
#if PROXIMITY_ENABLED == ENABLED
        copter.g2.proximity.handle_msg(msg);
#endif
        break;
    }

    case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
    {
#if PROXIMITY_ENABLED == ENABLED
        copter.g2.proximity.handle_msg(msg);
#endif
        break;
    }

#if HIL_MODE != HIL_MODE_DISABLED
    case MAVLINK_MSG_ID_HIL_STATE:          // MAV ID: 90
    {
        mavlink_hil_state_t packet;
        mavlink_msg_hil_state_decode(msg, &packet);

        // sanity check location
        if (!check_latlng(packet.lat, packet.lon)) {
            break;
        }

        // set gps hil sensor
        Location loc;
        loc.lat = packet.lat;
        loc.lng = packet.lon;
        loc.alt = packet.alt/10;
        Vector3f vel(packet.vx, packet.vy, packet.vz);
        vel *= 0.01f;

        gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
                   packet.time_usec/1000,
                   loc, vel, 10, 0);

        // rad/sec
        Vector3f gyros;
        gyros.x = packet.rollspeed;
        gyros.y = packet.pitchspeed;
        gyros.z = packet.yawspeed;

        // m/s/s
        Vector3f accels;
        accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
        accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
        accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);

        ins.set_gyro(0, gyros);

        ins.set_accel(0, accels);

        AP::baro().setHIL(packet.alt*0.001f);
        copter.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
        copter.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);

        break;
    }
#endif //  HIL_MODE != HIL_MODE_DISABLED

    case MAVLINK_MSG_ID_RADIO:
    case MAVLINK_MSG_ID_RADIO_STATUS:       // MAV ID: 109
    {
        handle_radio_status(msg, copter.should_log(MASK_LOG_PM));
        break;
    }

#if PRECISION_LANDING == ENABLED
    case MAVLINK_MSG_ID_LANDING_TARGET:
        copter.precland.handle_msg(msg);
        break;
#endif

    case MAVLINK_MSG_ID_TERRAIN_DATA:
    case MAVLINK_MSG_ID_TERRAIN_CHECK:
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
        copter.terrain.handle_data(chan, msg);
#endif
        break;

    case MAVLINK_MSG_ID_SET_HOME_POSITION:
    {
        mavlink_set_home_position_t packet;
        mavlink_msg_set_home_position_decode(msg, &packet);
        if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
            if (!copter.set_home_to_current_location(true)) {
                // silently ignored
            }
        } else {
            Location new_home_loc;
            new_home_loc.lat = packet.latitude;
            new_home_loc.lng = packet.longitude;
            new_home_loc.alt = packet.altitude / 10;
            if (!copter.set_home(new_home_loc, true)) {
                // silently ignored
            }
        }
        break;
    }

    case MAVLINK_MSG_ID_ADSB_VEHICLE:
    case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
    case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
    case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
#if ADSB_ENABLED == ENABLED
        copter.adsb.handle_message(chan, msg);
#endif
        break;

#if TOY_MODE_ENABLED == ENABLED
    case MAVLINK_MSG_ID_NAMED_VALUE_INT:
        copter.g2.toy_mode.handle_message(msg);
        break;
#endif
        
    default:
        handle_common_message(msg);
        break;
    }     // end switch
} // end handle mavlink
Beispiel #2
0
 const value is_equal(::zval const& rhs) const {
     return is_equal(rhs BOOST_PHP_TSRM_DIRECT_CC);
 }
Beispiel #3
0
double ElasticNature::getAlpha() const {
	return (is_equal(alpha, UNAVAILABLE_DOUBLE)) ? 0 : alpha;
}
// update L1 control for waypoint navigation
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min)
{

    struct Location _current_loc;
    float Nu;
    float xtrackVel;
    float ltrackVel;

    uint32_t now = AP_HAL::micros();
    float dt = (now - _last_update_waypoint_us) * 1.0e-6f;
    if (dt > 0.1) {
        dt = 0.1;
        _L1_xtrack_i = 0.0f;
    }
    _last_update_waypoint_us = now;

    // Calculate L1 gain required for specified damping
    float K_L1 = 4.0f * _L1_damping * _L1_damping;

    // Get current position and velocity
    if (_ahrs.get_position(_current_loc) == false) {
        // if no GPS loc available, maintain last nav/target_bearing
        _data_is_stale = true;
        return;
    }

    Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();

    // update _target_bearing_cd
    _target_bearing_cd = get_bearing_cd(_current_loc, next_WP);

    //Calculate groundspeed
    float groundSpeed = _groundspeed_vector.length();
    if (groundSpeed < 0.1f) {
        // use a small ground speed vector in the right direction,
        // allowing us to use the compass heading at zero GPS velocity
        groundSpeed = 0.1f;
        _groundspeed_vector = Vector2f(cosf(get_yaw()), sinf(get_yaw())) * groundSpeed;
    }

    // Calculate time varying control parameters
    // Calculate the L1 length required for specified period
    // 0.3183099 = 1/1/pipi
    _L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);

    // Calculate the NE position of WP B relative to WP A
    Vector2f AB = location_diff(prev_WP, next_WP);
    float AB_length = AB.length();

    // Check for AB zero length and track directly to the destination
    // if too small
    if (AB.length() < 1.0e-6f) {
        AB = location_diff(_current_loc, next_WP);
        if (AB.length() < 1.0e-6f) {
            AB = Vector2f(cosf(get_yaw()), sinf(get_yaw()));
        }
    }
    AB.normalize();

    // Calculate the NE position of the aircraft relative to WP A
    Vector2f A_air = location_diff(prev_WP, _current_loc);

    // calculate distance to target track, for reporting
    _crosstrack_error = A_air % AB;

    //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
    //and further than L1 distance from WP A. Then use WP A as the L1 reference point
    //Otherwise do normal L1 guidance
    float WP_A_dist = A_air.length();
    float alongTrackDist = A_air * AB;
    if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f)
    {
        //Calc Nu to fly To WP A
        Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
        xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
        ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
        Nu = atan2f(xtrackVel,ltrackVel);
        _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
    } else if (alongTrackDist > AB_length + groundSpeed*3) {
        // we have passed point B by 3 seconds. Head towards B
        // Calc Nu to fly To WP B
        Vector2f B_air = location_diff(next_WP, _current_loc);
        Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft
        xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line
        ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line
        Nu = atan2f(xtrackVel,ltrackVel);
        _nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point
    } else { //Calc Nu to fly along AB line

        //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
        xtrackVel = _groundspeed_vector % AB; // Velocity cross track
        ltrackVel = _groundspeed_vector * AB; // Velocity along track
        float Nu2 = atan2f(xtrackVel,ltrackVel);
        //Calculate Nu1 angle (Angle to L1 reference point)
        float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f);
        //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
        sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
        float Nu1 = asinf(sine_Nu1);

        // compute integral error component to converge to a crosstrack of zero when traveling
        // straight but reset it when disabled or if it changes. That allows for much easier
        // tuning by having it re-converge each time it changes.
        if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
            _L1_xtrack_i = 0;
            _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
        } else if (fabsf(Nu1) < radians(5)) {
            _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt;

            // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at
            _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f);
        }

        // to converge to zero we must push Nu1 harder
        Nu1 += _L1_xtrack_i;

        Nu = Nu1 + Nu2;
        _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point
    }

    _prevent_indecision(Nu);
    _last_Nu = Nu;

    //Limit Nu to +-(pi/2)
    Nu = constrain_float(Nu, -1.5708f, +1.5708f);
    _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);

    // Waypoint capture status is always false during waypoint following
    _WPcircle = false;

    _bearing_error = Nu; // bearing error angle (radians), +ve to left of track

    _data_is_stale = false; // status are correctly updated with current waypoint data
}
Beispiel #5
0
// flowhold_run - runs the flowhold controller
// should be called at 100hz or more
void Copter::ModeFlowHold::run()
{
    FlowHoldModeState flowhold_state;
    float takeoff_climb_rate = 0.0f;

    update_height_estimate();

    // initialize vertical speeds and acceleration
    copter.pos_control->set_max_speed_z(-get_pilot_speed_dn(), copter.g.pilot_speed_up);
    copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z);

    // apply SIMPLE mode transform to pilot inputs
    copter.update_simple_mode();

    // check for filter change
    if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) {
        flow_filter.set_cutoff_frequency(flow_filter_hz.get());
    }

    // get pilot desired climb rate
    float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in());
    target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up);

    // get pilot's desired yaw rate
    float target_yaw_rate = copter.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());

    if (!copter.motors->armed() || !copter.motors->get_interlock()) {
        flowhold_state = FlowHold_MotorStopped;
    } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
        flowhold_state = FlowHold_Takeoff;
    } else if (!copter.ap.auto_armed || copter.ap.land_complete) {
        flowhold_state = FlowHold_Landed;
    } else {
        flowhold_state = FlowHold_Flying;
    }

    if (copter.optflow.healthy()) {
        const float filter_constant = 0.95;
        quality_filtered = filter_constant * quality_filtered + (1-filter_constant) * copter.optflow.quality();
    } else {
        quality_filtered = 0;
    }

    Vector2f bf_angles;

    // calculate alt-hold angles
    int16_t roll_in = copter.channel_roll->get_control_in();
    int16_t pitch_in = copter.channel_pitch->get_control_in();
    float angle_max = copter.attitude_control->get_althold_lean_angle_max();
    get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y,angle_max, attitude_control->get_althold_lean_angle_max());

    if (quality_filtered >= flow_min_quality &&
        AP_HAL::millis() - copter.arm_time_ms > 3000) {
        // don't use for first 3s when we are just taking off
        Vector2f flow_angles;

        flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0));
        flow_angles.x = constrain_float(flow_angles.x, -angle_max/2, angle_max/2);
        flow_angles.y = constrain_float(flow_angles.y, -angle_max/2, angle_max/2);
        bf_angles += flow_angles;
    }
    bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
    bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);

    // Flow Hold State Machine
    switch (flowhold_state) {

    case FlowHold_MotorStopped:

        copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
        copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
#if FRAME_CONFIG == HELI_FRAME    
        // force descent rate and call position controller
        copter.pos_control->set_alt_target_from_climb_rate(-abs(copter.g.land_speed), copter.G_Dt, false);
#else
        copter.attitude_control->reset_rate_controller_I_terms();
        copter.attitude_control->set_yaw_target_to_current_heading();
        copter.pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
#endif
        flow_pi_xy.reset_I();
        copter.pos_control->update_z_controller();
        break;

    case FlowHold_Takeoff:
        // set motors to full range
        copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

        // initiate take-off
        if (!takeoff.running()) {
            takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
            // indicate we are taking off
            copter.set_land_complete(false);
            // clear i terms
            copter.set_throttle_takeoff();
        }

        // get take-off adjusted pilot and takeoff climb rates
        takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);

        // get avoidance adjusted climb rate
        target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);

        // call attitude controller
        copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);

        // call position controller
        copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false);
        copter.pos_control->add_takeoff_climb_rate(takeoff_climb_rate, copter.G_Dt);
        copter.pos_control->update_z_controller();
        break;

    case FlowHold_Landed:
#if FRAME_CONFIG == HELI_FRAME
        // helicopters do not spool down when landed.  Only when commanded to go to ground idle by pilot.
        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#else
        // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
        if (target_climb_rate < 0.0f) {
            copter.motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
        } else {
            copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
        }
#endif
        copter.attitude_control->reset_rate_controller_I_terms();
        copter.attitude_control->set_yaw_target_to_current_heading();
        copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
        copter.pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
        copter.pos_control->update_z_controller();
        break;

    case FlowHold_Flying:
        copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

#if AC_AVOID_ENABLED == ENABLED
        // apply avoidance
        copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max);
#endif

        // call attitude controller
        copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);

        // adjust climb rate using rangefinder
        target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt);

        // get avoidance adjusted climb rate
        target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);

        // call position controller
        copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false);
        copter.pos_control->update_z_controller();
        break;
    }
}
inline bool MacroAssembler::is_load_pcrelative_long(unsigned long inst) {
  // Load relative, 32-bit offset.
  return is_equal(inst, LRL_ZOPC, REL_LONG_MASK) || is_equal(inst, LGRL_ZOPC, REL_LONG_MASK); // off 16, len 32
}
inline bool MacroAssembler::is_load_addr_pcrel(address a) {
  return is_equal(a, LARL_ZOPC, LARL_MASK);
}
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
    // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
    // magnetometer, then declare the magnetometers as failed for this flight
    uint8_t maxCount = _ahrs->get_compass()->get_count();
    if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
        allMagSensorsFailed = true;
        return;
    }

    // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
    // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
    if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
        // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
        // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
        // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
        if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {

            // search through the list of magnetometers
            for (uint8_t i=1; i<maxCount; i++) {
                uint8_t tempIndex = magSelectIndex + i;
                // loop back to the start index if we have exceeded the bounds
                if (tempIndex >= maxCount) {
                    tempIndex -= maxCount;
                }
                // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
                if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
                    magSelectIndex = tempIndex;
                    hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex);
                    // reset the timeout flag and timer
                    magTimeout = false;
                    lastHealthyMagTime_ms = imuSampleTime_ms;
                    // zero the learned magnetometer bias states
                    stateStruct.body_magfield.zero();
                    // clear the measurement buffer
                    storedMag.reset();
                    }
            }
        }

        // detect changes to magnetometer offset parameters and reset states
        Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
        bool changeDetected = lastMagOffsetsValid && (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
        if (changeDetected) {
            // zero the learned magnetometer bias states
            stateStruct.body_magfield.zero();
            // clear the measurement buffer
            storedMag.reset();
        }
        lastMagOffsets = nowMagOffsets;
        lastMagOffsetsValid = true;

        // store time of last measurement update
        lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);

        // estimate of time magnetometer measurement was taken, allowing for delays
        magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;

        // Correct for the average intersampling delay due to the filter updaterate
        magDataNew.time_ms -= localFilterTimeStep_ms/2;

        // read compass data and scale to improve numerical conditioning
        magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;

        // check for consistent data between magnetometers
        consistentMagData = _ahrs->get_compass()->consistent();

        // save magnetometer measurement to buffer to be fused later
        storedMag.push(magDataNew);
    }
}
int* div(int* a,int* b,int size_a,int size_b,bool sign_a,bool sign_b,bool &sign_result,int &size_result,bool act)
{
	int* res;
	if(is_bigger(b,size_b,a,size_a))
	{
		if(!act)
		{
			size_result=1;
			res=(int*) malloc((size_result + 2)*sizeof(int));
			res[0]=0;
			res[1]=0;
			return res;
		}
		else
			{
			if(sign_a && sign_b)
				sign_result=1;
			if(!sign_a && sign_b)
				sign_result=0;
			size_result=size_a;
			res=(int*) malloc((size_result + 2)*sizeof(int));
			for(int i=0;i<size_result;++i)
				res[i]=a[i];
			return res;
		}
	}
	reverse_num(a,size_a);
	int size_q=size_a;
	int* q=(int*) malloc((size_q + 2)*sizeof(int));
	int size_cur=0;
	int* cur=(int*) malloc((size_q + 2)*sizeof(int));
	int size_r=size_b;
	for(int i=0;i<size_a;++i)
	{
		q[i]=0;
		cur[i]=0;
	}
	if(sign_a && sign_b)
		sign_result=0;
	else
		sign_result= sign_a || sign_b;
	int iq=0;
	for(int i=0;i<size_a;++i)
	{
		cur[size_cur]=a[i];
		
		++size_cur;
		
		reverse_num(cur,size_cur);
		if(is_bigger(b,size_b,cur,size_cur))
		{
			iq++;
			reverse_num(cur,size_cur);
			continue;
		}
		bool sign=0;
		
		while((is_bigger(cur,size_cur,b,size_b) || is_equal(cur,size_cur,b,size_b)) && !sign)
		{
			++q[iq];
			int aaa=size_cur;
			
			int* new_cur=sub(cur,b,aaa,size_b,0,0,sign,size_cur);
			free(cur);
			cur=new_cur;
			

		}
		++iq;
		reverse_num(cur,size_cur);
		
	}
	size_q=iq;
	reverse_num(cur,size_cur);	
	reverse_num(q,size_q);
	while (q[size_q-1] == 0)
		size_q-- ;
	while (cur[size_cur-1] == 0)
		size_cur-- ;
	if(act)
	{
		if(sign_a && sign_b)
				sign_result=1;
		if(!sign_a && sign_b)
				sign_result=0;
		free(q);
		res=cur;
		size_result=size_cur;
	}
	else
	{
		free(cur);
		res=q;
		size_result=size_q;
	}
  return res;
}
Beispiel #10
0
void SoloGimbal::send_controls(mavlink_channel_t chan)
{
    if (_state == GIMBAL_STATE_PRESENT_RUNNING) {
        // get the gimbal quaternion estimate
        Quaternion quatEst;
        _ekf.getQuat(quatEst);

        // run rate controller
        _ang_vel_dem_rads.zero();
        switch(get_mode()) {
            case GIMBAL_MODE_POS_HOLD_FF: {
                _ang_vel_dem_rads += get_ang_vel_dem_body_lock();
                _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
                float _ang_vel_dem_radsLen = _ang_vel_dem_rads.length();
                if (_ang_vel_dem_radsLen > radians(400)) {
                    _ang_vel_dem_rads *= radians(400)/_ang_vel_dem_radsLen;
                }
                mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
                                                _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
                break;
            }
            case GIMBAL_MODE_STABILIZE: {
                _ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst);
                _ang_vel_dem_rads += get_ang_vel_dem_tilt(quatEst);
                _ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst);
                _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
                float ang_vel_dem_norm = _ang_vel_dem_rads.length();
                if (ang_vel_dem_norm > radians(400)) {
                    _ang_vel_dem_rads *= radians(400)/ang_vel_dem_norm;
                }
                mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
                                                _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
                break;
            }
            default:
            case GIMBAL_MODE_IDLE:
            case GIMBAL_MODE_POS_HOLD:
                break;
        }
    }

    // set GMB_POS_HOLD
    if (get_mode() == GIMBAL_MODE_POS_HOLD) {
        _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1);
    } else {
        _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
    }

    // set GMB_MAX_TORQUE
    float max_torque;
    _gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0);
    if (!is_equal(max_torque,_max_torque) && !is_zero(max_torque)) {
        _max_torque = max_torque;
    }

    if (!hal.util->get_soft_armed() || joints_near_limits()) {
        _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, _max_torque);
    } else {
        _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, 0);
    }
}
static void
test_code(lzma_index *i)
{
	const size_t alloc_size = 128 * 1024;
	uint8_t *buf = malloc(alloc_size);
	expect(buf != NULL);

	// Encode
	lzma_stream strm = LZMA_STREAM_INIT;
	expect(lzma_index_encoder(&strm, i) == LZMA_OK);
	const lzma_vli index_size = lzma_index_size(i);
	succeed(coder_loop(&strm, NULL, 0, buf, index_size,
			LZMA_STREAM_END, LZMA_RUN));

	// Decode
	lzma_index *d;
	expect(lzma_index_decoder(&strm, &d, MEMLIMIT) == LZMA_OK);
	expect(d == NULL);
	succeed(decoder_loop(&strm, buf, index_size));

	expect(is_equal(i, d));

	lzma_index_end(d, NULL);
	lzma_end(&strm);

	// Decode with hashing
	lzma_index_hash *h = lzma_index_hash_init(NULL, NULL);
	expect(h != NULL);
	lzma_index_iter r;
	lzma_index_iter_init(&r, i);
	while (!lzma_index_iter_next(&r, LZMA_INDEX_ITER_BLOCK))
		expect(lzma_index_hash_append(h, r.block.unpadded_size,
				r.block.uncompressed_size) == LZMA_OK);
	size_t pos = 0;
	while (pos < index_size - 1)
		expect(lzma_index_hash_decode(h, buf, &pos, pos + 1)
				== LZMA_OK);
	expect(lzma_index_hash_decode(h, buf, &pos, pos + 1)
			== LZMA_STREAM_END);

	lzma_index_hash_end(h, NULL);

	// Encode buffer
	size_t buf_pos = 1;
	expect(lzma_index_buffer_encode(i, buf, &buf_pos, index_size)
			== LZMA_BUF_ERROR);
	expect(buf_pos == 1);

	succeed(lzma_index_buffer_encode(i, buf, &buf_pos, index_size + 1));
	expect(buf_pos == index_size + 1);

	// Decode buffer
	buf_pos = 1;
	uint64_t memlimit = MEMLIMIT;
	expect(lzma_index_buffer_decode(&d, &memlimit, NULL, buf, &buf_pos,
			index_size) == LZMA_DATA_ERROR);
	expect(buf_pos == 1);
	expect(d == NULL);

	succeed(lzma_index_buffer_decode(&d, &memlimit, NULL, buf, &buf_pos,
			index_size + 1));
	expect(buf_pos == index_size + 1);
	expect(is_equal(i, d));

	lzma_index_end(d, NULL);

	free(buf);
}
Beispiel #12
0
int openair0_device_init(openair0_device* device, openair0_config_t *openair0_cfg)
{
  uhd::set_thread_priority_safe(1.0);
  usrp_state_t *s = (usrp_state_t*)malloc(sizeof(usrp_state_t));
  memset(s, 0, sizeof(usrp_state_t));

  // Initialize USRP device
  std::string args = "type=b200";

  /*  std::string rx_subdev = "A:A A:B";
      std::string tx_subdev = "A:A A:B";*/

  uhd::device_addrs_t device_adds = uhd::device::find(args);
  size_t i;

  if(device_adds.size() == 0)
  {
    std::cerr<<"No USRP Device Found. " << std::endl;
    free(s);
    return -1;
  }
  s->usrp = uhd::usrp::multi_usrp::make(args);

  //  s->usrp->set_rx_subdev_spec(rx_subdev);
  //  s->usrp->set_tx_subdev_spec(tx_subdev);

  // lock mboard clocks
  s->usrp->set_clock_source("internal");
  // set master clock rate and sample rate for tx & rx for streaming
  s->usrp->set_master_clock_rate(30.72e6);




  for(i=0;i<s->usrp->get_rx_num_channels();i++) {
    if (i<openair0_cfg[0].rx_num_channels) {
      s->usrp->set_rx_rate(openair0_cfg[0].sample_rate,i);
      s->usrp->set_rx_bandwidth(openair0_cfg[0].rx_bw);
      printf("Setting rx freq/gain on channel %d/%d\n",i,s->usrp->get_rx_num_channels());
      s->usrp->set_rx_freq(openair0_cfg[0].rx_freq[i],i);
      s->usrp->set_rx_gain(openair0_cfg[0].rx_gain[i],i);
    }
  }
  for(i=0;i<s->usrp->get_tx_num_channels();i++) {
    if (i<openair0_cfg[0].tx_num_channels) {
      s->usrp->set_tx_rate(openair0_cfg[0].sample_rate,i);
      s->usrp->set_tx_bandwidth(openair0_cfg[0].tx_bw,i);
      printf("Setting tx freq/gain on channel %d/%d\n",i,s->usrp->get_tx_num_channels());
      s->usrp->set_tx_freq(openair0_cfg[0].tx_freq[i],i);
      s->usrp->set_tx_gain(openair0_cfg[0].tx_gain[i],i);
    }
  }



  // create tx & rx streamer
  uhd::stream_args_t stream_args_rx("sc16", "sc16");
  stream_args_rx.args["spp"] = str(boost::format("%d") % openair0_cfg[0].samples_per_packet);
  
  uhd::stream_args_t stream_args_tx("sc16", "sc16");
  stream_args_tx.args["spp"] = str(boost::format("%d") % openair0_cfg[0].samples_per_packet);
  for (i = 0; i<openair0_cfg[0].rx_num_channels; i++)
      stream_args_rx.channels.push_back(i);
  for (i = 0; i<openair0_cfg[0].tx_num_channels; i++)
      stream_args_tx.channels.push_back(i);

  s->tx_stream = s->usrp->get_tx_stream(stream_args_tx);
  s->rx_stream = s->usrp->get_rx_stream(stream_args_rx);

  s->usrp->set_time_now(uhd::time_spec_t(0.0));

  // display USRP settings
  for (i=0;i<openair0_cfg[0].rx_num_channels;i++) {
    if (i<openair0_cfg[0].rx_num_channels) {
      printf("RX Channel %d\n",i);
      std::cout << boost::format("Actual RX sample rate: %fMSps...") % (s->usrp->get_rx_rate(i)/1e6) << std::endl;
      std::cout << boost::format("Actual RX frequency: %fGHz...") % (s->usrp->get_rx_freq(i)/1e9) << std::endl;
      std::cout << boost::format("Actual RX gain: %f...") % (s->usrp->get_rx_gain(i)) << std::endl;
      std::cout << boost::format("Actual RX bandwidth: %fM...") % (s->usrp->get_rx_bandwidth(i)/1e6) << std::endl;
      std::cout << boost::format("Actual RX antenna: %s...") % (s->usrp->get_rx_antenna(i)) << std::endl;
    }
  }

  for (i=0;i<openair0_cfg[0].tx_num_channels;i++) {

    if (i<openair0_cfg[0].tx_num_channels) { 
      printf("TX Channel %d\n",i);
      std::cout << std::endl<<boost::format("Actual TX sample rate: %fMSps...") % (s->usrp->get_tx_rate(i)/1e6) << std::endl;
      std::cout << boost::format("Actual TX frequency: %fGHz...") % (s->usrp->get_tx_freq(i)/1e9) << std::endl;
      std::cout << boost::format("Actual TX gain: %f...") % (s->usrp->get_tx_gain(i)) << std::endl;
      std::cout << boost::format("Actual TX bandwidth: %fM...") % (s->usrp->get_tx_bandwidth(i)/1e6) << std::endl;
      std::cout << boost::format("Actual TX antenna: %s...") % (s->usrp->get_tx_antenna(i)) << std::endl;
    }
  }

  std::cout << boost::format("Device timestamp: %f...") % (s->usrp->get_time_now().get_real_secs()) << std::endl;

  device->priv = s;
  device->trx_start_func = trx_usrp_start;
  device->trx_end_func   = trx_usrp_end;
  device->trx_read_func  = trx_usrp_read;
  device->trx_write_func = trx_usrp_write;

  s->sample_rate = openair0_cfg[0].sample_rate;
  // TODO:
  // init tx_forward_nsamps based usrp_time_offset ex
  if(is_equal(s->sample_rate, (double)30.72e6))
    s->tx_forward_nsamps  = 176;
  if(is_equal(s->sample_rate, (double)15.36e6))
    s->tx_forward_nsamps = 90;
  if(is_equal(s->sample_rate, (double)7.68e6))
    s->tx_forward_nsamps = 50;

  return 0;
}
Beispiel #13
0
bool Bvector::is_proper_subset(const Bvector* super) const
{
  return (is_subset(super) && !is_equal(super));
}
Beispiel #14
0
// Save the variable to EEPROM, if supported
//
bool AP_Param::save(bool force_save)
{
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);
    const AP_Param *ap;

    if (info == NULL) {
        // we don't have any info on how to store it
        return false;
    }

    struct Param_header phdr;

    // create the header we will use to store the variable
    if (ginfo != NULL) {
        phdr.type = PGM_UINT8(&ginfo->type);
    } else {
        phdr.type = PGM_UINT8(&info->type);
    }
    phdr.key  = PGM_UINT8(&info->key);
    phdr.group_element = group_element;

    ap = this;
    if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
        // only vector3f can have non-zero idx for now
        return false;
    }
    if (idx != 0) {
        ap = (const AP_Param *)((uintptr_t)ap) - (idx*sizeof(float));
    }

    // scan EEPROM to find the right location
    uint16_t ofs;
    if (scan(&phdr, &ofs)) {
        // found an existing copy of the variable
        eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
        return true;
    }
    if (ofs == (uint16_t) ~0) {
        return false;
    }

    // if the value is the default value then don't save
    if (phdr.type <= AP_PARAM_FLOAT) {
        float v1 = cast_to_float((enum ap_var_type)phdr.type);
        float v2;
        if (ginfo != NULL) {
            v2 = get_default_value(&ginfo->def_value);
        } else {
            v2 = get_default_value(&info->def_value);
        }
        if (is_equal(v1,v2) && !force_save) {
            return true;
        }
        if (phdr.type != AP_PARAM_INT32 &&
            (fabsf(v1-v2) < 0.0001f*fabsf(v1))) {
            // for other than 32 bit integers, we accept values within
            // 0.01 percent of the current value as being the same
            return true;
        }
    }

    if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _storage.size()) {
        // we are out of room for saving variables
        hal.console->println_P(PSTR("EEPROM full"));
        return false;
    }

    // write a new sentinal, then the data, then the header
    write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type));
    eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
    eeprom_write_check(&phdr, ofs, sizeof(phdr));
    return true;
}
inline bool MacroAssembler::is_branchoncount_pcrelative_short(unsigned long inst) {
  // Branch relative on count, 16-bit offset.
  return is_equal(inst, BRCT_ZOPC) || is_equal(inst, BRCTG_ZOPC); // off 16, len 16
}
Beispiel #16
0
bool ogc_cs :: is_equal(
   const ogc_cs * p) const
{
   return is_equal(this, p);
}
inline bool MacroAssembler::is_branchonindex64_pcrelative_short(unsigned long inst) {
  // Branch relative on index (64bit), 16-bit offset.
  return is_equal(inst, BRXHG_ZOPC) || is_equal(inst, BRXLG_ZOPC); // off 16, len 16
}
void Shootout(const std::string &sCaption,
              std::vector<Benchmark*> vBenchmarks,
              std::vector<std::string> vExpr,
              int iCount,
              bool writeResultTable = false)
{
   char outstr[1024] = {0};
   char file  [1024] = {0};
   time_t t          = time(NULL);
   std::size_t excessive_failure_cnt = 0;

   sprintf(outstr, "Shootout_%%Y%%m%%d_%%H%%M%%S.txt");
   strftime(file, sizeof(file), outstr, localtime(&t));

   FILE* pRes = fopen(file, "w");
   assert(pRes);

   output(pRes, "Benchmark (Shootout for file \"%s\")\n", sCaption.c_str());

   Benchmark* pRefBench = vBenchmarks[0];

   std::map<double, std::vector<Benchmark*>> results;

   for (std::size_t i = 0; i < vExpr.size(); ++i)
   {
      std::size_t failure_count = 0;
      const std::string current_expr = vExpr[i];

      output(pRes, "\nExpression %d of %d: \"%s\"; Progress: ",
             (int)(i + 1),
             vExpr.size(),
             current_expr.c_str());

      double fRefResult = 0.0;
      double fRefSum    = 0.0;

      // Setup Reference parser result and total sum.
      {
         // Use the first as the reference parser.
         Benchmark *pBench = vBenchmarks[0];

         pBench->DoBenchmark(current_expr + " ", iCount);

         fRefResult = pBench->GetRes();
         fRefSum    = pBench->GetSum();

         pBench->IgnoreLastRate();

         if (
              (fRefResult ==  std::numeric_limits<double>::infinity()) ||
              (fRefResult == -std::numeric_limits<double>::infinity()) ||
              (fRefResult != fRefResult)
            )
         {
            output(pRes, "\nWARNING: Expression rejected due to non-numeric result.");
            continue;
         }
      }

      for (std::size_t j = 0; j < vBenchmarks.size(); ++j)
      {
         output(pRes, "#");  // <- "Progress" indicator for debugging, if a parser crashes we'd
                             //    like to know which one.

         Benchmark* pBench = vBenchmarks[j];

         std::string sExpr = current_expr;

         // Assign the current expression anew for each parser, furthermore preprocess the
         // expression string as some parsers use fancy characters to signal variables and
         // constants.
         pBench->PreprocessExpr(sExpr);

         double time = 1000000.0 * pBench->DoBenchmark(sExpr + " ", iCount);

         // The first parser is used for obtaining reference results.
         // If the reference result is a NaA the reference parser is
         // disqualified too.
         if (pBench->DidNotEvaluate())
         {
            pBench->AddFail(vExpr[i]);
            ++failure_count;
         }
         else if (
                   !is_equal(pBench->GetRes(),fRefResult)
                   ||
                   //Instead of 5, perhaps something proportional to iCount, but no less than 1?
                   (std::abs(static_cast<long long>(pBench->GetSum()) - static_cast<long long>(fRefSum)) > 5)
                 )
         {
            // Check the sum of all results and if the sum is ok,
            // check the last result of the benchmark run.
            pBench->AddFail(vExpr[i]);
            ++failure_count;
         }

         results[time].push_back(pBench);
      }

      output(pRes, "\n");

      int ct = 1;
      int parser_index = 0;
      for (auto it = results.begin(); it != results.end(); ++it)
      {
         const std::vector<Benchmark*>& vBench = it->second;

         for (std::size_t k = 0; k < vBench.size(); ++k)
         {
            Benchmark* pBench = vBench[k];

            if (pBench->ExpressionFailed(current_expr))
            {
               continue;
            }

            pBench->AddPoints(vBenchmarks.size() - ct + 1);
            pBench->AddScore(pRefBench->GetTime() / pBench->GetTime() );

            output(pRes, "[%02d] %-20s (%9.3f ns, %26.18f, %26.18f)\n",
                   static_cast<int>(++parser_index),
                   pBench->GetShortName().c_str(),
                   it->first,
                   pBench->GetRes(),
                   pBench->GetSum());
         }

         ct += vBench.size();
      }

      if (failure_count)
      {
         output(pRes, "DNQ List\n");
         parser_index = 0;
         for (auto it = results.begin(); it != results.end(); ++it)
         {
            const std::vector<Benchmark*>& vBench = it->second;

            for (std::size_t k = 0; k < vBench.size(); ++k)
            {
               Benchmark* pBench = vBench[k];

               if (!pBench->ExpressionFailed(current_expr))
                  continue;

               pBench->AddPoints(0);
               pBench->AddScore(0);

               if (pBench->DidNotEvaluate())
               {
                  output(pRes, "[%02d] %-20s (%s)\n",
                         static_cast<int>(++parser_index),
                         pBench->GetShortName().c_str(),
                         pBench->GetFailReason().c_str());
               }
               else
               {
                  output(pRes, "[%02d] %-20s (%9.3f ns, %26.18f, %26.18f)\n",
                         static_cast<int>(++parser_index),
                         pBench->GetShortName().c_str(),
                         it->first,
                         (pBench->GetRes() == pBench->GetRes()) ? pBench->GetRes() : 0.0,
                         (pBench->GetSum() == pBench->GetSum()) ? pBench->GetSum() : 0.0);
               }
            }
         }
      }

      if (failure_count > 2)
      {
         output(pRes, "**** ERROR ****   Excessive number of evaluation failures!  [%d]\n\n",
                failure_count);
         ++excessive_failure_cnt;
      }

      results.clear();
   }

   output(pRes, "\n\nBenchmark settings:\n");
   output(pRes, "  - Expressions File is \"%s\"\n"   , sCaption.c_str());
   output(pRes, "  - Reference parser is %s\n"       , pRefBench->GetShortName().c_str());
   output(pRes, "  - Iterations per expression: %d\n", iCount);
   output(pRes, "  - Number of expressions: %d\n"    , vExpr.size());
   if (excessive_failure_cnt)
   output(pRes, "  - Number of excessive failures: %d\n", excessive_failure_cnt);

   #if defined(_DEBUG)
   output(pRes, "  - Debug build\n");
   #else
   output(pRes, "  - Release build\n");
   #endif

   #if defined (__GNUC__)
   output(pRes, "  - Compiled with GCC Version %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__);
   #elif defined(_MSC_VER)
   output(pRes, "  - Compiled with MSVC Version %d\n", _MSC_VER);
   #endif

   output(pRes, "  - IEEE 754 (IEC 559) is %s\n", (std::numeric_limits<double>::is_iec559) ? "Available" : " NOT AVAILABLE");
   output(pRes, "  - %d-bit build\n", sizeof(void*)*8);

   dump_cpuid(pRes);

   output(pRes, "\n\nScores:\n");

   // Dump scores
   std::deque<std::pair<int,Benchmark*> > order_list;

   for (std::size_t i = 0; i < vBenchmarks.size(); ++i)
   {
      order_list.push_back(std::make_pair(vBenchmarks[i]->GetPoints(),vBenchmarks[i]));
   }

   std::sort(order_list.begin(),order_list.end());
   std::reverse(order_list.begin(),order_list.end());

   bool bHasFailures = false;

   output(pRes,  "  #     Parser                  Type            Points   Score   Failures\n");
   output(pRes,  "  -----------------------------------------------------------------------\n");

   for (std::size_t i = 0; i < order_list.size(); ++i)
   {
      Benchmark* pBench = order_list[i].second;
      bHasFailures |= (pBench->GetFails().size() > 0);

      output(pRes,  "  %02d\t%-20s\t%-10s\t%6d\t%6d\t%4d\n",
             i,
             pBench->GetShortName().c_str(),
             pBench->GetBaseType().c_str(),
             pBench->GetPoints(),
             (int)((pBench->GetScore() / (double)vExpr.size()) * 100.0),
             pBench->GetFails().size());
   }

   // Dump failures
   if (bHasFailures)
   {
      output(pRes, "\n\nFailures:\n");

      for (std::size_t i = 0; i < vBenchmarks.size(); ++i)
      {
         Benchmark *pBench = vBenchmarks[i];

         const std::map<std::string, std::string> &allFailures = pBench->GetFails();

         if (!allFailures.empty())
         {
            output(pRes, "  %-15s (%3d):\n",
                   pBench->GetShortName().c_str(),
                   allFailures.size());

            for (auto it = allFailures.begin(); it != allFailures.end(); ++it)
            {
               output(pRes, "         \"%s\" - \"%s\"\n",
                      it->first.c_str(),
                      it->second.c_str());
            }
         }
      }
   }

   if (writeResultTable)
   {
      WriteResultTable(pRes,vBenchmarks,vExpr);
   }

   fclose(pRes);
}
inline bool MacroAssembler::is_misc_pcrelative_long(unsigned long inst) {
  // Load address, execute relative, 32-bit offset.
  return is_equal(inst, LARL_ZOPC, REL_LONG_MASK) || is_equal(inst, EXRL_ZOPC, REL_LONG_MASK); // off 16, len 32
}
Beispiel #20
0
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
//   http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
    Vector3f velocity;
    uint32_t last_correction_time;

    // perform yaw drift correction if we have a new yaw reference
    // vector
    drift_correction_yaw();

    // rotate accelerometer values into the earth frame
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (_ins.get_accel_health(i)) {
            /*
              by using get_delta_velocity() instead of get_accel() the
              accel value is sampled over the right time delta for
              each sensor, which prevents an aliasing effect
             */
            Vector3f delta_velocity;
            float delta_velocity_dt;
            _ins.get_delta_velocity(i, delta_velocity);
            delta_velocity_dt = _ins.get_delta_velocity_dt(i);
            if (delta_velocity_dt > 0) {
                _accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
                // integrate the accel vector in the earth frame between GPS readings
                _ra_sum[i] += _accel_ef[i] * deltat;
            }
        }
    }

    //update _accel_ef_blended
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
    if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) {
        float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
        // slew _imu1_weight over one second
        _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
        _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
    } else {
        _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
    }
#else
    _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
#endif // HAL_CPU_CLASS >= HAL_CPU_CLASS_75

    // keep a sum of the deltat values, so we know how much time
    // we have integrated over
    _ra_deltat += deltat;

    if (!have_gps() ||
            _gps.status() < AP_GPS::GPS_OK_FIX_3D ||
            _gps.num_sats() < _gps_minsats) {
        // no GPS, or not a good lock. From experience we need at
        // least 6 satellites to get a really reliable velocity number
        // from the GPS.
        //
        // As a fallback we use the fixed wing acceleration correction
        // if we have an airspeed estimate (which we only have if
        // _fly_forward is set), otherwise no correction
        if (_ra_deltat < 0.2f) {
            // not enough time has accumulated
            return;
        }
        float airspeed;
        if (airspeed_sensor_enabled()) {
            airspeed = _airspeed->get_airspeed();
        } else {
            airspeed = _last_airspeed;
        }
        // use airspeed to estimate our ground velocity in
        // earth frame by subtracting the wind
        velocity = _dcm_matrix.colx() * airspeed;

        // add in wind estimate
        velocity += _wind;

        last_correction_time = hal.scheduler->millis();
        _have_gps_lock = false;
    } else {
        if (_gps.last_fix_time_ms() == _ra_sum_start) {
            // we don't have a new GPS fix - nothing more to do
            return;
        }
        velocity = _gps.velocity();
        last_correction_time = _gps.last_fix_time_ms();
        if (_have_gps_lock == false) {
            // if we didn't have GPS lock in the last drift
            // correction interval then set the velocities equal
            _last_velocity = velocity;
        }
        _have_gps_lock = true;

        // keep last airspeed estimate for dead-reckoning purposes
        Vector3f airspeed = velocity - _wind;
        airspeed.z = 0;
        _last_airspeed = airspeed.length();
    }

    if (have_gps()) {
        // use GPS for positioning with any fix, even a 2D fix
        _last_lat = _gps.location().lat;
        _last_lng = _gps.location().lng;
        _position_offset_north = 0;
        _position_offset_east = 0;

        // once we have a single GPS lock, we can update using
        // dead-reckoning from then on
        _have_position = true;
    } else {
        // update dead-reckoning position estimate
        _position_offset_north += velocity.x * _ra_deltat;
        _position_offset_east  += velocity.y * _ra_deltat;
    }

    // see if this is our first time through - in which case we
    // just setup the start times and return
    if (_ra_sum_start == 0) {
        _ra_sum_start = last_correction_time;
        _last_velocity = velocity;
        return;
    }

    // equation 9: get the corrected acceleration vector in earth frame. Units
    // are m/s/s
    Vector3f GA_e;
    GA_e = Vector3f(0, 0, -1.0f);

    if (_ra_deltat <= 0) {
        // waiting for more data
        return;
    }
    
    bool using_gps_corrections = false;
    float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);

    if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
        float v_scale = gps_gain.get() * ra_scale;
        Vector3f vdelta = (velocity - _last_velocity) * v_scale;
        GA_e += vdelta;
        GA_e.normalize();
        if (GA_e.is_inf()) {
            // wait for some non-zero acceleration information
            _last_failure_ms = hal.scheduler->millis();
            return;
        }
        using_gps_corrections = true;
    }

    // calculate the error term in earth frame.
    // we do this for each available accelerometer then pick the
    // accelerometer that leads to the smallest error term. This takes
    // advantage of the different sample rates on different
    // accelerometers to dramatically reduce the impact of aliasing
    // due to harmonics of vibrations that match closely the sampling
    // rate of our accelerometers. On the Pixhawk we have the LSM303D
    // running at 800Hz and the MPU6000 running at 1kHz, by combining
    // the two the effects of aliasing are greatly reduced.
    Vector3f error[INS_MAX_INSTANCES];
    float error_dirn[INS_MAX_INSTANCES];
    Vector3f GA_b[INS_MAX_INSTANCES];
    int8_t besti = -1;
    float best_error = 0;
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (!_ins.get_accel_health(i)) {
            // only use healthy sensors
            continue;
        }
        _ra_sum[i] *= ra_scale;

        // get the delayed ra_sum to match the GPS lag
        if (using_gps_corrections) {
            GA_b[i] = ra_delayed(i, _ra_sum[i]);
        } else {
            GA_b[i] = _ra_sum[i];
        }
        if (GA_b[i].is_zero()) {
            // wait for some non-zero acceleration information
            continue;
        }
        GA_b[i].normalize();
        if (GA_b[i].is_inf()) {
            // wait for some non-zero acceleration information
            continue;
        }
        error[i] = GA_b[i] % GA_e;
        // Take dot product to catch case vectors are opposite sign and parallel
        error_dirn[i] = GA_b[i] * GA_e;
        float error_length = error[i].length();
        if (besti == -1 || error_length < best_error) {
            besti = i;
            best_error = error_length;
        }
        // Catch case where orientation is 180 degrees out
        if (error_dirn[besti] < 0.0f) {
            best_error = 1.0f;
        }

    }

    if (besti == -1) {
        // no healthy accelerometers!
        _last_failure_ms = hal.scheduler->millis();
        return;
    }

    _active_accel_instance = besti;

#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
    // step 2 calculate earth_error_Z
    float earth_error_Z = error.z;

    // equation 10
    float tilt = pythagorous2(GA_e.x, GA_e.y);

    // equation 11
    float theta = atan2f(GA_b[besti].y, GA_b[besti].x);

    // equation 12
    Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);

    // step 6
    error = GA_b[besti] % GA_e2;
    error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION

    // to reduce the impact of two competing yaw controllers, we
    // reduce the impact of the gps/accelerometers on yaw when we are
    // flat, but still allow for yaw correction using the
    // accelerometers at high roll angles as long as we have a GPS
    if (AP_AHRS_DCM::use_compass()) {
        if (have_gps() && is_equal(gps_gain,1.0f)) {
            error[besti].z *= sinf(fabsf(roll));
        } else {
            error[besti].z = 0;
        }
    }

    // if ins is unhealthy then stop attitude drift correction and
    // hope the gyros are OK for a while. Just slowly reduce _omega_P
    // to prevent previous bad accels from throwing us off
    if (!_ins.healthy()) {
        error[besti].zero();
    } else {
        // convert the error term to body frame
        error[besti] = _dcm_matrix.mul_transpose(error[besti]);
    }

    if (error[besti].is_nan() || error[besti].is_inf()) {
        // don't allow bad values
        check_matrix();
        _last_failure_ms = hal.scheduler->millis();
        return;
    }

    _error_rp = 0.8f * _error_rp + 0.2f * best_error;

    // base the P gain on the spin rate
    float spin_rate = _omega.length();

    // sanity check _kp value
    if (_kp < AP_AHRS_RP_P_MIN) {
        _kp = AP_AHRS_RP_P_MIN;
    }

    // we now want to calculate _omega_P and _omega_I. The
    // _omega_P value is what drags us quickly to the
    // accelerometer reading.
    _omega_P = error[besti] * _P_gain(spin_rate) * _kp;
    if (use_fast_gains()) {
        _omega_P *= 8;
    }

    if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
            _gps.ground_speed() < GPS_SPEED_MIN &&
            _ins.get_accel().x >= 7 &&
            pitch_sensor > -3000 && pitch_sensor < 3000) {
        // assume we are in a launch acceleration, and reduce the
        // rp gain by 50% to reduce the impact of GPS lag on
        // takeoff attitude when using a catapult
        _omega_P *= 0.5f;
    }

    // accumulate some integrator error
    if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
        _omega_I_sum += error[besti] * _ki * _ra_deltat;
        _omega_I_sum_time += _ra_deltat;
    }

    if (_omega_I_sum_time >= 5) {
        // limit the rate of change of omega_I to the hardware
        // reported maximum gyro drift rate. This ensures that
        // short term errors don't cause a buildup of omega_I
        // beyond the physical limits of the device
        float change_limit = _gyro_drift_limit * _omega_I_sum_time;
        _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
        _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
        _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
        _omega_I += _omega_I_sum;
        _omega_I_sum.zero();
        _omega_I_sum_time = 0;
    }

    // zero our accumulator ready for the next GPS step
    memset(&_ra_sum[0], 0, sizeof(_ra_sum));
    _ra_deltat = 0;
    _ra_sum_start = last_correction_time;

    // remember the velocity for next time
    _last_velocity = velocity;
}
Beispiel #21
0
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
    switch (msg->msgid) {

    // If we are currently operating as a proxy for a remote, 
    // alas we have to look inside each packet to see if its for us or for the remote
    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
    {
        handle_request_data_stream(msg, true);
        break;
    }


    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
    {
        handle_param_request_list(msg);
        break;
    }

    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
    {
        handle_param_request_read(msg);
        break;
    }

    case MAVLINK_MSG_ID_PARAM_SET:
    {
        handle_param_set(msg, NULL);
        break;
    }

    case MAVLINK_MSG_ID_HEARTBEAT:
        break;

    case MAVLINK_MSG_ID_COMMAND_LONG:
    {
        // decode
        mavlink_command_long_t packet;
        mavlink_msg_command_long_decode(msg, &packet);
        
        uint8_t result = MAV_RESULT_UNSUPPORTED;
        
        // do command
        send_text_P(SEVERITY_LOW,PSTR("command received: "));
        
        switch(packet.command) {
            
            case MAV_CMD_PREFLIGHT_CALIBRATION:
            {
                if (is_equal(packet.param1,1.0f)) {
                    tracker.ins.init_gyro();
                    if (tracker.ins.gyro_calibrated_ok_all()) {
                        tracker.ahrs.reset_gyro_drift();
                        result = MAV_RESULT_ACCEPTED;
                    } else {
                        result = MAV_RESULT_FAILED;
                    }
                } 
                if (is_equal(packet.param3,1.0f)) {
                    tracker.init_barometer();
                    // zero the altitude difference on next baro update
                    tracker.nav_status.need_altitude_calibration = true;
                }
                if (is_equal(packet.param4,1.0f)) {
                    // Cant trim radio
                } else if (is_equal(packet.param5,1.0f)) {
                    float trim_roll, trim_pitch;
                    AP_InertialSensor_UserInteract_MAVLink interact(this);
                    if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
                        // reset ahrs's trim to suggested values from calibration routine
                        tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
                    }
                } else if (is_equal(packet.param5,2.0f)) {
                    // accel trim
                    float trim_roll, trim_pitch;
                    if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
                        // reset ahrs's trim to suggested values from calibration routine
                        tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
                        result = MAV_RESULT_ACCEPTED;
                    } else {
                        result = MAV_RESULT_FAILED;
                    }
                }
                result = MAV_RESULT_ACCEPTED;
                break;
            }

            case MAV_CMD_COMPONENT_ARM_DISARM:
                if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
                    if (is_equal(packet.param1,1.0f)) {
                        tracker.arm_servos();
                        result = MAV_RESULT_ACCEPTED;
                    } else if (is_zero(packet.param1))  {
                        tracker.disarm_servos();
                        result = MAV_RESULT_ACCEPTED;
                    } else {
                        result = MAV_RESULT_UNSUPPORTED;
                    }
                } else {
                    result = MAV_RESULT_UNSUPPORTED;
                }
            break;

            case MAV_CMD_DO_SET_MODE:
                switch ((uint16_t)packet.param1) {
                    case MAV_MODE_MANUAL_ARMED:
                    case MAV_MODE_MANUAL_DISARMED:
                        tracker.set_mode(MANUAL);
                        result = MAV_RESULT_ACCEPTED;
                        break;

                    case MAV_MODE_AUTO_ARMED:
                    case MAV_MODE_AUTO_DISARMED:
                        tracker.set_mode(AUTO);
                        result = MAV_RESULT_ACCEPTED;
                        break;

                    default:
                        result = MAV_RESULT_UNSUPPORTED;
                }
                break;

            case MAV_CMD_DO_SET_SERVO:
                if (tracker.servo_test_set_servo(packet.param1, packet.param2)) {
                    result = MAV_RESULT_ACCEPTED;
                }
                break;

                // mavproxy/mavutil sends this when auto command is entered 
            case MAV_CMD_MISSION_START:
                tracker.set_mode(AUTO);
                result = MAV_RESULT_ACCEPTED;
                break;

            case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
            {
                if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
                    // when packet.param1 == 3 we reboot to hold in bootloader
                    hal.scheduler->reboot(is_equal(packet.param1,3.0f));
                    result = MAV_RESULT_ACCEPTED;
                }
                break;
            }

            case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
                if (is_equal(packet.param1,1.0f)) {
                    tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
                    result = MAV_RESULT_ACCEPTED;
                }
                break;
            }

            default:
                break;
        }
        mavlink_msg_command_ack_send(
            chan,
            packet.command,
            result);
        
        break;
    }
         
    // When mavproxy 'wp sethome' 
    case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
    {
        // decode
        mavlink_mission_write_partial_list_t packet;
        mavlink_msg_mission_write_partial_list_decode(msg, &packet);
        if (packet.start_index == 0)
        {
            // New home at wp index 0. Ask for it
            waypoint_receiving = true;
            waypoint_request_i = 0;
            waypoint_request_last = 0;
            send_message(MSG_NEXT_WAYPOINT);
            waypoint_receiving = true;
        }
        break;
    }

    // XXX receive a WP from GCS and store in EEPROM if it is HOME
    case MAVLINK_MSG_ID_MISSION_ITEM:
    {
        // decode
        mavlink_mission_item_t packet;
        uint8_t result = MAV_MISSION_ACCEPTED;

        mavlink_msg_mission_item_decode(msg, &packet);

        struct Location tell_command = {};

        switch (packet.frame)
        {
        case MAV_FRAME_MISSION:
        case MAV_FRAME_GLOBAL:
        {
            tell_command.lat = 1.0e7f*packet.x;                                     // in as DD converted to * t7
            tell_command.lng = 1.0e7f*packet.y;                                     // in as DD converted to * t7
            tell_command.alt = packet.z*1.0e2f;                                     // in as m converted to cm
            tell_command.options = 0;                                     // absolute altitude
            break;
        }

#ifdef MAV_FRAME_LOCAL_NED
        case MAV_FRAME_LOCAL_NED:                         // local (relative to home position)
        {
            tell_command.lat = 1.0e7f*ToDeg(packet.x/
                                           (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
            tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
            tell_command.alt = -packet.z*1.0e2f;
            tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
            break;
        }
#endif

#ifdef MAV_FRAME_LOCAL
        case MAV_FRAME_LOCAL:                         // local (relative to home position)
        {
            tell_command.lat = 1.0e7f*ToDeg(packet.x/
                                           (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
            tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
            tell_command.alt = packet.z*1.0e2f;
            tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
            break;
        }
#endif

        case MAV_FRAME_GLOBAL_RELATIVE_ALT:                         // absolute lat/lng, relative altitude
        {
            tell_command.lat = 1.0e7f * packet.x;                                     // in as DD converted to * t7
            tell_command.lng = 1.0e7f * packet.y;                                     // in as DD converted to * t7
            tell_command.alt = packet.z * 1.0e2f;
            tell_command.options = MASK_OPTIONS_RELATIVE_ALT;                                     // store altitude relative!! Always!!
            break;
        }

        default:
            result = MAV_MISSION_UNSUPPORTED_FRAME;
            break;
        }

        if (result != MAV_MISSION_ACCEPTED) goto mission_failed;

        // Check if receiving waypoints (mission upload expected)
        if (!waypoint_receiving) {
            result = MAV_MISSION_ERROR;
            goto mission_failed;
        }

        // check if this is the HOME wp
        if (packet.seq == 0) {
            tracker.set_home(tell_command); // New home in EEPROM
            send_text_P(SEVERITY_LOW,PSTR("new HOME received"));
            waypoint_receiving = false;
        }

mission_failed:
        // we are rejecting the mission/waypoint
        mavlink_msg_mission_ack_send(
            chan,
            msg->sysid,
            msg->compid,
            result);
        break;
    }

    case MAVLINK_MSG_ID_MANUAL_CONTROL:
    {
        mavlink_manual_control_t packet;
        mavlink_msg_manual_control_decode(msg, &packet);
        tracker.tracking_manual_control(packet);
        break;
    }

    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: 
    {
        // decode
        mavlink_global_position_int_t packet;
        mavlink_msg_global_position_int_decode(msg, &packet);
        tracker.tracking_update_position(packet);
        break;
    }

    case MAVLINK_MSG_ID_SCALED_PRESSURE: 
    {
        // decode
        mavlink_scaled_pressure_t packet;
        mavlink_msg_scaled_pressure_decode(msg, &packet);
        tracker.tracking_update_pressure(packet);
        break;
    }

    case MAVLINK_MSG_ID_SET_MODE:
    {
        handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t));
        break;
    }

#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
    case MAVLINK_MSG_ID_SERIAL_CONTROL:
        handle_serial_control(msg, tracker.gps);
        break;

    case MAVLINK_MSG_ID_GPS_INJECT_DATA:
        handle_gps_inject(msg, tracker.gps);
        break;

#endif

    case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
        tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
        break;

    } // end switch
} // end handle mavlink
Beispiel #22
0
bool Vector3<T>::operator ==(const Vector3<T> &v) const
{
    return (is_equal(x,v.x) && is_equal(y,v.y) && is_equal(z,v.z));
}
Beispiel #23
0
bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
    // Have we reached the waypoint i.e. are we within waypoint_radius of the waypoint
    if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
        // check if we are loitering at this waypoint - the message sent to the GCS is different
        if (loiter_duration > 0) {
            // Check if this is the first time we have reached the waypoint
            if (!previously_reached_wp) {
                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
                        static_cast<uint32_t>(cmd.index),
                        static_cast<uint32_t>(loiter_duration));
                // record the current time i.e. start timer
                loiter_start_time = millis();
                previously_reached_wp = true;
            }

            distance_past_wp = wp_distance;

            // Check if we have loiter long enough
            if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
                return false;
            }
        } else {
            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Distance %dm",
                    static_cast<uint32_t>(cmd.index),
                    static_cast<int32_t>(fabsf(get_distance(current_loc, next_WP))));
        }
        // set loiter_duration to 0 so we know we aren't or have finished loitering
        loiter_duration = 0;
        return true;
    }

    // have we gone past the waypoint?
    // We should always go through the waypoint i.e. the above code
    // first before we go past it but sometimes we don't.
    // OR have we reached the waypoint previously be we aren't actively loitering
    // This second check is required for when we roll past the waypoint radius
    if (location_passed_point(current_loc, prev_WP, next_WP) ||
        (!active_loiter && previously_reached_wp)) {
        // As we have passed the waypoint navigation needs to be done from current location
        prev_WP = current_loc;
        // Check if this is the first time we have reached the waypoint even though we have gone past it
        if (!previously_reached_wp) {
            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
                    static_cast<uint32_t>(cmd.index),
                    static_cast<uint32_t>(loiter_duration));
            // record the current time i.e. start timer
            loiter_start_time = millis();
            previously_reached_wp = true;
            distance_past_wp = wp_distance;
        }

        // check if distance to the WP has changed and output new message if it has
        const float dist_to_wp = get_distance(current_loc, next_WP);
        if (!is_equal(distance_past_wp, dist_to_wp)) {
            distance_past_wp = dist_to_wp;
            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%u. Distance %dm",
                    static_cast<uint32_t>(cmd.index),
                    static_cast<int32_t>(fabsf(distance_past_wp)));
        }

        // Check if we need to loiter at this waypoint
        if (loiter_duration > 0) {
            if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
                return false;
            }
        }
        // set loiter_duration to 0 so we know we aren't or have finished loitering
        loiter_duration = 0;
        return true;
    }

    return false;
}
Beispiel #24
0
bool Vector3<T>::operator !=(const Vector3<T> &v) const
{
    return (!is_equal(x,v.x) || !is_equal(y,v.y) || !is_equal(z,v.z));
}
Beispiel #25
0
bool ogc_method :: is_equal(
   const ogc_method * p) const
{
   return is_equal(this, p);
}
inline bool MacroAssembler::is_call_pcrelative_long(unsigned long inst) {
  return is_equal(inst, BRASL_ZOPC); // off 16, len 32
}
Beispiel #27
0
double ElasticNature::getRhoAsForceDensity() const {
	return (is_equal(rho, UNAVAILABLE_DOUBLE)) ? 0 : rho;
}
inline bool MacroAssembler::is_branch_pcrelative_long(unsigned long inst) {
  // Branch relative, 32-bit offset.
  return is_equal(inst, BRCL_ZOPC); // off 16, len 32
}
Beispiel #29
0
double ElasticNature::getTref() const {
	return (is_equal(tref, UNAVAILABLE_DOUBLE)) ? 0 : tref;
}
Beispiel #30
0
MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet)
{
    switch(packet.command) {

    case MAV_CMD_NAV_TAKEOFF: {
        // param3 : horizontal navigation by pilot acceptable
        // param4 : yaw angle   (not supported)
        // param5 : latitude    (not supported)
        // param6 : longitude   (not supported)
        // param7 : altitude [metres]

        float takeoff_alt = packet.param7 * 100;      // Convert m to cm

        if (!copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;
    }

    case MAV_CMD_NAV_LOITER_UNLIM:
        if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
        if (!copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;

    case MAV_CMD_NAV_LAND:
        if (!copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;

#if MODE_FOLLOW_ENABLED == ENABLED
    case MAV_CMD_DO_FOLLOW:
        // param1: sysid of target to follow
        if ((packet.param1 > 0) && (packet.param1 <= 255)) {
            copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
#endif

    case MAV_CMD_CONDITION_YAW:
        // param1 : target angle [0-360]
        // param2 : speed during change [deg per second]
        // param3 : direction (-1:ccw, +1:cw)
        // param4 : relative offset (1) or absolute angle (0)
        if ((packet.param1 >= 0.0f)   &&
            (packet.param1 <= 360.0f) &&
            (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
            copter.flightmode->auto_yaw.set_fixed_yaw(
                packet.param1,
                packet.param2,
                (int8_t)packet.param3,
                is_positive(packet.param4));
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;

    case MAV_CMD_DO_CHANGE_SPEED:
        // param1 : unused
        // param2 : new speed in m/s
        // param3 : unused
        // param4 : unused
        if (packet.param2 > 0.0f) {
            if (packet.param1 > 2.9f) { // 3 = speed down
                copter.wp_nav->set_speed_down(packet.param2 * 100.0f);
            } else if (packet.param1 > 1.9f) { // 2 = speed up
                copter.wp_nav->set_speed_up(packet.param2 * 100.0f);
            } else {
                copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
            }
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;

#if MODE_AUTO_ENABLED == ENABLED
    case MAV_CMD_MISSION_START:
        if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
            copter.set_auto_armed(true);
            if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) {
                copter.mode_auto.mission.start_or_resume();
            }
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
#endif

    case MAV_CMD_COMPONENT_ARM_DISARM:
        if (is_equal(packet.param1,1.0f)) {
            // attempt to arm and return success or failure
            const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value);
            if (copter.init_arm_motors(AP_Arming::Method::MAVLINK, do_arming_checks)) {
                return MAV_RESULT_ACCEPTED;
            }
        } else if (is_zero(packet.param1))  {
            if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value)) {
                // force disarming by setting param2 = 21196 is deprecated
                copter.init_disarm_motors();
                return MAV_RESULT_ACCEPTED;
            } else {
                return MAV_RESULT_FAILED;
            }
        } else {
            return MAV_RESULT_UNSUPPORTED;
        }
        return MAV_RESULT_FAILED;

#if PARACHUTE == ENABLED
    case MAV_CMD_DO_PARACHUTE:
        // configure or release parachute
        switch ((uint16_t)packet.param1) {
        case PARACHUTE_DISABLE:
            copter.parachute.enabled(false);
            copter.Log_Write_Event(DATA_PARACHUTE_DISABLED);
            return MAV_RESULT_ACCEPTED;
        case PARACHUTE_ENABLE:
            copter.parachute.enabled(true);
            copter.Log_Write_Event(DATA_PARACHUTE_ENABLED);
            return MAV_RESULT_ACCEPTED;
        case PARACHUTE_RELEASE:
            // treat as a manual release which performs some additional check of altitude
            copter.parachute_manual_release();
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
#endif

    case MAV_CMD_DO_MOTOR_TEST:
        // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
        // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
        // param3 : throttle (range depends upon param2)
        // param4 : timeout (in seconds)
        // param5 : num_motors (in sequence)
        // param6 : compass learning (0: disabled, 1: enabled)
        return copter.mavlink_motor_test_start(chan,
                                               (uint8_t)packet.param1,
                                               (uint8_t)packet.param2,
                                               (uint16_t)packet.param3,
                                               packet.param4,
                                               (uint8_t)packet.param5);

#if WINCH_ENABLED == ENABLED
    case MAV_CMD_DO_WINCH:
        // param1 : winch number (ignored)
        // param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum.
        if (!copter.g2.winch.enabled()) {
            return MAV_RESULT_FAILED;
        }
        switch ((uint8_t)packet.param2) {
        case WINCH_RELAXED:
            copter.g2.winch.relax();
            copter.Log_Write_Event(DATA_WINCH_RELAXED);
            return MAV_RESULT_ACCEPTED;
        case WINCH_RELATIVE_LENGTH_CONTROL: {
            copter.g2.winch.release_length(packet.param3, fabsf(packet.param4));
            copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
            return MAV_RESULT_ACCEPTED;
        }
        case WINCH_RATE_CONTROL:
            if (fabsf(packet.param4) <= copter.g2.winch.get_rate_max()) {
                copter.g2.winch.set_desired_rate(packet.param4);
                copter.Log_Write_Event(DATA_WINCH_RATE_CONTROL);
                return MAV_RESULT_ACCEPTED;
            }
            return MAV_RESULT_FAILED;
        default:
            break;
        }
        return MAV_RESULT_FAILED;
#endif

        case MAV_CMD_AIRFRAME_CONFIGURATION: {
            // Param 1: Select which gear, not used in ArduPilot
            // Param 2: 0 = Deploy, 1 = Retract
            // For safety, anything other than 1 will deploy
            switch ((uint8_t)packet.param2) {
                case 1:
                    copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
                    return MAV_RESULT_ACCEPTED;
                default:
                    copter.landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
                    return MAV_RESULT_ACCEPTED;
            }
            return MAV_RESULT_FAILED;
        }

        /* Solo user presses Fly button */
    case MAV_CMD_SOLO_BTN_FLY_CLICK: {
        if (copter.failsafe.radio) {
            return MAV_RESULT_ACCEPTED;
        }

        // set mode to Loiter or fall back to AltHold
        if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
            copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
        }
        return MAV_RESULT_ACCEPTED;
    }

        /* Solo user holds down Fly button for a couple of seconds */
    case MAV_CMD_SOLO_BTN_FLY_HOLD: {
        if (copter.failsafe.radio) {
            return MAV_RESULT_ACCEPTED;
        }

        if (!copter.motors->armed()) {
            // if disarmed, arm motors
            copter.init_arm_motors(AP_Arming::Method::MAVLINK);
        } else if (copter.ap.land_complete) {
            // if armed and landed, takeoff
            if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
                copter.flightmode->do_user_takeoff(packet.param1*100, true);
            }
        } else {
            // if flying, land
            copter.set_mode(LAND, MODE_REASON_GCS_COMMAND);
        }
        return MAV_RESULT_ACCEPTED;
    }

        /* Solo user presses pause button */
    case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {
        if (copter.failsafe.radio) {
            return MAV_RESULT_ACCEPTED;
        }

        if (copter.motors->armed()) {
            if (copter.ap.land_complete) {
                // if landed, disarm motors
                copter.init_disarm_motors();
            } else {
                // assume that shots modes are all done in guided.
                // NOTE: this may need to change if we add a non-guided shot mode
                bool shot_mode = (!is_zero(packet.param1) && (copter.control_mode == GUIDED || copter.control_mode == GUIDED_NOGPS));

                if (!shot_mode) {
#if MODE_BRAKE_ENABLED == ENABLED
                    if (copter.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) {
                        copter.mode_brake.timeout_to_loiter_ms(2500);
                    } else {
                        copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
                    }
#else
                    copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
#endif
                } else {
                    // SoloLink is expected to handle pause in shots
                }
            }
        }
        return MAV_RESULT_ACCEPTED;
    }

    default:
        return GCS_MAVLINK::handle_command_long_packet(packet);
    }
}