#include "Rover.h" #include // initialise compass void Rover::init_compass() { if (!g.compass_enabled) { return; } if (!compass.init()|| !compass.read()) { hal.console->printf("Compass initialisation failed!\n"); g.compass_enabled = false; } else { ahrs.set_compass(&compass); } } /* if the compass is enabled then try to accumulate a reading also update initial location used for declination */ void Rover::compass_accumulate(void) { if (!g.compass_enabled) { return; } compass.accumulate(); // update initial location used for declination if (!compass_init_location) { Location loc; if (ahrs.get_position(loc)) { compass.set_initial_location(loc.lat, loc.lng); compass_init_location = true; } } } void Rover::init_rangefinder(void) { rangefinder.init(); } // init beacons used for non-gps position estimates void Rover::init_beacon() { g2.beacon.init(); } // init visual odometry sensor void Rover::init_visual_odom() { g2.visual_odom.init(); } // update visual odometry sensor void Rover::update_visual_odom() { // check for updates if (g2.visual_odom.enabled() && (g2.visual_odom.get_last_update_ms() != visual_odom_last_update_ms)) { visual_odom_last_update_ms = g2.visual_odom.get_last_update_ms(); const float time_delta_sec = g2.visual_odom.get_time_delta_usec() / 1000000.0f; ahrs.writeBodyFrameOdom(g2.visual_odom.get_confidence(), g2.visual_odom.get_position_delta(), g2.visual_odom.get_angle_delta(), time_delta_sec, visual_odom_last_update_ms, g2.visual_odom.get_pos_offset()); // log sensor data DataFlash.Log_Write_VisualOdom(time_delta_sec, g2.visual_odom.get_angle_delta(), g2.visual_odom.get_position_delta(), g2.visual_odom.get_confidence()); } } // update wheel encoders void Rover::update_wheel_encoder() { // exit immediately if not enabled if (g2.wheel_encoder.num_sensors() == 0) { return; } // update encoders g2.wheel_encoder.update(); // initialise on first iteration const uint32_t now = AP_HAL::millis(); if (wheel_encoder_last_ekf_update_ms == 0) { wheel_encoder_last_ekf_update_ms = now; for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) { wheel_encoder_last_angle_rad[i] = g2.wheel_encoder.get_delta_angle(i); wheel_encoder_last_update_ms[i] = g2.wheel_encoder.get_last_reading_ms(i); } return; } // calculate delta angle and delta time and send to EKF // use time of last ping from wheel encoder // send delta time (time between this wheel encoder time and previous wheel encoder time) // in case where wheel hasn't moved, count = 0 (cap the delta time at 50ms - or system time) // use system clock of last update instead of time of last ping const float system_dt = (now - wheel_encoder_last_ekf_update_ms) / 1000.0f; for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) { // calculate angular change (in radians) const float curr_angle_rad = g2.wheel_encoder.get_delta_angle(i); const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i]; wheel_encoder_last_angle_rad[i] = curr_angle_rad; // calculate delta time float delta_time; const uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i); const uint32_t sensor_diff_ms = latest_sensor_update_ms - wheel_encoder_last_update_ms[i]; // if we have not received any sensor updates, or time difference is too high then use time since last update to the ekf // check for old or insane sensor update times if (sensor_diff_ms == 0 || sensor_diff_ms > 100) { delta_time = system_dt; wheel_encoder_last_update_ms[i] = wheel_encoder_last_ekf_update_ms; } else { delta_time = sensor_diff_ms / 1000.0f; wheel_encoder_last_update_ms[i] = latest_sensor_update_ms; } /* delAng is the measured change in angular position from the previous measurement where a positive rotation is produced by forward motion of the vehicle (rad) * delTime is the time interval for the measurement of delAng (sec) * timeStamp_ms is the time when the rotation was last measured (msec) * posOffset is the XYZ body frame position of the wheel hub (m) */ EKF3.writeWheelOdom(delta_angle, delta_time, wheel_encoder_last_update_ms[i], g2.wheel_encoder.get_position(i), g2.wheel_encoder.get_wheel_radius(i)); // calculate rpm for reporting to GCS if (is_positive(delta_time)) { wheel_encoder_rpm[i] = (delta_angle / M_2PI) / (delta_time / 60.0f); } else { wheel_encoder_rpm[i] = 0.0f; } } // record system time update for next iteration wheel_encoder_last_ekf_update_ms = now; } // Calibrate compass void Rover::compass_cal_update() { if (!hal.util->get_soft_armed()) { compass.compass_cal_update(); } } // Accel calibration void Rover::accel_cal_update() { if (hal.util->get_soft_armed()) { return; } ins.acal_update(); // check if new trim values, and set them float trim_roll, trim_pitch; float trim_roll, trim_pitch; if (ins.get_new_trim(trim_roll, trim_pitch)) { ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } // read the rangefinders void Rover::read_rangefinders(void) { rangefinder.update(); AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0); AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1); if (s0 == nullptr || s0->status() == RangeFinder::RangeFinder_NotConnected) { // this makes it possible to disable rangefinder at runtime return; } if (s1 != nullptr && s1->has_data()) { // we have two rangefinders obstacle.rangefinder1_distance_cm = s0->distance_cm(); obstacle.rangefinder2_distance_cm = s1->distance_cm(); if (obstacle.rangefinder1_distance_cm < static_cast(g.rangefinder_trigger_cm) && obstacle.rangefinder1_distance_cm < static_cast(obstacle.rangefinder2_distance_cm)) { // we have an object on the left if (obstacle.detected_count < 127) { obstacle.detected_count++; } if (obstacle.detected_count == g.rangefinder_debounce) { gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder1 obstacle %u cm", static_cast(obstacle.rangefinder1_distance_cm)); } obstacle.detected_time_ms = AP_HAL::millis(); obstacle.turn_angle = g.rangefinder_turn_angle; } else if (obstacle.rangefinder2_distance_cm < static_cast(g.rangefinder_trigger_cm)) { // we have an object on the right if (obstacle.detected_count < 127) { obstacle.detected_count++; } if (obstacle.detected_count == g.rangefinder_debounce) { gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder2 obstacle %u cm", static_cast(obstacle.rangefinder2_distance_cm)); } obstacle.detected_time_ms = AP_HAL::millis(); obstacle.turn_angle = -g.rangefinder_turn_angle; } } else { // we have a single rangefinder obstacle.rangefinder1_distance_cm = s0->distance_cm(); obstacle.rangefinder2_distance_cm = 0; if (obstacle.rangefinder1_distance_cm < static_cast(g.rangefinder_trigger_cm)) { // obstacle detected in front if (obstacle.detected_count < 127) { obstacle.detected_count++; } if (obstacle.detected_count == g.rangefinder_debounce) { gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder obstacle %u cm", static_cast(obstacle.rangefinder1_distance_cm)); } obstacle.detected_time_ms = AP_HAL::millis(); obstacle.turn_angle = g.rangefinder_turn_angle; } } Log_Write_Rangefinder(); Log_Write_Depth(); // no object detected - reset after the turn time if (obstacle.detected_count >= g.rangefinder_debounce && AP_HAL::millis() > obstacle.detected_time_ms + g.rangefinder_turn_time*1000) { gcs().send_text(MAV_SEVERITY_INFO, "Obstacle passed"); obstacle.detected_count = 0; obstacle.turn_angle = 0; } } // initialise proximity sensor void Rover::init_proximity(void) { g2.proximity.init(); g2.proximity.set_rangefinder(&rangefinder); } // update error mask of sensors and subsystems. The mask // uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set // then it indicates that the sensor or subsystem is present but // not functioning correctly. void Rover::update_sensor_status_flags(void) { // default sensors present control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; // first what sensors/controllers we have if (g.compass_enabled) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present } if (gps.status() > AP_GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; } if (g2.visual_odom.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } if (rover.DataFlash.logging_present()) { // primary logging only (usually File) control_sensors_present |= MAV_SYS_STATUS_LOGGING; } if (rover.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING & ~MAV_SYS_STATUS_SENSOR_BATTERY); if (control_mode->attitude_stabilized()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control } if (control_mode->is_autopilot_mode()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control } if (rover.DataFlash.logging_enabled()) { control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; } // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED) if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; } if (battery.num_instances() > 0) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY; } // default to all healthy except compass and gps which we set individually control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS); if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } if (gps.is_healthy()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; } if (g2.visual_odom.enabled() && !g2.visual_odom.healthy()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; } if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; } if (!ins.get_accel_health_all()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL; } if (ahrs.initialised() && !ahrs.healthy()) { // AHRS subsystem is unhealthy control_sensors_health &= ~MAV_SYS_STATUS_AHRS; } if (rangefinder.num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; AP_RangeFinder_Backend *s = rangefinder.get_backend(0); if (s != nullptr && s->has_data()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } if (rover.g2.proximity.get_status() == AP_Proximity::Proximity_NoData) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; } if (rover.DataFlash.logging_failed()) { control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; } if (!battery.healthy() || battery.has_failsafed()) { control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY; } if (!initialised || ins.calibrating()) { // while initialising the gyros and accels are not enabled control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL); control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL); } #if FRSKY_TELEM_ENABLED == ENABLED // give mask of error flags to Frsky_Telemetry frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present); #endif }