mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-07 08:28:30 -04:00
73be185414
moved ground start to first arming added ground start flag moved throttle_integrator to 50hz loop CAMERA_STABILIZER deprecated - now always on renamed current logging bit mask to match APM added MA filter to PID - D term Adjusted PIDs based on continued testing and new PID filter added MASK_LOG_SET_DEFAULTS to match APM moved some stuff out of ground start into system start where it belonged Added slower Yaw gains for DCM when the copter is in the air changed camera output to be none scaled PWM fixed bug where ground_temperature was unfiltered shortened Baro startup time fixed issue with Nav_WP integrator not being reset RTL no longer yaws towards home Circle mode for flying a 10m circle around the point where it was engaged. - Not tested at all! Consider Circle mode as alpha. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2966 f9c3cf11-9bcb-44bc-f272-b75c42450872
118 lines
3.0 KiB
Plaintext
118 lines
3.0 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
// Sensors are not available in HIL_MODE_ATTITUDE
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
|
|
static void ReadSCP1000(void) {}
|
|
|
|
static void init_barometer(void)
|
|
{
|
|
#if HIL_MODE == HIL_MODE_SENSORS
|
|
hil.update(); // look for inbound hil packets for initialization
|
|
#endif
|
|
|
|
ground_temperature = barometer.Temp;
|
|
|
|
// We take some readings...
|
|
for(int i = 0; i < 20; i++){
|
|
delay(20);
|
|
|
|
#if HIL_MODE == HIL_MODE_SENSORS
|
|
hil.update(); // look for inbound hil packets
|
|
#endif
|
|
|
|
// Get initial data from absolute pressure sensor
|
|
ground_pressure = read_baro_filtered();
|
|
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
|
|
}
|
|
|
|
abs_pressure = ground_pressure;
|
|
|
|
//Serial.printf("init %ld\n", abs_pressure);
|
|
//SendDebugln("barometer calibration complete.");
|
|
}
|
|
|
|
static long read_baro_filtered(void)
|
|
{
|
|
long pressure = 0;
|
|
|
|
// get new data from absolute pressure sensor
|
|
barometer.Read();
|
|
|
|
|
|
// add new data into our filter
|
|
baro_filter[baro_filter_index] = barometer.Press;
|
|
baro_filter_index++;
|
|
|
|
// loop our filter
|
|
if(baro_filter_index >= BARO_FILTER_SIZE)
|
|
baro_filter_index = 0;
|
|
|
|
// zero out our accumulator
|
|
|
|
// sum our filter
|
|
for(byte i = 0; i < BARO_FILTER_SIZE; i++){
|
|
pressure += baro_filter[i];
|
|
}
|
|
|
|
|
|
// average our sampels
|
|
return pressure /= BARO_FILTER_SIZE;
|
|
}
|
|
|
|
static long read_barometer(void)
|
|
{
|
|
float x, scaling, temp;
|
|
|
|
abs_pressure = read_baro_filtered();
|
|
|
|
//Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure);
|
|
|
|
scaling = (float)ground_pressure / (float)abs_pressure;
|
|
temp = ((float)ground_temperature / 10.0f) + 273.15f;
|
|
x = log(scaling) * temp * 29271.267f;
|
|
return (x / 10);
|
|
}
|
|
|
|
// in M/S * 100
|
|
static void read_airspeed(void)
|
|
{
|
|
|
|
}
|
|
|
|
static void zero_airspeed(void)
|
|
{
|
|
|
|
}
|
|
|
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
|
|
|
static void read_battery(void)
|
|
{
|
|
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
|
|
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
|
|
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
|
|
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
|
|
|
|
if(g.battery_monitoring == 1)
|
|
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
|
|
if(g.battery_monitoring == 2)
|
|
battery_voltage = battery_voltage4;
|
|
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
|
|
battery_voltage = battery_voltage1;
|
|
if(g.battery_monitoring == 4) {
|
|
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
|
|
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
|
|
}
|
|
|
|
#if BATTERY_EVENT == 1
|
|
if(battery_voltage < LOW_VOLTAGE)
|
|
low_battery_event();
|
|
|
|
if(g.battery_monitoring == 4 && current_total > g.pack_capacity)
|
|
low_battery_event();
|
|
#endif
|
|
}
|
|
|
|
//v: 10.9453, a: 17.4023, mah: 8.2
|