aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

continued APvar int

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1701 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-02-20 23:09:28 +00:00
parent 747f01769d
commit 73f3fc6193
8 changed files with 105 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ArduCopterMega/ArduCopterMega.pde
View File

@ -38,6 +38,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // ArduPilot Mega RC Library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <GCS_MAVLink.h> // MAVLink GCS definitions
@ -125,6 +126,8 @@ GCS_MAVLINK gcs;
GCS_Class gcs;
#endif
AP_RangeFinder_MaxsonarXL sonar;
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
@ -257,7 +260,13 @@ unsigned long abs_pressure;
unsigned long ground_pressure;
int ground_temperature;
byte altitude_sensor = BARO; // used to know whic sensor is active, BARO or SONAR
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// Sonar Sensor variables
// ----------------------
long sonar_alt;
long baro_alt;
// flight mode specific
// --------------------
@ -505,6 +514,14 @@ void medium_loop()
// ------------------
read_barometer();
if(g.sonar_enabled){
sonar.read();
update_alt();
}else{
// no sonar altitude
current_loc.alt = baro_alt;
}
// altitude smoothing
// ------------------
calc_altitude_error();
@ -851,6 +868,7 @@ void update_current_flight_mode(void)
//if(g.rc_3.control_in)
// get desired height from the throttle
next_WP.alt = home.alt + (g.rc_3.control_in * 4); // 0 - 1000 (40 meters)
next_WP.alt = max(next_WP.alt, 30);
// !!! testing
//next_WP.alt -= 500;
@ -978,3 +996,16 @@ void update_trig(void){
cos_roll_x = temp.c.z / cos_pitch_x;
sin_roll_y = temp.c.y / cos_pitch_x;
}
void
update_alt(){
altitude_sensor = (target_altitude > 500) ? BARO : SONAR;
if(altitude_sensor == BARO){
current_loc.alt = baro_alt;
}else{
current_loc.alt = sonar_alt;
}
}

5
ArduCopterMega/Parameters.h
View File

@ -60,6 +60,7 @@ public:
k_param_milliamp_hours,
k_param_compass_enabled,
k_param_compass,
k_param_sonar,
//
// 160: Navigation parameters
@ -174,6 +175,7 @@ public:
AP_Int16 RTL_altitude;
AP_Int8 frame_type;
AP_Int8 sonar_enabled;
AP_Int8 current_enabled;
AP_Int16 milliamp_hours;
AP_Int8 compass_enabled;
@ -218,6 +220,7 @@ public:
frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
milliamp_hours (CURR_AMP_HOURS, k_param_milliamp_hours, PSTR("MAH")),
compass_enabled (DISABLED, k_param_compass_enabled, PSTR("COMPASS_ENABLE")),
@ -239,7 +242,7 @@ public:
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLIGHT_MODE_CH")),
flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")),
pitch_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_max, PSTR("PITCH_MAX_CENTIDEGREE")),
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),

7
ArduCopterMega/config.h
View File

@ -72,6 +72,13 @@
# define FRAME_CONFIG PLUS_FRAME
#endif
//////////////////////////////////////////////////////////////////////////////
// Sonar
//
#ifndef SONAR_PIN
# define SONAR_PIN AP_RANGEFINDER_PITOT_TUBE
#endif
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL

8
ArduCopterMega/motors.pde
View File

@ -60,7 +60,7 @@ set_servos_4()
//Serial.printf("yaw: %d ", g.rc_4.radio_out);
if(g.frame_type == PLUS_FRAME){
Serial.println("P_FRAME");
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out;
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
@ -73,7 +73,7 @@ set_servos_4()
}else if(g.frame_type == X_FRAME){
Serial.println("X_FRAME");
int roll_out = g.rc_1.pwm_out / 2;
int pitch_out = g.rc_2.pwm_out / 2;
@ -94,6 +94,7 @@ set_servos_4()
}else if(g.frame_type == TRI_FRAME){
Serial.println("TRI_FRAME");
// Tri-copter power distribution
int roll_out = (float)g.rc_1.pwm_out * .866;
@ -111,6 +112,7 @@ set_servos_4()
}else if (g.frame_type == HEXA_FRAME) {
Serial.println("6_FRAME");
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
@ -176,7 +178,7 @@ set_servos_4()
init_pids();
//*/
/*
///*
write_int(motor_out[CH_1]);
write_int(motor_out[CH_2]);
write_int(motor_out[CH_3]);

10
ArduCopterMega/sensors.pde
View File

@ -7,22 +7,22 @@ void init_pressure_ground(void)
delay(20);
barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
}
abs_pressure = barometer.Press;
}
void read_barometer(void){
float x, scaling, temp;
barometer.Read(); // Get new data from absolute pressure sensor
//abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
scaling = (float)ground_pressure / (float)abs_pressure;
temp = ((float)ground_temperature / 10.f) + 273.15;
x = log(scaling) * temp * 29271.267f;
current_loc.alt = (long)(x / 10) + home.alt;// + baro_offset; // Pressure altitude in centimeters
baro_alt = (long)(x / 10) + home.alt;// + baro_offset; // Pressure altitude in centimeters
}
// in M/S * 100
@ -44,7 +44,7 @@ void read_battery(void)
low_battery_event();
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
#endif
#if BATTERY_TYPE == 1
if(battery_voltage4 < LOW_VOLTAGE)
low_battery_event();

50
ArduCopterMega/setup.pde
View File

@ -10,6 +10,7 @@ static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_pid (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_current (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
@ -28,6 +29,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"modes", setup_flightmodes},
{"frame", setup_frame},
{"current", setup_current},
{"sonar", setup_sonar},
{"compass", setup_compass},
{"mag_offset", setup_mag_offset},
{"declination", setup_declination},
@ -66,6 +68,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_radio();
report_frame();
report_current();
report_sonar();
report_gains();
report_xtrack();
report_throttle();
@ -504,6 +507,25 @@ setup_current(uint8_t argc, const Menu::arg *argv)
return 0;
}
static int8_t
setup_sonar(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.sonar_enabled.set_and_save(true);
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.sonar_enabled.set_and_save(false);
} else {
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
report_sonar();
return 0;
}
report_sonar();
return 0;
}
static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
{
@ -759,7 +781,17 @@ void report_current()
print_enabled(g.current_enabled.get());
Serial.printf_P(PSTR("mah: %d"),(int)g.milliamp_hours.get());
print_blanks(1);
print_blanks(2);
}
void report_sonar()
{
//print_blanks(2);
g.sonar_enabled.load();
Serial.printf_P(PSTR("Sonar Sensor\n"));
print_divider();
print_enabled(g.sonar_enabled.get());
print_blanks(2);
}
@ -781,7 +813,7 @@ void report_frame()
Serial.printf_P(PSTR("HEXA "));
Serial.printf_P(PSTR("frame (%d)"), (int)g.frame_type);
print_blanks(1);
print_blanks(2);
}
void report_radio()
@ -792,7 +824,7 @@ void report_radio()
// radio
read_EEPROM_radio();
print_radio_values();
print_blanks(1);
print_blanks(2);
}
void report_gains()
@ -830,7 +862,7 @@ void report_gains()
print_PID(&g.pid_baro_throttle);
Serial.printf_P(PSTR("sonar throttle:\n"));
print_PID(&g.pid_sonar_throttle);
print_blanks(1);
print_blanks(2);
}
void report_xtrack()
@ -846,7 +878,7 @@ void report_xtrack()
(float)g.crosstrack_gain,
(int)g.crosstrack_entry_angle,
(long)g.pitch_max);
print_blanks(1);
print_blanks(2);
}
void report_throttle()
@ -866,7 +898,7 @@ void report_throttle()
(int)g.throttle_cruise,
(int)g.throttle_fs_enabled,
(int)g.throttle_fs_value);
print_blanks(1);
print_blanks(2);
}
void report_imu()
@ -877,7 +909,7 @@ void report_imu()
print_gyro_offsets();
print_accel_offsets();
print_blanks(1);
print_blanks(2);
}
void report_compass()
@ -903,7 +935,7 @@ void report_compass()
offsets.x,
offsets.y,
offsets.z);
print_blanks(1);
print_blanks(2);
}
@ -917,7 +949,7 @@ void report_flight_modes()
for(int i = 0; i < 6; i++ ){
print_switch(i, g.flight_modes[i]);
}
print_blanks(1);
print_blanks(2);
}
/***************************************************************************/

11
ArduCopterMega/system.pde
View File

@ -137,6 +137,10 @@ void init_ardupilot()
if(g.compass_enabled)
init_compass();
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
}
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
@ -177,7 +181,7 @@ void init_ardupilot()
// read in the flight switches
//update_servo_switches();
imu.init(IMU::WARM_START); // offsets are loaded from EEPROM
//imu.init_gyro(IMU::WARM_START); // offsets are loaded from EEPROM
startup_ground();
@ -201,6 +205,8 @@ void startup_ground(void)
delay(GROUND_START_DELAY * 1000);
#endif
setup_show(NULL,NULL);
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < g.waypoint_total + 1; i++) {
@ -209,7 +215,8 @@ void startup_ground(void)
// Warm up and read Gyro offsets
// -----------------------------
imu.init(IMU::COLD_START);
imu.init_gyro();
report_imu();
// read the radio to set trims
// ---------------------------

2
ArduCopterMega/test.pde
View File

@ -400,7 +400,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("Calibrating."));
report_imu();
imu.init_gyro();
report_imu();
print_hit_enter();
delay(1000);