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

AntennaTracker: notify armed after receiving vehicle position

Browse Source
This commit is contained in:
Randy Mackay 2014-09-29 19:40:25 +09:00
parent dbc1c03d49
commit 72053a8a74
3 changed files with 25 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
AntennaTracker/AntennaTracker.pde
View File

@ -258,7 +258,6 @@ void setup()
AP_Param::setup_sketch_defaults();
// arduplane does not use arming nor pre-arm checks
AP_Notify::flags.armed = true;
AP_Notify::flags.pre_arm_check = true;
AP_Notify::flags.failsafe_battery = false;
@ -296,6 +295,9 @@ static void one_second_loop()
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
// updated armed/disarmed status LEDs
update_armed_disarmed();
static uint8_t counter;
counter++;

7
AntennaTracker/config.h
View File

@ -55,6 +55,13 @@
# define SERIAL2_BUFSIZE 256
#endif
//////////////////////////////////////////////////////////////////////////////
// Tracking definitions
//
#ifndef TRACKING_TIMEOUT_MS
# define TRACKING_TIMEOUT_MS 5000 // consider we've lost track of vehicle after 5 seconds with no position update. Used to update armed/disarmed status leds
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//

16
AntennaTracker/tracking.pde
View File

@ -6,7 +6,8 @@
static struct {
Location location; // lat, long in degrees * 10^7; alt in meters * 100
int32_t relative_alt; // meters * 100
uint32_t last_update_us;
uint32_t last_update_us; // last position update in micxroseconds
uint32_t last_update_ms; // last position update in milliseconds
float heading; // degrees
float ground_speed; // m/s
} vehicle;
@ -392,6 +393,7 @@ static void tracking_update_position(const mavlink_global_position_int_t &msg)
vehicle.heading = msg.hdg * 0.01f;
vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
vehicle.last_update_us = hal.scheduler->micros();
vehicle.last_update_ms = hal.scheduler->millis();
}
@ -430,3 +432,15 @@ static void tracking_manual_control(const mavlink_manual_control_t &msg)
nav_status.manual_control_pitch = (msg.y != 0x7FFF);
// z, r and buttons are not used
}
/**
update_armed_disarmed - set armed LED if we have received a position update within the last 5 seconds
*/
static void update_armed_disarmed()
{
if (vehicle.last_update_ms != 0 && (hal.scheduler->millis() - vehicle.last_update_ms) < TRACKING_TIMEOUT_MS) {
AP_Notify::flags.armed = true;
} else {
AP_Notify::flags.armed = false;
}
}