2015-05-13 00:16:45 -03:00
# include "Rover.h"
2017-08-08 02:58:52 -03:00
# include <AP_RangeFinder/RangeFinder_Backend.h>
2017-06-06 22:28:34 -03:00
// initialise compass
void Rover : : init_compass ( )
{
if ( ! g . compass_enabled ) {
return ;
}
if ( ! compass . init ( ) | | ! compass . read ( ) ) {
2017-08-11 00:49:03 -03:00
hal . console - > printf ( " Compass initialisation failed! \n " ) ;
2017-06-06 22:28:34 -03:00
g . compass_enabled = false ;
} else {
ahrs . set_compass ( & compass ) ;
}
}
2017-06-06 22:26:51 -03:00
/*
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 ;
}
}
}
2016-05-23 22:31:27 -03:00
void Rover : : init_barometer ( bool full_calibration )
2014-02-23 18:25:50 -04:00
{
2017-07-08 22:40:59 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Calibrating barometer " ) ;
2016-05-23 22:31:27 -03:00
if ( full_calibration ) {
barometer . calibrate ( ) ;
} else {
barometer . update_calibration ( ) ;
}
2017-07-08 22:40:59 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Barometer calibration complete " ) ;
2014-02-23 18:25:50 -04:00
}
2017-07-13 08:36:44 -03:00
void Rover : : init_rangefinder ( void )
2012-05-14 12:47:08 -03:00
{
2017-07-13 08:36:44 -03:00
rangefinder . init ( ) ;
2012-05-14 12:47:08 -03:00
}
2017-04-03 17:46:12 -03:00
// init beacons used for non-gps position estimates
void Rover : : init_beacon ( )
{
g2 . beacon . init ( ) ;
}
// update beacons
void Rover : : update_beacon ( )
{
g2 . beacon . update ( ) ;
}
2017-06-01 04:39:49 -03:00
// 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 ( ) ) ;
}
}
2017-07-11 23:02:51 -03:00
// update wheel encoders
void Rover : : update_wheel_encoder ( )
{
2017-07-20 03:12:09 -03:00
// exit immediately if not enabled
if ( g2 . wheel_encoder . num_sensors ( ) = = 0 ) {
return ;
}
// update encoders
2017-07-11 23:02:51 -03:00
g2 . wheel_encoder . update ( ) ;
2017-07-20 03:12:09 -03:00
// initialise on first iteration
2017-08-03 06:53:22 -03:00
const uint32_t now = AP_HAL : : millis ( ) ;
2017-07-20 03:12:09 -03:00
if ( wheel_encoder_last_ekf_update_ms = = 0 ) {
wheel_encoder_last_ekf_update_ms = now ;
2017-08-03 06:53:22 -03:00
for ( uint8_t i = 0 ; i < g2 . wheel_encoder . num_sensors ( ) ; i + + ) {
2017-07-20 03:12:09 -03:00
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
2017-08-03 06:53:22 -03:00
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 + + ) {
2017-07-20 03:12:09 -03:00
// calculate angular change (in radians)
2017-08-03 06:53:22 -03:00
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 ] ;
2017-07-20 03:12:09 -03:00
wheel_encoder_last_angle_rad [ i ] = curr_angle_rad ;
// calculate delta time
float delta_time ;
2017-08-03 06:53:22 -03:00
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 ] ;
2017-07-20 03:12:09 -03:00
// 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 ) ) ;
2017-07-20 23:56:39 -03:00
// 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 ;
}
2017-07-20 03:12:09 -03:00
}
// record system time update for next iteration
wheel_encoder_last_ekf_update_ms = now ;
2017-07-11 23:02:51 -03:00
}
2013-10-02 03:07:28 -03:00
// read_battery - reads battery voltage and current and invokes failsafe
// should be called at 10hz
2015-05-12 02:03:23 -03:00
void Rover : : read_battery ( void )
2012-04-30 04:17:14 -03:00
{
2013-10-02 03:07:28 -03:00
battery . read ( ) ;
2012-04-30 04:17:14 -03:00
}
2013-03-14 18:08:35 -03:00
// read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
2015-05-12 04:00:25 -03:00
void Rover : : read_receiver_rssi ( void )
2013-03-14 18:08:35 -03:00
{
2015-08-28 03:00:12 -03:00
receiver_rssi = rssi . read_receiver_rssi_uint8 ( ) ;
2013-03-14 18:08:35 -03:00
}
2013-03-21 18:49:51 -03:00
2016-12-20 09:33:45 -04:00
// Calibrate compass
2015-07-30 22:24:32 -03:00
void Rover : : compass_cal_update ( ) {
if ( ! hal . util - > get_soft_armed ( ) ) {
compass . compass_cal_update ( ) ;
}
}
2015-10-21 18:22:45 -03:00
// 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;
2016-12-20 09:33:45 -04:00
float trim_roll , trim_pitch ;
if ( ins . get_new_trim ( trim_roll , trim_pitch ) ) {
2015-10-21 18:22:45 -03:00
ahrs . set_trim ( Vector3f ( trim_roll , trim_pitch , 0 ) ) ;
}
}
2017-07-13 08:36:44 -03:00
// read the rangefinders
void Rover : : read_rangefinders ( void )
2013-03-21 18:49:51 -03:00
{
2017-07-13 08:36:44 -03:00
rangefinder . update ( ) ;
2014-06-27 00:18:20 -03:00
2017-08-08 02:58:52 -03:00
AP_RangeFinder_Backend * s0 = rangefinder . get_backend ( 0 ) ;
AP_RangeFinder_Backend * s1 = rangefinder . get_backend ( 1 ) ;
if ( s0 = = nullptr | | s0 - > status ( ) = = RangeFinder : : RangeFinder_NotConnected ) {
2017-07-13 08:36:44 -03:00
// this makes it possible to disable rangefinder at runtime
2013-03-21 18:49:51 -03:00
return ;
}
2017-08-08 02:58:52 -03:00
if ( s1 ! = nullptr & & s1 - > has_data ( ) ) {
2017-07-13 08:36:44 -03:00
// we have two rangefinders
2017-08-08 02:58:52 -03:00
obstacle . rangefinder1_distance_cm = s0 - > distance_cm ( ) ;
obstacle . rangefinder2_distance_cm = s1 - > distance_cm ( ) ;
2017-07-13 08:36:44 -03:00
if ( obstacle . rangefinder1_distance_cm < static_cast < uint16_t > ( g . rangefinder_trigger_cm ) & &
obstacle . rangefinder1_distance_cm < static_cast < uint16_t > ( obstacle . rangefinder2_distance_cm ) ) {
2013-03-21 18:49:51 -03:00
// we have an object on the left
2013-03-28 20:49:08 -03:00
if ( obstacle . detected_count < 127 ) {
obstacle . detected_count + + ;
}
2017-07-13 08:36:44 -03:00
if ( obstacle . detected_count = = g . rangefinder_debounce ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Rangefinder1 obstacle %u cm " ,
static_cast < uint32_t > ( obstacle . rangefinder1_distance_cm ) ) ;
2013-03-28 18:53:02 -03:00
}
2015-11-19 23:04:16 -04:00
obstacle . detected_time_ms = AP_HAL : : millis ( ) ;
2017-07-13 08:36:44 -03:00
obstacle . turn_angle = g . rangefinder_turn_angle ;
} else if ( obstacle . rangefinder2_distance_cm < static_cast < uint16_t > ( g . rangefinder_trigger_cm ) ) {
2013-03-21 18:49:51 -03:00
// we have an object on the right
2013-03-28 20:49:08 -03:00
if ( obstacle . detected_count < 127 ) {
obstacle . detected_count + + ;
}
2017-07-13 08:36:44 -03:00
if ( obstacle . detected_count = = g . rangefinder_debounce ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Rangefinder2 obstacle %u cm " ,
static_cast < uint32_t > ( obstacle . rangefinder2_distance_cm ) ) ;
2013-03-28 18:53:02 -03:00
}
2015-11-19 23:04:16 -04:00
obstacle . detected_time_ms = AP_HAL : : millis ( ) ;
2017-07-13 08:36:44 -03:00
obstacle . turn_angle = - g . rangefinder_turn_angle ;
2013-03-21 18:49:51 -03:00
}
2013-03-28 18:08:14 -03:00
} else {
2017-07-13 08:36:44 -03:00
// we have a single rangefinder
2017-08-08 02:58:52 -03:00
obstacle . rangefinder1_distance_cm = s0 - > distance_cm ( ) ;
2017-07-13 08:36:44 -03:00
obstacle . rangefinder2_distance_cm = 0 ;
if ( obstacle . rangefinder1_distance_cm < static_cast < uint16_t > ( g . rangefinder_trigger_cm ) ) {
2016-12-20 09:33:45 -04:00
// obstacle detected in front
2013-03-28 20:49:08 -03:00
if ( obstacle . detected_count < 127 ) {
obstacle . detected_count + + ;
}
2017-07-13 08:36:44 -03:00
if ( obstacle . detected_count = = g . rangefinder_debounce ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Rangefinder obstacle %u cm " ,
static_cast < uint32_t > ( obstacle . rangefinder1_distance_cm ) ) ;
2013-03-28 18:53:02 -03:00
}
2015-11-19 23:04:16 -04:00
obstacle . detected_time_ms = AP_HAL : : millis ( ) ;
2017-07-13 08:36:44 -03:00
obstacle . turn_angle = g . rangefinder_turn_angle ;
2013-03-28 18:08:14 -03:00
}
2013-03-21 18:49:51 -03:00
}
2017-07-13 08:36:44 -03:00
Log_Write_Rangefinder ( ) ;
2013-04-18 21:23:57 -03:00
2013-03-21 18:49:51 -03:00
// no object detected - reset after the turn time
2017-07-13 08:36:44 -03:00
if ( obstacle . detected_count > = g . rangefinder_debounce & &
AP_HAL : : millis ( ) > obstacle . detected_time_ms + g . rangefinder_turn_time * 1000 ) {
2017-07-08 22:40:59 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Obstacle passed " ) ;
2013-03-28 20:49:08 -03:00
obstacle . detected_count = 0 ;
obstacle . turn_angle = 0 ;
2013-03-21 18:49:51 -03:00
}
}
2016-07-21 22:24:35 -03:00
/*
update AP_Button
*/
void Rover : : button_update ( void )
{
button . update ( ) ;
}
2016-10-27 23:20:56 -03:00
// 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 ) {
2016-12-20 09:33:45 -04:00
control_sensors_present | = MAV_SYS_STATUS_SENSOR_3D_MAG ; // compass present
2016-10-27 23:20:56 -03:00
}
if ( gps . status ( ) > AP_GPS : : NO_GPS ) {
control_sensors_present | = MAV_SYS_STATUS_SENSOR_GPS ;
}
2017-06-01 04:39:49 -03:00
if ( g2 . visual_odom . enabled ( ) ) {
control_sensors_present | = MAV_SYS_STATUS_SENSOR_VISION_POSITION ;
}
2016-12-20 09:33:45 -04:00
if ( rover . DataFlash . logging_present ( ) ) { // primary logging only (usually File)
2016-10-27 23:20:56 -03:00
control_sensors_present | = MAV_SYS_STATUS_LOGGING ;
}
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually
2016-12-20 09:33:45 -04:00
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 ) ;
2017-07-18 23:19:08 -03:00
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
2016-10-27 23:20:56 -03:00
}
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 ;
}
// 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 . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
control_sensors_health | = MAV_SYS_STATUS_SENSOR_GPS ;
}
2017-06-01 04:39:49 -03:00
if ( g2 . visual_odom . enabled ( ) & & ! g2 . visual_odom . healthy ( ) ) {
control_sensors_health & = ~ MAV_SYS_STATUS_SENSOR_VISION_POSITION ;
}
2016-10-27 23:20:56 -03:00
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 ;
}
2017-07-13 08:36:44 -03:00
if ( rangefinder . num_sensors ( ) > 0 ) {
2016-10-27 23:20:56 -03:00
control_sensors_present | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
2017-07-13 08:36:44 -03:00
if ( g . rangefinder_trigger_cm > 0 ) {
2016-10-27 23:20:56 -03:00
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
}
2017-08-08 02:58:52 -03:00
AP_RangeFinder_Backend * s = rangefinder . get_backend ( 0 ) ;
if ( s ! = nullptr & & s - > has_data ( ) ) {
2016-10-27 23:20:56 -03:00
control_sensors_health | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
}
}
if ( rover . DataFlash . logging_failed ( ) ) {
control_sensors_health & = ~ MAV_SYS_STATUS_LOGGING ;
}
if ( AP_Notify : : flags . initialising ) {
// 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
2016-10-29 16:46:36 -03:00
frsky_telemetry . update_sensor_status_flags ( ~ control_sensors_health & control_sensors_enabled & control_sensors_present ) ;
2016-10-27 23:20:56 -03:00
# endif
}