/* * a delay() callback that processes MAVLink packets. We set this as the * callback in long running library initialisation routines to allow * MAVLink to process packets while waiting for the initialisation to * complete */ void Rover::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; if (!gcs().chan(0).initialised) { return; } // don't allow potentially expensive logging calls: DataFlash.EnableWrites(false); const uint32_t tnow = millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs().send_message(MSG_HEARTBEAT); gcs().send_message(MSG_EXTENDED_STATUS1); } if (tnow - last_50hz > 20) { last_50hz = tnow; gcs_update(); gcs_data_stream_send(); notify.update(); } if (tnow - last_5s > 5000) { last_5s = tnow; gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM"); } DataFlash.EnableWrites(true); }
/* * a delay() callback that processes MAVLink packets. We set this as the * callback in long running library initialisation routines to allow * MAVLink to process packets while waiting for the initialisation to * complete */ void Tracker::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; if (!gcs[0].initialised) return; in_mavlink_delay = true; uint32_t tnow = hal.scheduler->millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs_send_message(MSG_HEARTBEAT); gcs_send_message(MSG_EXTENDED_STATUS1); } if (tnow - last_50hz > 20) { last_50hz = tnow; gcs_update(); gcs_data_stream_send(); notify.update(); } if (tnow - last_5s > 5000) { last_5s = tnow; gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM...")); } in_mavlink_delay = false; }
// update AHRS system void Rover::ahrs_update() { hal.util->set_soft_armed(arming.is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); #if HIL_MODE != HIL_MODE_DISABLED // update hil before AHRS update gcs_update(); #endif // when in reverse we need to tell AHRS not to assume we are a // 'fly forward' vehicle, otherwise it will see a large // discrepancy between the mag and the GPS heading and will try to // correct for it, leading to a large yaw error ahrs.set_fly_forward(!in_reverse); ahrs.update(); // if using the EKF get a speed update now (from accelerometers) Vector3f velocity; if (ahrs.get_velocity_NED(velocity)) { ground_speed = pythagorous2(velocity.x, velocity.y); } if (should_log(MASK_LOG_ATTITUDE_FAST)) Log_Write_Attitude(); if (should_log(MASK_LOG_IMU)) { DataFlash.Log_Write_IMU(ins); DataFlash.Log_Write_IMUDT(ins); } }
/* * a delay() callback that processes MAVLink packets. We set this as the * callback in long running library initialisation routines to allow * MAVLink to process packets while waiting for the initialisation to * complete */ void Tracker::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; if (!gcs().chan(0).initialised) { return; } DataFlash.EnableWrites(false); uint32_t tnow = AP_HAL::millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs().send_message(MSG_HEARTBEAT); gcs().send_message(MSG_EXTENDED_STATUS1); } if (tnow - last_50hz > 20) { last_50hz = tnow; gcs_update(); gcs_data_stream_send(); notify.update(); } if (tnow - last_5s > 5000) { last_5s = tnow; gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM"); } DataFlash.EnableWrites(true); }
/* * a delay() callback that processes MAVLink packets. We set this as the * callback in long running library initialisation routines to allow * MAVLink to process packets while waiting for the initialisation to * complete */ void Rover::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; if (!gcs[0].initialised || in_mavlink_delay) return; in_mavlink_delay = true; uint32_t tnow = millis(); if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs_send_message(MSG_HEARTBEAT); gcs_send_message(MSG_EXTENDED_STATUS1); } if (tnow - last_50hz > 20) { last_50hz = tnow; gcs_update(); gcs_data_stream_send(); notify.update(); } if (tnow - last_5s > 5000) { last_5s = tnow; gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM"); } check_usb_mux(); in_mavlink_delay = false; }
// update AHRS system void Plane::ahrs_update() { hal.util->set_soft_armed(arming.is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); #if HIL_SUPPORT if (g.hil_mode == 1) { // update hil before AHRS update gcs_update(); } #endif ahrs.update(); if (should_log(MASK_LOG_IMU)) { Log_Write_IMU(); } // calculate a scaled roll limit based on current pitch roll_limit_cd = aparm.roll_limit_cd * cosf(ahrs.pitch); pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll)); // updated the summed gyro used for ground steering and // auto-takeoff. Dot product of DCM.c with gyro vector gives earth // frame yaw rate steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt; steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err); // update inertial_nav for quadplane quadplane.inertial_nav.update(G_Dt); }
// update AHRS system void Plane::ahrs_update() { update_soft_armed(); #if HIL_SUPPORT if (g.hil_mode == 1) { // update hil before AHRS update gcs_update(); } #endif ahrs.update(); if (should_log(MASK_LOG_IMU)) { DataFlash.Log_Write_IMU(); } // calculate a scaled roll limit based on current pitch roll_limit_cd = aparm.roll_limit_cd; pitch_limit_min_cd = aparm.pitch_limit_min_cd; if (!quadplane.tailsitter_active()) { roll_limit_cd *= ahrs.cos_pitch(); pitch_limit_min_cd *= fabsf(ahrs.cos_roll()); } // updated the summed gyro used for ground steering and // auto-takeoff. Dot product of DCM.c with gyro vector gives earth // frame yaw rate steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt; steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err); // update inertial_nav for quadplane quadplane.inertial_nav.update(G_Dt); }
// update AHRS system void Rover::ahrs_update() { update_soft_armed(); #if HIL_MODE != HIL_MODE_DISABLED // update hil before AHRS update gcs_update(); #endif // AHRS may use movement to calculate heading update_ahrs_flyforward(); ahrs.update(); // update position have_position = ahrs.get_position(current_loc); // update home from EKF if necessary update_home_from_EKF(); // if using the EKF get a speed update now (from accelerometers) Vector3f velocity; if (ahrs.get_velocity_NED(velocity)) { ground_speed = norm(velocity.x, velocity.y); } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { ground_speed = ahrs.groundspeed(); } if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); } if (should_log(MASK_LOG_IMU)) { DataFlash.Log_Write_IMU(); } }