2015-05-13 00:16:45 -03:00
# include "Rover.h"
2016-05-23 22:31:27 -03:00
void Rover : : init_barometer ( bool full_calibration )
2014-02-23 18:25:50 -04:00
{
2015-11-03 01:19:03 -04: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 ( ) ;
}
2015-11-18 14:53:14 -04:00
gcs_send_text ( MAV_SEVERITY_INFO , " Barometer calibration complete " ) ;
2014-02-23 18:25:50 -04:00
}
2015-05-12 02:03:23 -03:00
void Rover : : init_sonar ( void )
2012-05-14 12:47:08 -03:00
{
2014-06-27 00:18:20 -03:00
sonar . init ( ) ;
2012-05-14 12:47:08 -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 ) ) ;
}
}
2013-03-21 18:49:51 -03:00
// read the sonars
2015-05-12 02:03:23 -03:00
void Rover : : read_sonars ( void )
2013-03-21 18:49:51 -03:00
{
2014-06-27 00:18:20 -03:00
sonar . update ( ) ;
2017-02-09 07:48:39 -04:00
if ( sonar . status ( 0 ) = = RangeFinder : : RangeFinder_NotConnected ) {
2013-03-21 18:49:51 -03:00
// this makes it possible to disable sonar at runtime
return ;
}
2015-04-17 03:41:00 -03:00
if ( sonar . has_data ( 1 ) ) {
2013-03-21 18:49:51 -03:00
// we have two sonars
2014-06-27 00:18:20 -03:00
obstacle . sonar1_distance_cm = sonar . distance_cm ( 0 ) ;
obstacle . sonar2_distance_cm = sonar . distance_cm ( 1 ) ;
2017-03-30 11:07:24 -03:00
if ( obstacle . sonar1_distance_cm < static_cast < uint16_t > ( g . sonar_trigger_cm ) & &
obstacle . sonar1_distance_cm < static_cast < uint16_t > ( obstacle . sonar2_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 + + ;
}
if ( obstacle . detected_count = = g . sonar_debounce ) {
2015-11-03 01:19:03 -04:00
gcs_send_text_fmt ( MAV_SEVERITY_INFO , " Sonar1 obstacle %u cm " ,
2017-03-30 11:07:24 -03:00
static_cast < uint32_t > ( obstacle . sonar1_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 ( ) ;
2013-03-21 18:49:51 -03:00
obstacle . turn_angle = g . sonar_turn_angle ;
2017-03-30 11:07:24 -03:00
} else if ( obstacle . sonar2_distance_cm < static_cast < uint16_t > ( g . sonar_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 + + ;
}
if ( obstacle . detected_count = = g . sonar_debounce ) {
2015-11-03 01:19:03 -04:00
gcs_send_text_fmt ( MAV_SEVERITY_INFO , " Sonar2 obstacle %u cm " ,
2017-03-30 11:07:24 -03:00
static_cast < uint32_t > ( obstacle . sonar2_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 ( ) ;
2013-03-21 18:49:51 -03:00
obstacle . turn_angle = - g . sonar_turn_angle ;
}
2013-03-28 18:08:14 -03:00
} else {
// we have a single sonar
2014-06-27 00:18:20 -03:00
obstacle . sonar1_distance_cm = sonar . distance_cm ( 0 ) ;
2013-04-18 21:23:57 -03:00
obstacle . sonar2_distance_cm = 0 ;
2017-03-30 11:07:24 -03:00
if ( obstacle . sonar1_distance_cm < static_cast < uint16_t > ( g . sonar_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 + + ;
}
if ( obstacle . detected_count = = g . sonar_debounce ) {
2015-11-18 14:53:14 -04:00
gcs_send_text_fmt ( MAV_SEVERITY_INFO , " Sonar obstacle %u cm " ,
2017-03-30 11:07:24 -03:00
static_cast < uint32_t > ( obstacle . sonar1_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 ( ) ;
2013-03-28 18:08:14 -03:00
obstacle . turn_angle = g . sonar_turn_angle ;
}
2013-03-21 18:49:51 -03:00
}
2013-04-18 21:23:57 -03:00
Log_Write_Sonar ( ) ;
2013-03-21 18:49:51 -03:00
// no object detected - reset after the turn time
2013-03-28 20:49:08 -03:00
if ( obstacle . detected_count > = g . sonar_debounce & &
2016-12-20 09:33:45 -04:00
AP_HAL : : millis ( ) > obstacle . detected_time_ms + g . sonar_turn_time * 1000 ) {
2015-11-03 01:19:03 -04:00
gcs_send_text_fmt ( 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 ;
}
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 ) ;
2016-10-27 23:20:56 -03:00
switch ( control_mode ) {
case MANUAL :
case HOLD :
break ;
case LEARNING :
case STEERING :
2016-12-20 09:33:45 -04:00
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL ; // 3D angular rate control
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION ; // attitude stabilisation
2016-10-27 23:20:56 -03:00
break ;
case AUTO :
case RTL :
case GUIDED :
2016-12-20 09:33:45 -04:00
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL ; // 3D angular rate control
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION ; // attitude stabilisation
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
break ;
case INITIALISING :
break ;
}
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 ;
}
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 ( sonar . num_sensors ( ) > 0 ) {
control_sensors_present | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
if ( g . sonar_trigger_cm > 0 ) {
control_sensors_enabled | = MAV_SYS_STATUS_SENSOR_LASER_POSITION ;
}
2017-02-09 07:48:39 -04:00
if ( sonar . has_data ( 0 ) ) {
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
}