zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

fix for Logging, motors_out buffer overrun, formatting. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1769 f9c3cf11-9bcb-44bc-f272-b75c42450872
master
jasonshort 14 years ago
parent
f52f27ef67
commit
90aa2d6d83
8 changed files with 178 additions and 165 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. 2
      ArduCopterMega/ArduCopterMega.pde
  2. 275
      ArduCopterMega/GCS_Mavlink.pde
  3. 4
      ArduCopterMega/Parameters.h
  4. 1
      ArduCopterMega/defines.h
  5. 21
      ArduCopterMega/motors.pde
  6. 1
      ArduCopterMega/setup.pde
  7. 12
      ArduCopterMega/system.pde
  8. 27
      ArduCopterMega/test.pde

2
ArduCopterMega/ArduCopterMega.pde
Unescape View File

@ -205,7 +205,7 @@ const char* flight_mode_strings[] = { @@ -205,7 +205,7 @@ const char* flight_mode_strings[] = {
// Radio
// -----
int motor_out[4];
int motor_out[8];
Vector3f omega;
// Failsafe

275
ArduCopterMega/GCS_Mavlink.pde
Unescape View File

@ -52,19 +52,13 @@ GCS_MAVLINK::update(void) @@ -52,19 +52,13 @@ GCS_MAVLINK::update(void)
_queued_send();
// stop waypoint sending if timeout
if (global_data.waypoint_sending &&
millis() - global_data.waypoint_timelast_send >
global_data.waypoint_send_timeout)
{
if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){
send_text(SEVERITY_LOW,"waypoint send timeout");
global_data.waypoint_sending = false;
}
// stop waypoint receiving if timeout
if (global_data.waypoint_receiving &&
millis() - global_data.waypoint_timelast_receive >
global_data.waypoint_receive_timeout)
{
if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){
send_text(SEVERITY_LOW,"waypoint receive timeout");
global_data.waypoint_receiving = false;
}
@ -74,7 +68,7 @@ void @@ -74,7 +68,7 @@ void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
{
if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax))
send_message(MSG_RAW_IMU);
send_message(MSG_RAW_IMU);
if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax)) {
send_message(MSG_EXTENDED_STATUS);
@ -84,7 +78,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) @@ -84,7 +78,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
}
if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax))
send_message(MSG_LOCATION);
send_message(MSG_LOCATION);
if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
@ -100,10 +94,9 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) @@ -100,10 +94,9 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
send_message(MSG_ATTITUDE);
if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) // Use Extra 3 for additional HIL info
send_message(MSG_VFR_HUD);
send_message(MSG_VFR_HUD);
if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax))
{
if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)){
// Available datastream
}
}
@ -146,14 +139,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -146,14 +139,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch(packet.req_stream_id)
{
case MAV_DATA_STREAM_ALL:
global_data.streamRateRawSensors = freq;
global_data.streamRateExtendedStatus = freq;
global_data.streamRateRCChannels = freq;
global_data.streamRateRawController = freq;
global_data.streamRatePosition = freq;
global_data.streamRateExtra1 = freq;
global_data.streamRateExtra2 = freq;
global_data.streamRateExtra3 = freq;
global_data.streamRateRawSensors = freq;
global_data.streamRateExtendedStatus = freq;
global_data.streamRateRCChannels = freq;
global_data.streamRateRawController = freq;
global_data.streamRatePosition = freq;
global_data.streamRateExtra1 = freq;
global_data.streamRateExtra2 = freq;
global_data.streamRateExtra3 = freq;
break;
case MAV_DATA_STREAM_RAW_SENSORS:
global_data.streamRateRawSensors = freq;
@ -197,89 +190,88 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -197,89 +190,88 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// do action
send_text(SEVERITY_LOW,"action received");
switch(packet.action)
{
case MAV_ACTION_LAUNCH:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_RETURN:
set_mode(RTL);
break;
case MAV_ACTION_EMCY_LAND:
//set_mode(LAND);
break;
case MAV_ACTION_HALT:
do_hold_position();
break;
// can't implement due to APM configuration
// just setting to manual to be safe
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
set_mode(ACRO);
break;
case MAV_ACTION_CONTINUE:
process_next_command();
break;
case MAV_ACTION_SET_MANUAL:
set_mode(ACRO);
break;
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
break;
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
break;
case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all();
break;
case MAV_ACTION_CALIBRATE_RC: break;
trim_radio();
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_ground();
break;
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
case MAV_ACTION_TAKEOFF:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
break;
case MAV_ACTION_LAND:
//set_mode(LAND);
break;
case MAV_ACTION_LOITER:
//set_mode(LOITER);
break;
default: break;
}
switch(packet.action){
case MAV_ACTION_LAUNCH:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_RETURN:
set_mode(RTL);
break;
case MAV_ACTION_EMCY_LAND:
//set_mode(LAND);
break;
case MAV_ACTION_HALT:
do_hold_position();
break;
// can't implement due to APM configuration
// just setting to manual to be safe
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
set_mode(ACRO);
break;
case MAV_ACTION_CONTINUE:
process_next_command();
break;
case MAV_ACTION_SET_MANUAL:
set_mode(ACRO);
break;
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
break;
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
break;
case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all();
break;
case MAV_ACTION_CALIBRATE_RC: break;
trim_radio();
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_ground();
break;
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
case MAV_ACTION_TAKEOFF:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
break;
case MAV_ACTION_LAND:
//set_mode(LAND);
break;
case MAV_ACTION_LOITER:
//set_mode(LOITER);
break;
default: break;
}
break;
}
@ -290,17 +282,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -290,17 +282,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_waypoint_request_list_t packet;
mavlink_msg_waypoint_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system,packet.target_component))
break;
// Start sending waypoints
mavlink_msg_waypoint_count_send(chan,msg->sysid,
msg->compid,g.waypoint_total + 1); // + home
global_data.waypoint_timelast_send = millis();
global_data.waypoint_sending = true;
global_data.waypoint_receiving = false;
global_data.waypoint_dest_sysid = msg->sysid;
global_data.waypoint_dest_compid = msg->compid;
global_data.requested_interface = chan;
mavlink_msg_waypoint_count_send(
chan,msg->sysid,
msg->compid,
g.waypoint_total + 1); // + home
global_data.waypoint_timelast_send = millis();
global_data.waypoint_sending = true;
global_data.waypoint_receiving = false;
global_data.waypoint_dest_sysid = msg->sysid;
global_data.waypoint_dest_compid = msg->compid;
global_data.requested_interface = chan;
break;
}
@ -444,9 +441,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -444,9 +441,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
g.waypoint_total.set_and_save(packet.count - 1);
global_data.waypoint_timelast_receive = millis();
global_data.waypoint_receiving = true;
global_data.waypoint_sending = false;
global_data.waypoint_request_i = 0;
global_data.waypoint_receiving = true;
global_data.waypoint_sending = false;
global_data.waypoint_request_i = 0;
break;
}
@ -520,7 +517,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -520,7 +517,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (global_data.waypoint_request_i > g.waypoint_total)
{
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
type);
send_text(SEVERITY_LOW,"flight plan received");
global_data.waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
@ -574,9 +577,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -574,9 +577,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
// Report back new value
mavlink_msg_param_value_send(chan, (int8_t *)key, packet.param_value,
_count_parameters(),
-1); // XXX we don't actually know what its index is...
mavlink_msg_param_value_send(
chan,
(int8_t *)key,
packet.param_value,
_count_parameters(),
-1); // XXX we don't actually know what its index is...
break;
} // end case
@ -748,14 +754,10 @@ GCS_MAVLINK::_queued_send() @@ -748,14 +754,10 @@ GCS_MAVLINK::_queued_send()
AP_Var *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
_queued_parameter = _queued_parameter->next();
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float();
if (!isnan(value)) {
@ -763,11 +765,12 @@ GCS_MAVLINK::_queued_send() @@ -763,11 +765,12 @@ GCS_MAVLINK::_queued_send()
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send(chan,
(int8_t*)param_name,
value,
_count_parameters(),
_queued_parameter_index);
mavlink_msg_param_value_send(
chan,
(int8_t*)param_name,
value,
_count_parameters(),
_queued_parameter_index);
_queued_parameter_index++;
break;
@ -783,12 +786,16 @@ GCS_MAVLINK::_queued_send() @@ -783,12 +786,16 @@ GCS_MAVLINK::_queued_send()
// request waypoints one by one
// XXX note that this is pan-interface
if (global_data.waypoint_receiving && global_data.requested_interface == chan &&
global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz
if (global_data.waypoint_receiving &&
(global_data.requested_interface == chan) &&
global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)) // limits to 3.33 hz
{
mavlink_msg_waypoint_request_send(chan,
global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid ,global_data.waypoint_request_i);
mavlink_msg_waypoint_request_send(
chan,
global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid,
global_data.waypoint_request_i);
mavdelay = 0;
}
}

4
ArduCopterMega/Parameters.h
Unescape View File

@ -239,12 +239,12 @@ public: @@ -239,12 +239,12 @@ public:
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")),
flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")),
flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")),
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")),
log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),

1
ArduCopterMega/defines.h
Unescape View File

@ -204,6 +204,7 @@ @@ -204,6 +204,7 @@
#define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
#define MASK_LOG_SET_DEFAULTS (1<<15)
// Waypoint Modes
// ----------------

21
ArduCopterMega/motors.pde
Unescape View File

@ -279,20 +279,21 @@ set_servos_4() @@ -279,20 +279,21 @@ set_servos_4()
motor_auto_safe = true;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
// fill the motor_out[] array for HIL use
for (unsigned char i=0; i<8; i++) {
motor_out[i] = 0;
for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
if (g.frame_type == HEXA_FRAME) {
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
}
// reset I terms of PID controls

1
ArduCopterMega/setup.pde
Unescape View File

@ -99,6 +99,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) @@ -99,6 +99,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
AP_Var::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
delay(1000);
default_log_bitmask();
default_gains();

12
ArduCopterMega/system.pde
Unescape View File

@ -141,7 +141,8 @@ void init_ardupilot() @@ -141,7 +141,8 @@ void init_ardupilot()
#endif
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
if(g.compass_enabled)
init_compass();
@ -199,6 +200,10 @@ void init_ardupilot() @@ -199,6 +200,10 @@ void init_ardupilot()
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
default_log_bitmask();
}
// set the correct flight mode
// ---------------------------
reset_control_switch();
@ -212,7 +217,7 @@ void startup_ground(void) @@ -212,7 +217,7 @@ void startup_ground(void)
gcs.send_text(SEVERITY_LOW,"<startup_ground> GROUND START");
#if(GROUND_START_DELAY > 0)
gcs.send_text(SEVERITY_LOW,"<startup_ground> With Delay");
//gcs.send_text(SEVERITY_LOW," With Delay");
delay(GROUND_START_DELAY * 1000);
#endif
@ -302,9 +307,6 @@ void set_mode(byte mode) @@ -302,9 +307,6 @@ void set_mode(byte mode)
// output control mode to the ground station
gcs.send_message(MSG_MODE_CHANGE);
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
void set_failsafe(boolean mode)

27
ArduCopterMega/test.pde
Unescape View File

@ -20,7 +20,7 @@ static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); @@ -20,7 +20,7 @@ static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(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_rawgps(uint8_t argc, const Menu::arg *argv);
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
@ -64,7 +64,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { @@ -64,7 +64,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"compass", test_mag},
{"xbee", test_xbee},
{"eedump", test_eedump},
{"rawgps", test_rawgps},
{"rawgps", test_rawgps},
};
// A Macro to create the Menu
@ -768,20 +768,20 @@ test_wp_print(struct Location *cmd, byte index) @@ -768,20 +768,20 @@ test_wp_print(struct Location *cmd, byte index)
static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
print_hit_enter();
delay(1000);
while(1){
if (Serial3.available())
Serial1.write(Serial3.read());
while(1){
if (Serial3.available())
Serial1.write(Serial3.read());
if (Serial1.available())
Serial3.write(Serial1.read());
if (Serial1.available())
Serial3.write(Serial1.read());
if(Serial.available() > 0){
if(Serial.available() > 0){
return (0);
}
}
}
}
}
static int8_t
@ -835,8 +835,9 @@ test_pressure(uint8_t argc, const Menu::arg *argv) @@ -835,8 +835,9 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
update_alt();
output_auto_throttle();
Serial.printf_P(PSTR("Alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
baro_alt,
sonar_alt,
next_WP.alt,
altitude_error,
(int)g.throttle_cruise,

Write Preview
Loading…
Cancel
Save