void Accelerometer::take_reading_angle(float* const readings) const{
// Take a reading
take_reading(readings);
// Convert the readings to G
for(int i = 0; i < 3; ++i){
readings[i] = ((readings[i] * VOLTAGE / ADC_STEPS) - (VOLTAGE / 2)) / 0.33;
}
// Calculate the angles in degrees
float x_angle = atan(readings[0] / sqrt((readings[1] * readings[1])
+ (readings[2] * readings[2]))) * 180 / PI;
float y_angle = atan(readings[1] / sqrt((readings[0] * readings[0])
+ (readings[2] * readings[2]))) * 180 / PI;
float z_angle = atan(sqrt((readings[0] * readings[0])
+ (readings[1] * readings[1])) / readings[2]) * 180 / PI;
// Store the readings
readings[0] = x_angle;
readings[1] = y_angle;
readings[2] = z_angle;
}
// RUN ... take data and store it.
void Scanner::run(){
static unsigned long command_time; //time servo ordered to move
static unsigned long ready_time; //time servo will be ready
static int target_heading = 90;
//if servo is ready then order next scan
if(servo_state & 0x01 == 0x01) { //B0001 tests servo_ready bit
target_heading = scan.heading_by_index( scan_order.current() );
#if DEBUG_SER == 1
Serial.print("target_heading is: ");
Serial.println(target_heading);
#endif
command_time=millis(); //record time move was ordered
ready_time = command_time + find_delay(target_heading);
servo.write(target_heading); //order servo to move
servo_state = 0x02; //set state to B0000 0010->move ordered
#if DEBUG_SER == 1
Serial.print("\tcommand time is: ");
Serial.println(command_time);
Serial.print("\tready time is: ");
Serial.println(ready_time);
Serial.print("\tservo state is: ");
Serial.println(servo_state, HEX);
#endif
}
//if servo move ordered and time has elapsed then move is complete
if((servo_state == 0x02) && ( millis()>=ready_time) ) { //B0000 0010->move_ordered
#if DEBUG_SER == 1
Serial.print("\tmove ordered and time expired. servo_state is: ");
Serial.println(servo_state, HEX);
#endif
servo_state = 0x04; //B0000 0100->move_complete
}
//if servo move complete, then pulse and update measurement
if(servo_state == 0x04) { //B0000 0100->move_complete
take_reading(target_heading);
servo_state = 0x01; //B0000 0001->ready
++scan_order; //advance current to the the next scan point
#if DEBUG_SER == 1
Serial.print("\tmove complete and servo_state is is: ");
Serial.println(servo_state, HEX);
#endif
}
}
// read - return last value measured by sensor
int16_t AP_RangeFinder_PulsedLightLRF::read()
{
uint8_t buff[2];
int16_t ret_value = 0;
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// exit immediately if we can't take the semaphore
if (i2c_sem == NULL || !i2c_sem->take(5)) {
healthy = false;
return healthy;
}
// assume the worst
healthy = false;
// read the high byte
if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 1, &buff[0]) == 0) {
// read the low byte
if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG, 1, &buff[1]) == 0) {
healthy = true;
// combine results into distance
ret_value = buff[0] << 8 | buff[1];
}
}
// ensure distance is within min and max
ret_value = constrain_int16(ret_value, min_distance, max_distance);
ret_value = _mode_filter->apply(ret_value);
// return semaphore
i2c_sem->give();
// kick off another reading for next time
// To-Do: replace this with continuous mode
take_reading();
// to-do: do we really want to return 0 if reading the distance fails?
return ret_value;
}
