ring_elem SchurRing2::add(const ring_elem f, const ring_elem g) const { if (is_zero(f)) return g; if (is_zero(g)) return f; const schur_poly *f1 = f.schur_poly_val; const schur_poly *g1 = g.schur_poly_val; ring_elem resultRE; schur_poly *result = new schur_poly; resultRE.schur_poly_val = result; schur_poly::iterator i = f1->begin(); schur_poly::iterator j = g1->begin(); schur_poly::iterator iend = f1->end(); schur_poly::iterator jend = g1->end(); bool done = false; while (!done) { int cmp = compare_partitions(i.getMonomial(), j.getMonomial()); switch (cmp) { case LT: result->appendTerm(j.getCoefficient(), j.getMonomial()); ++j; if (j == jend) { result->append(i, iend); done = true; } break; case GT: result->appendTerm(i.getCoefficient(), i.getMonomial()); ++i; if (i == iend) { result->append(j, jend); done = true; } break; case EQ: ring_elem c = coefficientRing->add(i.getCoefficient(), j.getCoefficient()); if (!coefficientRing->is_zero(c)) result->appendTerm(c, i.getMonomial()); ++j; ++i; if (j == jend) { result->append(i, iend); done = true; } else { if (i == iend) { result->append(j, jend); done = true; } } break; } } return resultRE; }
/// advance_spline_target_along_track - move target location along track from origin to destination void AC_WPNav::advance_spline_target_along_track(float dt) { if (!_flags.reached_destination) { Vector3f target_pos, target_vel; // update target position and velocity from spline calculator calc_spline_pos_vel(_spline_time, target_pos, target_vel); _pos_delta_unit = target_vel/target_vel.length(); calculate_wp_leash_length(); // get current location Vector3f curr_pos = _inav.get_position(); Vector3f track_error = curr_pos - target_pos; // calculate the horizontal error float track_error_xy = pythagorous2(track_error.x, track_error.y); // calculate the vertical error float track_error_z = fabsf(track_error.z); // get position control leash lengths float leash_xy = _pos_control.get_leash_xy(); float leash_z; if (track_error.z >= 0) { leash_z = _pos_control.get_leash_up_z(); }else{ leash_z = _pos_control.get_leash_down_z(); } // calculate how far along the track we could move the intermediate target before reaching the end of the leash float track_leash_slack = min(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); if (track_leash_slack < 0.0f) { track_leash_slack = 0.0f; } // update velocity float spline_dist_to_wp = (_destination - target_pos).length(); float vel_limit = _wp_speed_cms; if (!is_zero(dt)) { vel_limit = min(vel_limit, track_leash_slack/dt); } // if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) { _spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms); }else if(_spline_vel_scaler < vel_limit) { // increase velocity using acceleration _spline_vel_scaler += _wp_accel_cms * dt; } // constrain target velocity _spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit); // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator float target_vel_length = target_vel.length(); if (!is_zero(target_vel_length)) { _spline_time_scale = _spline_vel_scaler/target_vel_length; } // update target position _pos_control.set_pos_target(target_pos); // update the yaw _yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x)); // advance spline time to next step _spline_time += _spline_time_scale*dt; // we will reach the next waypoint in the next step so set reached_destination flag // To-Do: is this one step too early? if (_spline_time >= 1.0f) { _flags.reached_destination = true; } } }
// guided_posvel_control_run - runs the guided spline controller // called from guided_run void Copter::ModeGuided::posvel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { // set target position and velocity to current position and velocity pos_control->set_pos_target(inertial_nav.get_position()); pos_control->set_desired_velocity(Vector3f(0,0,0)); zero_throttle_and_relax_ac(); return; } // process pilot's yaw input float target_yaw_rate = 0; if (!copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { auto_yaw.set_mode(AUTO_YAW_HOLD); } } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { guided_vel_target_cms.zero(); if (auto_yaw.mode() == AUTO_YAW_RATE) { auto_yaw.set_rate(0.0f); } } // calculate dt float dt = pos_control->time_since_last_xy_update(); // sanity check dt if (dt >= 0.2f) { dt = 0.0f; } // advance position target using velocity target guided_pos_target_cm += guided_vel_target_cms * dt; // send position and velocity targets to position controller pos_control->set_pos_target(guided_pos_target_cm); pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y); // run position controllers pos_control->update_xy_controller(ekfNavVelGainScaler); pos_control->update_z_controller(); // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true); } }
/* update navigation for normal mission waypoints. Return true when the waypoint is complete */ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { steer_state.hold_course_cd = -1; // depending on the pass by flag either go to waypoint in regular manner or // fly past it for set distance along the line of waypoints Location flex_next_WP_loc = next_WP_loc; uint8_t cmd_passby = HIGHBYTE(cmd.p1); // distance in meters to pass beyond the wp uint8_t cmd_acceptance_distance = LOWBYTE(cmd.p1); // radius in meters to accept reaching the wp if (cmd_passby > 0) { float dist = get_distance(prev_WP_loc, flex_next_WP_loc); if (!is_zero(dist)) { float factor = (dist + cmd_passby) / dist; flex_next_WP_loc.lat = flex_next_WP_loc.lat + (flex_next_WP_loc.lat - prev_WP_loc.lat) * (factor - 1.0f); flex_next_WP_loc.lng = flex_next_WP_loc.lng + (flex_next_WP_loc.lng - prev_WP_loc.lng) * (factor - 1.0f); } } if (auto_state.no_crosstrack) { nav_controller->update_waypoint(current_loc, flex_next_WP_loc); } else { nav_controller->update_waypoint(prev_WP_loc, flex_next_WP_loc); } // see if the user has specified a maximum distance to waypoint // If override with p3 - then this is not used as it will overfly badly if (g.waypoint_max_radius > 0 && auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) { if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) { // this is needed to ensure completion of the waypoint if (cmd_passby == 0) { prev_WP_loc = current_loc; } } return false; } float acceptance_distance_m = 0; // default to: if overflown - let it fly up to the point if (cmd_acceptance_distance > 0) { // allow user to override acceptance radius acceptance_distance_m = cmd_acceptance_distance; } else if (cmd_passby == 0) { acceptance_distance_m = nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle); } else { } if (auto_state.wp_distance <= acceptance_distance_m) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um", (unsigned)mission.get_current_nav_cmd().index, (unsigned)get_distance(current_loc, flex_next_WP_loc)); return true; } // have we flown past the waypoint? if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um", (unsigned)mission.get_current_nav_cmd().index, (unsigned)get_distance(current_loc, flex_next_WP_loc)); return true; } return false; }
static void rx_data_eapol_key_2_of_4(struct wlantest *wt, const u8 *dst, const u8 *src, const u8 *data, size_t len) { struct wlantest_bss *bss; struct wlantest_sta *sta; const struct ieee802_1x_hdr *eapol; const struct wpa_eapol_key *hdr; const u8 *key_data, *kck; u16 key_info, key_data_len; struct wpa_eapol_ie_parse ie; wpa_printf(MSG_DEBUG, "EAPOL-Key 2/4 " MACSTR " -> " MACSTR, MAC2STR(src), MAC2STR(dst)); bss = bss_get(wt, dst); if (bss == NULL) return; sta = sta_get(bss, src); if (sta == NULL) return; eapol = (const struct ieee802_1x_hdr *) data; hdr = (const struct wpa_eapol_key *) (eapol + 1); if (is_zero(hdr->key_nonce, WPA_NONCE_LEN)) { add_note(wt, MSG_INFO, "EAPOL-Key 2/4 from " MACSTR " used zero nonce", MAC2STR(src)); } if (!is_zero(hdr->key_rsc, 8)) { add_note(wt, MSG_INFO, "EAPOL-Key 2/4 from " MACSTR " used non-zero Key RSC", MAC2STR(src)); } os_memcpy(sta->snonce, hdr->key_nonce, WPA_NONCE_LEN); key_info = WPA_GET_BE16(hdr->key_info); key_data_len = WPA_GET_BE16(hdr->key_data_length); derive_ptk(wt, bss, sta, key_info & WPA_KEY_INFO_TYPE_MASK, data, len); if (!sta->ptk_set && !sta->tptk_set) { add_note(wt, MSG_DEBUG, "No PTK known to process EAPOL-Key 2/4"); return; } kck = sta->ptk.kck; if (sta->tptk_set) { add_note(wt, MSG_DEBUG, "Use TPTK for validation EAPOL-Key MIC"); kck = sta->tptk.kck; } if (check_mic(kck, key_info & WPA_KEY_INFO_TYPE_MASK, data, len) < 0) { add_note(wt, MSG_INFO, "Mismatch in EAPOL-Key 2/4 MIC"); return; } add_note(wt, MSG_DEBUG, "Valid MIC found in EAPOL-Key 2/4"); key_data = (const u8 *) (hdr + 1); if (wpa_supplicant_parse_ies(key_data, key_data_len, &ie) < 0) { add_note(wt, MSG_INFO, "Failed to parse EAPOL-Key Key Data"); return; } if (ie.wpa_ie) { wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - WPA IE", ie.wpa_ie, ie.wpa_ie_len); if (os_memcmp(ie.wpa_ie, sta->rsnie, ie.wpa_ie_len) != 0) { struct ieee802_11_elems elems; add_note(wt, MSG_INFO, "Mismatch in WPA IE between EAPOL-Key 2/4 " "and (Re)Association Request from " MACSTR, MAC2STR(sta->addr)); wpa_hexdump(MSG_INFO, "WPA IE in EAPOL-Key", ie.wpa_ie, ie.wpa_ie_len); wpa_hexdump(MSG_INFO, "WPA IE in (Re)Association " "Request", sta->rsnie, sta->rsnie[0] ? 2 + sta->rsnie[1] : 0); /* * The sniffer may have missed (Re)Association * Request, so try to survive with the information from * EAPOL-Key. */ os_memset(&elems, 0, sizeof(elems)); elems.wpa_ie = ie.wpa_ie + 2; elems.wpa_ie_len = ie.wpa_ie_len - 2; wpa_printf(MSG_DEBUG, "Update STA data based on WPA " "IE in EAPOL-Key 2/4"); sta_update_assoc(sta, &elems); } } if (ie.rsn_ie) { wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - RSN IE", ie.rsn_ie, ie.rsn_ie_len); if (os_memcmp(ie.rsn_ie, sta->rsnie, ie.rsn_ie_len) != 0) { struct ieee802_11_elems elems; add_note(wt, MSG_INFO, "Mismatch in RSN IE between EAPOL-Key 2/4 " "and (Re)Association Request from " MACSTR, MAC2STR(sta->addr)); wpa_hexdump(MSG_INFO, "RSN IE in EAPOL-Key", ie.rsn_ie, ie.rsn_ie_len); wpa_hexdump(MSG_INFO, "RSN IE in (Re)Association " "Request", sta->rsnie, sta->rsnie[0] ? 2 + sta->rsnie[1] : 0); /* * The sniffer may have missed (Re)Association * Request, so try to survive with the information from * EAPOL-Key. */ os_memset(&elems, 0, sizeof(elems)); elems.rsn_ie = ie.rsn_ie + 2; elems.rsn_ie_len = ie.rsn_ie_len - 2; wpa_printf(MSG_DEBUG, "Update STA data based on RSN " "IE in EAPOL-Key 2/4"); sta_update_assoc(sta, &elems); } } }
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(MAV_SEVERITY_INFO,"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); // start with gyro calibration tracker.ins.init_gyro(); // reset ahrs gyro bias if (tracker.ins.gyro_calibrated_ok_all()) { tracker.ahrs.reset_gyro_drift(); } 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)) { // start with gyro calibration tracker.ins.init_gyro(); // 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_GET_HOME_POSITION: send_home(tracker.ahrs.get_home()); result = MAV_RESULT_ACCEPTED; 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(FIRMWARE_VERSION); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: result = tracker.compass.handle_mag_cal_command(packet); 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(MAV_SEVERITY_INFO,"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; } 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; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); break; } // end switch } // end handle mavlink
// circle_run - runs the circle flight mode // should be called at 100hz or more void Sub::circle_run() { float target_yaw_rate = 0; float target_climb_rate = 0; // initialize speeds and accelerations pos_control.set_speed_xy(wp_nav.get_speed_xy()); pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { // To-Do: add some initialisation of position controllers motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.set_alt_target_to_current_alt(); return; } // process pilot inputs if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { circle_pilot_yaw_override = true; } // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // check for pilot requested take-off if (ap.land_complete && target_climb_rate > 0) { // indicate we are taking off set_land_complete(false); // clear i term when we're taking off set_throttle_takeoff(); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run circle controller circle_nav.update(); // call attitude controller if (circle_pilot_yaw_override) { attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate); }else{ attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); } // run altitude controller if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { // if sonar is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); }
// poshold_run - runs the PosHold controller // should be called at 100hz or more void Copter::poshold_run() { float target_roll, target_pitch; // pilot's roll and pitch angle inputs float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec float target_climb_rate = 0; // pilot desired climb rate in centimeters/sec float takeoff_climb_rate = 0.0f; // takeoff induced climb rate float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions const Vector3f& vel = inertial_nav.get_velocity(); // initialize vertical speeds and acceleration pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); wp_nav.init_loiter_target(); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); return; } // process pilot inputs if (!failsafe.radio) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate (for alt-hold mode and take-off) target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off if (ap.land_complete && (takeoff_state.running || channel_throttle->get_control_in() > get_takeoff_trigger_throttle())) { if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } // indicate we are taking off set_land_complete(false); // clear i term when we're taking off set_throttle_takeoff(); } } // relax loiter target if we might be landed if (ap.land_complete_maybe) { wp_nav.loiter_soften_for_landing(); } // if landed initialise loiter targets, set throttle to zero and exit if (ap.land_complete) { // if throttle zero reset attitude and exit immediately if (ap.throttle_zero) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); }else{ motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } wp_nav.init_loiter_target(); // move throttle to between minimum and non-takeoff-throttle to keep us on the ground attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); return; }else{ // convert pilot input to lean angles get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // convert inertial nav earth-frame velocities to body-frame // To-Do: move this to AP_Math (or perhaps we already have a function to do this) vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER) poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch); // Roll state machine // Each state (aka mode) is responsible for: // 1. dealing with pilot input // 2. calculating the final roll output to the attitude controller // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state switch (poshold.roll_mode) { case POSHOLD_PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode poshold.brake_roll = 0; // initialise braking angle to zero poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated } // final lean angle should be pilot input plus wind compensation poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll; break; case POSHOLD_BRAKE: case POSHOLD_BRAKE_READY_TO_LOITER: // calculate brake_roll angle to counter-act velocity poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); // update braking time estimate if (!poshold.braking_time_updated_roll) { // check if brake angle is increasing if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) { poshold.brake_angle_max_roll = abs(poshold.brake_roll); } else { // braking angle has started decreasing so re-estimate braking time poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. poshold.braking_time_updated_roll = true; } } // if velocity is very low reduce braking time to 0.5seconds if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR; } // reduce braking timer if (poshold.brake_timeout_roll > 0) { poshold.brake_timeout_roll--; } else { // indicate that we are ready to move to Loiter. // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER // logic for engaging loiter is handled below the roll and pitch mode switch statements poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle poshold.roll = poshold.brake_roll + poshold.wind_comp_roll; // check for pilot input if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); } break; case POSHOLD_BRAKE_TO_LOITER: case POSHOLD_LOITER: // these modes are combined roll-pitch modes and are handled below break; case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); // count-down loiter to pilot timer if (poshold.controller_to_pilot_timer_roll > 0) { poshold.controller_to_pilot_timer_roll--; } else { // when timer runs out switch to full pilot override for next iteration poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll); break; } // Pitch state machine // Each state (aka mode) is responsible for: // 1. dealing with pilot input // 2. calculating the final pitch output to the attitude contpitcher // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state switch (poshold.pitch_mode) { case POSHOLD_PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode poshold.brake_pitch = 0; // initialise braking angle to zero poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated } // final lean angle should be pilot input plus wind compensation poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch; break; case POSHOLD_BRAKE: case POSHOLD_BRAKE_READY_TO_LOITER: // calculate brake_pitch angle to counter-act velocity poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); // update braking time estimate if (!poshold.braking_time_updated_pitch) { // check if brake angle is increasing if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) { poshold.brake_angle_max_pitch = abs(poshold.brake_pitch); } else { // braking angle has started decreasing so re-estimate braking time poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. poshold.braking_time_updated_pitch = true; } } // if velocity is very low reduce braking time to 0.5seconds if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR; } // reduce braking timer if (poshold.brake_timeout_pitch > 0) { poshold.brake_timeout_pitch--; } else { // indicate that we are ready to move to Loiter. // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER // logic for engaging loiter is handled below the pitch and pitch mode switch statements poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch; // check for pilot input if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); } break; case POSHOLD_BRAKE_TO_LOITER: case POSHOLD_LOITER: // these modes are combined pitch-pitch modes and are handled below break; case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); // count-down loiter to pilot timer if (poshold.controller_to_pilot_timer_pitch > 0) { poshold.controller_to_pilot_timer_pitch--; } else { // when timer runs out switch to full pilot override for next iteration poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch); break; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // // Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER) // // switch into LOITER mode when both roll and pitch are ready if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) { poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER; poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER; poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; // init loiter controller wp_nav.init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0)); // at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore poshold.loiter_reset_I = false; // set delay to start of wind compensation estimate updates poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; } // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) { // force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states) poshold.pitch_mode = poshold.roll_mode; // handle combined roll+pitch mode switch (poshold.roll_mode) { case POSHOLD_BRAKE_TO_LOITER: // reduce brake_to_loiter timer if (poshold.brake_to_loiter_timer > 0) { poshold.brake_to_loiter_timer--; } else { // progress to full loiter on next iteration poshold.roll_mode = POSHOLD_LOITER; poshold.pitch_mode = POSHOLD_LOITER; } // calculate percentage mix of loiter and brake control brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; // calculate brake_roll and pitch angles to counter-act velocity poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // calculate final roll and pitch output by mixing loiter and brake controls poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav.get_roll()); poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav.get_pitch()); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) // no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); if (is_zero(target_roll)) { // switch roll-mode to brake (but ready to go back to loiter anytime) // no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; } } } break; case POSHOLD_LOITER: // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // set roll angle based on loiter controller outputs poshold.roll = wp_nav.get_roll(); poshold.pitch = wp_nav.get_pitch(); // update wind compensation estimate poshold_update_wind_comp_estimate(); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle poshold.brake_pitch = 0; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time) if (is_zero(target_roll)) { poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; poshold.brake_roll = 0; } } } break; default: // do nothing for uncombined roll and pitch modes break; } } // constrain target pitch/roll angles poshold.roll = constrain_int16(poshold.roll, -aparm.angle_max, aparm.angle_max); poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max); // update attitude controller targets attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); // adjust climb rate using rangefinder if (rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); } }
/* zQRcondest -- returns an estimate of the 2-norm condition number of the matrix factorised by QRfactor() or QRCPfactor() -- note that as Q does not affect the 2-norm condition number, it is not necessary to pass the diag, beta (or pivot) vectors -- generates a lower bound on the true condition number -- if the matrix is exactly singular, HUGE_VAL is returned -- note that QRcondest() is likely to be more reliable for matrices factored using QRCPfactor() */ double zQRcondest(ZMAT *QR) { STATIC ZVEC *y=ZVNULL; Real norm, norm1, norm2, tmp1, tmp2; complex sum, tmp; int i, j, limit; if ( QR == ZMNULL ) error(E_NULL,"zQRcondest"); limit = min(QR->m,QR->n); for ( i = 0; i < limit; i++ ) /* if ( QR->me[i][i] == 0.0 ) */ if ( is_zero(QR->me[i][i]) ) return HUGE_VAL; y = zv_resize(y,limit); MEM_STAT_REG(y,TYPE_ZVEC); /* use the trick for getting a unit vector y with ||R.y||_inf small from the LU condition estimator */ for ( i = 0; i < limit; i++ ) { sum.re = sum.im = 0.0; for ( j = 0; j < i; j++ ) /* sum -= QR->me[j][i]*y->ve[j]; */ sum = zsub(sum,zmlt(QR->me[j][i],y->ve[j])); /* sum -= (sum < 0.0) ? 1.0 : -1.0; */ norm1 = zabs(sum); if ( norm1 == 0.0 ) sum.re = 1.0; else { sum.re += sum.re / norm1; sum.im += sum.im / norm1; } /* y->ve[i] = sum / QR->me[i][i]; */ y->ve[i] = zdiv(sum,QR->me[i][i]); } zUAmlt(QR,y,y); /* now apply inverse power method to R*.R */ for ( i = 0; i < 3; i++ ) { tmp1 = zv_norm2(y); zv_mlt(zmake(1.0/tmp1,0.0),y,y); zUAsolve(QR,y,y,0.0); tmp2 = zv_norm2(y); zv_mlt(zmake(1.0/tmp2,0.0),y,y); zUsolve(QR,y,y,0.0); } /* now compute approximation for ||R^{-1}||_2 */ norm1 = sqrt(tmp1)*sqrt(tmp2); /* now use complementary approach to compute approximation to ||R||_2 */ for ( i = limit-1; i >= 0; i-- ) { sum.re = sum.im = 0.0; for ( j = i+1; j < limit; j++ ) sum = zadd(sum,zmlt(QR->me[i][j],y->ve[j])); if ( is_zero(QR->me[i][i]) ) return HUGE_VAL; tmp = zdiv(sum,QR->me[i][i]); if ( is_zero(tmp) ) { y->ve[i].re = 1.0; y->ve[i].im = 0.0; } else { norm = zabs(tmp); y->ve[i].re = sum.re / norm; y->ve[i].im = sum.im / norm; } /* y->ve[i] = (sum >= 0.0) ? 1.0 : -1.0; */ /* y->ve[i] = (QR->me[i][i] >= 0.0) ? y->ve[i] : - y->ve[i]; */ } /* now apply power method to R*.R */ for ( i = 0; i < 3; i++ ) { tmp1 = zv_norm2(y); zv_mlt(zmake(1.0/tmp1,0.0),y,y); zUmlt(QR,y,y); tmp2 = zv_norm2(y); zv_mlt(zmake(1.0/tmp2,0.0),y,y); zUAmlt(QR,y,y); } norm2 = sqrt(tmp1)*sqrt(tmp2); /* printf("QRcondest: norm1 = %g, norm2 = %g\n",norm1,norm2); */ #ifdef THREADSAFE ZV_FREE(y); #endif return norm1*norm2; }
bool is_unit(const ElementType& f) const { return !is_zero(f); }
void archivebits(double *x, UL q, UL n, UL totalbits, UL b, UL c, double hi, double lo, const char *version_info, FILE *outfp, FILE *dupfp) { FILE *archfp = NULL; archfp = open_archive(); if (is_zero(x, n, 0, 0)) { if (outfp != NULL && !ferror(outfp)) fprintf(outfp, "M( " PRINTF_FMT_UL " )P, n = " PRINTF_FMT_UL ", %s\n", q, n, version_info); if (dupfp != NULL && !ferror(dupfp)) fprintf(dupfp, "M( " PRINTF_FMT_UL " )P, n = " PRINTF_FMT_UL ", %s\n", q, n, version_info); if (archfp != NULL) fprintf(archfp, "M( " PRINTF_FMT_UL " )P, n = " PRINTF_FMT_UL ", %s\n", q, n, version_info); if (archfp != NULL) close_archive(archfp, outfp, dupfp); return; } if (outfp != NULL && !ferror(outfp)) fprintf(outfp, "M( " PRINTF_FMT_UL " )C", q); if (dupfp != NULL && !ferror(dupfp)) fprintf(dupfp, "M( " PRINTF_FMT_UL " )C", q); if (archfp != NULL) fprintf(archfp, "M( " PRINTF_FMT_UL " )C", q); balancedtostdrep(x, n, b, c, hi, lo, 0, 0); static UL *hex = NULL; static UL prior_hex = 0; if (hex != NULL && totalbits/BITS_IN_LONG + 1 > prior_hex) { free(hex); hex = NULL; prior_hex = totalbits/BITS_IN_LONG + 1; } if (hex == NULL && (hex = (UL *)calloc(totalbits/BITS_IN_LONG + 1, sizeof(UL))) == NULL) { perror(program_name); fprintf(stderr, "%s: cannot get memory for residue bits; calloc() errno %d\n", program_name, errno); exit(errno); } int i = 0; int j = 0; do { UL k = (long)(ceil((double)q*(j + 1)/n) - ceil((double)q*j/n)); if (k > totalbits) k = totalbits; totalbits -= k; int word = (long)x[j]; for (j++; k > 0; k--, i++) { if (i % BITS_IN_LONG == 0) hex[i/BITS_IN_LONG] = 0L; hex[i/BITS_IN_LONG] |= ((word & 0x1) << (i % BITS_IN_LONG)); word >>= 1; } } while(totalbits > 0); if (outfp != NULL && !ferror(outfp)) fputs(", 0x", outfp); if (dupfp != NULL && !ferror(dupfp)) fputs(", 0x", dupfp); if (archfp != NULL) fputs(", 0x", archfp); static char bits_fmt[16] = "\0"; /* "%%0%ulx" -> "%08lx" or "%016lx" depending on sizeof(UL) */ if (bits_fmt[0] != '%') sprintf(bits_fmt, "%%0" PRINTF_FMT_UL "%s", (UL)(BITS_IN_LONG/4), "lx"); /* 4 bits per hex 'digit' */ for (j = (i - 1)/BITS_IN_LONG; j >= 0; j--) { if (outfp != NULL && !ferror(outfp)) fprintf(outfp, bits_fmt, hex[j]); if (dupfp != NULL && !ferror(dupfp)) fprintf(dupfp, bits_fmt, hex[j]); if (archfp != NULL) fprintf(archfp, bits_fmt, hex[j]); } if (outfp != NULL && !ferror(outfp)) { fprintf(outfp, ", n = " PRINTF_FMT_UL ", %s", n, version_info); fprintf(outfp, "\n"); fflush(outfp); } if (dupfp != NULL && !ferror(dupfp)) fprintf(dupfp, ", n = " PRINTF_FMT_UL ", %s\n", n, version_info); if (archfp != NULL) { fprintf(archfp, ", n = " PRINTF_FMT_UL ", %s\n", n, version_info); close_archive(archfp, outfp, dupfp); } return; }
// verify_payload_place - returns true if placing has been completed bool Copter::verify_payload_place() { const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds const float hover_throttle_placed_fraction = 0.8; // i.e. if throttle is less than 80% of hover we have placed const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed const float current_throttle_level = motors.get_throttle(); const uint32_t now = AP_HAL::millis(); // if we discover we've landed then immediately release the load: if (ap.land_complete) { switch (nav_payload_place.state) { case PayloadPlaceStateType_FlyToLocation: case PayloadPlaceStateType_Calibrating_Hover_Start: case PayloadPlaceStateType_Calibrating_Hover: case PayloadPlaceStateType_Descending_Start: case PayloadPlaceStateType_Descending: gcs_send_text_fmt(MAV_SEVERITY_INFO, "NAV_PLACE: landed"); nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; break; case PayloadPlaceStateType_Releasing_Start: case PayloadPlaceStateType_Releasing: case PayloadPlaceStateType_Released: case PayloadPlaceStateType_Ascending_Start: case PayloadPlaceStateType_Ascending: case PayloadPlaceStateType_Done: break; } } switch (nav_payload_place.state) { case PayloadPlaceStateType_FlyToLocation: if (!wp_nav.reached_wp_destination()) { return false; } // we're there; set loiter target auto_payload_place_start(wp_nav.get_wp_destination()); nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; // fall through case PayloadPlaceStateType_Calibrating_Hover_Start: // hover for 1 second to get an idea of what our hover // throttle looks like debug("Calibrate start"); nav_payload_place.hover_start_timestamp = now; nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover; // fall through case PayloadPlaceStateType_Calibrating_Hover: { if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) { // still calibrating... debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp); return false; } // we have a valid calibration. Hopefully. nav_payload_place.hover_throttle_level = current_throttle_level; const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors.get_throttle_hover()); gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast<double>(hover_throttle_delta)); nav_payload_place.state = PayloadPlaceStateType_Descending_Start; // fall through }; case PayloadPlaceStateType_Descending_Start: nav_payload_place.descend_start_timestamp = now; nav_payload_place.descend_start_altitude = inertial_nav.get_altitude(); nav_payload_place.descend_throttle_level = 0; nav_payload_place.state = PayloadPlaceStateType_Descending; // fall through case PayloadPlaceStateType_Descending: // make sure we don't descend too far: debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max); if (!is_zero(nav_payload_place.descend_max) && nav_payload_place.descend_start_altitude - inertial_nav.get_altitude() > nav_payload_place.descend_max) { nav_payload_place.state = PayloadPlaceStateType_Ascending; gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Reached maximum descent"); return false; // we'll do any cleanups required next time through the loop } // see if we've been descending long enough to calibrate a descend-throttle-level: if (is_zero(nav_payload_place.descend_throttle_level) && now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) { nav_payload_place.descend_throttle_level = current_throttle_level; } // watch the throttle to determine whether the load has been placed // debug("hover ratio: %f descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level)); if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction && (is_zero(nav_payload_place.descend_throttle_level) || current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) { // throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid) nav_payload_place.place_start_timestamp = 0; return false; } if (nav_payload_place.place_start_timestamp == 0) { // we've only just now hit the correct throttle level nav_payload_place.place_start_timestamp = now; return false; } else if (now - nav_payload_place.place_start_timestamp < placed_time) { // keep going down.... debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp); return false; } nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; // fall through case PayloadPlaceStateType_Releasing_Start: if (g2.gripper.valid()) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Releasing the gripper"); g2.gripper.release(); } else { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper not valid"); } nav_payload_place.state = PayloadPlaceStateType_Releasing; // fall through case PayloadPlaceStateType_Releasing: if (g2.gripper.valid() && !g2.gripper.released()) { return false; } nav_payload_place.state = PayloadPlaceStateType_Released; // fall through case PayloadPlaceStateType_Released: { nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; } // fall through case PayloadPlaceStateType_Ascending_Start: { Location_Class target_loc = inertial_nav.get_position(); target_loc.alt = nav_payload_place.descend_start_altitude; auto_wp_start(target_loc); nav_payload_place.state = PayloadPlaceStateType_Ascending; } //fall through case PayloadPlaceStateType_Ascending: if (!wp_nav.reached_wp_destination()) { return false; } nav_payload_place.state = PayloadPlaceStateType_Done; // fall through case PayloadPlaceStateType_Done: return true; default: // this should never happen // TO-DO: log an error return true; } // should never get here return true; }
Expr lower_lerp(Expr zero_val, Expr one_val, Expr weight) { Expr result; internal_assert(zero_val.type() == one_val.type()); internal_assert(weight.type().is_uint() || weight.type().is_float()); Type result_type = zero_val.type(); Expr bias_value = make_zero(result_type); Type computation_type = result_type; if (zero_val.type().is_int()) { computation_type = UInt(zero_val.type().bits(), zero_val.type().lanes()); bias_value = result_type.min(); } // For signed integer types, just convert everything to unsigned // and then back at the end to ensure proper rounding, etc. // There is likely a better way to handle this. if (result_type != computation_type) { zero_val = Cast::make(computation_type, zero_val - bias_value); one_val = Cast::make(computation_type, one_val - bias_value); } if (result_type.is_bool()) { Expr half_weight; if (weight.type().is_float()) half_weight = 0.5f; else { half_weight = weight.type().max() / 2; } result = select(weight > half_weight, one_val, zero_val); } else { Expr typed_weight; Expr inverse_typed_weight; if (weight.type().is_float()) { typed_weight = weight; if (computation_type.is_uint()) { // TODO: Verify this reduces to efficient code or // figure out a better way to express a multiply // of unsigned 2^32-1 by a double promoted weight if (computation_type.bits() == 32) { typed_weight = Cast::make(computation_type, cast<double>(Expr(65535.0f)) * cast<double>(Expr(65537.0f)) * Cast::make(Float(64, typed_weight.type().lanes()), typed_weight)); } else { typed_weight = Cast::make(computation_type, computation_type.max() * typed_weight); } inverse_typed_weight = computation_type.max() - typed_weight; } else { inverse_typed_weight = 1.0f - typed_weight; } } else { if (computation_type.is_float()) { int weight_bits = weight.type().bits(); if (weight_bits == 32) { // Should use ldexp, but can't make Expr from result // that is double typed_weight = Cast::make(computation_type, cast<double>(weight) / (pow(cast<double>(2), 32) - 1)); } else { typed_weight = Cast::make(computation_type, weight / ((float)ldexp(1.0f, weight_bits) - 1)); } inverse_typed_weight = 1.0f - typed_weight; } else { // This code rescales integer weights to the right number of bits. // It takes advantage of (2^n - 1) == (2^(n/2) - 1)(2^(n/2) + 1) // e.g. 65535 = 255 * 257. (Ditto for the 32-bit equivalent.) // To recale a weight of m bits to be n bits, we need to do: // scaled_weight = (weight / (2^m - 1)) * (2^n - 1) // which power of two values for m and n, results in a series like // so: // (2^(m/2) + 1) * (2^(m/4) + 1) ... (2^(n*2) + 1) // The loop below computes a scaling constant and either multiples // or divides by the constant and relies on lowering and llvm to // generate efficient code for the operation. int bit_size_difference = weight.type().bits() - computation_type.bits(); if (bit_size_difference == 0) { typed_weight = weight; } else { typed_weight = Cast::make(computation_type, weight); int bits_left = ::abs(bit_size_difference); int shift_amount = std::min(computation_type.bits(), weight.type().bits()); uint64_t scaling_factor = 1; while (bits_left != 0) { internal_assert(bits_left > 0); scaling_factor = scaling_factor + (scaling_factor << shift_amount); bits_left -= shift_amount; shift_amount *= 2; } if (bit_size_difference < 0) { typed_weight = Cast::make(computation_type, weight) * cast(computation_type, (int32_t)scaling_factor); } else { typed_weight = Cast::make(computation_type, weight / cast(weight.type(), (int32_t)scaling_factor)); } } inverse_typed_weight = Cast::make(computation_type, computation_type.max() - typed_weight); } } if (computation_type.is_float()) { result = zero_val * inverse_typed_weight + one_val * typed_weight; } else { int32_t bits = computation_type.bits(); switch (bits) { case 1: result = select(typed_weight, one_val, zero_val); break; case 8: case 16: case 32: { Expr zero_expand = Cast::make(UInt(2 * bits, computation_type.lanes()), zero_val); Expr one_expand = Cast::make(UInt(2 * bits, one_val.type().lanes()), one_val); Expr rounding = Cast::make(UInt(2 * bits), 1) << Cast::make(UInt(2 * bits), (bits - 1)); Expr divisor = Cast::make(UInt(2 * bits), 1) << Cast::make(UInt(2 * bits), bits); Expr prod_sum = zero_expand * inverse_typed_weight + one_expand * typed_weight + rounding; Expr divided = ((prod_sum / divisor) + prod_sum) / divisor; result = Cast::make(UInt(bits, computation_type.lanes()), divided); break; } case 64: // TODO: 64-bit lerp is not supported as current approach // requires double-width multiply. // There is an informative error message in IROperator.h. internal_error << "Can't do a 64-bit lerp.\n"; break; default: break; } } if (!is_zero(bias_value)) { result = Cast::make(result_type, result) + bias_value; } } return simplify(result); }
// auto_wp_run - runs the auto waypoint controller // called by auto_run at 100hz or more void Sub::auto_wp_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { // To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) // call attitude controller // Sub vehicles do not stabilize roll/pitch/yaw when disarmed motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.pilot_input) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller // TODO logic for terrain tracking target going below fence limit // TODO implement waypoint radius individually for each waypoint based on cmd.p2 // TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter failsafe_terrain_set_status(wp_nav.update_wpnav()); /////////////////////// // update xy outputs // float lateral_out, forward_out; translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); motors.set_forward(forward_out); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); //////////////////////////// // update attitude output // // get pilot desired lean angles float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain()); } }
void AP_TECS::_update_pitch(void) { // Calculate Speed/Height Control Weighting // This is used to determine how the pitch control prioritises speed and height control // A weighting of 1 provides equal priority (this is the normal mode of operation) // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed // rises above the demanded value, the pitch angle will be increased by the TECS controller. float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); if (!_ahrs.airspeed_sensor_enabled()) { SKE_weighting = 0.0f; } else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { SKE_weighting = 2.0f; } else if (_flags.is_doing_auto_land) { if (_spdWeightLand < 0) { // use sliding scale from normal weight down to zero at landing float scaled_weight = _spdWeight * (1.0f - constrain_float(_path_proportion,0,1)); SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f); } else { SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f); } } logging.SKE_weighting = SKE_weighting; float SPE_weighting = 2.0f - SKE_weighting; // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); logging.SKE_error = _SKE_dem - _SKE_est; logging.SPE_error = _SPE_dem - _SPE_est; // Calculate integrator state, constraining input if pitch limits are exceeded float integSEB_input = SEB_error * _get_i_gain(); if (_pitch_dem > _PITCHmaxf) { integSEB_input = MIN(integSEB_input, _PITCHmaxf - _pitch_dem); } else if (_pitch_dem < _PITCHminf) { integSEB_input = MAX(integSEB_input, _PITCHminf - _pitch_dem); } float integSEB_delta = integSEB_input * _DT; #if 0 if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) { ::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n", _hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem), SEB_dem, SEBdot_dem, SEB_error, SEBdot_error); } #endif // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. // During flare a different damping gain is used float gainInv = (_TAS_state * timeConstant() * GRAVITY_MSS); float temp = SEB_error + SEBdot_dem * timeConstant(); float pitch_damp = _ptchDamp; if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { pitch_damp = _landDamp; } else if (!is_zero(_land_pitch_damp) && _flags.is_doing_auto_land) { pitch_damp = _land_pitch_damp; } temp += SEBdot_error * pitch_damp; if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { temp += _PITCHminf * gainInv; } float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp; float integSEB_max = (gainInv * (_PITCHmaxf + 0.0783f)) - temp; float integSEB_range = integSEB_max - integSEB_min; logging.SEB_delta = integSEB_delta; // don't allow the integrator to rise by more than 20% of its full // range in one step. This prevents single value glitches from // causing massive integrator changes. See Issue#4066 integSEB_delta = constrain_float(integSEB_delta, -integSEB_range*0.1f, integSEB_range*0.1f); // integrate _integSEB_state = constrain_float(_integSEB_state + integSEB_delta, integSEB_min, integSEB_max); // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integSEB_state) / gainInv; // Constrain pitch demand _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); // Rate limit the pitch demand to comply with specified vertical // acceleration limit float ptchRateIncr = _DT * _vertAccLim / _TAS_state; if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { _pitch_dem = _last_pitch_dem + ptchRateIncr; } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { _pitch_dem = _last_pitch_dem - ptchRateIncr; } // re-constrain pitch demand _pitch_dem = constrain_float(_pitch_dem, _PITCHminf, _PITCHmaxf); _last_pitch_dem = _pitch_dem; }
void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) { /* Distance from polar axis. */ const double p = sqrt(ecef[0]*ecef[0] + ecef[1]*ecef[1]); /* Compute longitude first, this can be done exactly. */ if (!is_zero(p)) llh[1] = atan2(ecef[1], ecef[0]); else llh[1] = 0; /* If we are close to the pole then convergence is very slow, treat this is a * special case. */ if (p < WGS84_A*1e-16) { llh[0] = copysign(M_PI_2, ecef[2]); llh[2] = fabs(ecef[2]) - WGS84_B; return; } /* Caluclate some other constants as defined in the Fukushima paper. */ const double P = p / WGS84_A; const double e_c = sqrt(1. - WGS84_E*WGS84_E); const double Z = fabs(ecef[2]) * e_c / WGS84_A; /* Initial values for S and C correspond to a zero height solution. */ double S = Z; double C = e_c * P; /* Neither S nor C can be negative on the first iteration so * starting prev = -1 will not cause and early exit. */ double prev_C = -1; double prev_S = -1; double A_n, B_n, D_n, F_n; /* Iterate a maximum of 10 times. This should be way more than enough for all * sane inputs */ for (int i=0; i<10; i++) { /* Calculate some intermmediate variables used in the update step based on * the current state. */ A_n = sqrt(S*S + C*C); D_n = Z*A_n*A_n*A_n + WGS84_E*WGS84_E*S*S*S; F_n = P*A_n*A_n*A_n - WGS84_E*WGS84_E*C*C*C; B_n = 1.5*WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C); /* Update step. */ S = D_n*F_n - B_n*S; C = F_n*F_n - B_n*C; /* The original algorithm as presented in the paper by Fukushima has a * problem with numerical stability. S and C can grow very large or small * and over or underflow a double. In the paper this is acknowledged and * the proposed resolution is to non-dimensionalise the equations for S and * C. However, this does not completely solve the problem. The author caps * the solution to only a couple of iterations and in this period over or * underflow is unlikely but as we require a bit more precision and hence * more iterations so this is still a concern for us. * * As the only thing that is important is the ratio T = S/C, my solution is * to divide both S and C by either S or C. The scaling is chosen such that * one of S or C is scaled to unity whilst the other is scaled to a value * less than one. By dividing by the larger of S or C we ensure that we do * not divide by zero as only one of S or C should ever be zero. * * This incurs an extra division each iteration which the author was * explicityl trying to avoid and it may be that this solution is just * reverting back to the method of iterating on T directly, perhaps this * bears more thought? */ if (S > C) { C = C / S; S = 1; } else { S = S / C; C = 1; } /* Check for convergence and exit early if we have converged. */ if (fabs(S - prev_S) < 1e-16 && fabs(C - prev_C) < 1e-16) { break; } else { prev_S = S; prev_C = C; } } A_n = sqrt(S*S + C*C); llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C)); llh[2] = (p*e_c*C + fabs(ecef[2])*S - WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S); }
static inline bool is_one(const fixed value, const int accuracy=ACCURACY) { return is_zero(value - fixed(1), accuracy); }
bool is_zero(std::complex<value_t> const &x, value_t tolerance = 100 * std::numeric_limits<value_t>::epsilon()) { return is_zero(std::real(x), tolerance) && is_zero(std::imag(x), tolerance); }
literalt float_utilst::relation( const bvt &src1, relt rel, const bvt &src2) { if(rel==GT) return relation(src2, LT, src1); // swapped else if(rel==GE) return relation(src2, LE, src1); // swapped assert(rel==EQ || rel==LT || rel==LE); // special cases: -0 and 0 are equal literalt is_zero1=is_zero(src1); literalt is_zero2=is_zero(src2); literalt both_zero=prop.land(is_zero1, is_zero2); // NaN compares to nothing literalt is_NaN1=is_NaN(src1); literalt is_NaN2=is_NaN(src2); literalt NaN=prop.lor(is_NaN1, is_NaN2); if(rel==LT || rel==LE) { literalt bitwise_equal=bv_utils.equal(src1, src2); // signs different? trivial! Unless Zero. literalt signs_different= prop.lxor(sign_bit(src1), sign_bit(src2)); // as long as the signs match: compare like unsigned numbers // this works due to the BIAS literalt less_than1=bv_utils.unsigned_less_than(src1, src2); // if both are negative (and not the same), need to turn around! literalt less_than2= prop.lxor(less_than1, prop.land(sign_bit(src1), sign_bit(src2))); literalt less_than3= prop.lselect(signs_different, sign_bit(src1), less_than2); if(rel==LT) { bvt and_bv; and_bv.push_back(less_than3); and_bv.push_back(!bitwise_equal); // for the case of two negative numbers and_bv.push_back(!both_zero); and_bv.push_back(!NaN); return prop.land(and_bv); } else if(rel==LE) { bvt or_bv; or_bv.push_back(less_than3); or_bv.push_back(both_zero); or_bv.push_back(bitwise_equal); return prop.land(prop.lor(or_bv), !NaN); } else assert(false); } else if(rel==EQ) { literalt bitwise_equal=bv_utils.equal(src1, src2); return prop.land( prop.lor(bitwise_equal, both_zero), !NaN); } else assert(0); // not reached return const_literal(false); }
// sends commands to the motors void AP_MotorsCoax::output_armed_stabilizing() { float roll_thrust; // roll thrust input value, +/- 1.0 float pitch_thrust; // pitch thrust input value, +/- 1.0 float yaw_thrust; // yaw thrust input value, +/- 1.0 float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy float thrust_out; // float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits float actuator_allowed = 0.0f; // amount of yaw we can fit in // apply voltage and air pressure compensation roll_thrust = _roll_in * get_compensation_gain(); pitch_thrust = _pitch_in * get_compensation_gain(); yaw_thrust = _yaw_in * get_compensation_gain(); throttle_thrust = get_throttle() * get_compensation_gain(); // sanity check throttle is above zero and below current limited throttle if (throttle_thrust <= 0.0f) { throttle_thrust = 0.0f; limit.throttle_lower = true; } if (throttle_thrust >= _throttle_thrust_max) { throttle_thrust = _throttle_thrust_max; limit.throttle_upper = true; } _throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max); float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust)); // calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw if (is_zero(rp_thrust_max)) { rp_scale = 1.0f; } else { rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f*(float)_yaw_headroom/1000.0f)) / rp_thrust_max, 0.0f, 1.0f); if (rp_scale < 1.0f) { limit.roll_pitch = true; } } actuator_allowed = 2.0f * (1.0f - rp_scale * rp_thrust_max); if (fabsf(yaw_thrust) > actuator_allowed) { yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed); limit.yaw = true; } // calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces thrust_min_rpy = MAX(fabsf(rp_scale * rp_thrust_max), fabsf(yaw_thrust)); thr_adj = throttle_thrust - _throttle_avg_max; if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) { // Throttle can't be reduced to the desired level because this would mean roll or pitch control // would not be able to reach the desired level because of lack of thrust. thr_adj = MIN(thrust_min_rpy, _throttle_avg_max) - _throttle_avg_max; } // calculate the throttle setting for the lift fan thrust_out = _throttle_avg_max + thr_adj; if (fabsf(yaw_thrust) > thrust_out) { yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out); limit.yaw = true; } _thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust; _thrust_yt_cw = thrust_out - 0.5f * yaw_thrust; // limit thrust out for calculation of actuator gains float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f); if (is_zero(thrust_out)) { limit.roll_pitch = true; } // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared // static thrust is proportional to the airflow velocity squared // therefore the torque of the roll and pitch actuators should be approximately proportional to // the angle of attack multiplied by the static thrust. _actuator_out[0] = roll_thrust/thrust_out_actuator; _actuator_out[1] = pitch_thrust/thrust_out_actuator; if (fabsf(_actuator_out[0]) > 1.0f) { limit.roll_pitch = true; _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); } if (fabsf(_actuator_out[1]) > 1.0f) { limit.roll_pitch = true; _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f); } _actuator_out[2] = -_actuator_out[0]; _actuator_out[3] = -_actuator_out[1]; }
/* a special glide slope calculation for the landing approach During the land approach use a linear glide slope to a point projected through the landing point. We don't use the landing point itself as that leads to discontinuities close to the landing point, which can lead to erratic pitch control */ void Plane::setup_landing_glide_slope(void) { float total_distance = get_distance(prev_WP_loc, next_WP_loc); // If someone mistakenly puts all 0's in their LAND command then total_distance // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. if (total_distance < 1) { total_distance = 1; } // height we need to sink for this WP float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f; // current ground speed float groundspeed = ahrs.groundspeed(); if (groundspeed < 0.5f) { groundspeed = 0.5f; } // calculate time to lose the needed altitude float sink_time = total_distance / groundspeed; if (sink_time < 0.5f) { sink_time = 0.5f; } // find the sink rate needed for the target location float sink_rate = sink_height / sink_time; // the height we aim for is the one to give us the right flare point float aim_height = aparm.land_flare_sec * sink_rate; if (aim_height <= 0) { aim_height = g.land_flare_alt; } // don't allow the aim height to be too far above LAND_FLARE_ALT if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) { aim_height = g.land_flare_alt*2; } // calculate slope to landing point bool is_first_calc = is_zero(auto_state.land_slope); auto_state.land_slope = (sink_height - aim_height) / total_distance; if (is_first_calc) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(auto_state.land_slope))); } // time before landing that we will flare float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate(); // distance to flare is based on ground speed, adjusted as we // get closer. This takes into account the wind float flare_distance = groundspeed * flare_time; // don't allow the flare before half way along the final leg if (flare_distance > total_distance/2) { flare_distance = total_distance/2; } // project a point 500 meters past the landing point, passing // through the landing point const float land_projection = 500; int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); // now calculate our aim point, which is before the landing // point and above it Location loc = next_WP_loc; location_update(loc, land_bearing_cd*0.01f, -flare_distance); loc.alt += aim_height*100; // calculate point along that slope 500m ahead location_update(loc, land_bearing_cd*0.01f, land_projection); loc.alt -= auto_state.land_slope * land_projection * 100; // setup the offset_cm for set_target_altitude_proportion() target_altitude.offset_cm = loc.alt - prev_WP_loc.alt; // calculate the proportion we are to the target float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc); // now setup the glide slope for landing set_target_altitude_proportion(loc, 1.0f - land_proportion); // stay within the range of the start and end locations in altitude constrain_target_altitude_location(loc, prev_WP_loc); }
// althold_run - runs the althold controller // should be called at 100hz or more void Sub::althold_run() { uint32_t tnow = AP_HAL::millis(); // initialize vertical speeds and acceleration pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control.set_max_accel_z(g.pilot_accel_z); if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); last_pilot_heading = ahrs.yaw_sensor; return; } motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // get pilot desired lean angles float target_roll, target_pitch; // Check if set_attitude_target_no_gps is valid if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) { float target_yaw; Quaternion( set_attitude_target_no_gps.packet.q ).to_euler( target_roll, target_pitch, target_yaw ); target_roll = degrees(target_roll); target_pitch = degrees(target_pitch); target_yaw = degrees(target_yaw); attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true); return; } get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max()); // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); last_pilot_heading = ahrs.yaw_sensor; last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading // this check is required to prevent bounce back after very fast yaw maneuvers // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading target_yaw_rate = 0; // Stop rotation on yaw axis // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); last_pilot_heading = ahrs.yaw_sensor; // update heading to hold } else { // call attitude controller holding absolute absolute bearing attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); } } if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5% // output pilot's throttle attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); // reset z targets to current values pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); } else { // hold z if (ap.at_bottom) { pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom } else if (rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking float target_climb_rate = get_surface_tracking_climb_rate(0, pos_control.get_alt_target(), G_Dt); pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); } pos_control.update_z_controller(); } motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); }
/* initialise the barometer object, loading backend drivers */ void AP_Baro::init(void) { // ensure that there isn't a previous ground temperature saved if (!is_zero(_user_ground_temperature)) { _user_ground_temperature.set_and_save(0.0f); _user_ground_temperature.notify(); } if (_hil_mode) { drivers[0] = new AP_Baro_HIL(*this); _num_drivers = 1; return; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL ADD_BACKEND(new AP_Baro_SITL(*this)); return; #endif #if HAL_WITH_UAVCAN bool added; do { added = _add_backend(AP_Baro_UAVCAN::probe(*this)); if (_num_drivers == BARO_MAX_DRIVERS || _num_sensors == BARO_MAX_INSTANCES) { return; } } while (added); #endif #if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN switch (AP_BoardConfig::get_board_type()) { case AP_BoardConfig::PX4_BOARD_PX4V1: #ifdef HAL_BARO_MS5611_I2C_BUS ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR)))); #endif break; case AP_BoardConfig::PX4_BOARD_PIXHAWK: case AP_BoardConfig::PX4_BOARD_PHMINI: case AP_BoardConfig::PX4_BOARD_AUAV21: case AP_BoardConfig::PX4_BOARD_PH2SLIM: case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO: ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); break; case AP_BoardConfig::PX4_BOARD_PIXHAWK2: ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_EXT_NAME)))); ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); break; case AP_BoardConfig::PX4_BOARD_PIXRACER: ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_INT_NAME)))); break; case AP_BoardConfig::PX4_BOARD_AEROFC: #ifdef HAL_BARO_MS5607_I2C_BUS ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5607)); #endif break; default: break; } #elif HAL_BARO_DEFAULT == HAL_BARO_HIL drivers[0] = new AP_Baro_HIL(*this); _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_BMP085 drivers[0] = new AP_Baro_BMP085(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP085_BUS, HAL_BARO_BMP085_I2C_ADDR))); _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_I2C ADD_BACKEND(AP_Baro_BMP280::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP280_BUS, HAL_BARO_BMP280_I2C_ADDR)))); #elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_SPI ADD_BACKEND(AP_Baro_BMP280::probe(*this, std::move(hal.spi->get_device(HAL_BARO_BMP280_NAME)))); #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_I2C ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR)))); #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)))); #elif HAL_BARO_DEFAULT == HAL_BARO_MS5607_I2C ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5607)); #elif HAL_BARO_DEFAULT == HAL_BARO_MS5637_I2C ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5637_I2C_BUS, HAL_BARO_MS5637_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5637)); #elif HAL_BARO_DEFAULT == HAL_BARO_QFLIGHT drivers[0] = new AP_Baro_QFLIGHT(*this); _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_QURT drivers[0] = new AP_Baro_QURT(*this); _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H ADD_BACKEND(AP_Baro_LPS25H::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)))); #endif // can optionally have baro on I2C too if (_ext_bus >= 0) { #if APM_BUILD_TYPE(APM_BUILD_ArduSub) ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5837_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5837)); ADD_BACKEND(AP_Baro_KellerLD::probe(*this, std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_KELLERLD_I2C_ADDR)))); #else ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5611_I2C_ADDR)))); #endif } if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) { AP_BoardConfig::sensor_config_error("Baro: unable to initialise driver"); } }
/// check_fence - returns the fence type that has been breached (if any) /// curr_alt is the altitude above home in meters uint8_t AC_Fence::check_fence(float curr_alt) { uint8_t ret = AC_FENCE_TYPE_NONE; // return immediately if disabled if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { return AC_FENCE_TYPE_NONE; } // check if pilot is attempting to recover manually if (_manual_recovery_start_ms != 0) { // we ignore any fence breaches during the manual recovery period which is about 10 seconds if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { return AC_FENCE_TYPE_NONE; } else { // recovery period has passed so reset manual recovery time and continue with fence breach checks _manual_recovery_start_ms = 0; } } // altitude fence check if ((_enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) { // check if we are over the altitude fence if( curr_alt >= _alt_max ) { // record distance above breach _alt_max_breach_distance = curr_alt - _alt_max; // check for a new breach or a breach of the backup fence if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) == 0 || (!is_zero(_alt_max_backup) && curr_alt >= _alt_max_backup)) { // record that we have breached the upper limit record_breach(AC_FENCE_TYPE_ALT_MAX); ret |= AC_FENCE_TYPE_ALT_MAX; // create a backup fence 20m higher up _alt_max_backup = curr_alt + AC_FENCE_ALT_MAX_BACKUP_DISTANCE; } }else{ // clear alt breach if present if ((_breached_fences & AC_FENCE_TYPE_ALT_MAX) != 0) { clear_breach(AC_FENCE_TYPE_ALT_MAX); _alt_max_backup = 0.0f; _alt_max_breach_distance = 0.0f; } } } // circle fence check if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) != 0 ) { // check if we are outside the fence if (_home_distance >= _circle_radius) { // record distance outside the fence _circle_breach_distance = _home_distance - _circle_radius; // check for a new breach or a breach of the backup fence if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) == 0 || (!is_zero(_circle_radius_backup) && _home_distance >= _circle_radius_backup)) { // record that we have breached the circular distance limit record_breach(AC_FENCE_TYPE_CIRCLE); ret |= AC_FENCE_TYPE_CIRCLE; // create a backup fence 20m further out _circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE; } }else{ // clear circle breach if present if ((_breached_fences & AC_FENCE_TYPE_CIRCLE) != 0) { clear_breach(AC_FENCE_TYPE_CIRCLE); _circle_radius_backup = 0.0f; _circle_breach_distance = 0.0f; } } } // polygon fence check if ((_enabled_fences & AC_FENCE_TYPE_POLYGON) != 0 ) { // check consistency of number of points if (_boundary_num_points != _total) { _boundary_loaded = false; } // load fence if necessary if (!_boundary_loaded) { load_polygon_from_eeprom(); } else if (_boundary_valid) { // check if vehicle is outside the polygon fence const Vector3f& position = _inav.get_position(); if (_poly_loader.boundary_breached(Vector2f(position.x, position.y), _boundary_num_points, _boundary, true)) { // check if this is a new breach if ((_breached_fences & AC_FENCE_TYPE_POLYGON) == 0) { // record that we have breached the polygon record_breach(AC_FENCE_TYPE_POLYGON); ret |= AC_FENCE_TYPE_POLYGON; } } else { // clear breach if present if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) { clear_breach(AC_FENCE_TYPE_POLYGON); } } } } // return any new breaches that have occurred return ret; }
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance /// updated velocity sent directly to position controller void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) { // calculate a loiter speed limit which is the minimum of the value set by the WPNAV_LOITER_SPEED // parameter and the value set by the EKF to observe optical flow limits float gnd_speed_limit_cms = min(_loiter_speed_cms,ekfGndSpdLimit*100.0f); gnd_speed_limit_cms = max(gnd_speed_limit_cms, 10.0f); // range check nav_dt if( nav_dt < 0 ) { return; } // check loiter speed and avoid divide by zero if(gnd_speed_limit_cms < WPNAV_LOITER_SPEED_MIN) { gnd_speed_limit_cms = WPNAV_LOITER_SPEED_MIN; } _pos_control.set_speed_xy(gnd_speed_limit_cms); _pos_control.set_accel_xy(_loiter_accel_cmss); // rotate pilot input to lat/lon frame Vector2f desired_accel; desired_accel.x = (_pilot_accel_fwd_cms*_ahrs.cos_yaw() - _pilot_accel_rgt_cms*_ahrs.sin_yaw()); desired_accel.y = (_pilot_accel_fwd_cms*_ahrs.sin_yaw() + _pilot_accel_rgt_cms*_ahrs.cos_yaw()); // calculate the difference Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel); // constrain and scale the desired acceleration float des_accel_change_total = pythagorous2(des_accel_diff.x, des_accel_diff.y); float accel_change_max = _loiter_jerk_max_cmsss * nav_dt; if (_loiter_jerk_max_cmsss > 0.0f && des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) { des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total; des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total; } // adjust the desired acceleration _loiter_desired_accel += des_accel_diff; // get pos_control's feed forward velocity const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity(); Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y); // add pilot commanded acceleration desired_vel.x += _loiter_desired_accel.x * nav_dt; desired_vel.y += _loiter_desired_accel.y * nav_dt; float desired_speed = desired_vel.length(); if (!is_zero(desired_speed)) { Vector2f desired_vel_norm = desired_vel/desired_speed; float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms; if (_pilot_accel_fwd_cms == 0 && _pilot_accel_rgt_cms == 0) { drag_speed_delta = min(drag_speed_delta,-_loiter_accel_min_cmss*nav_dt); } desired_speed = max(desired_speed+drag_speed_delta,0.0f); desired_vel = desired_vel_norm*desired_speed; } // Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing) float horizSpdDem = sqrtf(sq(desired_vel.x) + sq(desired_vel.y)); if (horizSpdDem > gnd_speed_limit_cms) { desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem; desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem; } // send adjusted feed forward velocity back to position controller _pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y); }
static bool is_prop(expr type) { while (is_pi(type)) { type = binding_body(type); } return is_sort(type) && is_zero(sort_level(type)); }
DEVICE bool operator()(int idx) { return is_zero(rays[idx].dir); }
/* calculate throttle demand - airspeed enabled case */ void AP_TECS::_update_throttle_with_airspeed(void) { // Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem; float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem; // Calculate total energy error _STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est; float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast; _STEdotErrLast = STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_flags.underspeed) { _throttle_dem = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; float nomThr = aparm.throttle_cruise * 0.01f; const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); // Calculate PD + FF throttle float throttle_damp = _thrDamp; if (_flags.is_doing_auto_land && !is_zero(_land_throttle_damp)) { throttle_damp = _land_throttle_damp; } _throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle; // Constrain throttle demand _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (aparm.throttle_slewrate != 0) { float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * aparm.throttle_slewrate * 0.01f; _throttle_dem = constrain_float(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); _last_throttle_dem = _throttle_dem; } // Calculate integrator state upper and lower limits // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand // Additionally constrain the integrator state amplitude so that the integrator comes off limits faster. float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero); float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp); float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp); // Calculate integrator state, constraining state // Set integrator to a max throttle value during climbout _integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr; if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { if (!_flags.reached_speed_takeoff) { // ensure we run at full throttle until we reach the target airspeed _throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state); } _integTHR_state = integ_max; } else { _integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max); } // Sum the components. _throttle_dem = _throttle_dem + _integTHR_state; } // Constrain throttle demand _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); }
// *this *= 2 void PointGFp::mult2(std::vector<BigInt>& ws_bn) { if(is_zero()) return; else if(m_coord_y.is_zero()) { *this = PointGFp(m_curve); // setting myself to zero return; } /* http://hyperelliptic.org/EFD/g1p/auto-shortw-jacobian-3.html#doubling-dbl-1986-cc */ const BigInt& p = m_curve.get_p(); BigInt& y_2 = ws_bn[0]; BigInt& S = ws_bn[1]; BigInt& z4 = ws_bn[2]; BigInt& a_z4 = ws_bn[3]; BigInt& M = ws_bn[4]; BigInt& U = ws_bn[5]; BigInt& x = ws_bn[6]; BigInt& y = ws_bn[7]; BigInt& z = ws_bn[8]; curve_sqr(y_2, m_coord_y); curve_mult(S, m_coord_x, y_2); S <<= 2; // * 4 while(S >= p) S -= p; curve_sqr(z4, curve_sqr(m_coord_z)); curve_mult(a_z4, m_curve.get_a_rep(), z4); M = curve_sqr(m_coord_x); M *= 3; M += a_z4; while(M >= p) M -= p; curve_sqr(x, M); x -= (S << 1); while(x.is_negative()) x += p; curve_sqr(U, y_2); U <<= 3; while(U >= p) U -= p; S -= x; while(S.is_negative()) S += p; curve_mult(y, M, S); y -= U; if(y.is_negative()) y += p; curve_mult(z, m_coord_y, m_coord_z); z <<= 1; if(z >= p) z -= p; m_coord_x = x; m_coord_y = y; m_coord_z = z; }
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_STATUSTEXT: { // ignore any statustext messages not from our GCS: if (msg->sysid != rover.g.sysid_my_gcs) { break; } mavlink_statustext_t packet; mavlink_msg_statustext_decode(msg, &packet); char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G', 'C', 'S', ':'}; memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); rover.DataFlash.Log_Write_Message(text); break; } case MAVLINK_MSG_ID_COMMAND_INT: { // decode packet mavlink_command_int_t packet; mavlink_msg_command_int_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; switch (packet.command) { #if MOUNT == ENABLED case MAV_CMD_DO_SET_ROI: { // param1 : /* Region of interest mode (not used)*/ // param2 : /* MISSION index/ target ID (not used)*/ // param3 : /* ROI index (not used)*/ // param4 : /* empty */ // x : lat // y : lon // z : alt // sanity check location if (!check_latlng(packet.x, packet.y)) { break; } Location roi_loc; roi_loc.lat = packet.x; roi_loc.lng = packet.y; roi_loc.alt = (int32_t)(packet.z * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; } #endif default: result = MAV_RESULT_UNSUPPORTED; break; } // send ACK or NAK mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); 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 switch (packet.command) { case MAV_CMD_START_RX_PAIR: result = handle_rc_bind(packet); break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: rover.set_mode(RTL); result = MAV_RESULT_ACCEPTED; break; #if MOUNT == ENABLED // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; #endif #if CAMERA == ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: rover.camera.configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_DO_DIGICAM_CONTROL: if (rover.camera.control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6)) { rover.log_picture(); } result = MAV_RESULT_ACCEPTED; break; #endif // CAMERA == ENABLED case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif break; case MAV_CMD_MISSION_START: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_CALIBRATION: if (hal.util->get_soft_armed()) { result = MAV_RESULT_FAILED; break; } if (is_equal(packet.param1, 1.0f)) { rover.ins.init_gyro(); if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param3, 1.0f)) { rover.init_barometer(false); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param4, 1.0f)) { rover.trim_radio(); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param5, 1.0f)) { result = MAV_RESULT_ACCEPTED; // start with gyro calibration rover.ins.init_gyro(); // reset ahrs gyro bias if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); } else { result = MAV_RESULT_FAILED; } rover.ins.acal_init(); rover.ins.get_acal()->start(this); } else if (is_equal(packet.param5, 2.0f)) { // start with gyro calibration rover.ins.init_gyro(); // accel trim float trim_roll, trim_pitch; if (rover.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); } break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: { uint8_t compassNumber = -1; if (is_equal(packet.param1, 2.0f)) { compassNumber = 0; } else if (is_equal(packet.param1, 5.0f)) { compassNumber = 1; } else if (is_equal(packet.param1, 6.0f)) { compassNumber = 2; } if (compassNumber != (uint8_t) -1) { rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: rover.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: rover.set_mode(LEARNING); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_SERVO: if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_SET_RELAY: if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_RELAY: if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { 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_COMPONENT_ARM_DISARM: if (is_equal(packet.param1, 1.0f)) { // run pre_arm_checks and arm_checks and display failures if (rover.arm_motors(AP_Arming::MAVLINK)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_zero(packet.param1)) { if (rover.disarm_motors()) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_GET_HOME_POSITION: if (rover.home_is_set != HOME_UNSET) { send_home(rover.ahrs.get_home()); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } break; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1, 1.0f)) { send_autopilot_version(FIRMWARE_VERSION); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) // param5 : latitude // param6 : longitude // param7 : altitude result = MAV_RESULT_FAILED; // assume failure if (is_equal(packet.param1, 1.0f)) { rover.init_home(); } else { if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { // don't allow the 0,0 position break; } // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location new_home_loc {}; new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); rover.ahrs.set_home(new_home_loc); rover.home_is_set = HOME_SET_NOT_LOCKED; rover.Log_Write_Home_And_Origin(); GCS_MAVLINK::send_home_all(new_home_loc); result = MAV_RESULT_ACCEPTED; rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", (double)(new_home_loc.lat * 1.0e-7f), (double)(new_home_loc.lng * 1.0e-7f), (uint32_t)(new_home_loc.alt * 0.01f)); } break; } case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: result = rover.compass.handle_mag_cal_command(packet); break; case MAV_CMD_NAV_SET_YAW_SPEED: { // param1 : yaw angle to adjust direction by in centidegress // param2 : Speed - normalized to 0 .. 1 // exit if vehicle is not in Guided mode if (rover.control_mode != GUIDED) { break; } rover.guided_mode = Guided_Angle; rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis(); rover.guided_yaw_speed.turn_angle = packet.param1; rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f); rover.nav_set_yaw_speed(); result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_ACCELCAL_VEHICLE_POS: result = MAV_RESULT_FAILED; if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) { result = MAV_RESULT_ACCEPTED; } break; default: break; } mavlink_msg_command_ack_send_buf( msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t)); break; } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { handle_mission_request_list(rover.mission, msg); break; } // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST_INT: case MAVLINK_MSG_ID_MISSION_REQUEST: { handle_mission_request(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_ACK: { // not used break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // mark the firmware version in the tlog send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); #endif handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { handle_mission_clear_all(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { handle_mission_set_current(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_COUNT: { handle_mission_count(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { handle_mission_write_partial_list(rover.mission, msg); break; } // GCS has sent us a mission item, store to EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_MISSION_ITEM_INT: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, &rover.DataFlash); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; v[3] = packet.chan4_raw; v[4] = packet.chan5_raw; v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; hal.rcin->set_overrides(v, 8); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if (msg->sysid != rover.g.sysid_my_gcs) { break; } rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); 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 if (rover.control_mode != GUIDED) { 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; // prepare and send target position if (!pos_ignore) { Location loc = rover.current_loc; if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { // rotate from body-frame to NE frame float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw(); float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw(); // add offset to current location location_offset(loc, ne_x, ne_y); } else if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED) { // add offset to current location location_offset(loc, packet.x, packet.y); } else { // MAV_FRAME_LOCAL_NED interpret as an offset from home loc = rover.ahrs.get_home(); location_offset(loc, packet.x, packet.y); } rover.guided_WP = loc; rover.rtl_complete = false; rover.set_guided_WP(); } 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 if (rover.control_mode != GUIDED) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; // prepare and send target position if (!pos_ignore) { Location loc = rover.current_loc; loc.lat = packet.lat_int; loc.lng = packet.lon_int; rover.guided_WP = loc; rover.rtl_complete = false; rover.set_guided_WP(); } break; } case MAVLINK_MSG_ID_GPS_RTCM_DATA: case MAVLINK_MSG_ID_GPS_INPUT: { rover.gps.handle_msg(msg); break; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { 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); compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); break; } #endif // HIL_MODE #if CAMERA == ENABLED // deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: { break; } // deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONTROL: { rover.camera.control_msg(msg); rover.log_picture(); break; } #endif // CAMERA == ENABLED #if MOUNT == ENABLED // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } case MAVLINK_MSG_ID_LOG_REQUEST_DATA: case MAVLINK_MSG_ID_LOG_ERASE: rover.in_log_download = true; /* no break */ case MAVLINK_MSG_ID_LOG_REQUEST_LIST: if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_LOG_REQUEST_END: rover.in_log_download = false; if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, rover.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, rover.gps); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.sonar.handle_msg(msg); break; case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: rover.DataFlash.remote_log_block_status_msg(chan, msg); break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: send_autopilot_version(FIRMWARE_VERSION); break; case MAVLINK_MSG_ID_LED_CONTROL: // send message to Notify AP_Notify::handle_led_control(msg); break; case MAVLINK_MSG_ID_PLAY_TUNE: // send message to Notify AP_Notify::handle_play_tune(msg); break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink