mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 01:18:29 -04:00
updates - support
new test mission - Liftoff, spin, land updated scripted Yaw control Public Beta candidate... git-svn-id: https://arducopter.googlecode.com/svn/trunk@1814 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
db3b7ba12a
commit
bcc1bf65f2
@ -5,10 +5,9 @@
|
||||
// GPS is auto-selected
|
||||
|
||||
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
//#define GCS_PORT 0
|
||||
|
||||
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK
|
||||
|
||||
# define SERIAL0_BAUD 38400
|
||||
#define SERIAL0_BAUD 38400
|
||||
|
||||
//# define STABILIZE_ROLL_P 0.4
|
||||
//# define STABILIZE_PITCH_P 0.4
|
||||
|
||||
@ -42,6 +42,8 @@ version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
#define MAVLINK_COMM_NUM_BUFFERS 2
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
//#include <GCS_SIMPLE.h>
|
||||
|
||||
|
||||
// Configuration
|
||||
#include "config.h"
|
||||
@ -175,6 +177,8 @@ GPS *g_gps;
|
||||
GCS_Class gcs;
|
||||
#endif
|
||||
|
||||
//GCS_SIMPLE gcs_simple(&Serial);
|
||||
|
||||
AP_RangeFinder_MaxsonarXL sonar;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -337,6 +341,7 @@ long command_yaw_end; // what angle are we trying to be
|
||||
long command_yaw_delta; // how many degrees will we turn
|
||||
int command_yaw_speed; // how fast to turn
|
||||
byte command_yaw_dir;
|
||||
byte command_yaw_relative;
|
||||
|
||||
// Waypoints
|
||||
// ---------
|
||||
@ -758,6 +763,19 @@ void super_slow_loop()
|
||||
gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz
|
||||
// gcs.send_message(MSG_CPU_LOAD, load*100);
|
||||
|
||||
//if(gcs_simple.read()){
|
||||
// Serial.print("!");
|
||||
/*
|
||||
Location temp;
|
||||
temp.id = gcs_simple.id;
|
||||
temp.p1 = gcs_simple.p1;
|
||||
temp.alt = gcs_simple.altitude;
|
||||
temp.lat = gcs_simple.latitude;
|
||||
temp.lng = gcs_simple.longitude;
|
||||
set_wp_with_index(temp, gcs_simple.index);
|
||||
gcs_simple.ack();
|
||||
*/
|
||||
//}
|
||||
}
|
||||
|
||||
void update_GPS(void)
|
||||
@ -1127,11 +1145,9 @@ void update_alt()
|
||||
// decide which sensor we're usings
|
||||
sonar_alt = sonar.read();
|
||||
|
||||
if(baro_alt < 550){
|
||||
if(baro_alt < 500 && sonar_alt < 600){
|
||||
altitude_sensor = SONAR;
|
||||
}
|
||||
|
||||
if(sonar_alt > 600){
|
||||
}else{
|
||||
altitude_sensor = BARO;
|
||||
}
|
||||
|
||||
@ -1162,4 +1178,5 @@ void update_alt()
|
||||
// Amount of throttle to apply for hovering
|
||||
// ----------------------------------------
|
||||
calc_nav_throttle();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -89,7 +89,6 @@ clear_yaw_control()
|
||||
void
|
||||
output_yaw_with_hold(boolean hold)
|
||||
{
|
||||
//digitalWrite(B_LED_PIN, LOW);
|
||||
if(hold){
|
||||
// look to see if we have exited rate control properly - ie stopped turning
|
||||
if(rate_yaw_flag){
|
||||
@ -101,8 +100,6 @@ output_yaw_with_hold(boolean hold)
|
||||
|
||||
}else{
|
||||
|
||||
//digitalWrite(B_LED_PIN, HIGH);
|
||||
|
||||
// return to rate control until we slow down.
|
||||
hold = false;
|
||||
//Serial.print("D");
|
||||
@ -119,7 +116,6 @@ output_yaw_with_hold(boolean hold)
|
||||
}
|
||||
|
||||
if(hold){
|
||||
//Serial.println("H");
|
||||
// try and hold the current nav_yaw setting
|
||||
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
@ -139,11 +135,9 @@ output_yaw_with_hold(boolean hold)
|
||||
|
||||
}else{
|
||||
|
||||
//Serial.println("R");
|
||||
// rate control
|
||||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
|
||||
long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
|
||||
// -error = CCW, +error = CW
|
||||
|
||||
@ -161,10 +155,9 @@ output_yaw_with_hold(boolean hold)
|
||||
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// slight left rudder give right roll.
|
||||
// slight left rudder gives right roll.
|
||||
|
||||
void
|
||||
output_rate_roll()
|
||||
|
||||
@ -16,7 +16,7 @@ B Checksum byte 2
|
||||
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
#if GCS_PORT == 3
|
||||
# define SendSerw Serial3.write
|
||||
# define SendSer Serial3.print
|
||||
@ -80,23 +80,15 @@ void flush(byte id)
|
||||
byte mess_ck_a = 0;
|
||||
byte mess_ck_b = 0;
|
||||
byte i;
|
||||
|
||||
|
||||
SendSer("4D"); // This is the message preamble
|
||||
SendSerw(buff_pointer); // Length
|
||||
SendSerw(2); // id
|
||||
//SendSerw(0x01); // Version
|
||||
|
||||
for (i = 0; i < buff_pointer; i++) {
|
||||
SendSerw(mess_buffer[i]);
|
||||
}
|
||||
|
||||
//for (i = 0; i < buff_pointer; i++) {
|
||||
// mess_ck_a += mess_buffer[i]; // Calculates checksums
|
||||
// mess_ck_b += mess_ck_a;
|
||||
//}
|
||||
|
||||
//SendSerw(mess_ck_a);
|
||||
//SendSerw(mess_ck_b);
|
||||
|
||||
buff_pointer = 0;
|
||||
}
|
||||
*/
|
||||
@ -34,8 +34,6 @@ GCS_MAVLINK::update(void)
|
||||
{
|
||||
uint8_t c = comm_receive_ch(chan);
|
||||
|
||||
|
||||
|
||||
// Try to get a new message
|
||||
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
|
||||
}
|
||||
@ -43,11 +41,6 @@ GCS_MAVLINK::update(void)
|
||||
// Update packet drops counter
|
||||
packet_drops += status.packet_rx_drop_count;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// send out queued params/ waypoints
|
||||
_queued_send();
|
||||
|
||||
|
||||
@ -22,10 +22,10 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
Serial.printf_P(PSTR("\n"
|
||||
"Commands:\n"
|
||||
" dump <n> dump log <n>\n"
|
||||
" erase erase all logs\n"
|
||||
" enable <name>|all enable logging <name> or everything\n"
|
||||
" disable <name>|all disable logging <name> or everything\n"
|
||||
" dump <n>"
|
||||
" erase (all logs)\n"
|
||||
" enable <name> | all\n"
|
||||
" disable <name> | all\n"
|
||||
"\n"));
|
||||
}
|
||||
|
||||
@ -75,13 +75,13 @@ print_log_menu(void)
|
||||
Serial.println();
|
||||
|
||||
if (last_log_num == 0) {
|
||||
Serial.printf_P(PSTR("\nNo logs available for download\n"));
|
||||
Serial.printf_P(PSTR("\nNo logs\n"));
|
||||
}else{
|
||||
|
||||
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
|
||||
Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
|
||||
for(int i=1;i<last_log_num+1;i++) {
|
||||
get_log_boundaries(last_log_num, i, log_start, log_end);
|
||||
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"),
|
||||
Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"),
|
||||
i, log_start, log_end);
|
||||
}
|
||||
Serial.println();
|
||||
@ -106,20 +106,20 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
|
||||
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
|
||||
Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"),
|
||||
dump_log,
|
||||
dump_log_start,
|
||||
dump_log_end);
|
||||
|
||||
Log_Read(dump_log_start, dump_log_end);
|
||||
Serial.printf_P(PSTR("Log read complete\n"));
|
||||
Serial.printf_P(PSTR("Complete\n"));
|
||||
}
|
||||
|
||||
static int8_t
|
||||
erase_logs(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
for(int i = 10 ; i > 0; i--) {
|
||||
Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i);
|
||||
Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds.\n"), i);
|
||||
delay(1000);
|
||||
}
|
||||
Serial.printf_P(PSTR("\nErasing log...\n"));
|
||||
@ -253,7 +253,7 @@ void start_new_log(byte num_existing_logs)
|
||||
DataFlash.FinishWrite();
|
||||
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
|
||||
}else{
|
||||
gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full - logging discontinued"));
|
||||
gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full"));
|
||||
}
|
||||
}
|
||||
|
||||
@ -708,7 +708,7 @@ void Log_Read(int start_page, int end_page)
|
||||
}
|
||||
page = DataFlash.GetPage();
|
||||
}
|
||||
Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count);
|
||||
Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -8,7 +8,6 @@
|
||||
uint16_t system_type = MAV_FIXED_WING;
|
||||
byte mavdelay = 0;
|
||||
|
||||
|
||||
// what does this do?
|
||||
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
||||
{
|
||||
@ -24,7 +23,6 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
|
||||
{
|
||||
uint64_t timeStamp = micros();
|
||||
|
||||
@ -23,22 +23,16 @@ Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reach
|
||||
***********************************
|
||||
Command ID Name Parameter 1 Altitude Latitude Longitude
|
||||
0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
|
||||
|
||||
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) altitude lat lon
|
||||
|
||||
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (forever) altitude lat lon
|
||||
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
|
||||
|
||||
0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
|
||||
|
||||
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
|
||||
|
||||
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
|
||||
|
||||
0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
|
||||
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
|
||||
|
||||
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
|
||||
|
||||
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
|
||||
|
||||
|
||||
***********************************
|
||||
May Commands - these commands are optional to finish
|
||||
|
||||
@ -40,6 +40,9 @@ struct Location get_wp_with_index(int i)
|
||||
mem = (WP_START_BYTE) + (i * WP_SIZE);
|
||||
temp.id = eeprom_read_byte((uint8_t*)mem);
|
||||
|
||||
mem++;
|
||||
temp.options = eeprom_read_byte((uint8_t*)mem);
|
||||
|
||||
mem++;
|
||||
temp.p1 = eeprom_read_byte((uint8_t*)mem);
|
||||
|
||||
@ -70,6 +73,9 @@ void set_wp_with_index(struct Location temp, int i)
|
||||
|
||||
eeprom_write_byte((uint8_t *) mem, temp.id);
|
||||
|
||||
mem++;
|
||||
eeprom_write_byte((uint8_t *) mem, temp.options);
|
||||
|
||||
mem++;
|
||||
eeprom_write_byte((uint8_t *) mem, temp.p1);
|
||||
|
||||
@ -194,7 +200,7 @@ void init_home()
|
||||
home.alt = max(g_gps->altitude, 0);
|
||||
home_is_set = true;
|
||||
|
||||
Serial.printf("gps alt: %ld", home.alt);
|
||||
//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
|
||||
|
||||
// Save Home to EEPROM
|
||||
// -------------------
|
||||
|
||||
@ -230,7 +230,8 @@ void do_land()
|
||||
velocity_land = 1000;
|
||||
|
||||
Location temp = current_loc;
|
||||
temp.alt = home.alt;
|
||||
//temp.alt = home.alt;
|
||||
temp.alt = -1000;
|
||||
|
||||
set_next_WP(&temp);
|
||||
}
|
||||
@ -269,17 +270,21 @@ bool verify_takeoff()
|
||||
|
||||
bool verify_land()
|
||||
{
|
||||
// XXX not sure
|
||||
|
||||
velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
|
||||
old_alt = current_loc.alt;
|
||||
|
||||
if((current_loc.alt < home.alt + 100) && velocity_land == 0){
|
||||
land_complete = true;
|
||||
return true;
|
||||
}
|
||||
if(g.sonar_enabled){
|
||||
// decide which sensor we're usings
|
||||
if(sonar_alt < 20){
|
||||
land_complete = true;
|
||||
return true;
|
||||
}
|
||||
} else {
|
||||
//land_complete = true;
|
||||
//return true;
|
||||
}
|
||||
|
||||
update_crosstrack();
|
||||
//update_crosstrack();
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -353,62 +358,67 @@ void do_within_distance()
|
||||
|
||||
void do_yaw()
|
||||
{
|
||||
// p1: bearing
|
||||
// alt: speed
|
||||
// lat: direction (-1,1),
|
||||
// lng: rel (1) abs (0)
|
||||
// { // CMD opt dir angle/deg deg/s relative
|
||||
// Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
|
||||
|
||||
|
||||
// target angle in degrees
|
||||
command_yaw_start = nav_yaw; // current position
|
||||
command_yaw_start_time = millis();
|
||||
|
||||
// which direction to turn
|
||||
// 1 = clockwise, -1 = counterclockwise
|
||||
command_yaw_dir = next_command.lat;
|
||||
command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise
|
||||
command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute
|
||||
|
||||
// 1 = Relative or 0 = Absolute
|
||||
if (next_command.lng == 1) {
|
||||
// relative
|
||||
command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
|
||||
command_yaw_end += nav_yaw;
|
||||
command_yaw_end = wrap_360(command_yaw_end);
|
||||
}else{
|
||||
// absolute
|
||||
command_yaw_end = next_command.p1 * 100;
|
||||
}
|
||||
command_yaw_speed = next_command.lat * 100;
|
||||
|
||||
|
||||
// if unspecified go 10° a second
|
||||
if(command_yaw_speed == 0)
|
||||
command_yaw_speed = 10;
|
||||
command_yaw_speed = 6000;
|
||||
|
||||
// if unspecified go clockwise
|
||||
// if unspecified go counterclockwise
|
||||
if(command_yaw_dir == 0)
|
||||
command_yaw_dir = 1;
|
||||
command_yaw_dir = -1;
|
||||
|
||||
// calculate the delta travel
|
||||
if(command_yaw_dir == 1){
|
||||
if(command_yaw_start > command_yaw_end){
|
||||
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
|
||||
}else{
|
||||
command_yaw_delta = command_yaw_end - command_yaw_start;
|
||||
}
|
||||
if (command_yaw_relative){
|
||||
// relative
|
||||
//command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
|
||||
//command_yaw_end += nav_yaw;
|
||||
//command_yaw_end = wrap_360(command_yaw_end);
|
||||
command_yaw_delta = next_command.alt * 100;
|
||||
}else{
|
||||
if(command_yaw_start > command_yaw_end){
|
||||
command_yaw_delta = command_yaw_start - command_yaw_end;
|
||||
}else{
|
||||
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
|
||||
}
|
||||
// absolute
|
||||
command_yaw_end = next_command.alt * 100;
|
||||
|
||||
// calculate the delta travel in deg * 100
|
||||
if(command_yaw_dir == 1){
|
||||
if(command_yaw_start >= command_yaw_end){
|
||||
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
|
||||
}else{
|
||||
command_yaw_delta = command_yaw_end - command_yaw_start;
|
||||
}
|
||||
}else{
|
||||
if(command_yaw_start > command_yaw_end){
|
||||
command_yaw_delta = command_yaw_start - command_yaw_end;
|
||||
}else{
|
||||
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
|
||||
}
|
||||
}
|
||||
command_yaw_delta = wrap_360(command_yaw_delta);
|
||||
}
|
||||
command_yaw_delta = wrap_360(command_yaw_delta);
|
||||
|
||||
|
||||
// rate to turn deg per second - default is ten
|
||||
command_yaw_speed = next_command.alt;
|
||||
command_yaw_time = command_yaw_delta / command_yaw_speed;
|
||||
command_yaw_time *= 1000;
|
||||
|
||||
|
||||
//
|
||||
//9000 turn in 10 seconds
|
||||
//command_yaw_time = 9000/ 10 = 900° per second
|
||||
}
|
||||
|
||||
|
||||
/********************************************************************************/
|
||||
// Verify Condition (May) commands
|
||||
/********************************************************************************/
|
||||
@ -452,13 +462,17 @@ bool verify_within_distance()
|
||||
bool verify_yaw()
|
||||
{
|
||||
if((millis() - command_yaw_start_time) > command_yaw_time){
|
||||
// time out
|
||||
nav_yaw = command_yaw_end;
|
||||
return true;
|
||||
|
||||
}else{
|
||||
// else we need to be at a certain place
|
||||
// power is a ratio of the time : .5 = half done
|
||||
float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
|
||||
|
||||
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
|
||||
nav_yaw = wrap_360(nav_yaw);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -121,7 +121,7 @@
|
||||
// GCS_PORT
|
||||
//
|
||||
#ifndef GCS_PROTOCOL
|
||||
# define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
@ -346,7 +346,7 @@
|
||||
// YAW Control
|
||||
//
|
||||
#ifndef YAW_P
|
||||
# define YAW_P 0.5
|
||||
# define YAW_P 0.25
|
||||
#endif
|
||||
#ifndef YAW_I
|
||||
# define YAW_I 0.0
|
||||
|
||||
@ -19,17 +19,17 @@ void read_control_switch()
|
||||
}
|
||||
|
||||
byte readSwitch(void){
|
||||
#if FLIGHT_MODE_CHANNEL == CH_5
|
||||
int pulsewidth = g.rc_5.radio_in; // default for Arducopter
|
||||
#elif FLIGHT_MODE_CHANNEL == CH_6
|
||||
int pulsewidth = g.rc_6.radio_in; //
|
||||
#elif FLIGHT_MODE_CHANNEL == CH_7
|
||||
int pulsewidth = g.rc_7.radio_in; //
|
||||
#elif FLIGHT_MODE_CHANNEL == CH_8
|
||||
int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
|
||||
#else
|
||||
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
|
||||
#endif
|
||||
#if FLIGHT_MODE_CHANNEL == CH_5
|
||||
int pulsewidth = g.rc_5.radio_in; // default for Arducopter
|
||||
#elif FLIGHT_MODE_CHANNEL == CH_6
|
||||
int pulsewidth = g.rc_6.radio_in; //
|
||||
#elif FLIGHT_MODE_CHANNEL == CH_7
|
||||
int pulsewidth = g.rc_7.radio_in; //
|
||||
#elif FLIGHT_MODE_CHANNEL == CH_8
|
||||
int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
|
||||
#else
|
||||
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
|
||||
#endif
|
||||
|
||||
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
|
||||
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
|
||||
@ -75,14 +75,14 @@ void read_trim_switch()
|
||||
if(trim_flag){
|
||||
// switch was just released
|
||||
if((millis() - trim_timer) > 2000){
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
imu.save();
|
||||
#endif
|
||||
#endif
|
||||
}else{
|
||||
// set the throttle nominal
|
||||
if(g.rc_3.control_in > 50){
|
||||
g.throttle_cruise.set(g.rc_3.control_in);
|
||||
Serial.printf("tnom %d\n", g.throttle_cruise.get());
|
||||
//Serial.printf("tnom %d\n", g.throttle_cruise.get());
|
||||
//save_EEPROM_throttle_cruise();
|
||||
g.throttle_cruise.save();
|
||||
|
||||
@ -114,7 +114,11 @@ void trim_accel()
|
||||
imu.ax(imu.ax() - 1);
|
||||
}
|
||||
|
||||
Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"), dcm.roll_sensor, dcm.pitch_sensor,
|
||||
(double)imu.ax(), (double)imu.ay(), (double)imu.az());
|
||||
/*Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"),
|
||||
dcm.roll_sensor,
|
||||
dcm.pitch_sensor,
|
||||
(double)imu.ax(),
|
||||
(double)imu.ay(),
|
||||
(double)imu.az());*/
|
||||
}
|
||||
|
||||
|
||||
@ -255,4 +255,4 @@
|
||||
#define EEPROM_MAX_ADDR 4096
|
||||
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
|
||||
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
|
||||
#define WP_SIZE 14
|
||||
#define WP_SIZE 15
|
||||
|
||||
@ -208,6 +208,7 @@ long get_altitude_above_home(void)
|
||||
return current_loc.alt - home.alt;
|
||||
}
|
||||
|
||||
// distance is returned in meters
|
||||
long get_distance(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
//if(loc1->lat == 0 || loc1->lng == 0)
|
||||
|
||||
@ -20,7 +20,7 @@ void init_barometer(void)
|
||||
ground_pressure = barometer.Press;
|
||||
ground_temperature = barometer.Temp;
|
||||
delay(20);
|
||||
Serial.printf("barometer.Press %ld\n", barometer.Press);
|
||||
//Serial.printf("barometer.Press %ld\n", barometer.Press);
|
||||
}
|
||||
|
||||
for(int i = 0; i < 30; i++){ // We take some readings...
|
||||
|
||||
@ -12,7 +12,6 @@ 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);
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
@ -31,7 +30,6 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
{"current", setup_current},
|
||||
{"sonar", setup_sonar},
|
||||
{"compass", setup_compass},
|
||||
// {"mag_offset", setup_mag_offset},
|
||||
{"declination", setup_declination},
|
||||
{"show", setup_show}
|
||||
};
|
||||
@ -44,12 +42,12 @@ int8_t
|
||||
setup_mode(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
// Give the user some guidance
|
||||
Serial.printf_P(PSTR("Setup Mode\n"
|
||||
"\n"
|
||||
"IMPORTANT: if you have not previously set this system up, use the\n"
|
||||
"'reset' command to initialize the EEPROM to sensible default values\n"
|
||||
"and then the 'radio' command to configure for your radio.\n"
|
||||
"\n"));
|
||||
Serial.printf_P(PSTR("Setup Mode\n"));
|
||||
//"\n"
|
||||
//"IMPORTANT: if you have not previously set this system up, use the\n"
|
||||
//"'reset' command to initialize the EEPROM to sensible default values\n"
|
||||
//"and then the 'radio' command to configure for your radio.\n"
|
||||
//"\n"));
|
||||
|
||||
// Run the setup menu. When the menu exits, we will return to the main menu.
|
||||
setup_menu.run();
|
||||
@ -88,7 +86,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
uint8_t i;
|
||||
int c;
|
||||
|
||||
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort:\n"));
|
||||
Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n"));
|
||||
|
||||
do {
|
||||
c = Serial.read();
|
||||
@ -97,7 +95,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
if (('y' != c) && ('Y' != c))
|
||||
return(-1);
|
||||
AP_Var::erase_all();
|
||||
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
|
||||
Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM"));
|
||||
|
||||
delay(1000);
|
||||
default_log_bitmask();
|
||||
@ -124,7 +122,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
if(g.rc_1.radio_in < 500){
|
||||
while(1){
|
||||
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
|
||||
//Serial.printf_P(PSTR("\nNo radio; Check connectors."));
|
||||
delay(1000);
|
||||
// stop here
|
||||
}
|
||||
@ -303,7 +301,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
setup_accel(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
|
||||
//Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
|
||||
|
||||
imu.init_accel();
|
||||
print_accel_offsets();
|
||||
@ -318,35 +316,52 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
|
||||
if (!strcmp_P(argv[1].str, PSTR("default"))) {
|
||||
default_gains();
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) {
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("stabilize"))) {
|
||||
g.pid_stabilize_roll.kP(argv[2].f);
|
||||
g.pid_stabilize_pitch.kP(argv[2].f);
|
||||
save_EEPROM_PID();
|
||||
g.stabilize_dampener.set_and_save(argv[3].f);
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
|
||||
g.stabilize_dampener = argv[2].f;
|
||||
save_EEPROM_PID();
|
||||
g.pid_stabilize_roll.save_gains();
|
||||
g.pid_stabilize_pitch.save_gains();
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) {
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("yaw"))) {
|
||||
g.pid_yaw.kP(argv[2].f);
|
||||
save_EEPROM_PID();
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("y_kd"))) {
|
||||
g.pid_yaw.kD(argv[2].f);
|
||||
save_EEPROM_PID();
|
||||
g.pid_yaw.save_gains();
|
||||
g.hold_yaw_dampener.set_and_save(argv[3].f);
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("t_kp"))) {
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("nav"))) {
|
||||
g.pid_nav_lat.kP(argv[2].f);
|
||||
g.pid_nav_lat.kI(argv[3].f);
|
||||
g.pid_nav_lat.imax(argv[4].i);
|
||||
|
||||
g.pid_nav_lon.kP(argv[2].f);
|
||||
g.pid_nav_lon.kI(argv[3].f);
|
||||
g.pid_nav_lon.imax(argv[4].i);
|
||||
|
||||
g.pid_nav_lon.save_gains();
|
||||
g.pid_nav_lat.save_gains();
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("baro"))) {
|
||||
g.pid_baro_throttle.kP(argv[2].f);
|
||||
save_EEPROM_PID();
|
||||
g.pid_baro_throttle.kI(argv[3].f);
|
||||
g.pid_baro_throttle.kD(0);
|
||||
g.pid_baro_throttle.imax(argv[4].i);
|
||||
|
||||
g.pid_baro_throttle.save_gains();
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("sonar"))) {
|
||||
g.pid_sonar_throttle.kP(argv[2].f);
|
||||
g.pid_sonar_throttle.kI(argv[3].f);
|
||||
g.pid_sonar_throttle.kD(argv[4].f);
|
||||
g.pid_sonar_throttle.imax(argv[5].i);
|
||||
|
||||
g.pid_sonar_throttle.save_gains();
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("t_kd"))) {
|
||||
g.pid_baro_throttle.kD(argv[2].f);
|
||||
save_EEPROM_PID();
|
||||
}else{
|
||||
default_gains();
|
||||
}
|
||||
|
||||
|
||||
report_gains();
|
||||
}
|
||||
|
||||
@ -379,7 +394,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
// look for stick input
|
||||
if (radio_input_switch() == true){
|
||||
if (radio_input_switch() == true){
|
||||
mode++;
|
||||
if(mode >= NUM_MODES)
|
||||
mode = 0;
|
||||
@ -408,6 +423,7 @@ setup_declination(uint8_t argc, const Menu::arg *argv)
|
||||
report_compass();
|
||||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
setup_erase(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
@ -747,11 +763,34 @@ default_gains()
|
||||
/***************************************************************************/
|
||||
// CLI reports
|
||||
/***************************************************************************/
|
||||
void report_wp(byte index = 255)
|
||||
{
|
||||
if(index == 255){
|
||||
for(byte i = 0; i <= g.waypoint_total; i++){
|
||||
struct Location temp = get_wp_with_index(i);
|
||||
print_wp(&temp, i);
|
||||
}
|
||||
}else{
|
||||
struct Location temp = get_wp_with_index(index);
|
||||
print_wp(&temp, index);
|
||||
}
|
||||
}
|
||||
|
||||
void print_wp(struct Location *cmd, byte index)
|
||||
{
|
||||
Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
|
||||
(int)index,
|
||||
(int)cmd->id,
|
||||
(int)cmd->p1,
|
||||
cmd->alt,
|
||||
cmd->lat,
|
||||
cmd->lng);
|
||||
}
|
||||
|
||||
void report_current()
|
||||
{
|
||||
read_EEPROM_current();
|
||||
Serial.printf_P(PSTR("Current Sensor\n"));
|
||||
Serial.printf_P(PSTR("Current \n"));
|
||||
print_divider();
|
||||
print_enabled(g.current_enabled.get());
|
||||
|
||||
@ -762,7 +801,7 @@ void report_current()
|
||||
void report_sonar()
|
||||
{
|
||||
g.sonar_enabled.load();
|
||||
Serial.printf_P(PSTR("Sonar Sensor\n"));
|
||||
Serial.printf_P(PSTR("Sonar\n"));
|
||||
print_divider();
|
||||
print_enabled(g.sonar_enabled.get());
|
||||
print_blanks(2);
|
||||
@ -822,8 +861,8 @@ void report_gains()
|
||||
Serial.printf_P(PSTR("yaw:\n"));
|
||||
print_PID(&g.pid_yaw);
|
||||
|
||||
Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), (float)g.stabilize_dampener);
|
||||
Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), (float)g.hold_yaw_dampener);
|
||||
Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener);
|
||||
Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);
|
||||
|
||||
// Nav
|
||||
Serial.printf_P(PSTR("Nav:\nlat:\n"));
|
||||
@ -839,7 +878,7 @@ void report_gains()
|
||||
|
||||
void report_xtrack()
|
||||
{
|
||||
Serial.printf_P(PSTR("Crosstrack\n"));
|
||||
Serial.printf_P(PSTR("XTrack\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
read_EEPROM_nav();
|
||||
@ -889,7 +928,7 @@ void report_compass()
|
||||
print_enabled(g.compass_enabled);
|
||||
|
||||
// mag declination
|
||||
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
|
||||
Serial.printf_P(PSTR("Mag Dec: %4.4f\n"),
|
||||
degrees(compass.get_declination()));
|
||||
|
||||
Vector3f offsets = compass.get_offsets();
|
||||
@ -920,12 +959,35 @@ void report_flight_modes()
|
||||
void
|
||||
print_PID(PID * pid)
|
||||
{
|
||||
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax());
|
||||
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
|
||||
pid->kP(),
|
||||
pid->kI(),
|
||||
pid->kD(),
|
||||
(long)pid->imax());
|
||||
}
|
||||
|
||||
void
|
||||
print_radio_values()
|
||||
{
|
||||
/*Serial.printf_P(PSTR( "CH1: %d | %d\n"
|
||||
"CH2: %d | %d\n"
|
||||
"CH3: %d | %d\n"
|
||||
"CH4: %d | %d\n"
|
||||
"CH5: %d | %d\n"
|
||||
"CH6: %d | %d\n"
|
||||
"CH7: %d | %d\n"
|
||||
"CH8: %d | %d\n"),
|
||||
g.rc_1.radio_min, g.rc_1.radio_max,
|
||||
g.rc_2.radio_min, g.rc_2.radio_max,
|
||||
g.rc_3.radio_min, g.rc_3.radio_max,
|
||||
g.rc_4.radio_min, g.rc_4.radio_max,
|
||||
g.rc_5.radio_min, g.rc_5.radio_max,
|
||||
g.rc_6.radio_min, g.rc_6.radio_max,
|
||||
g.rc_7.radio_min, g.rc_7.radio_max,
|
||||
g.rc_8.radio_min, g.rc_8.radio_max);*/
|
||||
|
||||
|
||||
///*
|
||||
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
|
||||
Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
|
||||
Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
|
||||
@ -934,6 +996,7 @@ print_radio_values()
|
||||
Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
|
||||
Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
|
||||
Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
|
||||
//*/
|
||||
}
|
||||
|
||||
void
|
||||
@ -967,7 +1030,8 @@ print_divider(void)
|
||||
Serial.println("");
|
||||
}
|
||||
|
||||
int8_t
|
||||
// read at 50Hz
|
||||
bool
|
||||
radio_input_switch(void)
|
||||
{
|
||||
static int8_t bouncer = 0;
|
||||
|
||||
@ -6,7 +6,7 @@ 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_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);
|
||||
@ -22,6 +22,7 @@ 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
|
||||
@ -47,7 +48,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"radio", test_radio},
|
||||
{"failsafe", test_failsafe},
|
||||
{"stabilize", test_stabilize},
|
||||
{"fbw", test_fbw},
|
||||
// {"fbw", test_fbw},
|
||||
{"gps", test_gps},
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
{"adc", test_adc},
|
||||
@ -67,6 +68,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"xbee", test_xbee},
|
||||
{"eedump", test_eedump},
|
||||
{"rawgps", test_rawgps},
|
||||
{"mission", test_mission},
|
||||
};
|
||||
|
||||
// A Macro to create the Menu
|
||||
@ -315,7 +317,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
static int8_t
|
||||
test_fbw(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
@ -408,6 +410,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
static int8_t
|
||||
@ -737,36 +740,22 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
|
||||
// save the alitude above home option
|
||||
Serial.printf_P(PSTR("Hold altitude "));
|
||||
if(g.RTL_altitude < 0){
|
||||
Serial.printf_P(PSTR("Hold current altitude\n"));
|
||||
Serial.printf_P(PSTR("\n"));
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude / 100);
|
||||
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);
|
||||
//Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
|
||||
|
||||
for(byte i = 0; i <= g.waypoint_total; i++){
|
||||
struct Location temp = get_wp_with_index(i);
|
||||
test_wp_print(&temp, i);
|
||||
}
|
||||
report_wp();
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
void
|
||||
test_wp_print(struct Location *cmd, byte index)
|
||||
{
|
||||
Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
|
||||
(int)index,
|
||||
(int)cmd->id,
|
||||
(int)cmd->p1,
|
||||
cmd->alt,
|
||||
cmd->lat,
|
||||
cmd->lng);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_rawgps(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
@ -925,6 +914,53 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
test_mission(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
//write out a basic mission to the EEPROM
|
||||
Location t;
|
||||
/*{
|
||||
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
|
||||
}*/
|
||||
|
||||
{
|
||||
Location t = {0, 0, 0, 0, 0, 0};
|
||||
set_wp_with_index(t,0);
|
||||
}
|
||||
|
||||
{ // CMD opt pitch alt/cm
|
||||
Location t = {MAV_CMD_NAV_TAKEOFF, 0, 0, 300, 0, 0};
|
||||
set_wp_with_index(t,1);
|
||||
}
|
||||
{ // CMD opt time/ms
|
||||
Location t = {MAV_CMD_CONDITION_DELAY, 0, 0, 0, 3000, 0};
|
||||
set_wp_with_index(t,2);
|
||||
|
||||
}
|
||||
{ // CMD opt dir angle/deg time/ms relative
|
||||
Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 1000, 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);
|
||||
g.waypoint_radius.set_and_save(3);
|
||||
|
||||
test_wp(NULL, NULL);
|
||||
|
||||
}
|
||||
|
||||
void print_hit_enter()
|
||||
{
|
||||
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user