mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 08:58:29 -04:00
35bf288abd
Before: -> After Stabilize P –> Stabilize P (Use NG values, or 8.3 x the older AC2 value) Stabilize I –> Stabilize I (Stays same value) Stabilize D –> Rate P (Stays same value) –> Rate I (new) Added a new value – an I term for rate. The old stabilization routines did not use this term. Please refer to the config.h file to read more about the new PIDs. Added framework for using DCM corrected Accelerometer rates. Code is commented out for now. Added set home at Arming. Crosstrack is now a full PID loop, rather than just a P gain for more control. Throttle now slews when switching out of Alt hold or Auto modes for less jarring transitions Sonar and Baro PIDs are now combined into a throttle PID Yaw control is completely re-written. Added Octa_Quad support - Max git-svn-id: https://arducopter.googlecode.com/svn/trunk@2836 f9c3cf11-9bcb-44bc-f272-b75c42450872
1022 lines
23 KiB
Plaintext
1022 lines
23 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
// These are function definitions so the Menu can be constructed before the functions
|
|
// are defined below. Order matters to the compiler.
|
|
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
|
|
//static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_tri(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
|
|
//static int8_t test_dcm(uint8_t argc, const Menu::arg *argv);
|
|
//static int8_t test_omega(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
|
//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_tuning(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_baro(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
|
|
static int8_t test_mission(uint8_t argc, const Menu::arg *argv);
|
|
|
|
// This is the help function
|
|
// PSTR is an AVR macro to read strings from flash memory
|
|
// printf_P is a version of printf that reads from flash memory
|
|
/*static int8_t help_test(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
Serial.printf_P(PSTR("\n"
|
|
"Commands:\n"
|
|
" radio\n"
|
|
" servos\n"
|
|
" g_gps\n"
|
|
" imu\n"
|
|
" battery\n"
|
|
"\n"));
|
|
}*/
|
|
|
|
// Creates a constant array of structs representing menu options
|
|
// and stores them in Flash memory, not RAM.
|
|
// User enters the string in the console to call the functions on the right.
|
|
// See class Menu in AP_Coommon for implementation details
|
|
const struct Menu::command test_menu_commands[] PROGMEM = {
|
|
{"pwm", test_radio_pwm},
|
|
{"radio", test_radio},
|
|
{"failsafe", test_failsafe},
|
|
// {"stabilize", test_stabilize},
|
|
{"gps", test_gps},
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
{"adc", test_adc},
|
|
#endif
|
|
{"imu", test_imu},
|
|
//{"dcm", test_dcm},
|
|
//{"omega", test_omega},
|
|
{"battery", test_battery},
|
|
{"tune", test_tuning},
|
|
{"tri", test_tri},
|
|
{"current", test_current},
|
|
{"relay", test_relay},
|
|
{"waypoints", test_wp},
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
{"altitude", test_baro},
|
|
#endif
|
|
{"sonar", test_sonar},
|
|
{"compass", test_mag},
|
|
{"xbee", test_xbee},
|
|
{"eedump", test_eedump},
|
|
{"rawgps", test_rawgps},
|
|
{"mission", test_mission},
|
|
//{"wp", test_wp_nav},
|
|
};
|
|
|
|
// A Macro to create the Menu
|
|
MENU(test_menu, "test", test_menu_commands);
|
|
|
|
int8_t
|
|
test_mode(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
Serial.printf_P(PSTR("Test Mode\n\n"));
|
|
test_menu.run();
|
|
}
|
|
|
|
static int8_t
|
|
test_eedump(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
int i, j;
|
|
|
|
// hexdump the EEPROM
|
|
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
|
|
Serial.printf_P(PSTR("%04x:"), i);
|
|
for (j = 0; j < 16; j++)
|
|
Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
|
|
Serial.println();
|
|
}
|
|
return(0);
|
|
}
|
|
|
|
static int8_t
|
|
test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
delay(20);
|
|
|
|
// Filters radio input - adjust filters in the radio.pde file
|
|
// ----------------------------------------------------------
|
|
read_radio();
|
|
|
|
// servo Yaw
|
|
//APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
|
|
|
|
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
|
|
g.rc_1.radio_in,
|
|
g.rc_2.radio_in,
|
|
g.rc_3.radio_in,
|
|
g.rc_4.radio_in,
|
|
g.rc_5.radio_in,
|
|
g.rc_6.radio_in,
|
|
g.rc_7.radio_in,
|
|
g.rc_8.radio_in);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_tri(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
delay(20);
|
|
|
|
// Filters radio input - adjust filters in the radio.pde file
|
|
// ----------------------------------------------------------
|
|
read_radio();
|
|
g.rc_4.servo_out = g.rc_4.control_in;
|
|
g.rc_4.calc_pwm();
|
|
|
|
Serial.printf_P(PSTR("input: %d\toutput%d\n"),
|
|
g.rc_4.control_in,
|
|
g.rc_4.radio_out);
|
|
|
|
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_radio(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
delay(20);
|
|
read_radio();
|
|
|
|
|
|
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
|
|
g.rc_1.control_in,
|
|
g.rc_2.control_in,
|
|
g.rc_3.control_in,
|
|
g.rc_4.control_in,
|
|
g.rc_5.control_in,
|
|
g.rc_6.control_in,
|
|
g.rc_7.control_in);
|
|
|
|
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
|
|
|
|
/*Serial.printf_P(PSTR( "min: %d"
|
|
"\t in: %d"
|
|
"\t pwm_in: %d"
|
|
"\t sout: %d"
|
|
"\t pwm_out %d\n"),
|
|
g.rc_3.radio_min,
|
|
g.rc_3.control_in,
|
|
g.rc_3.radio_in,
|
|
g.rc_3.servo_out,
|
|
g.rc_3.pwm_out
|
|
);
|
|
*/
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
static int8_t
|
|
test_wp_nav(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
dTnav = 200;
|
|
current_loc.lat = 32.9513090 * t7;
|
|
current_loc.lng = -117.2381700 * t7;
|
|
|
|
do_loiter_at_location();
|
|
|
|
wp_control = LOITER_MODE;
|
|
|
|
//dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305
|
|
while(1){
|
|
read_radio();
|
|
delay(dTnav);
|
|
|
|
current_loc.lng = (-117.2381700 * t7) + g.rc_1.control_in / 2;
|
|
current_loc.lat = (32.9513090 * t7) + g.rc_2.control_in / 2;
|
|
|
|
navigate();
|
|
update_nav_wp();
|
|
|
|
Serial.printf("Lon_e: %ld, nLon, %ld, Lat_e %ld, nLat %ld\n", long_error, nav_lon, lat_error, nav_lat);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
//*/
|
|
|
|
/*
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
g.rc_6.set_range(0,900);
|
|
g.rc_4.set_angle(9000);
|
|
dTnav = 200;
|
|
|
|
//dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305
|
|
|
|
while(1){
|
|
delay(20);
|
|
read_radio();
|
|
target_bearing = 0;
|
|
g_gps->ground_course = g.rc_4.control_in;
|
|
g_gps->ground_speed = g.rc_6.control_in;
|
|
calc_rate_nav();
|
|
Serial.printf(" gps_GC: %ld, gps_GS: %d\n", g_gps->ground_course, g_gps->ground_speed);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
static int8_t
|
|
test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
byte fail_test;
|
|
print_hit_enter();
|
|
for(int i = 0; i < 50; i++){
|
|
delay(20);
|
|
read_radio();
|
|
}
|
|
|
|
oldSwitchPosition = readSwitch();
|
|
|
|
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
|
|
while(g.rc_3.control_in > 0){
|
|
delay(20);
|
|
read_radio();
|
|
}
|
|
|
|
while(1){
|
|
delay(20);
|
|
read_radio();
|
|
|
|
if(g.rc_3.control_in > 0){
|
|
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in);
|
|
fail_test++;
|
|
}
|
|
|
|
if(oldSwitchPosition != readSwitch()){
|
|
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
|
|
Serial.println(flight_mode_strings[readSwitch()]);
|
|
fail_test++;
|
|
}
|
|
|
|
if(g.throttle_fs_enabled && g.rc_3.get_failsafe()){
|
|
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
|
|
Serial.println(flight_mode_strings[readSwitch()]);
|
|
fail_test++;
|
|
}
|
|
|
|
if(fail_test > 0){
|
|
return (0);
|
|
}
|
|
if(Serial.available() > 0){
|
|
Serial.printf_P(PSTR("LOS caused no change in ACM.\n"));
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*static int8_t
|
|
test_stabilize(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
static byte ts_num;
|
|
|
|
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
// setup the radio
|
|
// ---------------
|
|
init_rc_in();
|
|
|
|
control_mode = STABILIZE;
|
|
Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP());
|
|
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
|
|
|
|
motor_auto_armed = false;
|
|
motor_armed = true;
|
|
|
|
while(1){
|
|
// 50 hz
|
|
if (millis() - fast_loopTimer > 19) {
|
|
delta_ms_fast_loop = millis() - fast_loopTimer;
|
|
fast_loopTimer = millis();
|
|
G_Dt = (float)delta_ms_fast_loop / 1000.f;
|
|
|
|
if(g.compass_enabled){
|
|
medium_loopCounter++;
|
|
if(medium_loopCounter == 5){
|
|
compass.read(); // Read magnetometer
|
|
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
|
|
compass.null_offsets(dcm.get_dcm_matrix());
|
|
medium_loopCounter = 0;
|
|
}
|
|
}
|
|
|
|
// for trim features
|
|
read_trim_switch();
|
|
|
|
// Filters radio input - adjust filters in the radio.pde file
|
|
// ----------------------------------------------------------
|
|
read_radio();
|
|
|
|
// IMU
|
|
// ---
|
|
read_AHRS();
|
|
|
|
// allow us to zero out sensors with control switches
|
|
if(g.rc_5.control_in < 600){
|
|
dcm.roll_sensor = dcm.pitch_sensor = 0;
|
|
}
|
|
|
|
// custom code/exceptions for flight modes
|
|
// ---------------------------------------
|
|
update_current_flight_mode();
|
|
|
|
// write out the servo PWM values
|
|
// ------------------------------
|
|
set_servos_4();
|
|
|
|
ts_num++;
|
|
if (ts_num > 10){
|
|
ts_num = 0;
|
|
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
|
|
(int)(dcm.roll_sensor/100),
|
|
(int)(dcm.pitch_sensor/100),
|
|
g.rc_1.pwm_out);
|
|
|
|
print_motor_out();
|
|
}
|
|
// R: 1417, L: 1453 F: 1453 B: 1417
|
|
|
|
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
|
|
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
|
|
|
|
if(Serial.available() > 0){
|
|
if(g.compass_enabled){
|
|
compass.save_offsets();
|
|
report_compass();
|
|
}
|
|
return (0);
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
static int8_t
|
|
test_adc(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
Serial.printf_P(PSTR("ADC\n"));
|
|
delay(1000);
|
|
|
|
while(1){
|
|
for(int i = 0; i < 9; i++){
|
|
Serial.printf_P(PSTR("%d,"),adc.Ch(i));
|
|
}
|
|
Serial.println();
|
|
delay(20);
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
static int8_t
|
|
test_imu(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
//Serial.printf_P(PSTR("Calibrating."));
|
|
|
|
//dcm.kp_yaw(0.02);
|
|
//dcm.ki_yaw(0);
|
|
|
|
report_imu();
|
|
imu.init_gyro();
|
|
report_imu();
|
|
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
//float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
|
|
fast_loopTimer = millis();
|
|
|
|
while(1){
|
|
//delay(20);
|
|
if (millis() - fast_loopTimer >= 5) {
|
|
delta_ms_fast_loop = millis() - fast_loopTimer;
|
|
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
|
fast_loopTimer = millis();
|
|
|
|
// IMU
|
|
// ---
|
|
read_AHRS();
|
|
|
|
Vector3f accels = imu.get_accel();
|
|
Vector3f gyros = imu.get_gyro();
|
|
//Vector3f accel_filt = imu.get_accel_filtered();
|
|
//accels_rot = dcm.get_dcm_matrix() * accel_filt;
|
|
|
|
|
|
medium_loopCounter++;
|
|
|
|
if(medium_loopCounter == 4){
|
|
update_trig();
|
|
}
|
|
|
|
if(medium_loopCounter == 20){
|
|
read_radio();
|
|
medium_loopCounter = 0;
|
|
//tuning();
|
|
//dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0);
|
|
|
|
/*
|
|
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld, kp:%1.4f, kp:%1.4f \n"),
|
|
dcm.roll_sensor,
|
|
dcm.pitch_sensor,
|
|
dcm.yaw_sensor,
|
|
dcm.kp_roll_pitch(),
|
|
(float)g.rc_6.control_in / 2000.0);
|
|
*/
|
|
Serial.printf_P(PSTR("%ld, %ld, %ld\n"),
|
|
dcm.roll_sensor,
|
|
dcm.pitch_sensor,
|
|
dcm.yaw_sensor);
|
|
|
|
if(g.compass_enabled){
|
|
compass.read(); // Read magnetometer
|
|
compass.calculate(dcm.get_dcm_matrix());
|
|
}
|
|
}
|
|
|
|
// We are using the IMU
|
|
// ---------------------
|
|
/*
|
|
Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
|
|
"G: %4.4f, %4.4f, %4.4f\t"),
|
|
accels.x, accels.y, accels.z,
|
|
gyros.x, gyros.y, gyros.z);
|
|
*/
|
|
/*
|
|
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
|
|
cos_pitch_x,
|
|
sin_pitch_y,
|
|
cos_roll_x,
|
|
sin_roll_y,
|
|
cos_yaw_x, // x
|
|
sin_yaw_y); // y
|
|
//*/
|
|
|
|
//
|
|
Log_Write_Raw();
|
|
}
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_gps(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
delay(333);
|
|
|
|
// Blink GPS LED if we don't have a fix
|
|
// ------------------------------------
|
|
update_GPS_light();
|
|
|
|
g_gps->update();
|
|
|
|
if (g_gps->new_data){
|
|
Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
|
|
g_gps->latitude,
|
|
g_gps->longitude,
|
|
g_gps->altitude/100,
|
|
g_gps->num_sats);
|
|
g_gps->new_data = false;
|
|
}else{
|
|
Serial.print(".");
|
|
}
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
static int8_t
|
|
test_dcm(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("Gyro | Accel\n"));
|
|
Vector3f _cam_vector;
|
|
Vector3f _out_vector;
|
|
|
|
G_Dt = .02;
|
|
|
|
while(1){
|
|
for(byte i = 0; i <= 50; i++){
|
|
delay(20);
|
|
// IMU
|
|
// ---
|
|
read_AHRS();
|
|
}
|
|
|
|
Matrix3f temp = dcm.get_dcm_matrix();
|
|
Matrix3f temp_t = dcm.get_dcm_transposed();
|
|
|
|
Serial.printf_P(PSTR("dcm\n"
|
|
"%4.4f \t %4.4f \t %4.4f \n"
|
|
"%4.4f \t %4.4f \t %4.4f \n"
|
|
"%4.4f \t %4.4f \t %4.4f \n\n"),
|
|
temp.a.x, temp.a.y, temp.a.z,
|
|
temp.b.x, temp.b.y, temp.b.z,
|
|
temp.c.x, temp.c.y, temp.c.z);
|
|
|
|
int _pitch = degrees(-asin(temp.c.x));
|
|
int _roll = degrees(atan2(temp.c.y, temp.c.z));
|
|
int _yaw = degrees(atan2(temp.b.x, temp.a.x));
|
|
Serial.printf_P(PSTR( "angles\n"
|
|
"%d \t %d \t %d\n\n"),
|
|
_pitch,
|
|
_roll,
|
|
_yaw);
|
|
|
|
//_out_vector = _cam_vector * temp;
|
|
//Serial.printf_P(PSTR( "cam\n"
|
|
// "%d \t %d \t %d\n\n"),
|
|
// (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
/*
|
|
static int8_t
|
|
test_dcm(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("Gyro | Accel\n"));
|
|
delay(1000);
|
|
|
|
while(1){
|
|
Vector3f accels = dcm.get_accel();
|
|
Serial.print("accels.z:");
|
|
Serial.print(accels.z);
|
|
Serial.print("omega.z:");
|
|
Serial.print(omega.z);
|
|
delay(100);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
|
|
/*static int8_t
|
|
test_omega(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
static byte ts_num;
|
|
float old_yaw;
|
|
|
|
print_hit_enter();
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("Omega"));
|
|
delay(1000);
|
|
|
|
G_Dt = .02;
|
|
|
|
while(1){
|
|
delay(20);
|
|
// IMU
|
|
// ---
|
|
read_AHRS();
|
|
|
|
float my_oz = (dcm.yaw - old_yaw) * 50;
|
|
|
|
old_yaw = dcm.yaw;
|
|
|
|
ts_num++;
|
|
if (ts_num > 2){
|
|
ts_num = 0;
|
|
//Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz);
|
|
Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz);
|
|
}
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
return (0);
|
|
}
|
|
//*/
|
|
|
|
static int8_t
|
|
test_battery(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
#if BATTERY_EVENT == 1
|
|
for (int i = 0; i < 20; i++){
|
|
delay(20);
|
|
read_battery();
|
|
}
|
|
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
|
|
battery_voltage1,
|
|
battery_voltage2,
|
|
battery_voltage3,
|
|
battery_voltage4);
|
|
#else
|
|
Serial.printf_P(PSTR("Not enabled\n"));
|
|
|
|
#endif
|
|
return (0);
|
|
}
|
|
|
|
static int8_t
|
|
test_tuning(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
|
|
while(1){
|
|
delay(200);
|
|
read_radio();
|
|
|
|
//Outer Loop : Attitude
|
|
#if CHANNEL_6_TUNING == CH6_NONE
|
|
Serial.printf_P(PSTR("disabled\n"));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KP
|
|
Serial.printf_P(PSTR("stab kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
|
|
Serial.printf_P(PSTR("stab kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_YAW_KP
|
|
Serial.printf_P(PSTR("yaw Hold kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); // range from 0 ~ 5.0
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_YAW_KI
|
|
Serial.printf_P(PSTR("yaw Hold kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
//Inner Loop : Rate
|
|
#elif CHANNEL_6_TUNING == CH6_RATE_KP
|
|
Serial.printf_P(PSTR("rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_RATE_KI
|
|
Serial.printf_P(PSTR("rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
|
|
Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI
|
|
Serial.printf_P(PSTR("yaw rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
|
|
//Altitude Hold
|
|
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
|
|
Serial.printf_P(PSTR("throttle kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
|
|
Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
|
|
//Extras
|
|
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
|
|
Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
|
|
|
#elif CHANNEL_6_TUNING == CH6_PMAX
|
|
Serial.printf_P(PSTR("Y6: %d\n"), (g.rc_6.control_in * 2));
|
|
|
|
#endif
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_current(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delta_ms_medium_loop = 100;
|
|
|
|
while(1){
|
|
delay(100);
|
|
read_radio();
|
|
read_battery();
|
|
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
|
|
battery_voltage,
|
|
current_amps,
|
|
current_total);
|
|
|
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
|
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_relay(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
|
|
while(1){
|
|
Serial.printf_P(PSTR("Relay on\n"));
|
|
relay_on();
|
|
delay(3000);
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
|
|
Serial.printf_P(PSTR("Relay off\n"));
|
|
relay_off();
|
|
delay(3000);
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_wp(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
delay(1000);
|
|
|
|
// save the alitude above home option
|
|
Serial.printf_P(PSTR("Hold altitude "));
|
|
if(g.RTL_altitude < 0){
|
|
Serial.printf_P(PSTR("\n"));
|
|
}else{
|
|
Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
|
|
}
|
|
|
|
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total);
|
|
Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
|
|
//Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
|
|
|
|
report_wp();
|
|
|
|
return (0);
|
|
}
|
|
|
|
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) {
|
|
print_hit_enter();
|
|
delay(1000);
|
|
while(1){
|
|
if (Serial3.available()){
|
|
digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
|
|
Serial1.write(Serial3.read());
|
|
digitalWrite(B_LED_PIN, LOW);
|
|
}
|
|
if (Serial1.available()){
|
|
digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS
|
|
Serial3.write(Serial1.read());
|
|
digitalWrite(C_LED_PIN, LOW);
|
|
}
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int8_t
|
|
test_xbee(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
delay(1000);
|
|
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
|
|
|
|
while(1){
|
|
if (Serial3.available())
|
|
Serial3.write(Serial3.read());
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
static int8_t
|
|
test_baro(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
print_hit_enter();
|
|
init_barometer();
|
|
|
|
while(1){
|
|
delay(100);
|
|
|
|
baro_alt = read_barometer();
|
|
|
|
Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
static int8_t
|
|
test_mag(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if(g.compass_enabled) {
|
|
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
|
|
|
|
print_hit_enter();
|
|
|
|
while(1){
|
|
delay(100);
|
|
compass.read();
|
|
compass.calculate(dcm.get_dcm_matrix());
|
|
Vector3f maggy = compass.get_offsets();
|
|
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
|
|
(wrap_360(ToDeg(compass.heading) * 100)) /100,
|
|
compass.mag_x,
|
|
compass.mag_y,
|
|
compass.mag_z,
|
|
maggy.x,
|
|
maggy.y,
|
|
maggy.z);
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
} else {
|
|
Serial.printf_P(PSTR("Compass: "));
|
|
print_enabled(false);
|
|
return (0);
|
|
}
|
|
}
|
|
|
|
/*
|
|
test the sonar
|
|
*/
|
|
static int8_t
|
|
test_sonar(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
if(g.sonar_enabled == false){
|
|
Serial.printf_P(PSTR("Sonar disabled\n"));
|
|
return (0);
|
|
}
|
|
|
|
print_hit_enter();
|
|
while(1) {
|
|
delay(100);
|
|
|
|
Serial.printf_P(PSTR("Sonar: %d cm\n"), sonar.read());
|
|
|
|
if(Serial.available() > 0){
|
|
return (0);
|
|
}
|
|
}
|
|
|
|
return (0);
|
|
}
|
|
|
|
static int8_t
|
|
test_mission(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
//write out a basic mission to the EEPROM
|
|
|
|
/*{
|
|
uint8_t id; ///< command id
|
|
uint8_t options; ///< options bitmask (1<<0 = relative altitude)
|
|
uint8_t p1; ///< param 1
|
|
int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100)
|
|
int32_t lat; ///< param 3 - Lattitude * 10**7
|
|
int32_t lng; ///< param 4 - Longitude * 10**7
|
|
}*/
|
|
|
|
// clear home
|
|
{Location t = {0, 0, 0, 0, 0, 0};
|
|
set_command_with_index(t,0);}
|
|
|
|
// CMD opt pitch alt/cm
|
|
{Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0};
|
|
set_command_with_index(t,1);}
|
|
|
|
if (!strcmp_P(argv[1].str, PSTR("wp"))) {
|
|
|
|
// CMD opt
|
|
{Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0};
|
|
set_command_with_index(t,2);}
|
|
// CMD opt
|
|
{Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0};
|
|
set_command_with_index(t,3);}
|
|
|
|
// CMD opt
|
|
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
|
|
set_command_with_index(t,4);}
|
|
|
|
} else {
|
|
//2250 = 25 meteres
|
|
// CMD opt p1 //alt //NS //WE
|
|
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19
|
|
set_command_with_index(t,2);}
|
|
|
|
// CMD opt dir angle/deg deg/s relative
|
|
{Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
|
|
set_command_with_index(t,3);}
|
|
|
|
// CMD opt
|
|
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
|
|
set_command_with_index(t,4);}
|
|
|
|
}
|
|
|
|
g.RTL_altitude.set_and_save(300);
|
|
g.waypoint_total.set_and_save(4);
|
|
g.waypoint_radius.set_and_save(3);
|
|
|
|
test_wp(NULL, NULL);
|
|
return (0);
|
|
}
|
|
|
|
void print_hit_enter()
|
|
{
|
|
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
|
|
}
|
|
|
|
void fake_out_gps()
|
|
{
|
|
static float rads;
|
|
g_gps->new_data = true;
|
|
g_gps->fix = true;
|
|
|
|
//int length = g.rc_6.control_in;
|
|
rads += .05;
|
|
|
|
if (rads > 6.28){
|
|
rads = 0;
|
|
}
|
|
|
|
g_gps->latitude = 377696000; // Y
|
|
g_gps->longitude = -1224319000; // X
|
|
g_gps->altitude = 9000; // meters * 100
|
|
|
|
//next_WP.lng = home.lng - length * sin(rads); // X
|
|
//next_WP.lat = home.lat + length * cos(rads); // Y
|
|
}
|
|
|
|
|
|
|
|
void print_motor_out(){
|
|
Serial.printf("out: R: %d, L: %d F: %d B: %d\n",
|
|
(motor_out[CH_1] - g.rc_3.radio_min),
|
|
(motor_out[CH_2] - g.rc_3.radio_min),
|
|
(motor_out[CH_3] - g.rc_3.radio_min),
|
|
(motor_out[CH_4] - g.rc_3.radio_min));
|
|
}
|