Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: sensors.cpp correct whitespace, remove tabs

Browse Source
This commit is contained in:
Pierre Kancir 2016-12-20 14:33:45 +01:00 committed by Randy Mackay
parent 25fbfeb5cf
commit 51aaa7efa7
1 changed files with 19 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
APMrover2/sensors.cpp
View File

@ -30,7 +30,7 @@ void Rover::read_receiver_rssi(void)
receiver_rssi = rssi.read_receiver_rssi_uint8(); receiver_rssi = rssi.read_receiver_rssi_uint8();
} }
//Calibrate compass // Calibrate compass
void Rover::compass_cal_update() { void Rover::compass_cal_update() {
if (!hal.util->get_soft_armed()) { if (!hal.util->get_soft_armed()) {
compass.compass_cal_update(); compass.compass_cal_update();
@ -45,8 +45,8 @@ void Rover::accel_cal_update() {
} }
ins.acal_update(); ins.acal_update();
// check if new trim values, and set them float trim_roll, trim_pitch; // check if new trim values, and set them float trim_roll, trim_pitch;
float trim_roll,trim_pitch; float trim_roll, trim_pitch;
if(ins.get_new_trim(trim_roll, trim_pitch)) { if (ins.get_new_trim(trim_roll, trim_pitch)) {
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
} }
} }
@ -94,7 +94,7 @@ void Rover::read_sonars(void)
obstacle.sonar1_distance_cm = sonar.distance_cm(0); obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = 0; obstacle.sonar2_distance_cm = 0;
if (obstacle.sonar1_distance_cm < (uint16_t)g.sonar_trigger_cm) { if (obstacle.sonar1_distance_cm < (uint16_t)g.sonar_trigger_cm) {
// obstacle detected in front // obstacle detected in front
if (obstacle.detected_count < 127) { if (obstacle.detected_count < 127) {
obstacle.detected_count++; obstacle.detected_count++;
} }
@ -111,7 +111,7 @@ void Rover::read_sonars(void)
// no object detected - reset after the turn time // no object detected - reset after the turn time
if (obstacle.detected_count >= g.sonar_debounce && if (obstacle.detected_count >= g.sonar_debounce &&
AP_HAL::millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { AP_HAL::millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Obstacle passed"); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Obstacle passed");
obstacle.detected_count = 0; obstacle.detected_count = 0;
obstacle.turn_angle = 0; obstacle.turn_angle = 0;
@ -137,19 +137,24 @@ void Rover::update_sensor_status_flags(void)
// first what sensors/controllers we have // first what sensors/controllers we have
if (g.compass_enabled) { if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
} }
if (gps.status() > AP_GPS::NO_GPS) { if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
} }
if (rover.DataFlash.logging_present()) { // primary logging only (usually File) if (rover.DataFlash.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING; 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 // 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); 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);
switch (control_mode) { switch (control_mode) {
case MANUAL: case MANUAL:
@ -158,17 +163,17 @@ void Rover::update_sensor_status_flags(void)
case LEARNING: case LEARNING:
case STEERING: case STEERING:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control 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_ATTITUDE_STABILIZATION; // attitude stabilisation
break; break;
case AUTO: case AUTO:
case RTL: case RTL:
case GUIDED: case GUIDED:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control 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_ATTITUDE_STABILIZATION; // attitude stabilisation
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position 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 control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
break; break;
case INITIALISING: case INITIALISING:
@ -223,7 +228,6 @@ void Rover::update_sensor_status_flags(void)
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL); 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); control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
} }
#if FRSKY_TELEM_ENABLED == ENABLED #if FRSKY_TELEM_ENABLED == ENABLED
// give mask of error flags to Frsky_Telemetry // give mask of error flags to Frsky_Telemetry
frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present); frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);