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
const value is_equal(::zval const& rhs) const { return is_equal(rhs BOOST_PHP_TSRM_DIRECT_CC); }
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 }
// 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; }
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); }
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; }
bool Bvector::is_proper_subset(const Bvector* super) const { return (is_subset(super) && !is_equal(super)); }
// 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 }
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 }
// 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; }
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
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)); }
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; }
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)); }
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 }
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 }
double ElasticNature::getTref() const { return (is_equal(tref, UNAVAILABLE_DOUBLE)) ? 0 : tref; }
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); } }