int main(int argc, char const *argv[]) {
char strings[][73] = { "$GPGGA,152606.000,3732.7200,N,07726.9881,W,1,4,1.56,159.0,M,-33.6,M,,*64",
"$GPGSA,A,3,21,14,22,24,,,,,,,,,1.85,1.56,0.99*0C",
"$GPRMC,152606.000,A,3732.7200,N,07726.9881,W,0.29,81.28,110413,,,A*48",
"$GPVTG,81.28,T,,M,0.29,N,0.53,K,A*03",
"$PGTOP,11,2*6E" };
for (int i = 0; i < 5; i++) {
nmea_parse(strings[i], strlen(strings[i]));
}
// Validate state
within_range(get_latitude(), 37.545333, 0.00001f);
within_range(get_longitude(), -77.449802, 0.00001f);
within_range(get_altitude(), 159.0, 0.1f);
validate_int(get_sat_count(), 4);
validate_int(get_fix_type(), 3);
nmea_parser_t *parser = nmea_parser_new();
for (int i = 0; i < 5; i++) {
nmea_parse_r(strings[i], strlen(strings[i]), parser);
}
within_range(get_latitude_r(parser), 37.545333, 0.00001f);
within_range(get_longitude_r(parser), -77.449802, 0.00001f);
within_range(get_altitude_r(parser), 159.0, 0.1f);
validate_int(get_sat_count_r(parser), 4);
validate_int(get_fix_type_r(parser), 3);
nmea_parser_free(&parser);
assert(NULL == parser);
return error;
}
/*
* Updates our gps coordinate structure with the newest buffer
* data.
*/
void update_coordinates(char *buffer, uint32_t index)
{
char *latitude = get_latitude();
char *longitude = get_longitude();
char *altitude = get_altitude();
index = index%BUFFERSIZE;
// skip the time
index = _skip_data(buffer, index);
index = set_location(buffer, index, latitude, 10);
index = set_location(buffer, index, longitude, 11);
// skip the fix
index = _skip_data(buffer, index);
//sendIndex(index);
//index = (index+2)%BUFFERSIZE;
// get satalites tracked
index = set_satellites_tracked(buffer, index);
//sendIndex(index);
//sendIndex(index);
// skip Horizontal dilution of position
index = _skip_data(buffer, index);
// altitude
index = set_location(buffer, index, altitude, 15);
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (fabsf(_alt_offset - _alt_offset_active) > 0.1f) {
// if there's more than 10cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy
_alt_offset_active = (0.9f*_alt_offset_active) + (0.1f*_alt_offset);
} else {
_alt_offset_active = _alt_offset;
}
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = AP_HAL::millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure);
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (is_zero(sensors[i].ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset_active;
}
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
// return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate
float AP_Baro::get_EAS2TAS(void)
{
float altitude = get_altitude();
if ((fabsf(altitude - _last_altitude_EAS2TAS) < 100.0f) && !is_zero(_EAS2TAS)) {
// not enough change to require re-calculating
return _EAS2TAS;
}
float tempK = get_calibration_temperature() + 273.15f - 0.0065f * altitude;
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
_last_altitude_EAS2TAS = altitude;
return _EAS2TAS;
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = AP_HAL::millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure);
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (is_zero(sensors[i].ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset;
}
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
// return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate
float AP_Baro::get_EAS2TAS(void)
{
float altitude = get_altitude();
if ((fabsf(altitude - _last_altitude_EAS2TAS) < 25.0f) && !is_zero(_EAS2TAS)) {
// not enough change to require re-calculating
return _EAS2TAS;
}
// only estimate lapse rate for the difference from the ground location
// provides a more consistent reading then trying to estimate a complete
// ISA model atmosphere
float tempK = get_ground_temperature() + C_TO_KELVIN - ISA_LAPSE_RATE * altitude;
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK)));
_last_altitude_EAS2TAS = altitude;
return _EAS2TAS;
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = hal.scheduler->millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && sensors[i].pressure != 0;
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (sensors[i].ground_pressure == 0) {
sensors[i].ground_pressure = sensors[i].pressure;
}
sensors[i].altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(sensors[i].altitude) || isinf(sensors[i].altitude));
}
}
// choose primary sensor
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
// ensure the climb rate filter is updated
_climb_rate_filter.update(get_altitude(), get_last_update());
}
BootloaderHandleMessageResponse handle_message(const void *message, void *response) {
switch(tfp_get_fid_from_message(message)) {
case FID_GET_COORDINATES:
return get_coordinates(message, response);
case FID_GET_STATUS:
return get_status(message, response);
case FID_GET_ALTITUDE:
return get_altitude(message, response);
case FID_GET_MOTION:
return get_motion(message, response);
case FID_GET_DATE_TIME:
return get_date_time(message, response);
case FID_RESTART:
return restart(message);
case FID_SET_COORDINATES_CALLBACK_PERIOD:
return set_coordinates_callback_period(message);
case FID_GET_COORDINATES_CALLBACK_PERIOD:
return get_coordinates_callback_period(message, response);
case FID_SET_STATUS_CALLBACK_PERIOD:
return set_status_callback_period(message);
case FID_GET_STATUS_CALLBACK_PERIOD:
return get_status_callback_period(message, response);
case FID_SET_ALTITUDE_CALLBACK_PERIOD:
return set_altitude_callback_period(message);
case FID_GET_ALTITUDE_CALLBACK_PERIOD:
return get_altitude_callback_period(message, response);
case FID_SET_MOTION_CALLBACK_PERIOD:
return set_motion_callback_period(message);
case FID_GET_MOTION_CALLBACK_PERIOD:
return get_motion_callback_period(message, response);
case FID_SET_DATE_TIME_CALLBACK_PERIOD:
return set_date_time_callback_period(message);
case FID_GET_DATE_TIME_CALLBACK_PERIOD:
return get_date_time_callback_period(message, response);
default:
return HANDLE_MESSAGE_RESPONSE_NOT_SUPPORTED;
}
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (fabsf(_alt_offset - _alt_offset_active) > 0.01f) {
// If there's more than 1cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy.
_alt_offset_active = (0.95f*_alt_offset_active) + (0.05f*_alt_offset);
} else {
_alt_offset_active = _alt_offset;
}
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->backend_update(i);
}
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
float ground_pressure = sensors[i].ground_pressure;
if (!is_positive(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = sensors[i].altitude;
if (sensors[i].type == BARO_TYPE_AIR) {
float pressure = sensors[i].pressure + sensors[i].p_correction;
altitude = get_altitude_difference(sensors[i].ground_pressure, pressure);
} else if (sensors[i].type == BARO_TYPE_WATER) {
//101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
//No temperature or depth compensation for density of water.
altitude = (sensors[i].ground_pressure - sensors[i].pressure) / 9800.0f / _specific_gravity;
}
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset_active;
}
}
if (_hil.have_alt) {
sensors[0].altitude = _hil.altitude;
}
if (_hil.have_last_update) {
sensors[0].last_update_ms = _hil.last_update_ms;
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
int main()
{
#ifdef FULL_SIMU
initialize_simulation();
trajectory();
finish_simulation();
#else
signal(SIGINT, Emergency_exit);
pthread_t th_navdata;
pthread_t th_watchdog;
// pthread_t th_simu;
initialize_connection_with_drone();
pthread_create(&th_navdata, NULL, navdata_thread, NULL);
pthread_mutex_lock(&mutex_navdata_cond);
pthread_cond_wait(&navdata_initialised, &mutex_navdata_cond);
pthread_mutex_unlock(&mutex_navdata_cond);
pthread_create(&th_watchdog, NULL, watchdog_thread, NULL);
// pthread_create(&th_simu, NULL,simu_thread, NULL);
printf("It's on\n");
sleep(2);
take_off();
printf("First Altitude : %d\n", (int) get_altitude());
sleep(2);
calibrate_magnetometer();
//sleep(7);
// Indefinitely print magneto
/*while (1) {
print_navdata_magneto();
usleep(500000);
}*/
/*************************
* Normal behaviour *
*************************/
trajectory();
/*************************
* Tests *
*************************/
/*sleep(2) ;
printf("orientation\n") ;
printf("first heading : %f\n", get_heading()) ;
//forward(10, 2.0);
//forward_mag(100, 150, get_heading());
//sleep(2) ;
*/
//orientate_mag(100, get_heading() + 10.0);
//sleep(2) ;
land();
close_commands_socket();
#endif
return 0;
}
