Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

renamed some command variables to align with Arduplane

Browse Source 
reworked the arming code and moved the DCM gains out.
updated climb_rate to include sonar data.
This commit is contained in:
Jason Short 2011-11-04 21:41:51 -07:00
parent 5f85c0dc52
commit 80c7e135d5
15 changed files with 239 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/APM_Config.h
View File

@ -39,7 +39,7 @@
CH7_SAVE_WP CH7_SAVE_WP
*/ */
#define ACCEL_ALT_HOLD 1 // disabled by default, work in progress #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress
// See the config.h and defines.h files for how to set this up! // See the config.h and defines.h files for how to set this up!
// //

41
ArduCopter/ArduCopter.pde
View File

@ -303,7 +303,6 @@ static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static byte wp_control; // used to control - navgation or loiter static byte wp_control; // used to control - navgation or loiter
static byte command_must_index; // current command memory location static byte command_must_index; // current command memory location
@ -335,9 +334,6 @@ static int airspeed; // m/s * 100
// Location Errors // Location Errors
// --------------- // ---------------
static long altitude_error; // meters * 100 we are off in altitude
static long old_altitude;
static int old_rate;
static long yaw_error; // how off are we pointed static long yaw_error; // how off are we pointed
static long long_error, lat_error; // temp for debugging static long long_error, lat_error; // temp for debugging
@ -361,10 +357,20 @@ static int ground_temperature;
// Altitude Sensor variables // Altitude Sensor variables
// ---------------------- // ----------------------
static int sonar_alt;
static int baro_alt;
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int altitude_rate; static long altitude_error; // meters * 100 we are off in altitude
static int climb_rate; // m/s * 100
static int sonar_alt;
static int old_sonar_alt;
static int sonar_rate;
static int baro_alt;
static int old_baro_alt;
static int baro_rate;
// flight mode specific // flight mode specific
// -------------------- // --------------------
@ -1206,6 +1212,11 @@ static void update_altitude()
return; return;
#else #else
// calc the vertical accel rate
int temp_alt = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale
baro_rate = (temp_alt - old_baro_alt) * 10;
old_baro_alt = temp_alt;
if(g.sonar_enabled){ if(g.sonar_enabled){
// filter out offset // filter out offset
float scale; float scale;
@ -1213,6 +1224,10 @@ static void update_altitude()
// read barometer // read barometer
baro_alt = (baro_alt + read_barometer()) >> 1; baro_alt = (baro_alt + read_barometer()) >> 1;
// calc rate of change for Sonar
sonar_rate = (sonar_alt - old_sonar_alt) * 10;
old_sonar_alt = sonar_alt;
if(baro_alt < 1000){ if(baro_alt < 1000){
#if SONAR_TILT_CORRECTION == 1 #if SONAR_TILT_CORRECTION == 1
@ -1224,21 +1239,23 @@ static void update_altitude()
scale = (sonar_alt - 400) / 200; scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1); scale = constrain(scale, 0, 1);
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
}else{ }else{
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + home.alt;
climb_rate = baro_rate;
} }
}else{ }else{
// No Sonar Case
baro_alt = read_barometer(); baro_alt = read_barometer();
// no sonar altitude
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + home.alt;
climb_rate = baro_rate;
} }
// calc the vertical accel rate
int temp_rate = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale
altitude_rate = (temp_rate - old_rate) * 10;
old_rate = temp_rate;
#endif #endif
} }

2
ArduCopter/Attitude.pde
View File

@ -91,7 +91,7 @@ get_nav_throttle(long z_error)
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
rate_error = rate_error - altitude_rate; rate_error = rate_error - climb_rate;
// limit the rate // limit the rate
rate_error = constrain(rate_error, -80, 140); rate_error = constrain(rate_error, -80, 140);

20
ArduCopter/GCS_Mavlink.pde
View File

@ -273,7 +273,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{ {
mavlink_msg_waypoint_current_send( mavlink_msg_waypoint_current_send(
chan, chan,
g.waypoint_index); g.command_index);
} }
static void NOINLINE send_statustext(mavlink_channel_t chan) static void NOINLINE send_statustext(mavlink_channel_t chan)
@ -579,7 +579,7 @@ GCS_MAVLINK::update(void)
uint32_t tnow = millis(); uint32_t tnow = millis();
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total && waypoint_request_i <= (unsigned)g.command_total &&
tnow > waypoint_timelast_request + 500) { tnow > waypoint_timelast_request + 500) {
waypoint_timelast_request = tnow; waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT); send_message(MSG_NEXT_WAYPOINT);
@ -947,7 +947,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_waypoint_count_send( mavlink_msg_waypoint_count_send(
chan,msg->sysid, chan,msg->sysid,
msg->compid, msg->compid,
g.waypoint_total + 1); // + home g.command_total + 1); // + home
waypoint_timelast_send = millis(); waypoint_timelast_send = millis();
waypoint_sending = true; waypoint_sending = true;
@ -974,7 +974,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
// send waypoint // send waypoint
tell_command = get_command_with_index(packet.seq); tell_command = get_cmd_with_index(packet.seq);
// set frame of waypoint // set frame of waypoint
uint8_t frame; uint8_t frame;
@ -990,7 +990,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// time that the mav should loiter in milliseconds // time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false) uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == (uint16_t)g.waypoint_index) if (packet.seq == (uint16_t)g.command_index)
current = 1; current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false) uint8_t autocontinue = 1; // 1 (true), 0 (false)
@ -1115,7 +1115,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// clear all waypoints // clear all waypoints
uint8_t type = 0; // ok (0), error(1) uint8_t type = 0; // ok (0), error(1)
g.waypoint_total.set_and_save(0); g.command_total.set_and_save(0);
// send acknowledgement 3 times to makes sure it is received // send acknowledgement 3 times to makes sure it is received
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
@ -1136,7 +1136,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// set current command // set current command
change_command(packet.seq); change_command(packet.seq);
mavlink_msg_waypoint_current_send(chan, g.waypoint_index); mavlink_msg_waypoint_current_send(chan, g.command_index);
break; break;
} }
@ -1153,7 +1153,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.count > MAX_WAYPOINTS) { if (packet.count > MAX_WAYPOINTS) {
packet.count = MAX_WAYPOINTS; packet.count = MAX_WAYPOINTS;
} }
g.waypoint_total.set_and_save(packet.count - 1); g.command_total.set_and_save(packet.count - 1);
waypoint_timelast_receive = millis(); waypoint_timelast_receive = millis();
waypoint_receiving = true; waypoint_receiving = true;
@ -1308,7 +1308,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_timelast_request = 0; waypoint_timelast_request = 0;
waypoint_request_i++; waypoint_request_i++;
if (waypoint_request_i > (uint16_t)g.waypoint_total){ if (waypoint_request_i > (uint16_t)g.command_total){
uint8_t type = 0; // ok (0), error(1) uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send( mavlink_msg_waypoint_ack_send(
@ -1643,7 +1643,7 @@ void
GCS_MAVLINK::queued_waypoint_send() GCS_MAVLINK::queued_waypoint_send()
{ {
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total) { waypoint_request_i <= (unsigned)g.command_total) {
mavlink_msg_waypoint_request_send( mavlink_msg_waypoint_request_send(
chan, chan,
waypoint_dest_sysid, waypoint_dest_sysid,

2
ArduCopter/Log.pde
View File

@ -762,7 +762,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp)
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG); DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(g.waypoint_total); DataFlash.WriteByte(g.command_total);
DataFlash.WriteByte(num); DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id); DataFlash.WriteByte(wp->id);

12
ArduCopter/Parameters.h
View File

@ -146,8 +146,8 @@ public:
// 220: Waypoint data // 220: Waypoint data
// //
k_param_waypoint_mode = 220, k_param_waypoint_mode = 220,
k_param_waypoint_total, k_param_command_total,
k_param_waypoint_index, k_param_command_index,
k_param_command_must_index, k_param_command_must_index,
k_param_waypoint_radius, k_param_waypoint_radius,
k_param_loiter_radius, k_param_loiter_radius,
@ -193,8 +193,8 @@ public:
// Waypoints // Waypoints
// //
AP_Int8 waypoint_mode; AP_Int8 waypoint_mode;
AP_Int8 waypoint_total; AP_Int8 command_total;
AP_Int8 waypoint_index; AP_Int8 command_index;
AP_Int8 command_must_index; AP_Int8 command_must_index;
AP_Int8 waypoint_radius; AP_Int8 waypoint_radius;
AP_Int8 loiter_radius; AP_Int8 loiter_radius;
@ -316,8 +316,8 @@ public:
low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")), low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")), waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), command_total (0, k_param_command_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), command_index (0, k_param_command_index, PSTR("WP_INDEX")),
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")), command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),

22
ArduCopter/commands.pde
View File

@ -3,7 +3,7 @@
static void init_commands() static void init_commands()
{ {
// zero is home, but we always load the next command (1), in the code. // zero is home, but we always load the next command (1), in the code.
g.waypoint_index = 0; g.command_index = 0;
// This are registers for the current may and must commands // This are registers for the current may and must commands
// setting to zero will allow them to be written to by new commands // setting to zero will allow them to be written to by new commands
@ -22,13 +22,13 @@ static void clear_command_queue(){
// Getters // Getters
// ------- // -------
static struct Location get_command_with_index(int i) static struct Location get_cmd_with_index(int i)
{ {
struct Location temp; struct Location temp;
// Find out proper location in memory by using the start_byte position + the index // Find out proper location in memory by using the start_byte position + the index
// -------------------------------------------------------------------------------- // --------------------------------------------------------------------------------
if (i > g.waypoint_total) { if (i > g.command_total) {
// we do not have a valid command to load // we do not have a valid command to load
// return a WP with a "Blank" id // return a WP with a "Blank" id
temp.id = CMD_BLANK; temp.id = CMD_BLANK;
@ -77,7 +77,7 @@ static struct Location get_command_with_index(int i)
// ------- // -------
static void set_command_with_index(struct Location temp, int i) static void set_command_with_index(struct Location temp, int i)
{ {
i = constrain(i, 0, g.waypoint_total.get()); i = constrain(i, 0, g.command_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE); uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id); eeprom_write_byte((uint8_t *) mem, temp.id);
@ -100,18 +100,18 @@ static void set_command_with_index(struct Location temp, int i)
static void increment_WP_index() static void increment_WP_index()
{ {
if (g.waypoint_index < g.waypoint_total) { if (g.command_index < g.command_total) {
g.waypoint_index++; g.command_index++;
} }
SendDebugln(g.waypoint_index,DEC); SendDebugln(g.command_index,DEC);
} }
/* /*
static void decrement_WP_index() static void decrement_WP_index()
{ {
if (g.waypoint_index > 0) { if (g.command_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1); g.command_index.set_and_save(g.command_index - 1);
} }
}*/ }*/
@ -135,7 +135,7 @@ static Location get_LOITER_home_wp()
next_WP = current_loc; next_WP = current_loc;
// read home position // read home position
struct Location temp = get_command_with_index(0); // 0 = home struct Location temp = get_cmd_with_index(0); // 0 = home
temp.id = MAV_CMD_NAV_LOITER_UNLIM; temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold(); temp.alt = read_alt_to_hold();
return temp; return temp;
@ -149,7 +149,7 @@ It precalculates all the necessary stuff.
static void set_next_WP(struct Location *wp) static void set_next_WP(struct Location *wp)
{ {
//SendDebug("MSG <set_next_wp> wp_index: "); //SendDebug("MSG <set_next_wp> wp_index: ");
//SendDebugln(g.waypoint_index, DEC); //SendDebugln(g.command_index, DEC);
// copy the current WP into the OldWP slot // copy the current WP into the OldWP slot
// --------------------------------------- // ---------------------------------------

4
ArduCopter/commands_logic.pde
View File

@ -660,7 +660,7 @@ static void do_jump()
command_may_index = 0; command_may_index = 0;
// set pointer to desired index // set pointer to desired index
g.waypoint_index = next_command.p1 - 1; g.command_index = next_command.p1 - 1;
} else if (jump == 0){ } else if (jump == 0){
// we're done, move along // we're done, move along
@ -668,7 +668,7 @@ static void do_jump()
} else if (jump == -1) { } else if (jump == -1) {
// repeat forever // repeat forever
g.waypoint_index = next_command.p1 - 1; g.command_index = next_command.p1 - 1;
} }
} }

26
ArduCopter/commands_process.pde
View File

@ -2,22 +2,22 @@
// For changing active command mid-mission // For changing active command mid-mission
//---------------------------------------- //----------------------------------------
static void change_command(uint8_t index) static void change_command(uint8_t cmd_index)
{ {
struct Location temp = get_command_with_index(index); struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){ if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd")); gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
} else { } else {
command_must_index = NO_COMMAND; command_must_index = NO_COMMAND;
next_command.id = NO_COMMAND; next_command.id = NO_COMMAND;
g.waypoint_index = index - 1; g.command_index = cmd_index - 1;
update_commands(); update_commands();
} }
} }
// called by 10 Hz Medium loop // called by 10 Hz loop
// --------------------------- // --------------------
static void update_commands(void) static void update_commands(void)
{ {
// fill command queue with a new command if available // fill command queue with a new command if available
@ -25,10 +25,10 @@ static void update_commands(void)
// fetch next command if the next command queue is empty // fetch next command if the next command queue is empty
// ----------------------------------------------------- // -----------------------------------------------------
if (g.waypoint_index < g.waypoint_total) { if (g.command_index < g.command_total) {
// only if we have a cmd stored in EEPROM // only if we have a cmd stored in EEPROM
next_command = get_command_with_index(g.waypoint_index + 1); next_command = get_cmd_with_index(g.command_index + 1);
//Serial.printf("queue CMD %d\n", next_command.id); //Serial.printf("queue CMD %d\n", next_command.id);
} }
} }
@ -50,7 +50,7 @@ static void update_commands(void)
// We acted on the queue - let's debug that // We acted on the queue - let's debug that
// ---------------------------------------- // ----------------------------------------
print_wp(&next_command, g.waypoint_index); print_wp(&next_command, g.command_index);
// invalidate command queue so a new one is loaded // invalidate command queue so a new one is loaded
// ----------------------------------------------- // -----------------------------------------------
@ -88,11 +88,11 @@ process_next_command()
if (next_command.id < MAV_CMD_NAV_LAST ){ if (next_command.id < MAV_CMD_NAV_LAST ){
// we remember the index of our mission here // we remember the index of our mission here
command_must_index = g.waypoint_index + 1; command_must_index = g.command_index + 1;
// Save CMD to Log // Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command); Log_Write_Cmd(g.command_index + 1, &next_command);
// Act on the new command // Act on the new command
process_must(); process_must();
@ -106,7 +106,7 @@ process_next_command()
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
// we remember the index of our mission here // we remember the index of our mission here
command_may_index = g.waypoint_index + 1; command_may_index = g.command_index + 1;
//SendDebug("MSG <pnc> new may "); //SendDebug("MSG <pnc> new may ");
//SendDebugln(next_command.id,DEC); //SendDebugln(next_command.id,DEC);
@ -115,7 +115,7 @@ process_next_command()
// Save CMD to Log // Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command); Log_Write_Cmd(g.command_index + 1, &next_command);
process_may(); process_may();
return true; return true;
@ -128,7 +128,7 @@ process_next_command()
//SendDebugln(next_command.id,DEC); //SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index + 1, &next_command); Log_Write_Cmd(g.command_index + 1, &next_command);
process_now(); process_now();
return true; return true;
} }

2
ArduCopter/control_modes.pde
View File

@ -99,7 +99,7 @@ static void read_trim_switch()
// set the next_WP // set the next_WP
CH7_wp_index++; CH7_wp_index++;
current_loc.id = MAV_CMD_NAV_WAYPOINT; current_loc.id = MAV_CMD_NAV_WAYPOINT;
g.waypoint_total.set_and_save(CH7_wp_index); g.command_total.set_and_save(CH7_wp_index);
set_command_with_index(current_loc, CH7_wp_index); set_command_with_index(current_loc, CH7_wp_index);
} }
} }

133
ArduCopter/motors.pde
View File

@ -1,9 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define ARM_DELAY 10 // one second // 10 = 1 second
#define DISARM_DELAY 10 // one second #define ARM_DELAY 30
#define LEVEL_DELAY 70 // twelve seconds #define DISARM_DELAY 20
#define AUTO_LEVEL_DELAY 90 // twentyfive seconds #define LEVEL_DELAY 100
// called at 10hz // called at 10hz
@ -11,54 +11,76 @@ static void arm_motors()
{ {
static int arming_counter; static int arming_counter;
// Arm motor output : Throttle down and full yaw right for more than 2 seconds // don't allow arming/disarming in anything but manual
if (g.rc_3.control_in == 0){ if ((g.rc_3.control_in > 0) || (control_mode >= ALT_HOLD) || (arming_counter > LEVEL_DELAY)){
arming_counter = 0;
return;
}
// full right // full right
if (g.rc_4.control_in > 4000) { if (g.rc_4.control_in > 4000) {
if (arming_counter == LEVEL_DELAY){
// don't allow arming in anything but manual Serial.printf("\nAL\n");
if (control_mode < ALT_HOLD){ // begin auto leveling
auto_level_counter = 200;
if (arming_counter > AUTO_LEVEL_DELAY){
auto_level_counter = 155;
arming_counter = 0; arming_counter = 0;
}else if (arming_counter == ARM_DELAY){ }else if (arming_counter == ARM_DELAY){
if(motor_armed == false){
// arm the motors and configure for flight
init_arm_motors();
}
// keep going up
arming_counter++;
} else{
arming_counter++;
}
// full left
}else if (g.rc_4.control_in < -4000) {
if (arming_counter == LEVEL_DELAY){
Serial.printf("\nLEV\n");
// begin manual leveling
imu.init_accel(mavlink_delay);
arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){
if(motor_armed == true){
// arm the motors and configure for flight
init_disarm_motors();
}
// keep going up
arming_counter++;
}else{
arming_counter++;
}
// Yaw is centered
}else{
arming_counter = 0;
}
}
static void init_arm_motors()
{
Serial.printf("\nARM\n");
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif #endif
motor_armed = true; motor_armed = true;
arming_counter = ARM_DELAY;
#if PIEZO_ARMING == 1 #if PIEZO_ARMING == 1
piezo_beep(); piezo_beep();
piezo_beep(); piezo_beep();
#endif #endif
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
//dcm.ki_roll_pitch(0.000006);
#endif
// tune down compass
// -----------------
dcm.kp_yaw(0.08);
dcm.ki_yaw(0);
// Remember Orientation // Remember Orientation
// -------------------- // --------------------
init_simple_bearing(); init_simple_bearing();
// init the Z damopener
// --------------------
#if ACCEL_ALT_HOLD == 1
init_z_damper();
#endif
// Reset home position // Reset home position
// ---------------------- // ----------------------
if(home_is_set) if(home_is_set)
@ -79,64 +101,23 @@ static void arm_motors()
// temp hack // temp hack
motor_light = true; motor_light = true;
digitalWrite(A_LED_PIN, HIGH); digitalWrite(A_LED_PIN, HIGH);
arming_counter++;
} else{
arming_counter++;
}
} }
// full left
}else if (g.rc_4.control_in < -4000) {
//Serial.print(arming_counter, DEC);
if (arming_counter > LEVEL_DELAY){
//Serial.print("init");
imu.init_accel(mavlink_delay);
arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){ static void init_disarm_motors()
{
Serial.printf("\nDISARM\n");
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif #endif
motor_armed = false; motor_armed = false;
arming_counter = DISARM_DELAY;
compass.save_offsets(); compass.save_offsets();
#if PIEZO_ARMING == 1 #if PIEZO_ARMING == 1
piezo_beep(); piezo_beep();
#endif #endif
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
//dcm.kp_roll_pitch(0.12); // higher for fast recovery
//dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif
// tune up compass
// -----------------
dcm.kp_yaw(0.8);
dcm.ki_yaw(0.00004);
// Clear throttle slew
// -------------------
//throttle_slew = 0;
arming_counter++;
}else{
arming_counter++;
} }
// centered
}else{
arming_counter = 0;
}
}else{
arming_counter = 0;
}
}
/***************************************** /*****************************************
* Set the flight control servos based on the current calculated values * Set the flight control servos based on the current calculated values

6
ArduCopter/setup.pde
View File

@ -815,12 +815,12 @@ static void report_batt_monitor()
static void report_wp(byte index = 255) static void report_wp(byte index = 255)
{ {
if(index == 255){ if(index == 255){
for(byte i = 0; i <= g.waypoint_total; i++){ for(byte i = 0; i <= g.command_total; i++){
struct Location temp = get_command_with_index(i); struct Location temp = get_cmd_with_index(i);
print_wp(&temp, i); print_wp(&temp, i);
} }
}else{ }else{
struct Location temp = get_command_with_index(index); struct Location temp = get_cmd_with_index(index);
print_wp(&temp, index); print_wp(&temp, index);
} }
} }

14
ArduCopter/system.pde
View File

@ -289,6 +289,20 @@ static void init_ardupilot()
// --------------------------- // ---------------------------
reset_control_switch(); reset_control_switch();
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
dcm.kp_yaw(0.08);
dcm.ki_yaw(0.00004);
#endif
// init the Z damopener
// --------------------
#if ACCEL_ALT_HOLD == 1
init_z_damper();
#endif
startup_ground(); startup_ground();
Log_Write_Startup(); Log_Write_Startup();

30
ArduCopter/test.pde
View File

@ -6,7 +6,7 @@
// are defined below. Order matters to the compiler. // 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_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(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_failsafe(uint8_t argc, const Menu::arg *argv);
//static int8_t test_stabilize(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_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_tri(uint8_t argc, const Menu::arg *argv); static int8_t test_tri(uint8_t argc, const Menu::arg *argv);
@ -56,7 +56,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
const struct Menu::command test_menu_commands[] PROGMEM = { const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm}, {"pwm", test_radio_pwm},
{"radio", test_radio}, {"radio", test_radio},
{"failsafe", test_failsafe}, // {"failsafe", test_failsafe},
// {"stabilize", test_stabilize}, // {"stabilize", test_stabilize},
{"gps", test_gps}, {"gps", test_gps},
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
@ -246,6 +246,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
} }
} }
/*
static int8_t static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv) test_failsafe(uint8_t argc, const Menu::arg *argv)
{ {
@ -299,6 +300,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
return (0); return (0);
#endif #endif
} }
*/
/*static int8_t /*static int8_t
test_stabilize(uint8_t argc, const Menu::arg *argv) test_stabilize(uint8_t argc, const Menu::arg *argv)
@ -429,7 +431,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
while(1){ while(1){
//delay(20); //delay(20);
if (millis() - fast_loopTimer >=5) { if (millis() - fast_loopTimer >=20) {
// IMU // IMU
// --- // ---
@ -447,7 +449,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
update_trig(); update_trig();
} }
if(medium_loopCounter == 20){ if(medium_loopCounter == 1){
//read_radio(); //read_radio();
medium_loopCounter = 0; medium_loopCounter = 0;
//tuning(); //tuning();
@ -461,10 +463,13 @@ test_imu(uint8_t argc, const Menu::arg *argv)
dcm.kp_roll_pitch(), dcm.kp_roll_pitch(),
(float)g.rc_6.control_in / 2000.0); (float)g.rc_6.control_in / 2000.0);
*/ */
Serial.printf_P(PSTR("%ld, %ld, %ld\n"), Serial.printf_P(PSTR("%ld, %ld, %ld, | %ld, %ld, %ld\n"),
dcm.roll_sensor, dcm.roll_sensor,
dcm.pitch_sensor, dcm.pitch_sensor,
dcm.yaw_sensor); dcm.yaw_sensor,
(long)(degrees(omega.x) * 100.0),
(long)(degrees(omega.y) * 100.0),
(long)(degrees(omega.z) * 100.0));
if(g.compass_enabled){ if(g.compass_enabled){
compass.read(); // Read magnetometer compass.read(); // Read magnetometer
@ -740,7 +745,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("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 wp\n"), (int)g.waypoint_total); Serial.printf_P(PSTR("%d wp\n"), (int)g.command_total);
Serial.printf_P(PSTR("Hit rad: %d\n"), (int)g.waypoint_radius); Serial.printf_P(PSTR("Hit rad: %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);
@ -800,11 +805,12 @@ test_baro(uint8_t argc, const Menu::arg *argv)
delay(100); delay(100);
baro_alt = read_barometer(); baro_alt = read_barometer();
int temp_rate = (barometer._offset_press - barometer.RawPress) << 1; int temp_alt = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale
altitude_rate = (temp_rate - old_rate) * 10; baro_rate = (temp_alt - old_baro_alt) * 10;
old_rate = temp_rate; old_baro_alt = temp_alt;
// 1 2 3 4 5 1 2 3 4 5 // 1 2 3 4 5 1 2 3 4 5
Serial.printf_P(PSTR("Baro: %dcm, rate:%d, %ld, %ld, %d\n"), baro_alt, altitude_rate, barometer.RawTemp, barometer.RawPress, temp_rate); Serial.printf_P(PSTR("Baro: %dcm, rate:%d, %ld, %ld, %d\n"), baro_alt, climb_rate, barometer.RawTemp, barometer.RawPress, temp_alt);
//Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2); //Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2);
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -995,7 +1001,7 @@ test_mission(uint8_t argc, const Menu::arg *argv)
} }
g.RTL_altitude.set_and_save(300); g.RTL_altitude.set_and_save(300);
g.waypoint_total.set_and_save(4); g.command_total.set_and_save(4);
g.waypoint_radius.set_and_save(3); g.waypoint_radius.set_and_save(3);
test_wp(NULL, NULL); test_wp(NULL, NULL);

4
ArduPlane/commands_process.pde
View File

@ -5,20 +5,22 @@
static void change_command(uint8_t cmd_index) static void change_command(uint8_t cmd_index)
{ {
struct Location temp = get_cmd_with_index(cmd_index); struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){ if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else { } else {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index); gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND; nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND; next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1; nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index - 1); g.command_index.set_and_save(cmd_index - 1);
process_next_command(); process_next_command();
} }
} }
// called by 10 Hz loop // called by 10 Hz loop
// -------------------- // --------------------
static void update_commands(void) static void update_commands(void)