Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
James Goppert 2011-11-28 00:56:29 -05:00
commit 480f92294c
11 changed files with 105 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Makefile
View File

@ -22,7 +22,7 @@ apm2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DCLI_SLIDER_ENABLED=DISABLED"
mavlink10:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10 -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
make -f Makefile EXTRAFLAGS="-DMAVLINK10"
etags:
cd .. && etags -f ArduCopter/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduCopter libraries)

4
ArduPlane/GCS_Mavlink.pde
View File

@ -1071,6 +1071,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
result=1;
// clearing failsafe should not be needed
// here. Added based on some puzzling results in
// the simulator (tridge)
failsafe = FAILSAFE_NONE;
break;
case MAV_ACTION_STORAGE_READ:

2
ArduPlane/Makefile
View File

@ -22,7 +22,7 @@ apm2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
mavlink10:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10 -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
make -f Makefile EXTRAFLAGS="-DMAVLINK10"
etags:
cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)

1
Tools/autotest/arduplane.py
View File

@ -286,6 +286,7 @@ def fly_ArduPlane(viewerip=None):
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
failed = False
e = 'None'

4
Tools/autotest/autotest.py
View File

@ -53,6 +53,7 @@ def dump_logs(atype):
lognums.append(int(mavproxy.match.group(1)))
mavproxy.expect("Log]")
for i in range(numlogs):
print("Dumping log %u (i=%u)" % (lognums[i], i))
mavproxy.send("dump %u\n" % lognums[i])
mavproxy.expect("logs enabled:", timeout=400)
mavproxy.expect("Log]")
@ -259,12 +260,11 @@ def run_tests(steps):
if not passed:
print("FAILED %u tests: %s" % (len(failed), failed))
results.addglob("Google Earth track", '*.kml')
results.addglob("Google Earth track", '*.kmz')
results.addfile('Full Logs', 'autotest-output.txt')
results.addglob('DataFlash Log', '*.flashlog')
results.addglob("MAVLink log", '*.mavlog')
results.addglob("GPX track", '*.gpx')
results.addglob("KMZ track", '*.kmz')
results.addfile('ArduPlane build log', 'ArduPlane.txt')
results.addfile('ArduPlane code size', 'ArduPlane.sizes.txt')
results.addfile('ArduPlane stack sizes', 'ArduPlane.framesizes.txt')

2
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -212,7 +212,7 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
// division. That costs us 36 bytes of stack, but I think its
// worth it.
for (i = 0; i < 6; i++) {
result[i] = sum[i] / count[i];
result[i] = (sum[i] + (count[i]/2)) / count[i];
}

47
libraries/Desktop/support/sitl.cpp
View File

@ -41,7 +41,16 @@ struct ADC_UDR2 UDR2;
struct RC_ICR4 ICR4;
extern AP_TimerProcess timer_scheduler;
extern Arduino_Mega_ISR_Registry isr_registry;
static volatile uint32_t sim_input_count;
static volatile struct {
double latitude, longitude; // degrees
double altitude; // MSL
double heading; // degrees
double speedN, speedE; // m/s
double roll, pitch, yaw; // degrees
double airspeed; // m/s
uint32_t update_count;
} sim_state;
/*
@ -123,14 +132,19 @@ static void sitl_fgear_input(void)
return;
}
sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude,
ft2m(d.fg_pkt.altitude),
ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE), true);
sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading, kt2mps(d.fg_pkt.airspeed));
sitl_update_barometer(ft2m(d.fg_pkt.altitude));
sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading);
sim_state.latitude = d.fg_pkt.latitude;
sim_state.longitude = d.fg_pkt.longitude;
sim_state.altitude = ft2m(d.fg_pkt.altitude);
sim_state.speedN = ft2m(d.fg_pkt.speedN);
sim_state.speedE = ft2m(d.fg_pkt.speedE);
sim_state.roll = d.fg_pkt.rollDeg;
sim_state.pitch = d.fg_pkt.pitchDeg;
sim_state.yaw = d.fg_pkt.yawDeg;
sim_state.heading = d.fg_pkt.heading;
sim_state.airspeed = kt2mps(d.fg_pkt.airspeed);
sim_state.update_count++;
count++;
sim_input_count++;
if (millis() - last_report > 1000) {
printf("SIM %u FPS\n", count);
count = 0;
@ -239,6 +253,8 @@ static void sitl_simulator_output(void)
*/
static void timer_handler(int signum)
{
static uint32_t last_update_count;
/* make sure we die if our parent dies */
if (kill(parent_pid, 0) != 0) {
exit(1);
@ -258,9 +274,22 @@ static void timer_handler(int signum)
// send RC output to flight sim
sitl_simulator_output();
if (sim_input_count == 0) {
if (sim_state.update_count == 0) {
sitl_update_gps(0, 0, 0, 0, 0, false);
return;
}
if (sim_state.update_count == last_update_count) {
return;
}
last_update_count = sim_state.update_count;
sitl_update_gps(sim_state.latitude, sim_state.longitude,
sim_state.altitude,
sim_state.speedN, sim_state.speedE, true);
sitl_update_adc(sim_state.roll, sim_state.pitch, sim_state.heading, sim_state.airspeed);
sitl_update_barometer(sim_state.altitude);
sitl_update_compass(sim_state.heading, sim_state.roll, sim_state.pitch, sim_state.heading);
}

44
libraries/Desktop/support/sitl_adc.cpp
View File

@ -52,16 +52,21 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
const float _gyro_gain_y = 0.41;
const float _gyro_gain_z = 0.41;
const float _accel_scale = 9.80665 / 423.8;
uint16_t adc[7];
float adc[7];
float xAccel, yAccel, zAccel, scale;
float rollRate, pitchRate, yawRate;
static uint32_t last_update;
static float last_roll, last_pitch, last_yaw;
unsigned long delta_t;
// 200Hz max
if (micros() - last_update < 5000) {
return;
}
/* map roll/pitch/yaw to values the accelerometer would see */
xAccel = sin(ToRad(pitch));
yAccel = -sin(ToRad(roll));
xAccel = sin(ToRad(pitch)) * cos(ToRad(roll));
yAccel = -sin(ToRad(roll)) * cos(ToRad(pitch));
zAccel = -cos(ToRad(roll)) * cos(ToRad(pitch));
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel));
xAccel *= scale;
@ -76,12 +81,13 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
delta_t = micros();
} else {
delta_t = micros() - last_update;
rollRate = 1.0e6 * (roll - last_roll) / delta_t;
pitchRate = 1.0e6 * (pitch - last_pitch) / delta_t;
yawRate = 1.0e6 * (yaw - last_yaw) / delta_t;
rollRate = normalise(rollRate, -180, 180);
pitchRate = normalise(pitchRate, -180, 180);
yawRate = normalise(yawRate, -180, 180);
float rollChange, pitchChange, yawChange;
rollChange = normalise180(roll - last_roll);
pitchChange = normalise180(pitch - last_pitch);
yawChange = normalise180(yaw - last_yaw);
rollRate = 1.0e6 * rollChange / delta_t;
pitchRate = 1.0e6 * pitchChange / delta_t;
yawRate = 1.0e6 * yawChange / delta_t;
}
last_update += delta_t;
@ -100,20 +106,30 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
/* tell the UDR2 register emulation what values to give to the driver */
for (uint8_t i=0; i<6; i++) {
UDR2.set(sensor_map[i], (adc[i]<<3) & 0x7FFF);
UDR2.set(sensor_map[i], adc[i]);
}
// set the airspeed sensor input
UDR2.set(7, airspeed_sensor(airspeed)<<3);
UDR2.set(7, airspeed_sensor(airspeed));
/* FIX: rubbish value for temperature for now */
UDR2.set(3, 2000<<3);
UDR2.set(3, 2000);
#if 0
extern AP_DCM_HIL dcm;
dcm.setHil(ToRad(roll), ToRad(pitch), ToRad(yaw),
ToRad(rollRate), ToRad(pitchRate), ToRad(yawRate));
#endif
#if 0
extern AP_DCM dcm;
printf("roll=%6.1f / %6.1f pitch=%6.1f / %6.1f rollRate=%6.1f pitchRate=%6.1f\n",
roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, pitchRate);
Vector3f omega = dcm.get_gyro();
printf("dt=%5u adc[2]=%6.1f roll=%6.1f / %6.1f yaw=%6.1f / %6.1f yawRate=%6.3f / %6.3f\n",
(unsigned)delta_t,
adc[2],
roll, dcm.roll_sensor/100.0, yaw, dcm.yaw_sensor/100.0, yawRate, ToDeg(omega.z));
#endif
}

31
libraries/Desktop/support/sitl_adc.h
View File

@ -6,11 +6,13 @@
#ifndef _SITL_ADC_H
#define _SITL_ADC_H
#include <stdlib.h>
// this implements the UDR2 register
struct ADC_UDR2 {
uint16_t value, next_value;
uint8_t idx;
uint16_t channels[8];
float channels[8];
ADC_UDR2() {
// constructor
@ -26,19 +28,26 @@ struct ADC_UDR2 {
to output next
*/
ADC_UDR2& operator=(uint8_t cmd) {
float next_analog;
switch (cmd) {
case 0x87: next_value = channels[0]; break;
case 0xC7: next_value = channels[1]; break;
case 0x97: next_value = channels[2]; break;
case 0xD7: next_value = channels[3]; break;
case 0xA7: next_value = channels[4]; break;
case 0xE7: next_value = channels[5]; break;
case 0xB7: next_value = channels[6]; break;
case 0xF7: next_value = channels[7]; break;
case 0x87: next_analog = channels[0]; break;
case 0xC7: next_analog = channels[1]; break;
case 0x97: next_analog = channels[2]; break;
case 0xD7: next_analog = channels[3]; break;
case 0xA7: next_analog = channels[4]; break;
case 0xE7: next_analog = channels[5]; break;
case 0xB7: next_analog = channels[6]; break;
case 0xF7: next_analog = channels[7]; break;
case 0:
default: return *this;
}
if (cmd != 0) {
idx = 1;
int noise = (((unsigned long)random()) % 3) - 1;
next_value = (unsigned)(next_analog + noise + 0.5);
if (next_value >= 0x1000) {
next_value = 0xFFF;
}
next_value = (next_value << 3);
return *this;
}
@ -60,7 +69,7 @@ struct ADC_UDR2 {
/*
interface to set a channel value from SITL
*/
void set(uint8_t i, uint16_t v) {
void set(uint8_t i, float v) {
channels[i] = v;
}
};

5
libraries/Desktop/support/util.cpp
View File

@ -90,3 +90,8 @@ double normalise(double v, double min, double max)
}
return v;
}
double normalise180(double v)
{
return normalise(v, -180, 180);
}

1
libraries/Desktop/support/util.h
View File

@ -9,3 +9,4 @@ float swap_float(float f);
void swap_floats(float *f, unsigned count);
void set_nonblocking(int fd);
double normalise(double v, double min, double max);
double normalise180(double v);