mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
added better value for Sonar minimal value for landing
removed FBW mode - no longer needed added set_throttle_cruise_flag to auto set the throttle value for alt hold added altitude minimum option for waypoints added support for relative WPs added support for Yaw tracking per WP in options bitmask lowered default sonar kD value increased minimal value to set the throttle cruise value with CH7 switch updated README.txt added additional stock test missions available in CLI git-svn-id: https://arducopter.googlecode.com/svn/trunk@1856 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
5e726335fa
commit
c3737db26c
@ -16,7 +16,6 @@
|
||||
|
||||
//#define ACRO_RATE_TRIGGER 4200
|
||||
// if you want full ACRO mode, set value to 0
|
||||
// if you want safe ACRO mode, set value to 100
|
||||
// if you want mostly stabilize with flips, set value to 4200
|
||||
|
||||
|
||||
|
||||
@ -195,7 +195,6 @@ const char* flight_mode_strings[] = {
|
||||
"ACRO",
|
||||
"ALT_HOLD",
|
||||
"SIMPLE",
|
||||
"FBW",
|
||||
"AUTO",
|
||||
"GCS_AUTO",
|
||||
"LOITER",
|
||||
@ -336,6 +335,8 @@ long nav_lon; // for error calcs
|
||||
int nav_throttle; // 0-1000 for throttle control
|
||||
int nav_throttle_old; // for filtering
|
||||
|
||||
bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
|
||||
|
||||
long command_yaw_start; // what angle were we to begin with
|
||||
long command_yaw_start_time; // when did we start turning
|
||||
int command_yaw_time; // how long we are turning
|
||||
@ -560,9 +561,6 @@ void medium_loop()
|
||||
if (control_mode == SIMPLE)
|
||||
break;
|
||||
|
||||
if (control_mode == FBW)
|
||||
break;
|
||||
|
||||
// Auto control modes:
|
||||
if(g_gps->new_data){
|
||||
g_gps->new_data = false;
|
||||
@ -975,45 +973,6 @@ void update_current_flight_mode(void)
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case FBW:
|
||||
// we are currently using manual throttle during alpha testing.
|
||||
flight_timer++;
|
||||
|
||||
// 10 hz
|
||||
if(flight_timer > 10){
|
||||
flight_timer = 0;
|
||||
|
||||
if(GPS_enabled == false){
|
||||
current_loc.lat = home.lat = 0;
|
||||
current_loc.lng = home.lng = 0;
|
||||
}
|
||||
|
||||
next_WP.lng = home.lng + g.rc_1.control_in / 2; // X: 4500 / 2 = 2250 = 25 meteres
|
||||
next_WP.lat = home.lat - g.rc_2.control_in / 2; // Y: 4500 / 2 = 2250 = 25 meteres
|
||||
|
||||
calc_loiter_nav();
|
||||
}
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
|
||||
// REMOVE AFTER TESTING !!!
|
||||
//nav_yaw = dcm.yaw_sensor;
|
||||
|
||||
// Yaw control
|
||||
output_manual_yaw();
|
||||
|
||||
// apply throttle control
|
||||
output_manual_throttle();
|
||||
|
||||
// apply nav_pitch and nav_roll to output
|
||||
fbw_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case ALT_HOLD:
|
||||
// clear any AP naviagtion values
|
||||
nav_pitch = 0;
|
||||
|
||||
@ -10,7 +10,6 @@ init_pids()
|
||||
max_yaw_dampener = g.pid_yaw.kP() * 6000; // = .5 * 6000 = 3000
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
control_nav_mixer()
|
||||
{
|
||||
@ -158,7 +157,6 @@ output_yaw_with_hold(boolean hold)
|
||||
}
|
||||
|
||||
// slight left rudder gives right roll.
|
||||
|
||||
void
|
||||
output_rate_roll()
|
||||
{
|
||||
@ -175,12 +173,12 @@ void
|
||||
output_rate_pitch()
|
||||
{
|
||||
// rate control
|
||||
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
long error = ((long)g.rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
|
||||
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
long error = ((long)g.rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
|
||||
|
||||
g.rc_2.servo_out = g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
|
||||
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
|
||||
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
|
||||
}
|
||||
|
||||
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
||||
@ -195,8 +193,6 @@ reset_I(void)
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*************************************************************
|
||||
throttle control
|
||||
****************************************************************/
|
||||
@ -248,7 +244,6 @@ float angle_boost()
|
||||
return temp;
|
||||
}
|
||||
|
||||
|
||||
/*************************************************************
|
||||
yaw control
|
||||
****************************************************************/
|
||||
|
||||
@ -47,7 +47,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
||||
case STABILIZE:
|
||||
mode = MAV_MODE_GUIDED;
|
||||
break;
|
||||
case FBW:
|
||||
case SIMPLE:
|
||||
mode = MAV_MODE_TEST1;
|
||||
break;
|
||||
case ALT_HOLD:
|
||||
|
||||
@ -63,6 +63,17 @@ struct Location get_wp_with_index(int i)
|
||||
temp.alt += home.alt;
|
||||
}
|
||||
|
||||
if(temp.options & WP_OPTION_RELATIVE){
|
||||
temp.lat += home.lat;
|
||||
temp.lng += home.lng;
|
||||
}
|
||||
|
||||
// XXX this is a little awkward. We have two methods to control Yaw tracking
|
||||
// one is global and one is per waypoint.
|
||||
if(temp.options & WP_OPTION_YAW){
|
||||
yaw_tracking = TRACK_NEXT_WP;
|
||||
}
|
||||
|
||||
return temp;
|
||||
}
|
||||
|
||||
@ -154,7 +165,7 @@ void set_next_WP(struct Location *wp)
|
||||
// ---------------------
|
||||
next_WP = *wp;
|
||||
|
||||
// used to control FBW and limit the rate of climb
|
||||
// used to control and limit the rate of climb
|
||||
// -----------------------------------------------
|
||||
target_altitude = current_loc.alt;
|
||||
|
||||
|
||||
@ -303,7 +303,7 @@ bool verify_land()
|
||||
|
||||
if(g.sonar_enabled){
|
||||
// decide which sensor we're usings
|
||||
if(sonar_alt < 20){
|
||||
if(sonar_alt < 40){
|
||||
land_complete = true;
|
||||
Serial.println("Y");
|
||||
return true;
|
||||
@ -322,22 +322,35 @@ bool verify_land()
|
||||
|
||||
bool verify_nav_wp()
|
||||
{
|
||||
bool alt = true;
|
||||
|
||||
update_crosstrack();
|
||||
|
||||
if (next_WP.options & WP_OPTION_ALT_REQUIRED){
|
||||
byte = current_loc.alt > next_WP.alt;
|
||||
}
|
||||
|
||||
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
|
||||
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
|
||||
//SendDebugln(command_must_index,DEC);
|
||||
char message[30];
|
||||
sprintf(message,"Reached Waypoint #%i",command_must_index);
|
||||
gcs.send_text(SEVERITY_LOW,message);
|
||||
return true;
|
||||
|
||||
if (alt){
|
||||
char message[30];
|
||||
sprintf(message,"Reached Waypoint #%i",command_must_index);
|
||||
gcs.send_text(SEVERITY_LOW,message);
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Have we passed the WP?
|
||||
if(loiter_sum > 90){
|
||||
if(alt && (loiter_sum > 90)){
|
||||
gcs.send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool verify_loiter_unlim()
|
||||
|
||||
@ -301,7 +301,7 @@
|
||||
# define ACRO_RATE_YAW_IMAX_CENTIDEGREE ACRO_RATE_YAW_IMAX * 100
|
||||
|
||||
#ifndef ACRO_RATE_TRIGGER
|
||||
# define ACRO_RATE_TRIGGER 100
|
||||
# define ACRO_RATE_TRIGGER 0
|
||||
#endif
|
||||
|
||||
|
||||
@ -429,7 +429,7 @@
|
||||
# define THROTTLE_SONAR_I 0.01
|
||||
#endif
|
||||
#ifndef THROTTLE_SONAR_D
|
||||
# define THROTTLE_SONAR_D 0.2
|
||||
# define THROTTLE_SONAR_D 0.03
|
||||
#endif
|
||||
#ifndef THROTTLE_SONAR_IMAX
|
||||
# define THROTTLE_SONAR_IMAX 50
|
||||
|
||||
@ -80,12 +80,9 @@ void read_trim_switch()
|
||||
#endif
|
||||
}else{
|
||||
// set the throttle nominal
|
||||
if(g.rc_3.control_in > 50){
|
||||
g.throttle_cruise.set(g.rc_3.control_in);
|
||||
if(g.rc_3.control_in > 200){
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
//Serial.printf("tnom %d\n", g.throttle_cruise.get());
|
||||
//save_EEPROM_throttle_cruise();
|
||||
g.throttle_cruise.save();
|
||||
|
||||
}
|
||||
|
||||
// this is a test for Max's tri-copter
|
||||
|
||||
@ -88,12 +88,11 @@
|
||||
#define ACRO 1 // rate control
|
||||
#define ALT_HOLD 2 // AUTO control
|
||||
#define SIMPLE 3 //
|
||||
#define FBW 4 // AUTO control
|
||||
#define AUTO 5 // AUTO control
|
||||
#define GCS_AUTO 6 // AUTO control
|
||||
#define LOITER 7 // Hold a single location
|
||||
#define RTL 8 // AUTO control
|
||||
#define NUM_MODES 9
|
||||
#define AUTO 4 // AUTO control
|
||||
#define GCS_AUTO 5 // AUTO control
|
||||
#define LOITER 6 // Hold a single location
|
||||
#define RTL 7 // AUTO control
|
||||
#define NUM_MODES 8
|
||||
|
||||
|
||||
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
|
||||
@ -105,6 +104,16 @@
|
||||
#define TRACK_NEXT_WP 2
|
||||
#define TRACK_TARGET_WP 4
|
||||
|
||||
// Waypoint options
|
||||
#define WP_OPTION_ALT_RELATIVE 1
|
||||
#define WP_OPTION_ALT_CHANGE 2
|
||||
#define WP_OPTION_YAW 4
|
||||
#define WP_OPTION_ALT_REQUIRED 8
|
||||
#define WP_OPTION_RELATIVE 16
|
||||
//#define WP_OPTION_ 32
|
||||
//#define WP_OPTION_ 64
|
||||
#define WP_OPTION_NEXT_CMD 128
|
||||
|
||||
//repeating events
|
||||
#define NO_REPEAT 0
|
||||
#define CH_5_TOGGLE 1
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
Make sure you update the libraries from the arducopter trunk and the latest code from the ardupilotmega branch.
|
||||
I have a lot of setup commands now. I may reduce and consolidate these soon.
|
||||
|
||||
Radio setup:
|
||||
ch1 = roll
|
||||
@ -9,16 +8,16 @@ ch4 = yaw
|
||||
ch5 = mode switch - use your 3 position switch
|
||||
ch6 = used for in-air tuning - not currently active, look for "debugging with Channel 6" in radio.pde to enable
|
||||
ch7 = use to set throttle hold value while hovering (quick toggle), hold to trim in air values - don't use your radio's trims!
|
||||
ch8 = not used - this is the hardware manual - it's dangerous to use for quads
|
||||
ch8 = NOT used!!! - this is the hardware manual - it's dangerous to use for quads BEWARE!!!
|
||||
|
||||
CLI interactive setup - You must go through each item and set the values to match your hardware
|
||||
|
||||
"setup" menu:
|
||||
erase - run this first! erases bad values from EEPROMS just in case
|
||||
erase - when installing ACM for the first time, run this to erases bad values from EEPROMS – just in case
|
||||
reset - Performs factory reset and initialization of EEPROM values
|
||||
radio - records the limits of ALL radio channels - very important!
|
||||
pid - restores default PID values
|
||||
frame - sets your frame config: [x, +, tri]
|
||||
radio - records the limits of ALL radio channels - very important!!!
|
||||
pid - restores default PID values if you have changed them.
|
||||
frame - sets your frame config: [x, +, tri, hexa, y6]
|
||||
motors - interactive setup of your ESC and motors, enter this mode, then plug-in battery,
|
||||
point at motors to make them spin
|
||||
throttle will output full range to each motor - this is good for esc calibration
|
||||
@ -26,50 +25,56 @@ level - sets initial value of accelerometers - hold copter level
|
||||
modes - sets the flight modes assigned to each switch position (you have 5 available)
|
||||
current - enables an Attopilot current sensor: [on, off, milliamp hours]
|
||||
compass - enables the compass [on, off]
|
||||
mag_offset - interactive mode to set the limits of your compass
|
||||
declination - look up your declination online for accuracy [decimal degrees]
|
||||
declination - sets your local declination value – lookup online for accuracy [decimal degrees]
|
||||
sonar - Sonar hooks to the "pitot" port which is an analog input [on, off]
|
||||
show - a formatted output of all the settings
|
||||
|
||||
"test" menu:
|
||||
|
||||
pwm - outputs the pwm values of all 8 radio channels
|
||||
radio - outputs the control values of all 8 radio channels in degrees * 100 or other value (see radio.pde)
|
||||
stabilize - lets you test the stabilization with debugging output to the terminal - may spin the props
|
||||
fbw - developers only
|
||||
stabilize - lets you test the stabilization with debugging output to the terminal – may spin the props – not recommended for flying.
|
||||
gps - outputs GPS data
|
||||
rawgps - outputs raw, unparsed GPS data
|
||||
adc - outputs raw adc values
|
||||
imu - outputs euler angles
|
||||
dcm - outputs DCM matrix values
|
||||
battery - outputs voltage readings to analog in 0-3
|
||||
current - outputs voltage and current from an AttoPilot current sensor
|
||||
relay - toggles the relay
|
||||
sonar - outputs sonar data in cm
|
||||
waypoints - dumps stored waypoint commands
|
||||
airpressure - raw output of absolute presure sensor
|
||||
compass - outputs compass angles in degrees * 100
|
||||
compass - outputs compass angles in degrees (0 = north)
|
||||
xbee - outputs an XBEE sequence used for range testing
|
||||
mission - writes a default mission to EEPROM [null, 'wp']
|
||||
- choosing 'wp' option will send the copter 15 meters North and back again.
|
||||
eedump - raw output of bytes in eeprom
|
||||
|
||||
Flight modes:
|
||||
stabilize - copter will hold -45 to 45° angle, throttle is manual.
|
||||
Alt_hold - You need to set your g. value by toggling ch_7 for less than 1 second. (Mine is 330),
|
||||
altitude is controlled by the throttle lever using absolute position - from 0 to 40 meters.
|
||||
this control method will change after testing.
|
||||
FBW - Simulates GPS Hold with the sticks being the position offset. Manual Throttle.
|
||||
position_hold
|
||||
- When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden temporarily with the radio.
|
||||
"logs" Menu:
|
||||
See the APM wiki to better understand how to dump logs and how to set the types of data you want to record.
|
||||
|
||||
|
||||
ACM Flight modes:
|
||||
Set these up in 'setup'/'modes'. Use your three position switch (channel 5) to select. Change the setting with your roll (Aileron) stick. Hit enter to save.
|
||||
All of the modes allow the user to control the copter with the normal controls. You can get yourself out of a jam sometimes by simply nudging the copter while in AUTO or LOITER modes.
|
||||
Options include:
|
||||
ACRO - rate control only. not for beginners
|
||||
STABILIZE - copter will hold -45 to 45° angle, throttle is manual.
|
||||
SIMPLE - Remembers the orientation of the copter when first entering SIMPLE mode, allowing the user to fly more intuitivey. Manual Throttle.
|
||||
ALT_HOLD - You need to set your throttle_cruise value by toggling ch_7 for less than 1 second. (Mine is 330 out of 1000),
|
||||
altitude is controlled by the throttle lever. Middle is hold, high = rise, low = fall.
|
||||
LOITER - When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden temporarily with the radio.
|
||||
altitude is controlled by the throttle lever. Middle is hold, high = rise, low = fall.
|
||||
RTL - Will try and fly back to home at the current altitude.
|
||||
Auto - Will fly the mission loaded by the Waypoint writer
|
||||
AUTO - Will fly the mission loaded by the Waypoint writer
|
||||
GCS_AUTO - A future mode where the copter can be flown interactively from the GCS
|
||||
|
||||
|
||||
what's new to commands for ACM:
|
||||
- CMD_ANGLE - will set the desired yaw with control of angle/second and direction.
|
||||
- CMD_ALTITUDE - will send the copter up from current position to desired altitude
|
||||
- CMD_TRACK_WAYPOINT - coming soon, will point the copter at the waypoint no matter the position
|
||||
|
||||
Special note:
|
||||
Any mode other than stabilize may cause the props to spin once the control switch is engaged.
|
||||
The props will NOT spin in stabilize when throttle is in the off position, even when armed.
|
||||
Arming is Yaw right for 1 sec, disarm is yaw left for 1 sec. Just give it some juice to confirm arming.
|
||||
|
||||
Auto modes will NOT engage until the throttle is above neutral. So if you put the control switch to positio hold while it's on the ground, it will no spin up. Or at leat it shouldn't ;)
|
||||
|
||||
Good luck,
|
||||
Jason
|
||||
|
||||
|
||||
@ -271,7 +271,8 @@ void startup_ground(void)
|
||||
GPS_enabled = true;
|
||||
break;
|
||||
}
|
||||
if (counter >= 4) {
|
||||
|
||||
if (counter >= 2) {
|
||||
GPS_enabled = false;
|
||||
break;
|
||||
}
|
||||
@ -314,23 +315,27 @@ void set_mode(byte mode)
|
||||
break;
|
||||
|
||||
case ALT_HOLD:
|
||||
init_throttle_cruise();
|
||||
do_loiter_at_location();
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
init_throttle_cruise();
|
||||
init_simple_bearing();
|
||||
init_auto();
|
||||
break;
|
||||
|
||||
case SIMPLE:
|
||||
//initial_simple_bearing = dcm.yaw_sensor;
|
||||
init_simple_bearing();
|
||||
break;
|
||||
|
||||
case LOITER:
|
||||
init_throttle_cruise();
|
||||
do_loiter_at_location();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
init_throttle_cruise();
|
||||
do_RTL();
|
||||
break;
|
||||
|
||||
@ -472,3 +477,12 @@ init_simple_bearing()
|
||||
//}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
init_throttle_cruise()
|
||||
{
|
||||
if(set_throttle_cruise_flag == false && g.rc_3.control_in > 250){
|
||||
set_throttle_cruise_flag = true;
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
}
|
||||
}
|
||||
@ -6,7 +6,6 @@ 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_fbw(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_gps(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);
|
||||
@ -48,7 +47,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"radio", test_radio},
|
||||
{"failsafe", test_failsafe},
|
||||
{"stabilize", test_stabilize},
|
||||
// {"fbw", test_fbw},
|
||||
{"gps", test_gps},
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
{"adc", test_adc},
|
||||
@ -317,100 +315,6 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
}
|
||||
}
|
||||
/*
|
||||
static int8_t
|
||||
test_fbw(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
static byte ts_num;
|
||||
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
// setup the radio
|
||||
// ---------------
|
||||
init_rc_in();
|
||||
|
||||
control_mode = FBW;
|
||||
//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_armed = true;
|
||||
trim_radio();
|
||||
|
||||
nav_yaw = 8000;
|
||||
scaleLongDown = 1;
|
||||
|
||||
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
|
||||
medium_loopCounter = 0;
|
||||
}
|
||||
}
|
||||
// for trim features
|
||||
read_trim_switch();
|
||||
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
|
||||
// IMU
|
||||
// ---
|
||||
read_AHRS();
|
||||
update_trig();
|
||||
|
||||
|
||||
// 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 == 5){
|
||||
update_alt();
|
||||
// 10 hz
|
||||
ts_num = 0;
|
||||
g_gps->longitude = 0;
|
||||
g_gps->latitude = 0;
|
||||
calc_loiter_nav();
|
||||
|
||||
Serial.printf_P(PSTR("ys:%ld, WP.lat:%ld, WP.lng:%ld, n_lat:%ld, n_lon:%ld, nroll:%ld, npitch:%ld, pmax:%ld, \t- "),
|
||||
dcm.yaw_sensor,
|
||||
next_WP.lat,
|
||||
next_WP.lng,
|
||||
nav_lat,
|
||||
nav_lon,
|
||||
nav_roll,
|
||||
nav_pitch,
|
||||
(long)g.pitch_max);
|
||||
|
||||
print_motor_out();
|
||||
}
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
static int8_t
|
||||
@ -863,35 +767,35 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
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);
|
||||
{
|
||||
if(g.compass_enabled) {
|
||||
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
|
||||
|
||||
print_hit_enter();
|
||||
print_hit_enter();
|
||||
|
||||
while(1){
|
||||
delay(250);
|
||||
compass.read();
|
||||
compass.calculate(0,0);
|
||||
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);
|
||||
while(1){
|
||||
delay(250);
|
||||
compass.read();
|
||||
compass.calculate(0,0);
|
||||
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);
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
} else {
|
||||
Serial.printf_P(PSTR("Compass: "));
|
||||
print_enabled(false);
|
||||
return (0);
|
||||
}
|
||||
} else {
|
||||
Serial.printf_P(PSTR("Compass: "));
|
||||
print_enabled(false);
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
@ -938,18 +842,34 @@ test_mission(uint8_t argc, const Menu::arg *argv)
|
||||
{Location t = {MAV_CMD_NAV_TAKEOFF, 0, 0, 300, 0, 0};
|
||||
set_wp_with_index(t,1);}
|
||||
|
||||
// CMD opt time/s
|
||||
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 15, 0, 0, 0};
|
||||
set_wp_with_index(t,2);}
|
||||
if (!strcmp_P(argv[1].str, PSTR("wp"))) {
|
||||
|
||||
// CMD opt dir angle/deg deg/s relative
|
||||
{Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
|
||||
set_wp_with_index(t,3);}
|
||||
// CMD opt
|
||||
{Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0};
|
||||
set_wp_with_index(t,2);}
|
||||
// CMD opt
|
||||
{Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0};
|
||||
set_wp_with_index(t,3);}
|
||||
|
||||
// CMD opt
|
||||
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
|
||||
set_wp_with_index(t,4);}
|
||||
// CMD opt
|
||||
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
|
||||
set_wp_with_index(t,4);}
|
||||
|
||||
} else {
|
||||
//2250 = 25 meteres
|
||||
// CMD opt p1 //alt //NS //WE
|
||||
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 0, 300, 1100, 0};
|
||||
set_wp_with_index(t,2);}
|
||||
|
||||
// CMD opt dir angle/deg deg/s relative
|
||||
{Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
|
||||
set_wp_with_index(t,3);}
|
||||
|
||||
// CMD opt
|
||||
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
|
||||
set_wp_with_index(t,4);}
|
||||
|
||||
}
|
||||
|
||||
g.RTL_altitude.set_and_save(300);
|
||||
g.waypoint_total.set_and_save(4);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user