From ca628ae5d402d0ae5889ace0cf11676380caef0a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 31 Jan 2016 15:30:12 +0100 Subject: [PATCH] [traffic_info] rename SetAcInfo to set_ac_info to follow naming conventions And add doxygen comments --- sw/airborne/modules/multi/formation.c | 2 +- sw/airborne/subsystems/datalink/datalink.c | 36 ++++++------- .../subsystems/navigation/traffic_info.c | 53 +++++++++---------- .../subsystems/navigation/traffic_info.h | 48 +++++++++++++---- 4 files changed, 80 insertions(+), 59 deletions(-) diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c index fdadf30d5b..fdcb7ec12e 100644 --- a/sw/airborne/modules/multi/formation.c +++ b/sw/airborne/modules/multi/formation.c @@ -145,7 +145,7 @@ int formation_flight(void) stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north; } // set info for this AC - SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir, + set_ac_info(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir, stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow); // broadcast info diff --git a/sw/airborne/subsystems/datalink/datalink.c b/sw/airborne/subsystems/datalink/datalink.c index 695399cd81..0646fcae2a 100644 --- a/sw/airborne/subsystems/datalink/datalink.c +++ b/sw/airborne/subsystems/datalink/datalink.c @@ -89,19 +89,19 @@ void dl_parse_msg(void) climb |= 0xFFFFF800; // fix for twos complements } - SetAcInfo(sender_id, - MOfCm(DL_GPS_SMALL_utm_east(dl_buffer)), /*m*/ - MOfCm(DL_GPS_SMALL_utm_north(dl_buffer)), /*m*/ - RadOfDeg(((float)course) / 10.), /*rad(CW)*/ - MOfCm(DL_GPS_SMALL_alt(dl_buffer)), /*m*/ - MOfCm(gspeed), /*m/s*/ - MOfCm(climb), /*m/s*/ - gps_tow_from_sys_ticks(sys_time.nb_tick)); + set_ac_info(sender_id, + MOfCm(DL_GPS_SMALL_utm_east(dl_buffer)), /*m*/ + MOfCm(DL_GPS_SMALL_utm_north(dl_buffer)), /*m*/ + RadOfDeg(((float)course) / 10.), /*rad(CW)*/ + MOfCm(DL_GPS_SMALL_alt(dl_buffer)), /*m*/ + MOfCm(gspeed), /*m/s*/ + MOfCm(climb), /*m/s*/ + gps_tow_from_sys_ticks(sys_time.nb_tick)); } break; case DL_GPS: { - SetAcInfo(sender_id, + set_ac_info(sender_id, MOfCm(DL_GPS_utm_east(dl_buffer)), /*m*/ MOfCm(DL_GPS_utm_north(dl_buffer)), /*m*/ RadOfDeg(((float)DL_GPS_course(dl_buffer)) / 10.), /*rad(CW)*/ @@ -112,14 +112,14 @@ void dl_parse_msg(void) } break; case DL_GPS_LLA: { - SetAcInfoLLA(sender_id, - DL_GPS_LLA_lat(dl_buffer), /*1e7deg*/ - DL_GPS_LLA_lon(dl_buffer), /*1e7deg*/ - DL_GPS_LLA_alt(dl_buffer), /*mm*/ - DL_GPS_LLA_course(dl_buffer), /*decideg*/ - DL_GPS_LLA_speed(dl_buffer), /*cm/s*/ - DL_GPS_LLA_climb(dl_buffer), /*cm/s*/ - DL_GPS_LLA_itow(dl_buffer)); /*ms*/ + set_ac_info_lla(sender_id, + DL_GPS_LLA_lat(dl_buffer), /*1e7deg*/ + DL_GPS_LLA_lon(dl_buffer), /*1e7deg*/ + DL_GPS_LLA_alt(dl_buffer), /*mm*/ + DL_GPS_LLA_course(dl_buffer), /*decideg*/ + DL_GPS_LLA_speed(dl_buffer), /*cm/s*/ + DL_GPS_LLA_climb(dl_buffer), /*cm/s*/ + DL_GPS_LLA_itow(dl_buffer)); /*ms*/ } break; #endif /* TRAFFIC_INFO */ @@ -250,7 +250,7 @@ void dl_parse_msg(void) float s = MOfCm(DL_ACINFO_speed(dl_buffer)); float cl = MOfCm(DL_ACINFO_climb(dl_buffer)); uint32_t t = DL_ACINFO_itow(dl_buffer); - SetAcInfo(id, ux, uy, c, a, s, cl, t); + set_ac_info(id, ux, uy, c, a, s, cl, t); } break; #endif diff --git a/sw/airborne/subsystems/navigation/traffic_info.c b/sw/airborne/subsystems/navigation/traffic_info.c index 5ffe7531e2..8bdd3b32ef 100644 --- a/sw/airborne/subsystems/navigation/traffic_info.c +++ b/sw/airborne/subsystems/navigation/traffic_info.c @@ -50,36 +50,31 @@ struct ac_info_ *get_ac_info(uint8_t _id) return &the_acs[the_acs_id[_id]]; } -// 0 is reserved for ground station (_id=0) -// 1 is reserved for this AC (_id=AC_ID) -void SetAcInfo(uint8_t _id, float _utm_x /*m*/, float _utm_y /*m*/, float _course/*rad(CW)*/, float _alt/*m*/, - float _gspeed/*m/s*/, float _climb, uint32_t _itow) +void set_ac_info(uint8_t id, float utm_east, float utm_north, float course, float alt, + float gspeed, float climb, uint32_t itow) { if (acs_idx < NB_ACS) { - if (_id > 0 && the_acs_id[_id] == 0) { - the_acs_id[_id] = acs_idx++; - the_acs[the_acs_id[_id]].ac_id = _id; + if (id > 0 && the_acs_id[id] == 0) { + the_acs_id[id] = acs_idx++; + the_acs[the_acs_id[id]].ac_id = id; } - the_acs[the_acs_id[_id]].east = _utm_x;// - nav_utm_east0; - the_acs[the_acs_id[_id]].north = _utm_y;// - nav_utm_north0; - the_acs[the_acs_id[_id]].course = _course; - the_acs[the_acs_id[_id]].alt = _alt;// +- NAV_MSL0; - the_acs[the_acs_id[_id]].gspeed = _gspeed; - the_acs[the_acs_id[_id]].climb = _climb; - the_acs[the_acs_id[_id]].itow = _itow; + the_acs[the_acs_id[id]].east = utm_east;// - nav_utm_east0; + the_acs[the_acs_id[id]].north = utm_north;// - nav_utm_north0; + the_acs[the_acs_id[id]].course = course; + the_acs[the_acs_id[id]].alt = alt;// +- NAV_MSL0; + the_acs[the_acs_id[id]].gspeed = gspeed; + the_acs[the_acs_id[id]].climb = climb; + the_acs[the_acs_id[id]].itow = itow; } } -// 0 is reserved for ground station (_id=0) -// 1 is reserved for this AC (_id=AC_ID) -void SetAcInfoLLA(uint8_t _id, int32_t lat/*1e7deg*/, int32_t lon/*1e7deg*/, int32_t alt/*mm*/, - int16_t course/*decideg*/, uint16_t gspeed/*cm/s*/, int16_t climb/*cm/s*/, - uint32_t itow/*ms*/) +void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt, + int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow) { if (acs_idx < NB_ACS) { - if (_id > 0 && the_acs_id[_id] == 0) { - the_acs_id[_id] = acs_idx++; - the_acs[the_acs_id[_id]].ac_id = _id; + if (id > 0 && the_acs_id[id] == 0) { + the_acs_id[id] = acs_idx++; + the_acs[the_acs_id[id]].ac_id = id; } struct LlaCoor_i lla_i = {.lat = lat, .lon = lon, .alt = alt}; @@ -89,12 +84,12 @@ void SetAcInfoLLA(uint8_t _id, int32_t lat/*1e7deg*/, int32_t lon/*1e7deg*/, int struct UtmCoor_f utm_f; utm_of_lla_f(&utm_f, &lla_f); - the_acs[the_acs_id[_id]].east = utm_f.east; - the_acs[the_acs_id[_id]].north = utm_f.north; - the_acs[the_acs_id[_id]].alt = utm_f.alt; - the_acs[the_acs_id[_id]].course = RadOfDeg((float)course / 10.); - the_acs[the_acs_id[_id]].gspeed = (float)gspeed * 100; - the_acs[the_acs_id[_id]].climb = (float)climb * 100; - the_acs[the_acs_id[_id]].itow = itow; + the_acs[the_acs_id[id]].east = utm_f.east; + the_acs[the_acs_id[id]].north = utm_f.north; + the_acs[the_acs_id[id]].alt = utm_f.alt; + the_acs[the_acs_id[id]].course = RadOfDeg((float)course / 10.); + the_acs[the_acs_id[id]].gspeed = (float)gspeed * 100; + the_acs[the_acs_id[id]].climb = (float)climb * 100; + the_acs[the_acs_id[id]].itow = itow; } } diff --git a/sw/airborne/subsystems/navigation/traffic_info.h b/sw/airborne/subsystems/navigation/traffic_info.h index ca64513703..78ca403b73 100644 --- a/sw/airborne/subsystems/navigation/traffic_info.h +++ b/sw/airborne/subsystems/navigation/traffic_info.h @@ -36,13 +36,13 @@ struct ac_info_ { uint8_t ac_id; - float east; /* m relative to nav_utm_east0 */ - float north; /* m relative to nav_utm_north0 */ - float course; /* rad (CW) */ - float alt; /* m */ - float gspeed; /* m/s */ - float climb; /* m/s */ - uint32_t itow;/* ms */ + float east; ///< m relative to nav_utm_east0 + float north; ///< m relative to nav_utm_north0 + float course; ///< rad (CW) + float alt; ///< m + float gspeed; ///< m/s + float climb; ///< m/s + uint32_t itow;///< ms }; extern uint8_t acs_idx; @@ -50,11 +50,37 @@ extern uint8_t the_acs_id[NB_ACS_ID]; extern struct ac_info_ the_acs[NB_ACS]; extern void traffic_info_init(void); -struct ac_info_ *get_ac_info(uint8_t id); +extern struct ac_info_ *get_ac_info(uint8_t id); -void SetAcInfo(uint8_t _id, float _utm_x /*m*/, float _utm_y /*m*/, float _course/*rad(CW)*/, float _alt/*m*/, - float _gspeed/*m/s*/, float _climb, uint32_t _itow/*ms*/); -void SetAcInfoLLA(uint8_t _id, int32_t lat/*1e7deg*/, int32_t lon/*1e7deg*/, int32_t alt/*mm*/, +/** + * Set Aircraft info. + * @param[in] id aircraft id, 0 is reserved for GCS, 1 for this aircraft (id=AC_ID) + * @param[in] utm_east UTM east in m relative to nav_utm_east0 + * @param[in] utm_north UTM north in m relative to nav_utm_north0 + * @param[in] course Course in rad (CW) + * @param[in] alt Altitude in m above MSL + * @param[in] gspeed Ground speed in m/s + * @param[in] climb Climb rate in m/s + * @param[in] itow GPS time of week in ms + */ +extern void set_ac_info(uint8_t id, float utm_east, float utm_north, float course, float alt, + float gspeed, float climb, uint32_t itow); + +/** + * Set Aircraft info. + * @param[in] id aircraft id, 0 is reserved for GCS, 1 for this aircraft (id=AC_ID) + * @param[in] lat Latitude in 1e7deg + * @param[in] lon Longitude in 1e7deg + * @param[in] alt Altitude in mm above MSL + * @param[in] course Course in decideg (CW) + * @param[in] gspeed Ground speed in cm/s + * @param[in] climb Climb rate in cm/s + * @param[in] itow GPS time of week in ms + */ +extern void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt, + int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow); + +void set_ac_info_lla(uint8_t _id, int32_t lat/*1e7deg*/, int32_t lon/*1e7deg*/, int32_t alt/*mm*/, int16_t course/*decideg*/, uint16_t gspeed/*cm/s*/, int16_t climb/*cm/s*/, uint32_t itow/*ms*/);