mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
Renamed the loop timers to gain a new medium loop delta. Fixed an issue with the navigation timer. Updated the current sensing. Need to add a low voltage or current trigger.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1508 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
dbb40ad9a4
commit
f60602d7a9
@ -225,6 +225,7 @@ float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 +
|
||||
|
||||
float current_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
|
||||
float current_amps;
|
||||
float current_total;
|
||||
|
||||
boolean current_sensor = false;
|
||||
|
||||
@ -380,7 +381,11 @@ byte medium_count;
|
||||
byte slow_loopCounter;
|
||||
byte superslow_loopCounter;
|
||||
|
||||
unsigned long deltaMiliSeconds; // Delta Time in miliseconds
|
||||
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
|
||||
uint8_t delta_ms_medium_loop;
|
||||
uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
|
||||
|
||||
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||
unsigned long elapsedTime; // for doing custom events
|
||||
float load; // % MCU cycles used
|
||||
@ -437,10 +442,10 @@ void loop()
|
||||
// We want this to execute at 100Hz
|
||||
// --------------------------------
|
||||
if (millis() - fast_loopTimer > 9) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
fast_loopTimer = millis();
|
||||
load = float(fast_loopTimeStamp - fast_loopTimer) / deltaMiliSeconds;
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
|
||||
load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
mainLoop_count++;
|
||||
|
||||
// Execute the fast loop
|
||||
@ -450,7 +455,8 @@ void loop()
|
||||
}
|
||||
|
||||
if (millis() - medium_loopTimer > 19) {
|
||||
medium_loopTimer = millis();
|
||||
delta_ms_medium_loop = millis() - medium_loopTimer;
|
||||
medium_loopTimer = millis();
|
||||
medium_loop();
|
||||
|
||||
/* commented out temporarily
|
||||
@ -474,8 +480,8 @@ void fast_loop()
|
||||
|
||||
// This is the fast loop - we want it to execute at 200Hz if possible
|
||||
// ------------------------------------------------------------------
|
||||
if (deltaMiliSeconds > G_Dt_max)
|
||||
G_Dt_max = deltaMiliSeconds;
|
||||
if (delta_ms_fast_loop > G_Dt_max)
|
||||
G_Dt_max = delta_ms_fast_loop;
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
@ -516,8 +522,8 @@ void medium_loop()
|
||||
|
||||
if(GPS.new_data){
|
||||
GPS.new_data = false;
|
||||
dTnav = millis() - medium_loopTimer;
|
||||
medium_loopTimer = millis();
|
||||
dTnav = millis() - nav_loopTimer;
|
||||
nav_loopTimer = millis();
|
||||
|
||||
// calculate the plane's desired bearing
|
||||
// -------------------------------------
|
||||
@ -717,6 +723,8 @@ void update_GPS(void)
|
||||
if (log_bitmask & MASK_LOG_CMD)
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
|
||||
// reset our nav loop timer
|
||||
nav_loopTimer = millis();
|
||||
init_home();
|
||||
// init altitude
|
||||
current_loc.alt = GPS.altitude;
|
||||
|
||||
@ -32,8 +32,8 @@ void output_stabilize()
|
||||
pitch_error = constrain(pitch_error, -2500, 2500);
|
||||
|
||||
// write out angles back to servo out - this will be converted to PWM by RC_Channel
|
||||
rc_1.servo_out = pid_stabilize_roll.get_pid(roll_error, deltaMiliSeconds, 1.0);
|
||||
rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, deltaMiliSeconds, 1.0);
|
||||
rc_1.servo_out = pid_stabilize_roll.get_pid(roll_error, delta_ms_fast_loop, 1.0);
|
||||
rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, delta_ms_fast_loop, 1.0);
|
||||
|
||||
// We adjust the output by the rate of rotation:
|
||||
// Rate control through bias corrected gyro rates
|
||||
@ -98,7 +98,7 @@ void output_yaw_with_hold(boolean hold)
|
||||
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
|
||||
|
||||
// Apply PID and save the new angle back to RC_Channel
|
||||
rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .5 * 6000 = 3000
|
||||
rc_4.servo_out = pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000
|
||||
|
||||
// We adjust the output by the rate of rotation
|
||||
long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
|
||||
@ -114,7 +114,7 @@ void output_yaw_with_hold(boolean hold)
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
long error = ((long)rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
|
||||
// -error = CCW, +error = CW
|
||||
rc_4.servo_out = pid_acro_rate_yaw.get_pid(error, deltaMiliSeconds, 1.0); // .075 * 36000 = 2700
|
||||
rc_4.servo_out = pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
|
||||
rc_4.servo_out = constrain(rc_4.servo_out, -2400, 2400); // limit to 2400
|
||||
|
||||
}
|
||||
|
||||
@ -215,8 +215,9 @@
|
||||
#define BATTERY_PIN2 1
|
||||
#define BATTERY_PIN3 2
|
||||
#define BATTERY_PIN4 3
|
||||
#define CURRENT_PIN1 4 // These are the pins for current sensor: voltage
|
||||
#define CURRENT_PIN2 5 // and current
|
||||
|
||||
#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
|
||||
#define CURRENT_PIN_1 1 // and current
|
||||
|
||||
#define RELAY_PIN 47
|
||||
|
||||
|
||||
@ -24,13 +24,13 @@ void calc_nav_throttle()
|
||||
long t_out;
|
||||
|
||||
if(altitude_sensor == BARO) {
|
||||
t_out = pid_baro_throttle.get_pid(altitude_error, deltaMiliSeconds, 1.0);
|
||||
t_out = pid_baro_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0);
|
||||
|
||||
// limit output of throttle control
|
||||
t_out = throttle_cruise + constrain(t_out, -50, 100);
|
||||
} else {
|
||||
// SONAR
|
||||
t_out = pid_sonar_throttle.get_pid(altitude_error, deltaMiliSeconds, 1.0);
|
||||
t_out = pid_sonar_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0);
|
||||
|
||||
// limit output of throttle control
|
||||
t_out = throttle_cruise + constrain(t_out, -60, 100);
|
||||
|
||||
@ -38,7 +38,7 @@ void read_battery(void)
|
||||
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 BATTERY_TYPE == 0
|
||||
if(battery_voltage3 < LOW_VOLTAGE)
|
||||
low_battery_event();
|
||||
@ -54,9 +54,12 @@ void read_battery(void)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
void read_current(void)
|
||||
{
|
||||
current_voltage = CURRENT_VOLTAGE(analogRead(CURRENT_PIN1)) * .1 + battery_voltage1 * .9; //reads power sensor voltage pin
|
||||
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN2)) * .1 + battery_voltage2 * .9; //reads power sensor current pin
|
||||
current_voltage = CURRENT_VOLTAGE(analogRead(VOLTAGE_PIN_0)) * .1 + current_voltage * .9; //reads power sensor voltage pin
|
||||
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
|
||||
current_total += (current_amps * 0.27777) / delta_ms_medium_loop;
|
||||
}
|
||||
|
||||
|
||||
//v: 10.9453, a: 17.4023, mah: 8.2
|
||||
|
||||
@ -713,9 +713,9 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
while(1){
|
||||
static float min[3], _max[3], offset[3];
|
||||
if (millis() - fast_loopTimer > 100) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
fast_loopTimer = millis();
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f;
|
||||
|
||||
|
||||
compass.read();
|
||||
|
||||
@ -170,9 +170,9 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
||||
while(1){
|
||||
// 50 hz
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
fast_loopTimer = millis();
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f;
|
||||
|
||||
if(compass_enabled){
|
||||
medium_loopCounter++;
|
||||
@ -236,8 +236,8 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
// R: 1417, L: 1453 F: 1453 B: 1417
|
||||
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
@ -272,9 +272,9 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
|
||||
while(1){
|
||||
// 50 hz
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
fast_loopTimer = millis();
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f;
|
||||
|
||||
|
||||
if(compass_enabled){
|
||||
@ -336,8 +336,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
// R: 1417, L: 1453 F: 1453 B: 1417
|
||||
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
@ -379,8 +379,8 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
||||
while(1){
|
||||
delay(20);
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
fast_loopTimer = millis();
|
||||
|
||||
|
||||
@ -615,15 +615,20 @@ static int8_t
|
||||
test_current(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delta_ms_medium_loop = 100;
|
||||
|
||||
while(1){
|
||||
for (int i = 0; i < 20; i++){
|
||||
delay(20);
|
||||
read_current();
|
||||
}
|
||||
Serial.printf_P(PSTR("Volts:"));
|
||||
Serial.print(battery_voltage1, 2); //power sensor voltage pin
|
||||
Serial.print(" Amps:");
|
||||
Serial.println(battery_voltage2, 2); //power sensor current pin
|
||||
delay(100);
|
||||
read_radio();
|
||||
read_current();
|
||||
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), current_voltage, current_amps, current_total);
|
||||
|
||||
//if(rc_3.control_in > 0){
|
||||
APM_RC.OutputCh(CH_1, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_2, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_3, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_4, rc_3.radio_in);
|
||||
//}
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
@ -729,8 +734,8 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
while(1){
|
||||
if (millis()-fast_loopTimer > 9) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
fast_loopTimer = millis();
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user