zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Rover: more cleanups 

get rid of flight_mode_strings and cleanup unused code

switch to using ahrs for have_position
mission-4.1.18
Andrew Tridgell 12 years ago
parent
547f154ba8
commit
5768244671
10 changed files with 97 additions and 160 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 57
      APMrover2/APMrover2.pde
  2. 2
      APMrover2/Log.pde
  3. 43
      APMrover2/commands.pde
  4. 2
      APMrover2/commands_logic.pde
  5. 91
      APMrover2/control_modes.pde
  6. 25
      APMrover2/defines.h
  7. 4
      APMrover2/navigation.pde
  8. 2
      APMrover2/setup.pde
  9. 25
      APMrover2/system.pde
  10. 6
      APMrover2/test.pde

57
APMrover2/APMrover2.pde
Unescape View File

@ -316,36 +316,12 @@ static bool usb_connected; @@ -316,36 +316,12 @@ static bool usb_connected;
static const char *comma = ",";
static const char* flight_mode_strings[] = {
"Manual",
"",
"Learning",
"",
"",
"",
"",
"",
"",
"",
"Auto",
"RTL",
"",
"",
"",
"",
"",
"",
"",
"",
"",
""};
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
1 Steering
2 ---
3 Throttle
4 Rudder (if we have ailerons)
4 ---
5 Aux5
6 Aux6
7 Aux7
@ -416,6 +392,11 @@ static int32_t gps_base_alt; @@ -416,6 +392,11 @@ static int32_t gps_base_alt;
// Constants
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
// true if we have a position estimate from AHRS
static bool have_position;
// This is the currently calculated direction to fly.
// deg * 100 : 0 to 360
static int32_t nav_bearing;
@ -789,16 +770,7 @@ cliSerial->println(tempaccel.z, DEC); @@ -789,16 +770,7 @@ cliSerial->println(tempaccel.z, DEC);
//------------------------------------------------
case 1:
medium_loopCounter++;
if(g_gps->new_data){
g_gps->new_data = false;
// calculate the plane's desired bearing
// -------------------------------------
navigate();
}
navigate();
break;
// command processing
@ -922,9 +894,9 @@ static void update_GPS(void) @@ -922,9 +894,9 @@ static void update_GPS(void)
g_gps->update();
update_GPS_light();
if (g_gps->new_data && g_gps->fix) {
// for performance
// ---------------
have_position = ahrs.get_position(&current_loc);
if (g_gps->new_data && g_gps->status() == GPS::GPS_OK) {
gps_fix_count++;
if(ground_start_count > 1){
@ -947,11 +919,6 @@ static void update_GPS(void) @@ -947,11 +919,6 @@ static void update_GPS(void)
ground_start_count = 0;
}
}
current_loc.lng = g_gps->longitude; // Lon * 10**7
current_loc.lat = g_gps->latitude; // Lat * 10**7
current_loc.alt = max((g_gps->altitude - home.alt),0);
ground_speed = g_gps->ground_speed;
}
}

2
APMrover2/Log.pde
Unescape View File

@ -517,7 +517,7 @@ static void Log_Read_Attitude() @@ -517,7 +517,7 @@ static void Log_Read_Attitude()
static void Log_Read_Mode()
{
cliSerial->printf_P(PSTR("MOD: "));
cliSerial->println(flight_mode_strings[DataFlash.ReadByte()]);
print_flight_mode(DataFlash.ReadByte());
}
// Read a GPS packet

43
APMrover2/commands.pde
Unescape View File

@ -155,40 +155,21 @@ static void set_guided_WP(void) @@ -155,40 +155,21 @@ static void set_guided_WP(void)
// -------------------------------
void init_home()
{
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
if (!have_position) {
// we need position information
return;
}
// block until we get a good fix
// -----------------------------
while (!g_gps->new_data || !g_gps->fix) {
g_gps->update();
}
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
home.id = MAV_CMD_NAV_WAYPOINT;
#if HIL_MODE != HIL_MODE_ATTITUDE
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
gps_base_alt = max(g_gps->altitude, 0);
home.alt = g_gps->altitude;;
// Home is always 0
#else
// struct Location temp = get_cmd_with_index(0); // JLN update - for HIL test only get the home param stored in the FPL
// if (temp.alt > 0) {
// home.lng = temp.lng; // Lon * 10**7
// home.lat = temp.lat; // Lat * 10**7
// } else {
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
// }
gps_base_alt = g_gps->altitude;; // get the stored home altitude as the base ref for AGL calculation.
home.alt = g_gps->altitude;;
#endif
gps_base_alt = max(g_gps->altitude, 0);
home.alt = g_gps->altitude;
home_is_set = true;
//gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
@ -204,9 +185,9 @@ void init_home() @@ -204,9 +185,9 @@ void init_home()
static void restart_nav()
{
reset_I();
prev_WP = current_loc;
nav_command_ID = NO_COMMAND;
nav_command_index = 0;
process_next_command();
reset_I();
prev_WP = current_loc;
nav_command_ID = NO_COMMAND;
nav_command_index = 0;
process_next_command();
}

2
APMrover2/commands_logic.pde
Unescape View File

@ -351,7 +351,7 @@ static void do_change_speed() @@ -351,7 +351,7 @@ static void do_change_speed()
static void do_set_home()
{
if(next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK) {
if(next_nonnav_command.p1 == 1 && have_position) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;

91
APMrover2/control_modes.pde
Unescape View File

@ -55,69 +55,62 @@ static void reset_control_switch() @@ -55,69 +55,62 @@ static void reset_control_switch()
// set this to your trainer switch
static void read_trim_switch()
{
if (g.ch7_option == CH7_SAVE_WP){ // set to 1
if (g.ch7_option == CH7_SAVE_WP) { // set to 1
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
} else { // switch is disengaged
if(trim_flag) {
trim_flag = false;
if(control_mode == MANUAL){ // if SW7 is ON in MANUAL = Erase the Flight Plan
// reset the mission
if (control_mode == MANUAL) {
// if SW7 is ON in MANUAL = Erase the Flight Plan
CH7_wp_index = 0;
g.command_total.set_and_save(CH7_wp_index);
g.command_total = 0;
g.command_index =0;
nav_command_index = 0;
if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
ground_start_count = 5;
#if X_PLANE == ENABLED
cliSerial->printf_P(PSTR("*** RESET the FPL\n"));
#endif
CH7_wp_index = 1;
g.command_total = 0;
g.command_index =0;
nav_command_index = 0;
if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
ground_start_count = 5;
CH7_wp_index = 1;
return;
} else if (control_mode == LEARNING) { // if SW7 is ON in LEARNING = record the Wp
// set the next_WP (home is stored at 0)
// max out at 100 since I think we need to stay under the EEPROM limit
CH7_wp_index = constrain(CH7_wp_index, 1, 100);
} else if (control_mode == LEARNING) {
// if SW7 is ON in LEARNING = record the Wp
// set the next_WP (home is stored at 0)
CH7_wp_index = constrain(CH7_wp_index, 1, MAX_WAYPOINTS);
current_loc.id = MAV_CMD_NAV_WAYPOINT;
current_loc.id = MAV_CMD_NAV_WAYPOINT;
// store the index
g.command_total.set_and_save(CH7_wp_index);
g.command_total = CH7_wp_index;
g.command_index = CH7_wp_index;
nav_command_index = 0;
// store the index
g.command_total.set_and_save(CH7_wp_index);
g.command_total = CH7_wp_index;
g.command_index = CH7_wp_index;
nav_command_index = 0;
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
#if X_PLANE == ENABLED
cliSerial->printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index);
#endif
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
// increment index
CH7_wp_index++;
// increment index
CH7_wp_index++;
} else if (control_mode == AUTO) { // if SW7 is ON in AUTO = set to RTL
set_mode(RTL);
}
}
}
}
else if (g.ch7_option == CH7_RTL){ // set to 6
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
} else if (control_mode == AUTO) {
// if SW7 is ON in AUTO = set to RTL
set_mode(RTL);
}
}
}
} else if (g.ch7_option == CH7_RTL) {
// set to 6
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
// switch is engaged
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
} else { // switch is disengaged
if (trim_flag) {
trim_flag = false;
if (control_mode == LEARNING) {
set_mode(RTL);
}
}
if (control_mode == LEARNING) {
set_mode(RTL);
}
}
}
}
}
}

25
APMrover2/defines.h
Unescape View File

@ -35,32 +35,7 @@ @@ -35,32 +35,7 @@
// CH 7 control
#define CH7_DO_NOTHING 0
#define CH7_SAVE_WP 1
#define CH7_LEO 2
#define CH7_THERMAL 3
#define CH7_SARSEC 4
#define CH7_SARGRID 5
#define CH7_RTL 6
#define CH7_TUNING 7
// CH_7 Tuning
// -----------
#define TUN_NONE 0
// Attitude
#define TUN_STABILIZE_KP 1
#define TUN_STABILIZE_KI 2
#define TUN_STABILIZE_KD 3
#define TUN_YAW_KP 4
#define TUN_YAW_KI 5
#define TUN_YAW_KD 6
#define TUN_STABROLL_KP 7
#define TUN_STABROLL_KI 8
#define TUN_STABROLL_KD 9
#define TUN_STABPITCH_KP 10
#define TUN_STABPITCH_KI 11
#define TUN_STABPITCH_KD 12
#define PITOT_SOURCE_ADC 1
#define PITOT_SOURCE_ANALOG_PIN 2
#define T6 1000000
#define T7 10000000

4
APMrover2/navigation.pde
Unescape View File

@ -7,9 +7,7 @@ static void navigate() @@ -7,9 +7,7 @@ static void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (g_gps->fix == 0)
{
g_gps->new_data = false;
if (!have_position) {
return;
}

2
APMrover2/setup.pde
Unescape View File

@ -552,7 +552,7 @@ static void @@ -552,7 +552,7 @@ static void
print_switch(byte p, byte m)
{
cliSerial->printf_P(PSTR("Pos %d: "),p);
cliSerial->println(flight_mode_strings[m]);
print_flight_mode(m);
}
static void

25
APMrover2/system.pde
Unescape View File

@ -453,7 +453,7 @@ static void startup_INS_ground(bool force_accel_level) @@ -453,7 +453,7 @@ static void startup_INS_ground(bool force_accel_level)
// when MANUAL_LEVEL is set to 1 we don't do accelerometer
// levelling on each boot, and instead rely on the user to do
// it once via the ground station
ins.init_accel(mavlink_delay, flash_leds);
ins.init_accel(mavlink_delay, flash_leds);
}
ahrs.set_fly_forward(true);
ahrs.reset();
@ -565,3 +565,26 @@ uint16_t board_voltage(void) @@ -565,3 +565,26 @@ uint16_t board_voltage(void)
return vcc.read_vcc();
}
#endif
static void
print_flight_mode(uint8_t mode)
{
switch (mode) {
case MANUAL:
cliSerial->println_P(PSTR("Manual"));
break;
case LEARNING:
cliSerial->println_P(PSTR("Learning"));
break;
case AUTO:
cliSerial->println_P(PSTR("AUTO"));
break;
case RTL:
cliSerial->println_P(PSTR("RTL"));
break;
default:
cliSerial->println_P(PSTR("---"));
break;
}
}

6
APMrover2/test.pde
Unescape View File

@ -226,15 +226,15 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) @@ -226,15 +226,15 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
fail_test++;
}
if(oldSwitchPosition != readSwitch()){
if (oldSwitchPosition != readSwitch()){
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
cliSerial->println(flight_mode_strings[readSwitch()]);
print_flight_mode(readSwitch());
fail_test++;
}
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
cliSerial->println(flight_mode_strings[readSwitch()]);
print_flight_mode(readSwitch());
fail_test++;
}

Write Preview
Loading…
Cancel
Save