mirror of https://github.com/ArduPilot/ardupilot
AP_Bootloader: use IP address from periph if available
This commit is contained in:
parent
82dea9c037
commit
212bde0b61
Tools/AP_Bootloader
|
@ -97,6 +97,10 @@ int main(void)
|
|||
stm32_flash_unprotect_flash();
|
||||
#endif
|
||||
|
||||
#if AP_BOOTLOADER_NETWORK_ENABLED
|
||||
network.save_comms_ip();
|
||||
#endif
|
||||
|
||||
#if AP_FASTBOOT_ENABLED
|
||||
enum rtc_boot_magic m = check_fast_reboot();
|
||||
bool was_watchdog = stm32_was_watchdog_reset();
|
||||
|
|
|
@ -10,7 +10,10 @@
|
|||
|
||||
struct app_bootloader_comms {
|
||||
uint32_t magic;
|
||||
uint32_t reserved[4];
|
||||
uint32_t ip;
|
||||
uint32_t netmask;
|
||||
uint32_t gateway;
|
||||
uint32_t reserved;
|
||||
uint8_t server_node_id;
|
||||
uint8_t my_node_id;
|
||||
uint8_t path[201];
|
||||
|
|
|
@ -725,9 +725,8 @@ bool can_check_update(void)
|
|||
fw_update.reads[i].offset = i*sizeof(uavcan_protocol_file_ReadResponse::data.data);
|
||||
}
|
||||
memcpy(fw_update.path, comms->path, sizeof(uavcan_protocol_file_Path::path.data)+1);
|
||||
memset(comms, 0, sizeof(*comms));
|
||||
ret = true;
|
||||
// clear comms region
|
||||
memset(comms, 0, sizeof(struct app_bootloader_comms));
|
||||
}
|
||||
#endif
|
||||
#if defined(CAN1_BASE) && defined(RCC_APB1ENR_CAN1EN)
|
||||
|
|
|
@ -24,9 +24,22 @@
|
|||
#include "support.h"
|
||||
#include "bl_protocol.h"
|
||||
#include <AP_CheckFirmware/AP_CheckFirmware.h>
|
||||
#include "app_comms.h"
|
||||
|
||||
#ifndef AP_NETWORKING_DEFAULT_MAC_ADDR
|
||||
#define AP_NETWORKING_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46"
|
||||
#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR
|
||||
#define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46"
|
||||
#endif
|
||||
|
||||
#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_IP
|
||||
#define AP_NETWORKING_BOOTLOADER_DEFAULT_IP "192.168.1.100"
|
||||
#endif
|
||||
|
||||
#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY
|
||||
#define AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY "192.168.1.1"
|
||||
#endif
|
||||
|
||||
#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK
|
||||
#define AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK "255.255.255.0"
|
||||
#endif
|
||||
|
||||
#ifndef AP_BOOTLOADER_NETWORK_USE_DHCP
|
||||
|
@ -186,21 +199,27 @@ void BL_Network::net_thread()
|
|||
|
||||
thread_sleep_ms(100);
|
||||
|
||||
AP_Networking::convert_str_to_macaddr(AP_NETWORKING_DEFAULT_MAC_ADDR, thisif->hwaddr);
|
||||
AP_Networking::convert_str_to_macaddr(AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR, thisif->hwaddr);
|
||||
|
||||
struct {
|
||||
ip4_addr_t ip, gateway, netmask;
|
||||
} addr {};
|
||||
|
||||
addr.ip.addr = htonl(SocketAPM::inet_str_to_addr("192.168.2.201"));
|
||||
addr.gateway.addr = htonl(SocketAPM::inet_str_to_addr("192.168.2.10"));
|
||||
addr.netmask.addr = htonl(SocketAPM::inet_str_to_addr("255.255.255.0"));
|
||||
ip4_addr_t ip, gateway, netmask;
|
||||
if (addr.ip == 0) {
|
||||
// no IP from AP_Periph, use defaults
|
||||
ip.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_IP));
|
||||
gateway.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY));
|
||||
netmask.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK));
|
||||
} else {
|
||||
// use addresses from AP_Periph
|
||||
ip.addr = htonl(addr.ip);
|
||||
netmask.addr = htonl(addr.netmask);
|
||||
gateway.addr = htonl(addr.gateway);
|
||||
}
|
||||
|
||||
const MACConfig mac_config = {thisif->hwaddr};
|
||||
macStart(ÐD1, &mac_config);
|
||||
|
||||
/* Add interface. */
|
||||
auto result = netifapi_netif_add(thisif, &addr.ip, &addr.netmask, &addr.gateway, NULL, ethernetif_init, tcpip_input);
|
||||
|
||||
auto result = netifapi_netif_add(thisif, &ip, &netmask, &gateway, NULL, ethernetif_init, tcpip_input);
|
||||
if (result != ERR_OK) {
|
||||
AP_HAL::panic("Failed to initialise netif");
|
||||
}
|
||||
|
@ -505,5 +524,18 @@ void BL_Network::init()
|
|||
this);
|
||||
}
|
||||
|
||||
/*
|
||||
save IP address from AP_Periph
|
||||
*/
|
||||
void BL_Network::save_comms_ip(void)
|
||||
{
|
||||
struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START;
|
||||
if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC && comms->ip != 0) {
|
||||
addr.ip = comms->ip;
|
||||
addr.netmask = comms->netmask;
|
||||
addr.gateway = comms->gateway;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // AP_BOOTLOADER_NETWORK_ENABLED
|
||||
|
||||
|
|
|
@ -13,6 +13,8 @@ class SocketAPM;
|
|||
class BL_Network {
|
||||
public:
|
||||
void init(void);
|
||||
void save_comms_ip(void);
|
||||
|
||||
private:
|
||||
struct netif *thisif;
|
||||
thread_t *net_thread_ctx;
|
||||
|
@ -37,6 +39,10 @@ private:
|
|||
const char *name;
|
||||
const char *value;
|
||||
} variables[];
|
||||
|
||||
struct {
|
||||
uint32_t ip, gateway, netmask;
|
||||
} addr;
|
||||
};
|
||||
|
||||
#endif // AP_BOOTLOADER_NETWORK_ENABLED
|
||||
|
|
|
@ -150,6 +150,9 @@ bool flash_func_is_erased(uint32_t sector)
|
|||
|
||||
bool flash_func_erase_sector(uint32_t sector, bool force_erase)
|
||||
{
|
||||
if (sector >= num_pages) {
|
||||
return false;
|
||||
}
|
||||
#if AP_BOOTLOADER_ALWAYS_ERASE
|
||||
return stm32_flash_erasepage(flash_base_page+sector);
|
||||
#else
|
||||
|
|
Loading…
Reference in New Issue