aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: more work

Browse Source
This commit is contained in:
Pat Hickey 2012-12-12 17:25:09 -08:00 committed by Andrew Tridgell
parent b90889dd11
commit 7af03127f6
11 changed files with 75 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/ArduCopter.pde
View File

@ -54,7 +54,9 @@
// Header includes
////////////////////////////////////////////////////////////////////////////////
// Libraries
#include <math.h>
#include <stdio.h>
#include <stdarg.h>
// Common dependencies
#include <AP_Common.h>
@ -65,6 +67,7 @@
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
// Application dependencies
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_GPS.h> // ArduPilot GPS library
@ -1175,7 +1178,7 @@ static void fifty_hz_loop()
// ----------
# if CONFIG_SONAR == ENABLED
if(g.sonar_enabled) {
sonar_alt = sonar.read();
sonar_alt = sonar->read();
}
#endif

14
ArduCopter/Attitude.pde
View File

@ -1007,7 +1007,7 @@ get_throttle_rate_stabilized(int16_t target_rate)
static float alt_rate = 0; // the desired climb rate in cm/s.
static uint32_t last_call_ms = 0;
float altitude_error;
float _altitude_error;
int16_t desired_rate;
int16_t alt_error_max;
uint32_t now = millis();
@ -1023,10 +1023,10 @@ get_throttle_rate_stabilized(int16_t target_rate)
alt_desired += alt_rate * 0.02;
alt_error_max = constrain(600-abs(target_rate),100,600);
altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max);
alt_desired = current_loc.alt + altitude_error;
_altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max);
alt_desired = current_loc.alt + _altitude_error;
desired_rate = g.pi_alt_hold.get_p(altitude_error);
desired_rate = g.pi_alt_hold.get_p(_altitude_error);
desired_rate = constrain(desired_rate, -200, 200) + target_rate;
desired_rate = constrain(desired_rate, -VELOCITY_MAX_Z, VELOCITY_MAX_Z); // TO-DO: replace constrains with ALTHOLD_MIN_CLIMB_RATE and ALTHOLD_MAX_CLIMB_RATE?
@ -1040,11 +1040,11 @@ get_throttle_rate_stabilized(int16_t target_rate)
static void
get_throttle_althold(int32_t target_alt, int16_t max_climb_rate)
{
int32_t altitude_error;
int32_t _altitude_error;
int16_t desired_rate;
altitude_error = target_alt - current_loc.alt;
desired_rate = g.pi_alt_hold.get_p(altitude_error);
_altitude_error = target_alt - current_loc.alt;
desired_rate = g.pi_alt_hold.get_p(_altitude_error);
desired_rate = constrain(desired_rate, ALTHOLD_MIN_CLIMB_RATE, max_climb_rate);
// call rate based throttle controller which will update accel based throttle controller targets

54
ArduCopter/GCS_Mavlink.pde
View File

@ -266,7 +266,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
mavlink_msg_hwstatus_send(
chan,
board_voltage(),
I2c.lockup_count());
hal.i2c->lockup_count());
}
#endif
@ -1090,14 +1090,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.param1 == 1 ||
packet.param2 == 1 ||
packet.param3 == 1) {
ins.init_accel(mavlink_delay, flash_leds);
ins.init_accel(flash_leds);
}
if (packet.param4 == 1) {
trim_radio();
}
if (packet.param5 == 1) {
// this blocks
ins.calibrate_accel(mavlink_delay, flash_leds, gcs_send_text_fmt, setup_wait_key);
ins.calibrate_accel(flash_leds, hal.console);
}
result = MAV_RESULT_ACCEPTED;
break;
@ -1750,7 +1750,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
v[5] = packet.chan6_raw;
v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw;
APM_RC.setHIL(v);
hal.rcin->set_overrides(v, 8);
break;
}
@ -2048,42 +2048,32 @@ GCS_MAVLINK::queued_waypoint_send()
* MAVLink to process packets while waiting for the initialisation to
* complete
*/
static void mavlink_delay(unsigned long t)
static void mavlink_delay_cb()
{
uint32_t tstart;
static uint32_t last_1hz, last_50hz, last_5s;
if (in_mavlink_delay) {
// this should never happen, but let's not tempt fate by
// letting the stack grow too much
delay(t);
return;
}
if (!gcs0.initialised) return;
in_mavlink_delay = true;
tstart = millis();
do {
uint32_t tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs_send_message(MSG_HEARTBEAT);
gcs_send_message(MSG_EXTENDED_STATUS1);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs_update();
gcs_data_stream_send();
}
if (tnow - last_5s > 5000) {
last_5s = tnow;
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
}
delay(1);
uint32_t tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs_send_message(MSG_HEARTBEAT);
gcs_send_message(MSG_EXTENDED_STATUS1);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs_update();
gcs_data_stream_send();
}
if (tnow - last_5s > 5000) {
last_5s = tnow;
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
}
#if USB_MUX_PIN > 0
check_usb_mux();
check_usb_mux();
#endif
} while (millis() - tstart < t);
in_mavlink_delay = false;
}

8
ArduCopter/Log.pde
View File

@ -156,7 +156,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
static void do_erase_logs(void)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs\n"));
DataFlash.EraseAll(mavlink_delay);
DataFlash.EraseAll();
gcs_send_text_P(SEVERITY_LOW, PSTR("Log erase complete\n"));
}
@ -228,7 +228,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
// print_latlon - prints an latitude or longitude value held in an int32_t
// probably this should be moved to AP_Common
void print_latlon(AL_HAL::BetterStream *s, int32_t lat_or_lon)
void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon)
{
int32_t dec_portion, frac_portion;
int32_t abs_lat_or_lon = labs(lat_or_lon);
@ -283,9 +283,9 @@ static void Log_Read_GPS()
cliSerial->printf_P(PSTR("GPS, %ld, %d, "),
(long)temp1, // 1 time
(int)temp2); // 2 sats
print_latlon(&Serial, temp3);
print_latlon(cliSerial, temp3);
cliSerial->print_P(PSTR(", "));
print_latlon(&Serial, temp4);
print_latlon(cliSerial, temp4);
cliSerial->printf_P(PSTR(", %4.4f, %4.4f, %d, %ld\n"),
temp5, // 5 gps alt
temp6, // 6 sensor alt

28
ArduCopter/commands.pde
View File

@ -32,24 +32,24 @@ static struct Location get_cmd_with_index(int i)
}else{
// we can load a command, we don't process it yet
// read WP position
uintptr_t mem = (WP_START_BYTE) + (i * WP_SIZE);
uint16_t mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem);
temp.id = hal.storage->read_byte(mem);
mem++;
temp.options = eeprom_read_byte((uint8_t*)mem);
temp.options = hal.storage->read_byte(mem);
mem++;
temp.p1 = eeprom_read_byte((uint8_t*)mem);
temp.p1 = hal.storage->read_byte(mem);
mem++;
temp.alt = eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative!
temp.alt = hal.storage->read_dword(mem); // alt is stored in CM! Alt is stored relative!
mem += 4;
temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
temp.lat = hal.storage->read_dword(mem); // lat is stored in decimal * 10,000,000
mem += 4;
temp.lng = eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
temp.lng = hal.storage->read_dword(mem); // lon is stored in decimal * 10,000,000
}
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
@ -81,24 +81,24 @@ static void set_cmd_with_index(struct Location temp, int i)
temp.id = MAV_CMD_NAV_WAYPOINT;
}
uintptr_t mem = WP_START_BYTE + (i * WP_SIZE);
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id);
hal.storage->write_byte(mem, temp.id);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.options);
hal.storage->write_byte(mem, temp.options);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.p1);
hal.storage->write_byte(mem, temp.p1);
mem++;
eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM!
hal.storage->write_dword(mem, temp.alt); // Alt is stored in CM!
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7
hal.storage->write_dword(mem, temp.lat); // Lat is stored in decimal degrees * 10^7
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7
hal.storage->write_dword(mem, temp.lng); // Long is stored in decimal degrees * 10^7
// Make sure our WP_total
if(g.command_total < (i+1))

4
ArduCopter/commands_logic.pde
View File

@ -782,8 +782,8 @@ static void do_set_servo()
// send output to channel
if (channel_num != 0xff) {
APM_RC.enable_out(channel_num);
APM_RC.OutputCh(channel_num, command_cond_queue.alt);
hal.rcout->enable_ch(channel_num);
hal.rcout->write(channel_num, command_cond_queue.alt);
}
}

3
ArduCopter/compat.h
View File

@ -11,5 +11,8 @@
/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
static void run_cli(AP_HAL::UARTDriver *port);
// XXX AP_HAL tofix - what is bitRead? temporarily disable.
#define bitRead(a,b) false
#endif // __COMPAT_H__

10
ArduCopter/compat.pde
View File

@ -25,11 +25,21 @@ void pinMode(uint8_t pin, uint8_t output)
hal.gpio->pinMode(pin, output);
}
void digitalWriteFast(uint8_t pin, uint8_t out)
{
hal.gpio->write(pin,out);
}
void digitalWrite(uint8_t pin, uint8_t out)
{
hal.gpio->write(pin,out);
}
uint8_t digitalReadFast(uint8_t pin)
{
return hal.gpio->read(pin);
}
uint8_t digitalRead(uint8_t pin)
{
return hal.gpio->read(pin);

4
ArduCopter/events.pde
View File

@ -111,9 +111,9 @@ static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
APM_RC.OutputCh(event_id, event_value); // send to Servos
hal.rcout->write(event_id, event_value); // send to Servos
} else {
APM_RC.OutputCh(event_id, event_undo_value);
hal.rcout->write(event_id, event_undo_value);
}
}

6
ArduCopter/limits.pde
View File

@ -43,10 +43,12 @@ static void limits_loop() {
lim_state = limits.state();
// Use limits channel to determine LIMITS_ENABLED or LIMITS_DISABLED state
if (lim_state != LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) < LIMITS_ENABLE_PWM) {
if (lim_state != LIMITS_DISABLED && limits.channel() !=0
&& hal.rcin->read(limits.channel()-1) < LIMITS_ENABLE_PWM) {
limits.set_state(LIMITS_DISABLED);
}
else if (lim_state == LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) >= LIMITS_ENABLE_PWM) {
else if (lim_state == LIMITS_DISABLED && limits.channel() !=0
&& hal.rcin->read(limits.channel()-1) >= LIMITS_ENABLE_PWM) {
limits.set_state(LIMITS_ENABLED);
}

4
ArduCopter/motors.pde
View File

@ -111,9 +111,9 @@ static void init_arm_motors()
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we arm
// the motors
cliSerial->set_blocking_writes(false);
hal.uartA->set_blocking_writes(false);
if (gcs3.initialised) {
Serial3.set_blocking_writes(false);
hal.uartC->set_blocking_writes(false);
}
#if COPTER_LEDS == ENABLED