// calculate steering output to drive along line from origin to destination waypoint // relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updated void AR_WPNav::update_steering(const Location& current_loc, float current_speed) { // calculate yaw error for determining if vehicle should pivot towards waypoint float yaw_cd = _reversed ? wrap_360_cd(_wp_bearing_cd + 18000) : _wp_bearing_cd; float yaw_error_cd = wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor); // calculate desired turn rate and update desired heading if (use_pivot_steering(yaw_error_cd)) { _cross_track_error = 0.0f; _desired_heading_cd = yaw_cd; _desired_lat_accel = 0.0f; _desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(yaw_cd * 0.01f), radians(_pivot_rate)); } else { // run L1 controller _nav_controller.set_reverse(_reversed); _nav_controller.update_waypoint((_reached_destination ? current_loc : _origin), _destination, _radius); // retrieve lateral acceleration, heading back towards line and crosstrack error _desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_turn_max_mss, _turn_max_mss); _desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd()); if (_reversed) { _desired_lat_accel *= -1.0f; _desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000); } _cross_track_error = _nav_controller.crosstrack_error(); _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed); } }
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { // get current yaw target int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z; // get final angle, 1 = Relative, 0 = Absolute if (relative_angle == 0) { // absolute angle yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle if (direction < 0) { angle_deg = -angle_deg; } yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); } // get turn speed if (is_zero(turn_rate_dps)) { // default to regular auto slew rate yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; }else{ int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec } // set yaw mode set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise }
static void test_wrap_cd(void) { for (uint8_t i=0; i < ARRAY_SIZE(wrap_180_tests); i++) { int32_t r = wrap_180_cd(wrap_180_tests[i].v); if (r != wrap_180_tests[i].wv) { hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld\n", (long)wrap_180_tests[i].v, (long)wrap_180_tests[i].wv, (long)r); } } for (uint8_t i=0; i < ARRAY_SIZE(wrap_360_tests); i++) { int32_t r = wrap_360_cd(wrap_360_tests[i].v); if (r != wrap_360_tests[i].wv) { hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld\n", (long)wrap_360_tests[i].v, (long)wrap_360_tests[i].wv, (long)r); } } for (uint8_t i=0; i < ARRAY_SIZE(wrap_PI_tests); i++) { float r = wrap_PI(wrap_PI_tests[i].v); if (fabsf(r - wrap_PI_tests[i].wv) > 0.001f) { hal.console->printf("wrap_PI: v=%f wv=%f r=%f\n", wrap_PI_tests[i].v, wrap_PI_tests[i].wv, r); } } hal.console->printf("wrap_cd tests done\n"); }
void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) { // select heading method. Either mission, gps bearing projection or yaw based // If prev_WP_loc and next_WP_loc are different then an accurate wp based bearing can // be computed. However, if we had just changed modes before this, such as an aborted landing // via mode change, the prev and next wps are the same. float bearing; if (!locations_are_same(prev_WP_loc, next_WP_loc)) { // use waypoint based bearing, this is the usual case steer_state.hold_course_cd = -1; } else if (ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) { // use gps ground course based bearing hold steer_state.hold_course_cd = -1; bearing = ahrs.get_gps().ground_course_cd() * 0.01f; location_update(next_WP_loc, bearing, 1000); // push it out 1km } else { // use yaw based bearing hold steer_state.hold_course_cd = wrap_360_cd(ahrs.yaw_sensor); bearing = ahrs.yaw_sensor * 0.01f; location_update(next_WP_loc, bearing, 1000); // push it out 1km } next_WP_loc.alt = cmd.content.location.alt + home.alt; condition_value = cmd.p1; reset_offset_altitude(); }
/* copy current data to CHEK message */ void Replay::log_check_generate(void) { Vector3f euler; Vector3f velocity; Location loc {}; _vehicle.EKF.getEulerAngles(euler); _vehicle.EKF.getVelNED(velocity); _vehicle.EKF.getLLH(loc); struct log_Chek packet = { LOG_PACKET_HEADER_INIT(LOG_CHEK_MSG), time_us : AP_HAL::micros64(), roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string) pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string) yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string) lat : loc.lat, lng : loc.lng, alt : loc.alt*0.01f, vnorth : velocity.x, veast : velocity.y, vdown : velocity.z }; _vehicle.dataflash.WriteBlock(&packet, sizeof(packet)); }
// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination // this function updates the _yaw_error_cd value void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed) { // record system time of call last_steer_to_wp_ms = AP_HAL::millis(); // Calculate the required turn of the wheels // negative error = left turn // positive error = right turn rover.nav_controller->set_reverse(reversed); rover.nav_controller->update_waypoint(origin, destination, g.waypoint_radius); float desired_lat_accel = rover.nav_controller->lateral_acceleration(); float desired_heading = rover.nav_controller->target_bearing_cd(); if (reversed) { desired_heading = wrap_360_cd(desired_heading + 18000); desired_lat_accel *= -1.0f; } _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor); if (rover.sailboat_use_indirect_route(desired_heading)) { // sailboats use heading controller when tacking upwind desired_heading = rover.sailboat_calc_heading(desired_heading); calc_steering_to_heading(desired_heading, g2.pivot_turn_rate); } else if (rover.use_pivot_steering(_yaw_error_cd)) { // for pivot turns use heading controller calc_steering_to_heading(desired_heading, g2.pivot_turn_rate); } else { // call lateral acceleration to steering controller calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); } }
/* report SITL state to DataFlash */ void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash) { float yaw; // convert to same conventions as DCM yaw = state.yawDeg; if (yaw > 180) { yaw -= 360; } struct log_AHRS pkt = { LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG), time_us : AP_HAL::micros64(), roll : (int16_t)(state.rollDeg*100), pitch : (int16_t)(state.pitchDeg*100), yaw : (uint16_t)(wrap_360_cd(yaw*100)), alt : (float)state.altitude, lat : (int32_t)(state.latitude*1.0e7), lng : (int32_t)(state.longitude*1.0e7), q1 : state.quaternion.q1, q2 : state.quaternion.q2, q3 : state.quaternion.q3, q4 : state.quaternion.q4, }; DataFlash->WriteBlock(&pkt, sizeof(pkt)); }
/* set HIL (hardware in the loop) status for a GPS instance */ void AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, const Location &_location, const Vector3f &_velocity, uint8_t _num_sats, uint16_t hdop, bool _have_vertical_velocity) { if (instance >= GPS_MAX_INSTANCES) { return; } uint32_t tnow = AP_HAL::millis(); GPS_State &istate = state[instance]; istate.status = _status; istate.location = _location; istate.location.options = 0; istate.velocity = _velocity; istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y); istate.ground_course_cd = wrap_360_cd(degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL); istate.hdop = hdop; istate.num_sats = _num_sats; istate.have_vertical_velocity |= _have_vertical_velocity; istate.last_gps_time_ms = tnow; uint64_t gps_time_ms = time_epoch_ms - (17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL); istate.time_week = gps_time_ms / (86400*7*(uint64_t)1000); istate.time_week_ms = gps_time_ms - istate.time_week*(86400*7*(uint64_t)1000); timing[instance].last_message_time_ms = tnow; timing[instance].last_fix_time_ms = tnow; _type[instance].set(GPS_TYPE_HIL); }
/* report SITL state to DataFlash */ void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash) { double p, q, r; float yaw; // we want the gyro values to be directly comparable to the // raw_imu message, which is in body frame convert_body_frame(state.rollDeg, state.pitchDeg, state.rollRate, state.pitchRate, state.yawRate, &p, &q, &r); // convert to same conventions as DCM yaw = state.yawDeg; if (yaw > 180) { yaw -= 360; } struct log_AHRS pkt = { LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG), time_ms : hal.scheduler->millis(), roll : (int16_t)(state.rollDeg*100), pitch : (int16_t)(state.pitchDeg*100), yaw : (uint16_t)(wrap_360_cd(yaw*100)), alt : (float)state.altitude, lat : (int32_t)(state.latitude*1.0e7), lng : (int32_t)(state.longitude*1.0e7) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); }
bool Plane::verify_loiter_heading(bool init) { if (quadplane.in_vtol_auto()) { // skip heading verify if in VTOL auto return true; } //Get the lat/lon of next Nav waypoint after this one: AP_Mission::Mission_Command next_nav_cmd; if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd)) { //no next waypoint to shoot for -- go ahead and break out of loiter return true; } if (get_distance(next_WP_loc, next_nav_cmd.content.location) < abs(aparm.loiter_radius)) { /* Whenever next waypoint is within the loiter radius, maintaining loiter would prevent us from ever pointing toward the next waypoint. Hence break out of loiter immediately */ return true; } // Bearing in degrees int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location); // get current heading. int32_t heading_cd = gps.ground_course_cd(); int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); if (init) { loiter.total_cd = wrap_360_cd(bearing_cd - heading_cd); loiter.sum_cd = 0; } /* Check to see if the the plane is heading toward the land waypoint. We use 20 degrees (+/-10 deg) of margin so that we can handle 200 degrees/second of yaw. Allow turn count to stop it too to ensure we don't loop around forever in case high winds are forcing us beyond 200 deg/sec at this particular moment. */ if (labs(heading_err_cd) <= 1000 || loiter.sum_cd > loiter.total_cd) { // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location if (next_WP_loc.flags.loiter_xtrack) { next_WP_loc = current_loc; } return true; } return false; }
bool Plane::verify_takeoff() { if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) { const float min_gps_speed = 5; if (auto_state.takeoff_speed_time_ms == 0 && gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.ground_speed() > min_gps_speed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { auto_state.takeoff_speed_time_ms = millis(); } if (auto_state.takeoff_speed_time_ms != 0 && millis() - auto_state.takeoff_speed_time_ms >= 2000) { // once we reach sufficient speed for good GPS course // estimation we save our current GPS ground course // corrected for summed yaw to set the take off // course. This keeps wings level until we are ready to // rotate, and also allows us to cope with arbitrary // compass errors for auto takeoff float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err; takeoff_course = wrap_PI(takeoff_course); steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)", steer_state.hold_course_cd, (double)gps.ground_speed(), (double)degrees(steer_state.locked_course_err)); } } if (steer_state.hold_course_cd != -1) { // call navigation controller for heading hold nav_controller->update_heading_hold(steer_state.hold_course_cd); } else { nav_controller->update_level_flight(); } // see if we have reached takeoff altitude int32_t relative_alt_cm = adjusted_relative_altitude_cm(); if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm", (double)(relative_alt_cm*0.01f)); steer_state.hold_course_cd = -1; auto_state.takeoff_complete = true; next_WP_loc = prev_WP_loc = current_loc; plane.complete_auto_takeoff(); // don't cross-track on completion of takeoff, as otherwise we // can end up doing too sharp a turn auto_state.next_wp_no_crosstrack = true; return true; } else { return false; } }
void AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v, int32_t yaw) { float v_length = v.length(); char arrow = SYM_ARROW_START; if (v_length > 1.0f) { int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - yaw); int32_t interval = 36000 / SYM_ARROW_COUNT; arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT; } backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, v_length), u_icon(SPEED)); }
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error, float overshoot_max) { // calculate angle error with maximum of +- max angle overshoot angle_ef_error.z = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor); angle_ef_error.z = constrain_float(angle_ef_error.z, -overshoot_max, overshoot_max); // update yaw angle target to be within max angle overshoot of our current heading _angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor; // increment the yaw angle target _angle_ef_target.z += yaw_rate_ef * _dt; _angle_ef_target.z = wrap_360_cd(_angle_ef_target.z); }
// Write an attitude packet void Tracker::Log_Write_Attitude() { Vector3f targets; targets.y = nav_status.pitch * 100.0f; targets.z = wrap_360_cd(nav_status.bearing * 100.0f); DataFlash.Log_Write_Attitude(ahrs, targets); DataFlash.Log_Write_EKF(ahrs,false); DataFlash.Log_Write_AHRS2(ahrs); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL sitl.Log_Write_SIMSTATE(&DataFlash); #endif DataFlash.Log_Write_POS(ahrs); }
void AP_OSD_Screen::draw_waypoint(uint8_t x, uint8_t y) { AP_AHRS &ahrs = AP::ahrs(); int32_t angle = wrap_360_cd(osd->nav_info.wp_bearing - ahrs.yaw_sensor); int32_t interval = 36000 / SYM_ARROW_COUNT; if (osd->nav_info.wp_distance < 2.0f) { //avoid fast rotating arrow at small distances angle = 0; } char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT; backend->write(x,y, false, "%c%2u%c",SYM_WPNO, osd->nav_info.wp_number, arrow); draw_distance(x+4, y, osd->nav_info.wp_distance); }
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error) { // calculate angle error with maximum of +- max angle overshoot angle_ef_error.z = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor); angle_ef_error.z = constrain_float(angle_ef_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); // update yaw angle target to be within max angle overshoot of our current heading _angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor; // increment the yaw angle target _angle_ef_target.z += yaw_rate_ef * _dt; _angle_ef_target.z = wrap_360_cd(_angle_ef_target.z); }
// decode pilot's input and return heading_out (in cd) and speed_out (in m/s) void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out) { // get steering and throttle in the -1 to +1 range const float desired_steering = constrain_float(rover.channel_steer->norm_input_dz(), -1.0f, 1.0f); const float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f); // calculate angle of input stick vector heading_out = wrap_360_cd(atan2f(desired_steering, desired_throttle) * DEGX100); // calculate throttle using magnitude of input stick vector const float throttle = MIN(safe_sqrt(sq(desired_throttle) + sq(desired_steering)), 1.0f); speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); }
void Copter::init_simple_bearing() { // capture current cos_yaw and sin_yaw values simple_cos_yaw = ahrs.cos_yaw(); simple_sin_yaw = ahrs.sin_yaw(); // initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000); super_simple_cos_yaw = simple_cos_yaw; super_simple_sin_yaw = simple_sin_yaw; // log the simple bearing to dataflash if (should_log(MASK_LOG_ANY)) { Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor); } }
// get_auto_heading - returns target heading depending upon auto_yaw_mode // 100hz update rate float Sub::get_auto_heading(void) { switch (auto_yaw_mode) { case AUTO_YAW_ROI: // point towards a location held in roi_WP return get_roi_yaw(); break; case AUTO_YAW_LOOK_AT_HEADING: // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed return yaw_look_at_heading; break; case AUTO_YAW_LOOK_AHEAD: // Commanded Yaw to automatically look ahead. return get_look_ahead_yaw(); break; case AUTO_YAW_RESETTOARMEDYAW: // changes yaw to be same as when quad was armed return initial_armed_bearing; break; case AUTO_YAW_CORRECT_XTRACK: { // TODO return current yaw if not in appropriate mode // Bearing of current track (centidegrees) float track_bearing = wp_nav.get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination()); // Bearing from current position towards intermediate position target (centidegrees) float desired_angle = wp_nav.get_loiter_bearing_to_target(); float angle_error = wrap_180_cd(desired_angle - track_bearing); float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); return wrap_360_cd(track_bearing + angle_limited); } break; case AUTO_YAW_LOOK_AT_NEXT_WP: default: // point towards next waypoint. // we don't use wp_bearing because we don't want the vehicle to turn too much during flight return wp_nav.get_yaw(); break; } }
void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y) { AP_AHRS &ahrs = AP::ahrs(); Location loc; if (ahrs.get_position(loc) && ahrs.home_is_set()) { const Location &home_loc = ahrs.get_home(); float distance = home_loc.get_distance(loc); int32_t angle = wrap_360_cd(loc.get_bearing_to(home_loc) - ahrs.yaw_sensor); int32_t interval = 36000 / SYM_ARROW_COUNT; if (distance < 2.0f) { //avoid fast rotating arrow at small distances angle = 0; } char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT; backend->write(x, y, false, "%c%c", SYM_HOME, arrow); draw_distance(x+2, y, distance); } else { backend->write(x, y, true, "%c", SYM_HOME); } }
// Write an attitude packet void Plane::Log_Write_Attitude(void) { Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude targets.x = nav_roll_cd; targets.y = nav_pitch_cd; if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) { // when VTOL active log the copter target yaw targets.z = wrap_360_cd(quadplane.attitude_control->get_att_target_euler_cd().z); } else { //Plane does not have the concept of navyaw. This is a placeholder. targets.z = 0; } if (quadplane.tailsitter_active()) { DataFlash.Log_Write_AttitudeView(*quadplane.ahrs_view, targets); } else { DataFlash.Log_Write_Attitude(ahrs, targets); } if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) { // log quadplane PIDs separately from fixed wing PIDs DataFlash.Log_Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() ); } DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDP_MSG, pitchController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDY_MSG, yawController.get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info()); #if AP_AHRS_NAVEKF_AVAILABLE DataFlash.Log_Write_EKF(ahrs); DataFlash.Log_Write_AHRS2(ahrs); #endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL sitl.Log_Write_SIMSTATE(&DataFlash); #endif DataFlash.Log_Write_POS(ahrs); }
void ModeSimple::update() { float desired_heading_cd, desired_speed; // get pilot input get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed); // rotate heading around based on initial heading if (g2.simple_type == Simple_InitialHeading) { desired_heading_cd = wrap_360_cd(_initial_heading_cd + desired_heading_cd); } // if sticks in middle, use previous desired heading (important when vehicle is slowing down) if (!is_positive(desired_speed)) { desired_heading_cd = _desired_heading_cd; } else { // record desired heading for next iteration _desired_heading_cd = desired_heading_cd; } // run throttle and steering controllers calc_steering_to_heading(desired_heading_cd); calc_throttle(desired_speed, true); }
TEST(MathWrapTest, Angle360) { // Full circle test for (int32_t i = 0; i <= 36000; i += 100) { if (i == 0) { // hit pole position EXPECT_EQ(i, wrap_360_cd(i)); EXPECT_EQ(i, wrap_360_cd(-i)); } else if (i < 36000) { // between pole position EXPECT_EQ(i, wrap_360_cd(i)); EXPECT_EQ(36000 - i, wrap_360_cd(-i)); } else if (i == 36000) { // hit pole position EXPECT_EQ(0, wrap_360_cd(i)); EXPECT_EQ(0, wrap_360_cd(-i)); } } EXPECT_EQ(4500.f, wrap_360_cd(4500.f)); EXPECT_EQ(9000.f, wrap_360_cd(9000.f)); EXPECT_EQ(18000.f, wrap_360_cd(18000.f)); EXPECT_EQ(27000.f, wrap_360_cd(27000.f)); EXPECT_EQ(0.f, wrap_360_cd(36000.f)); EXPECT_EQ(0.f, wrap_360_cd(72000.f)); EXPECT_EQ(0.f, wrap_360_cd(360000.f)); EXPECT_EQ(0.f, wrap_360_cd(720000.f)); EXPECT_EQ( 0.f, wrap_360_cd(-3600000000.f)); EXPECT_EQ(31500.f, wrap_360_cd(-4500.f)); EXPECT_EQ(27000.f, wrap_360_cd(-9000.f)); EXPECT_EQ(18000.f, wrap_360_cd(-18000.f)); EXPECT_EQ(9000.f, wrap_360_cd(-27000.f)); EXPECT_EQ(0.f, wrap_360_cd(-36000.f)); EXPECT_EQ(0.f, wrap_360_cd(-72000.f)); }
bool AP_GPS_ERB::_parse_gps(void) { switch (_msg_id) { case MSG_VER: Debug("Version of ERB protocol %u.%u.%u", _buffer.ver.ver_high, _buffer.ver.ver_medium, _buffer.ver.ver_low); break; case MSG_POS: Debug("Message POS"); _last_pos_time = _buffer.pos.time; state.location.lng = (int32_t)(_buffer.pos.longitude * 1e7); state.location.lat = (int32_t)(_buffer.pos.latitude * 1e7); state.location.alt = (int32_t)(_buffer.pos.altitude_msl * 1e2); state.status = next_fix; _new_position = true; state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f; state.vertical_accuracy = _buffer.pos.vertical_accuracy * 1.0e-3f; state.have_horizontal_accuracy = true; state.have_vertical_accuracy = true; break; case MSG_STAT: Debug("Message STAT fix_status=%u fix_type=%u", _buffer.stat.fix_status, _buffer.stat.fix_type); if (_buffer.stat.fix_status & STAT_FIX_VALID) { if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FIX) { next_fix = AP_GPS::GPS_OK_FIX_3D_RTK; } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FLOAT) { next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_SINGLE) { next_fix = AP_GPS::GPS_OK_FIX_3D; } else { next_fix = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX; } } else { next_fix = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX; } state.num_sats = _buffer.stat.satellites; if (next_fix >= AP_GPS::GPS_OK_FIX_3D) { state.last_gps_time_ms = AP_HAL::millis(); state.time_week_ms = _buffer.stat.time; state.time_week = _buffer.stat.week; } break; case MSG_DOPS: Debug("Message DOPS"); state.hdop = _buffer.dops.hDOP; state.vdop = _buffer.dops.vDOP; break; case MSG_VEL: Debug("Message VEL"); _last_vel_time = _buffer.vel.time; state.ground_speed = _buffer.vel.speed_2d * 0.01f; // m/s // Heading 2D deg * 100000 rescaled to deg * 100 state.ground_course_cd = wrap_360_cd(_buffer.vel.heading_2d / 1000); state.have_vertical_velocity = true; state.velocity.x = _buffer.vel.vel_north * 0.01f; state.velocity.y = _buffer.vel.vel_east * 0.01f; state.velocity.z = _buffer.vel.vel_down * 0.01f; state.have_speed_accuracy = true; state.speed_accuracy = _buffer.vel.speed_accuracy * 0.01f; _new_speed = true; break; default: Debug("Unexpected message 0x%02x", (unsigned)_msg_id); return false; } // we only return true when we get new position and speed data // this ensures we don't use stale data if (_new_position && _new_speed && _last_vel_time == _last_pos_time) { _new_speed = _new_position = false; _fix_count++; return true; } return false; }
bool Plane::verify_takeoff() { if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) { const float min_gps_speed = 5; if (auto_state.takeoff_speed_time_ms == 0 && gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.ground_speed() > min_gps_speed) { auto_state.takeoff_speed_time_ms = millis(); } if (auto_state.takeoff_speed_time_ms != 0 && millis() - auto_state.takeoff_speed_time_ms >= 2000) { // once we reach sufficient speed for good GPS course // estimation we save our current GPS ground course // corrected for summed yaw to set the take off // course. This keeps wings level until we are ready to // rotate, and also allows us to cope with arbitary // compass errors for auto takeoff float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err; takeoff_course = wrap_PI(takeoff_course); steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); gcs_send_text_fmt(PSTR("Holding course %ld at %.1fm/s (%.1f)"), steer_state.hold_course_cd, (double)gps.ground_speed(), (double)degrees(steer_state.locked_course_err)); } } if (steer_state.hold_course_cd != -1) { // call navigation controller for heading hold nav_controller->update_heading_hold(steer_state.hold_course_cd); } else { nav_controller->update_level_flight(); } // see if we have reached takeoff altitude int32_t relative_alt_cm = adjusted_relative_altitude_cm(); if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { gcs_send_text_fmt(PSTR("Takeoff complete at %.2fm"), (double)(relative_alt_cm*0.01f)); steer_state.hold_course_cd = -1; auto_state.takeoff_complete = true; next_WP_loc = prev_WP_loc = current_loc; #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable > 0) { if (! geofence_set_enabled(true, AUTO_TOGGLED)) { gcs_send_text_P(SEVERITY_HIGH, PSTR("Enable fence failed (cannot autoenable")); } else { gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence enabled. (autoenabled)")); } } #endif // don't cross-track on completion of takeoff, as otherwise we // can end up doing too sharp a turn auto_state.next_wp_no_crosstrack = true; return true; } else { return false; } }
int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv) { if (!g.compass_enabled) { cliSerial->print("Compass: "******"Compass initialisation failed!"); return 0; } ahrs.init(); ahrs.set_fly_forward(true); ahrs.set_compass(&compass); // we need the AHRS initialised for this test ins.init(scheduler.get_loop_rate_hz()); ahrs.reset(); int counter = 0; float heading = 0; print_hit_enter(); uint8_t medium_loopCounter = 0; while(1) { ins.wait_for_sample(); ahrs.update(); medium_loopCounter++; if(medium_loopCounter >= 5){ if (compass.read()) { // Calculate heading Matrix3f m = ahrs.get_rotation_body_to_ned(); heading = compass.calculate_heading(m); compass.learn_offsets(); } medium_loopCounter = 0; } counter++; if (counter>20) { if (compass.healthy()) { const Vector3f mag_ofs = compass.get_offsets(); const Vector3f mag = compass.get_field(); cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", (double)(wrap_360_cd(ToDeg(heading) * 100)) /100, (double)mag.x, (double)mag.y, (double)mag.z, (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z); } else { cliSerial->println("compass not healthy"); } counter=0; } if (cliSerial->available() > 0) { break; } } // save offsets. This allows you to get sane offset values using // the CLI before you go flying. cliSerial->println("saving offsets"); compass.save_offsets(); return (0); }
int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv) { if (!g.compass_enabled) { cliSerial->printf_P(PSTR("Compass: "******"Compass initialisation failed!")); return 0; } ahrs.init(); ahrs.set_fly_forward(true); ahrs.set_wind_estimation(true); ahrs.set_compass(&compass); // we need the AHRS initialised for this test ins.init(AP_InertialSensor::COLD_START, ins_sample_rate); ahrs.reset(); uint16_t counter = 0; float heading = 0; print_hit_enter(); while(1) { hal.scheduler->delay(20); if (micros() - fast_loopTimer_us > 19000UL) { fast_loopTimer_us = micros(); // INS // --- ahrs.update(); if(counter % 5 == 0) { if (compass.read()) { // Calculate heading const Matrix3f &m = ahrs.get_dcm_matrix(); heading = compass.calculate_heading(m); compass.learn_offsets(); } } counter++; if (counter>20) { if (compass.healthy()) { const Vector3f &mag_ofs = compass.get_offsets_milligauss(); const Vector3f &mag = compass.get_field_milligauss(); cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), (wrap_360_cd(ToDeg(heading) * 100)) /100, (double)mag.x, (double)mag.y, (double)mag.z, (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z); } else { cliSerial->println_P(PSTR("compass not healthy")); } counter=0; } } if (cliSerial->available() > 0) { break; } } // save offsets. This allows you to get sane offset values using // the CLI before you go flying. cliSerial->println_P(PSTR("saving offsets")); compass.save_offsets(); return (0); }
bool AP_GPS_UBLOX::_parse_gps(void) { if (_class == CLASS_ACK) { Debug("ACK %u", (unsigned)_msg_id); if(_msg_id == MSG_ACK_ACK && _buffer.ack.clsID == CLASS_CFG && _buffer.ack.msgID == MSG_CFG_CFG) { _cfg_saved = true; } return false; } if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) { Debug("Got settings %u min_elev %d drLimit %u\n", (unsigned)_buffer.nav_settings.dynModel, (int)_buffer.nav_settings.minElev, (unsigned)_buffer.nav_settings.drLimit); _buffer.nav_settings.mask = 0; if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE && _buffer.nav_settings.dynModel != gps._navfilter) { // we've received the current nav settings, change the engine // settings and send them back Debug("Changing engine setting from %u to %u\n", (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter); _buffer.nav_settings.dynModel = gps._navfilter; _buffer.nav_settings.mask |= 1; } if (gps._min_elevation != -100 && _buffer.nav_settings.minElev != gps._min_elevation) { Debug("Changing min elevation to %d\n", (int)gps._min_elevation); _buffer.nav_settings.minElev = gps._min_elevation; _buffer.nav_settings.mask |= 2; } if (_buffer.nav_settings.mask != 0) { _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, &_buffer.nav_settings, sizeof(_buffer.nav_settings)); _cfg_saved = false; //save configuration } return false; } #if UBLOX_GNSS_SETTINGS if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) { uint8_t gnssCount = 0; Debug("Got GNSS Settings %u %u %u %u:\n", (unsigned)_buffer.gnss.msgVer, (unsigned)_buffer.gnss.numTrkChHw, (unsigned)_buffer.gnss.numTrkChUse, (unsigned)_buffer.gnss.numConfigBlocks); #if UBLOX_DEBUG for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) { Debug(" %u %u %u 0x%08x\n", (unsigned)_buffer.gnss.configBlock[i].gnssId, (unsigned)_buffer.gnss.configBlock[i].resTrkCh, (unsigned)_buffer.gnss.configBlock[i].maxTrkCh, (unsigned)_buffer.gnss.configBlock[i].flags); } #endif for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) { if((gps._gnss_mode & (1 << i)) && i != GNSS_SBAS) { gnssCount++; } } for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) { // Reserve an equal portion of channels for all enabled systems if(gps._gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) { if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) { _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2); _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw; } else { _buffer.gnss.configBlock[i].resTrkCh = 1; _buffer.gnss.configBlock[i].maxTrkCh = 3; } _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001; } else { _buffer.gnss.configBlock[i].resTrkCh = 0; _buffer.gnss.configBlock[i].maxTrkCh = 0; _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE; } } _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks)); return false; } #endif if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) { Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", (unsigned)_buffer.sbas.mode, (unsigned)_buffer.sbas.usage, (unsigned)_buffer.sbas.maxSBAS, (unsigned)_buffer.sbas.scanmode2, (unsigned)_buffer.sbas.scanmode1); if (_buffer.sbas.mode != gps._sbas_mode) { _buffer.sbas.mode = gps._sbas_mode; _send_message(CLASS_CFG, MSG_CFG_SBAS, &_buffer.sbas, sizeof(_buffer.sbas)); _cfg_saved = false; } } #if UBLOX_HW_LOGGING if (_class == CLASS_MON) { if (_msg_id == MSG_MON_HW) { if (_payload_length == 60 || _payload_length == 68) { log_mon_hw(); } } else if (_msg_id == MSG_MON_HW2) { if (_payload_length == 28) { log_mon_hw2(); } } else { unexpected_message(); } return false; } #endif // UBLOX_HW_LOGGING #if UBLOX_RXM_RAW_LOGGING if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) { log_rxm_raw(_buffer.rxm_raw); return false; } else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) { log_rxm_rawx(_buffer.rxm_rawx); return false; } #endif // UBLOX_RXM_RAW_LOGGING if (_class != CLASS_NAV) { unexpected_message(); return false; } switch (_msg_id) { case MSG_POSLLH: Debug("MSG_POSLLH next_fix=%u", next_fix); _last_pos_time = _buffer.posllh.time; state.location.lng = _buffer.posllh.longitude; state.location.lat = _buffer.posllh.latitude; state.location.alt = _buffer.posllh.altitude_msl / 10; state.status = next_fix; _new_position = true; state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f; state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f; state.have_horizontal_accuracy = true; state.have_vertical_accuracy = true; #if UBLOX_FAKE_3DLOCK state.location.lng = 1491652300L; state.location.lat = -353632610L; state.location.alt = 58400; state.vertical_accuracy = 0; state.horizontal_accuracy = 0; #endif break; case MSG_STATUS: Debug("MSG_STATUS fix_status=%u fix_type=%u", _buffer.status.fix_status, _buffer.status.fix_type); if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) { if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) && (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) { next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) { next_fix = AP_GPS::GPS_OK_FIX_3D; }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) { next_fix = AP_GPS::GPS_OK_FIX_2D; }else{ next_fix = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX; } }else{ next_fix = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX; } #if UBLOX_FAKE_3DLOCK state.status = AP_GPS::GPS_OK_FIX_3D; next_fix = state.status; #endif break; case MSG_DOP: Debug("MSG_DOP"); noReceivedHdop = false; state.hdop = _buffer.dop.hDOP; state.vdop = _buffer.dop.vDOP; #if UBLOX_FAKE_3DLOCK state.hdop = 130; state.hdop = 170; #endif break; case MSG_SOL: Debug("MSG_SOL fix_status=%u fix_type=%u", _buffer.solution.fix_status, _buffer.solution.fix_type); if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) { if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) && (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) { next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) { next_fix = AP_GPS::GPS_OK_FIX_3D; }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) { next_fix = AP_GPS::GPS_OK_FIX_2D; }else{ next_fix = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX; } }else{ next_fix = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX; } if(noReceivedHdop) { state.hdop = _buffer.solution.position_DOP; } state.num_sats = _buffer.solution.satellites; if (next_fix >= AP_GPS::GPS_OK_FIX_2D) { state.last_gps_time_ms = AP_HAL::millis(); if (state.time_week == _buffer.solution.week && state.time_week_ms + 200 == _buffer.solution.time) { // we got a 5Hz update. This relies on the way // that uBlox gives timestamps that are always // multiples of 200 for 5Hz _last_5hz_time = state.last_gps_time_ms; } state.time_week_ms = _buffer.solution.time; state.time_week = _buffer.solution.week; } #if UBLOX_FAKE_3DLOCK next_fix = state.status; state.num_sats = 10; state.time_week = 1721; state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000; state.last_gps_time_ms = AP_HAL::millis(); state.hdop = 130; #endif break; case MSG_VELNED: Debug("MSG_VELNED"); _last_vel_time = _buffer.velned.time; state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s state.ground_course_cd = wrap_360_cd(_buffer.velned.heading_2d / 1000); // Heading 2D deg * 100000 rescaled to deg * 100 state.have_vertical_velocity = true; state.velocity.x = _buffer.velned.ned_north * 0.01f; state.velocity.y = _buffer.velned.ned_east * 0.01f; state.velocity.z = _buffer.velned.ned_down * 0.01f; state.have_speed_accuracy = true; state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; #if UBLOX_FAKE_3DLOCK state.speed_accuracy = 0; #endif _new_speed = true; break; #if UBLOX_VERSION_AUTODETECTION case MSG_NAV_SVINFO: { Debug("MSG_NAV_SVINFO\n"); static const uint8_t HardwareGenerationMask = 0x07; uint8_t hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask; switch (hardware_generation) { case UBLOX_5: case UBLOX_6: /*speed already configured */; break; case UBLOX_7: case UBLOX_M8: port->begin(4000000U); Debug("Changed speed to 5Mhzfor SPI-driven UBlox\n"); break; default: hal.console->printf("Wrong Ublox' Hardware Version%u\n", hardware_generation); break; }; /* We don't need that anymore */ _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0); break; } #endif default: Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id); if (++_disable_counter == 0) { Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id); _configure_message_rate(CLASS_NAV, _msg_id, 0); } return false; } // we only return true when we get new position and speed data // this ensures we don't use stale data if (_new_position && _new_speed && _last_vel_time == _last_pos_time) { _new_speed = _new_position = false; _fix_count++; if ((AP_HAL::millis() - _last_5hz_time) > 15000U && !need_rate_update) { // the GPS is running slow. It possibly browned out and // restarted with incorrect parameters. We will slowly // send out new parameters to fix it need_rate_update = true; rate_update_step = 0; _last_5hz_time = AP_HAL::millis(); } if (_fix_count == 50 && gps._sbas_mode != 2) { // ask for SBAS settings every 20 seconds Debug("Asking for SBAS setting\n"); _send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0); } if (_fix_count == 100) { // ask for nav settings every 20 seconds Debug("Asking for engine setting\n"); _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0); _fix_count = 0; } return true; } return false; }
void Replay::loop() { while (true) { char type[5]; if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) { if (!hal.util->get_soft_armed()) { hal.util->set_soft_armed(true); ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis()); } } if (!logreader.update(type)) { ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f); fclose(plotf); break; } read_sensors(type); if (streq(type,"ATT")) { Vector3f ekf_euler; Vector3f velNED; Vector3f posNED; Vector3f gyroBias; float accelWeighting; float accelZBias1; float accelZBias2; Vector3f windVel; Vector3f magNED; Vector3f magXYZ; Vector3f DCM_attitude; Vector3f ekf_relpos; Vector3f velInnov; Vector3f posInnov; Vector3f magInnov; float tasInnov; float velVar; float posVar; float hgtVar; Vector3f magVar; float tasVar; Vector2f offset; uint16_t faultStatus; const Matrix3f &dcm_matrix = _vehicle.ahrs.AP_AHRS_DCM::get_rotation_body_to_ned(); dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z); _vehicle.EKF.getEulerAngles(ekf_euler); _vehicle.EKF.getVelNED(velNED); _vehicle.EKF.getPosNED(posNED); _vehicle.EKF.getGyroBias(gyroBias); _vehicle.EKF.getIMU1Weighting(accelWeighting); _vehicle.EKF.getAccelZBias(accelZBias1, accelZBias2); _vehicle.EKF.getWind(windVel); _vehicle.EKF.getMagNED(magNED); _vehicle.EKF.getMagXYZ(magXYZ); _vehicle.EKF.getInnovations(velInnov, posInnov, magInnov, tasInnov); _vehicle.EKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); _vehicle.EKF.getFilterFaults(faultStatus); _vehicle.EKF.getPosNED(ekf_relpos); Vector3f inav_pos = _vehicle.inertial_nav.get_position() * 0.01f; float temp = degrees(ekf_euler.z); if (temp < 0.0f) temp = temp + 360.0f; fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n", AP_HAL::millis() * 0.001f, logreader.get_sim_attitude().x, logreader.get_sim_attitude().y, logreader.get_sim_attitude().z, _vehicle.barometer.get_altitude(), logreader.get_attitude().x, logreader.get_attitude().y, wrap_180_cd(logreader.get_attitude().z*100)*0.01f, logreader.get_inavpos().x, logreader.get_inavpos().y, logreader.get_ahr2_attitude().x, logreader.get_ahr2_attitude().y, wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f, degrees(DCM_attitude.x), degrees(DCM_attitude.y), degrees(DCM_attitude.z), degrees(ekf_euler.x), degrees(ekf_euler.y), degrees(ekf_euler.z), inav_pos.x, inav_pos.y, inav_pos.z, ekf_relpos.x, ekf_relpos.y, -ekf_relpos.z); fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", AP_HAL::millis() * 0.001f, degrees(ekf_euler.x), degrees(ekf_euler.y), temp, velNED.x, velNED.y, velNED.z, posNED.x, posNED.y, posNED.z, 60*degrees(gyroBias.x), 60*degrees(gyroBias.y), 60*degrees(gyroBias.z), windVel.x, windVel.y, magNED.x, magNED.y, magNED.z, magXYZ.x, magXYZ.y, magXYZ.z, logreader.get_attitude().x, logreader.get_attitude().y, logreader.get_attitude().z); // define messages for EKF1 data packet int16_t roll = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg) int16_t pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg) uint16_t yaw = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg) float velN = (float)(velNED.x); // velocity North (m/s) float velE = (float)(velNED.y); // velocity East (m/s) float velD = (float)(velNED.z); // velocity Down (m/s) float posN = (float)(posNED.x); // metres North float posE = (float)(posNED.y); // metres East float posD = (float)(posNED.z); // metres Down float gyrX = (float)(6000*degrees(gyroBias.x)); // centi-deg/min float gyrY = (float)(6000*degrees(gyroBias.y)); // centi-deg/min float gyrZ = (float)(6000*degrees(gyroBias.z)); // centi-deg/min // print EKF1 data packet fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %.0f %.0f %.0f\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), roll, pitch, yaw, velN, velE, velD, posN, posE, posD, gyrX, gyrY, gyrZ); // define messages for EKF2 data packet int8_t accWeight = (int8_t)(100*accelWeighting); int8_t acc1 = (int8_t)(100*accelZBias1); int8_t acc2 = (int8_t)(100*accelZBias2); int16_t windN = (int16_t)(100*windVel.x); int16_t windE = (int16_t)(100*windVel.y); int16_t magN = (int16_t)(magNED.x); int16_t magE = (int16_t)(magNED.y); int16_t magD = (int16_t)(magNED.z); int16_t magX = (int16_t)(magXYZ.x); int16_t magY = (int16_t)(magXYZ.y); int16_t magZ = (int16_t)(magXYZ.z); // print EKF2 data packet fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), accWeight, acc1, acc2, windN, windE, magN, magE, magD, magX, magY, magZ); // define messages for EKF3 data packet int16_t innovVN = (int16_t)(100*velInnov.x); int16_t innovVE = (int16_t)(100*velInnov.y); int16_t innovVD = (int16_t)(100*velInnov.z); int16_t innovPN = (int16_t)(100*posInnov.x); int16_t innovPE = (int16_t)(100*posInnov.y); int16_t innovPD = (int16_t)(100*posInnov.z); int16_t innovMX = (int16_t)(magInnov.x); int16_t innovMY = (int16_t)(magInnov.y); int16_t innovMZ = (int16_t)(magInnov.z); int16_t innovVT = (int16_t)(100*tasInnov); // print EKF3 data packet fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), innovVN, innovVE, innovVD, innovPN, innovPE, innovPD, innovMX, innovMY, innovMZ, innovVT); // define messages for EKF4 data packet int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX)); int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX)); int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX)); int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX)); int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX)); int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX)); // print EKF4 data packet fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, (unsigned)AP_HAL::millis(), (int)sqrtvarV, (int)sqrtvarP, (int)sqrtvarH, (int)sqrtvarMX, (int)sqrtvarMY, (int)sqrtvarMZ, (int)sqrtvarVT, (int)offsetNorth, (int)offsetEast, (int)faultStatus); } } flush_dataflash(); if (check_solution) { report_checks(); } exit(0); }
/* send a report to the vehicle control code over MAVLink */ void ADSB::send_report(void) { if (AP_HAL::millis() < 10000) { // simulated aircraft don't appear until 10s after startup. This avoids a windows // threading issue with non-blocking sockets and the initial wait on uartA return; } if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { ::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port); mavlink.connected = true; } if (!mavlink.connected) { return; } // check for incoming MAVLink messages uint8_t buf[100]; ssize_t ret; while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) { for (uint8_t i=0; i<ret; i++) { mavlink_message_t msg; mavlink_status_t status; if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, buf[i], &msg, &status) == MAVLINK_FRAMING_OK) { switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { if (!seen_heartbeat) { seen_heartbeat = true; vehicle_component_id = msg.compid; vehicle_system_id = msg.sysid; ::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id); } break; } } } } } if (!seen_heartbeat) { return; } uint32_t now = AP_HAL::millis(); mavlink_message_t msg; uint16_t len; if (now - last_heartbeat_ms >= 1000) { mavlink_heartbeat_t heartbeat; heartbeat.type = MAV_TYPE_ADSB; heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; heartbeat.base_mode = 0; heartbeat.system_status = 0; heartbeat.mavlink_version = 0; heartbeat.custom_mode = 0; /* save and restore sequence number for chan0, as it is used by generated encode functions */ mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_heartbeat_encode(vehicle_system_id, vehicle_component_id, &msg, &heartbeat); chan0_status->current_tx_seq = saved_seq; mav_socket.send(&msg.magic, len); last_heartbeat_ms = now; } /* send a ADSB_VEHICLE messages */ uint32_t now_us = AP_HAL::micros(); if (now_us - last_report_us >= reporting_period_ms*1000UL) { for (uint8_t i=0; i<num_vehicles; i++) { ADSB_Vehicle &vehicle = vehicles[i]; Location loc = home; location_offset(loc, vehicle.position.x, vehicle.position.y); // re-init when exceeding radius range if (get_distance(home, loc) > _sitl->adsb_radius_m) { vehicle.initialised = false; } mavlink_adsb_vehicle_t adsb_vehicle {}; last_report_us = now_us; adsb_vehicle.ICAO_address = vehicle.ICAO_address; adsb_vehicle.lat = loc.lat; adsb_vehicle.lon = loc.lng; adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; adsb_vehicle.altitude = -vehicle.position.z * 1000; adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x))); adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100; adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100; memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign)); adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE; adsb_vehicle.tslc = 1; adsb_vehicle.flags = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_VELOCITY | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED; adsb_vehicle.squawk = 0; // NOTE: ADSB_FLAGS_VALID_SQUAWK bit is not set mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &adsb_vehicle); chan0_status->current_tx_seq = saved_seq; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); } } } // ADSB_transceiever is enabled, send the status report. if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) { last_tx_report_ms = now; mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; uint8_t saved_flags = chan0_status->flags; chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; chan0_status->current_tx_seq = mavlink.seq; const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK}; len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &health_report); chan0_status->current_tx_seq = saved_seq; chan0_status->flags = saved_flags; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); ::printf("ADSBsim send tx health packet\n"); } } }